aintgit/hexdec
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added timestamp in milliseconds to messages

Browse Source
This commit is contained in:
Your Name
2026-09-29 15:00:06 +03:00
parent 92271ef376
commit 12f05d8f10
4 changed files with 260 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
servo_controller/src/command_message.c
+4 -1
View File
@@ -2,6 +2,7 @@
#include <string.h>
#include <zephyr/logging/log.h>
#include <zephyr/kernel.h>
LOG_MODULE_REGISTER(command_message, LOG_LEVEL_INF);
@@ -9,6 +10,7 @@ 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;
msg->tick = 0;
}
void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[]) {
@@ -20,6 +22,7 @@ void command_create_message(struct command_message_t *msg, uint8_t length, comma
command_message_init(msg);
msg->length = length;
msg->command = command;
msg->tick = k_uptime_get();
// Copy the data
if (data != NULL) {
@@ -38,7 +41,7 @@ uint8_t command_calculate_crc(struct command_message_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; }
if (i == (COMMAND_HEADER_SIZE - 1)) { continue; }
sum += byte_ptr[i];
}
servo_controller/src/command_message.h
+2 -1
View File
@@ -8,7 +8,7 @@
#define COMMAND_PREFIX 0x69
#define COMMAND_ID 0x00
#define COMMAND_DATA_SIZE 160
#define COMMAND_HEADER_SIZE 5
#define COMMAND_HEADER_SIZE 13
typedef enum {
COMMAND_ACK,
@@ -32,6 +32,7 @@ struct command_message_t {
uint8_t length;
uint8_t id;
uint8_t command;
int64_t tick;
uint8_t crc;
uint8_t data[COMMAND_DATA_SIZE];
} __attribute__((packed));
servo_controller/src/usb.c
+11 -4
View File
@@ -104,13 +104,14 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) {
len = ring_buf_get(&ringbuf, &buf_prefix, 1);
if (len && (buf_prefix == COMMAND_PREFIX)) {
uint8_t buf_header[4];
len = ring_buf_get(&ringbuf, buf_header, 4);
uint8_t buf_header[COMMAND_HEADER_SIZE];
len = ring_buf_get(&ringbuf, buf_header, (COMMAND_HEADER_SIZE - 1));
if ((len == 4) && (buf_header[1] == COMMAND_ID) && (buf_header[0] <= COMMAND_DATA_SIZE)) {
if ((len == (COMMAND_HEADER_SIZE - 1)) && (buf_header[1] == COMMAND_ID) && (buf_header[0] <= COMMAND_DATA_SIZE)) {
msg.length = buf_header[0];
msg.command = buf_header[2];
msg.crc = buf_header[3];
// Ignore the tick for now
msg.crc = buf_header[COMMAND_HEADER_SIZE - 2];
if (msg.length) {
len = ring_buf_get(&ringbuf, msg.data, msg.length);
@@ -120,6 +121,8 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) {
if (calculated_crc != msg.crc) {
if (RETURN_ACK) {
// Send NACK
usb_tx_buffer[TX_BUFFER_SIZE + 1].tick = k_uptime_get();
usb_tx_buffer[TX_BUFFER_SIZE + 1].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
}
continue;
@@ -129,12 +132,16 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) {
if (ret == 0) {
if (RETURN_ACK) {
// Send ACK
usb_tx_buffer[TX_BUFFER_SIZE].tick = k_uptime_get();
usb_tx_buffer[TX_BUFFER_SIZE].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE]);
usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE]);
}
}
else {
if (RETURN_ACK) {
// Send NACK
usb_tx_buffer[TX_BUFFER_SIZE + 1].tick = k_uptime_get();
usb_tx_buffer[TX_BUFFER_SIZE + 1].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
}
}