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

Centralized TX messages to command handler to support multiple outputs

Browse Source
This commit is contained in:
Your Name
2026-10-04 16:44:15 +03:00
parent 5d511f3aca
commit 36f406e69d
8 changed files with 99 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
servo_controller/src/command_handler.c
+47 -5
View File
@@ -11,7 +11,26 @@
LOG_MODULE_REGISTER(command_handler, LOG_LEVEL_INF);
int command_handler(struct command_message_t *msg) {
// BUFFER (add ack and nack at the and as static)
#define CMD_MSG_BUFFER_SIZE 10
struct command_message_t cmd_msg_buffer[CMD_MSG_BUFFER_SIZE + 2];
struct command_message_t *cmd_msg_buffer_ptr;
int command_handler_init() {
// BUFFER
memset(cmd_msg_buffer, 0, sizeof(cmd_msg_buffer));
cmd_msg_buffer_ptr = cmd_msg_buffer;
// CREATE ACK/NACK
command_create_ack(&cmd_msg_buffer[CMD_MSG_BUFFER_SIZE]);
command_create_nack(&cmd_msg_buffer[CMD_MSG_BUFFER_SIZE + 1]);
return 0;
}
int command_handler_rx(struct command_message_t *msg) {
if (msg == NULL) {
LOG_ERR("Received NULL message pointer");
return -EINVAL;
@@ -71,9 +90,9 @@ int command_handler(struct command_message_t *msg) {
int channel = msg->data[0];
int value = adc_read_id(channel);
struct command_message_t *reply = usb_get_next_tx_buf();
struct command_message_t *reply = cmd_get_next_tx_buf();
command_create_message(reply, sizeof(value), msg->command, (uint8_t *)&value);
usb_send_command(reply);
command_handler_tx(reply);
break;
}
@@ -83,9 +102,9 @@ int command_handler(struct command_message_t *msg) {
values[i] = adc_read_id(i);
}
struct command_message_t *reply = usb_get_next_tx_buf();
struct command_message_t *reply = cmd_get_next_tx_buf();
command_create_message(reply, sizeof(values), msg->command, (uint8_t *)values);
usb_send_command(reply);
command_handler_tx(reply);
break;
}
@@ -139,5 +158,28 @@ int command_handler(struct command_message_t *msg) {
}
}
return 0;
}
struct command_message_t* cmd_get_next_tx_buf() {
struct command_message_t *buf = cmd_msg_buffer_ptr;
// Increment the buffer pointer
cmd_msg_buffer_ptr++;
if (cmd_msg_buffer_ptr > &cmd_msg_buffer[CMD_MSG_BUFFER_SIZE-1]) {
cmd_msg_buffer_ptr = cmd_msg_buffer;
}
return buf;
}
int command_handler_tx(struct command_message_t *msg) {
// Send over USB if enabled and ready
if (usb_is_tx_enabled()) {
usb_send_command(msg);
}
return 0;
}