Your Name
75 lines
1.8 KiB
C
75 lines
1.8 KiB
C
#include "command_message.h"
|
|
|
|
#include <string.h>
|
|
#include <zephyr/logging/log.h>
|
|
|
|
LOG_MODULE_REGISTER(command_message, LOG_LEVEL_INF);
|
|
|
|
void command_message_init(struct command_message_t *msg) {
|
|
memset(msg, 0, sizeof(struct command_message_t));
|
|
msg->prefix = COMMAND_PREFIX;
|
|
msg->id = COMMAND_ID;
|
|
}
|
|
|
|
void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[160]) {
|
|
// Ensure length doesn't exceed available space
|
|
if (length > sizeof(msg->data) - 1) {
|
|
return;
|
|
}
|
|
|
|
command_message_init(msg);
|
|
msg->length = length;
|
|
msg->command = command;
|
|
|
|
// Copy the data
|
|
if (data != NULL) {
|
|
for (int i = 0; i < msg->length; i++) {
|
|
msg->data[i] = data[i];
|
|
}
|
|
}
|
|
msg->crc = command_calculate_crc(msg);
|
|
}
|
|
|
|
uint8_t command_calculate_crc(struct command_message_t *msg) {
|
|
uint32_t sum = 0;
|
|
uint8_t crc = 0;
|
|
|
|
uint8_t *byte_ptr = (uint8_t *)msg;
|
|
int loop_length = (sizeof(struct command_message_t) - sizeof(msg->data) + msg->length);
|
|
|
|
for (int i = 0; i < loop_length; i++) {
|
|
if (i == 4) { continue; }
|
|
sum += byte_ptr[i];
|
|
}
|
|
|
|
crc = 0x100 - (sum & 0xff);
|
|
|
|
return crc;
|
|
}
|
|
|
|
void command_create_ack(struct command_message_t *msg) {
|
|
command_create_message(msg, 0, COMMAND_ACK, NULL);
|
|
}
|
|
|
|
void command_create_nack(struct command_message_t *msg) {
|
|
command_create_message(msg, 0, COMMAND_NACK, NULL);
|
|
}
|
|
|
|
void command_log(struct command_message_t *msg) {
|
|
if (msg->length > sizeof(msg->data) - 1) {
|
|
LOG_ERR("Message length too long: %d", msg->length);
|
|
return;
|
|
}
|
|
|
|
LOG_INF("Prefix: %d\n\r", msg->prefix);
|
|
LOG_INF("Length: %d\n\r", msg->length);
|
|
LOG_INF("COMMAND_ID: %d\n\r", msg->id);
|
|
LOG_INF("Command: %d\n\r", msg->command);
|
|
LOG_INF("Data:\n\r");
|
|
|
|
for (int i = 0; i < msg->length; i++) {
|
|
LOG_INF("%d", msg->data[i]);
|
|
}
|
|
|
|
LOG_INF("CRC: %d\n\r", msg->crc);
|
|
} |