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

Added robot move commands its own message buffer

Browse Source
This commit is contained in:
Your Name
2026-10-06 13:36:36 +03:00
parent bb1e17b27a
commit 7a7e7d9c68
9 changed files with 296 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pins.drawio
+112 -6
View File
File diff suppressed because one or more lines are too long
servo_controller/scripts/send_cmd.py
+4
View File
@@ -48,6 +48,9 @@ COMMANDS = {
"DIGITAL_LEG_OUT": 12,
"POWER_MONITOR_SET_READ": 13,
"POWER_MONITOR_READ": 14,
"MOVE_COMMAND_START": 15,
"TEST": 16,
"MOVE_COMMAND_END": 17,
}
COMMAND_NAMES = {v: k for k, v in COMMANDS.items()}
@@ -60,6 +63,7 @@ COMMANDS_DATA_OPTIONAL = {
"LED_TOGGLE",
"ADC_READ_RAW_ALL",
"ADC_READ_ALL",
"TEST",
}
# struct formats for typed data tokens (all little-endian)
servo_controller/src/command_handler.c
+28 -14
View File
@@ -12,20 +12,21 @@
LOG_MODULE_REGISTER(command_handler, LOG_LEVEL_INF);
// 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;
// BUFFER
#define CMD_TX_MSG_BUFFER_SIZE 10
struct command_message_t cmd_tx_msg_buffer[CMD_TX_MSG_BUFFER_SIZE];
struct command_message_t *cmd_tx_msg_buffer_ptr;
#define CMD_RX_MSG_BUFFER_SIZE 10
struct command_message_t cmd_rx_msg_buffer[CMD_RX_MSG_BUFFER_SIZE];
struct command_message_t *cmd_rx_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]);
memset(cmd_tx_msg_buffer, 0, sizeof(cmd_tx_msg_buffer));
cmd_tx_msg_buffer_ptr = cmd_tx_msg_buffer;
memset(cmd_rx_msg_buffer, 0, sizeof(cmd_rx_msg_buffer));
cmd_rx_msg_buffer_ptr = cmd_rx_msg_buffer;
return 0;
}
@@ -162,13 +163,26 @@ int command_handler_rx(struct command_message_t *msg) {
}
struct command_message_t* cmd_get_next_tx_buf() {
struct command_message_t *buf = cmd_msg_buffer_ptr;
struct command_message_t *buf = cmd_tx_msg_buffer_ptr;
// Increment the buffer pointer
cmd_msg_buffer_ptr++;
cmd_tx_msg_buffer_ptr++;
if (cmd_msg_buffer_ptr > &cmd_msg_buffer[CMD_MSG_BUFFER_SIZE-1]) {
cmd_msg_buffer_ptr = cmd_msg_buffer;
if (cmd_tx_msg_buffer_ptr > &cmd_tx_msg_buffer[CMD_TX_MSG_BUFFER_SIZE-1]) {
cmd_tx_msg_buffer_ptr = cmd_tx_msg_buffer;
}
return buf;
}
struct command_message_t* cmd_get_next_rx_buf() {
struct command_message_t *buf = cmd_rx_msg_buffer_ptr;
// Increment the buffer pointer
cmd_rx_msg_buffer_ptr++;
if (cmd_rx_msg_buffer_ptr > &cmd_rx_msg_buffer[CMD_RX_MSG_BUFFER_SIZE-1]) {
cmd_rx_msg_buffer_ptr = cmd_rx_msg_buffer;
}
return buf;
servo_controller/src/command_handler.h
+1
View File
@@ -7,6 +7,7 @@
int command_handler_init();
int command_handler_rx(struct command_message_t *msg);
struct command_message_t* cmd_get_next_tx_buf();
struct command_message_t* cmd_get_next_rx_buf();
int command_handler_tx(struct command_message_t *msg);
servo_controller/src/command_message.h
+6 -2
View File
@@ -27,8 +27,12 @@ typedef enum {
POWER_MONITOR_SET_READ,
POWER_MONITOR_READ,
// Keep last
NUM_COMMANDS,
MOVE_COMMAND_START,
// All the commands going to robot thread go here
TEST_COMMAND, // 16
MOVE_COMMAND_END
} commands_e;
struct command_message_t {
servo_controller/src/main.c
+8
View File
@@ -6,6 +6,7 @@
#include "digital_out.h"
#include "power_monitor.h"
#include "command_handler.h"
#include "robot.h"
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
@@ -71,5 +72,12 @@ int main(void) {
return 0;
}
// ROBOT init
ret = robot_init();
if (ret != 0) {
LOG_ERR("Failed to enable ROBOT");
return 0;
}
return 0;
}
servo_controller/src/robot.c
+99
View File
@@ -0,0 +1,99 @@
/*
(1) (6)
\ /
\ /
\ /
*---(F)---*
/ \
/ \
/ \
(2)---* o *---(5)
\ /
\ /
\ /
*---(B)---*
/ \
/ \
/ \
(3) (4)
*/
#include "robot.h"
#include "led.h"
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(robot, LOG_LEVEL_INF);
// THREAD
static struct k_thread robot_thread_data;
static k_tid_t robot_thread_id = NULL;
#define ROBOT_THREAD_STACK_SIZE 2048
#define ROBOT_THREAD_PRIORITY 5
K_THREAD_STACK_DEFINE(robot_thread_stack, ROBOT_THREAD_STACK_SIZE);
// BUFFER
#define ROBOT_MSG_BUFFER_SIZE 10
struct command_message_t robot_msg_buffer[ROBOT_MSG_BUFFER_SIZE + 2];
struct command_message_t *robot_msg_buffer_ptr;
char robot_msgq_buffer[ROBOT_MSG_BUFFER_SIZE * sizeof(uint32_t)];
struct k_msgq robot_msgq;
static void robot_thread(void *p1, void *p2, void *p3) {
ARG_UNUSED(p1);
ARG_UNUSED(p2);
ARG_UNUSED(p3);
struct command_message_t *data;
while (1) {
k_msgq_get(&robot_msgq, &data, K_FOREVER);
// TODO: handle commands
led1_toggle();
}
}
int robot_init() {
// BUFFER
memset(robot_msg_buffer, 0, sizeof(robot_msg_buffer));
robot_msg_buffer_ptr = robot_msg_buffer;
k_msgq_init(&robot_msgq, robot_msgq_buffer, sizeof(struct command_message_t *), ROBOT_MSG_BUFFER_SIZE);
// THREAD
robot_thread_id = k_thread_create(
&robot_thread_data,
robot_thread_stack,
K_THREAD_STACK_SIZEOF(robot_thread_stack),
robot_thread,
NULL, NULL, NULL,
ROBOT_THREAD_PRIORITY,
0,
K_NO_WAIT
);
return 0;
}
struct command_message_t* robot_get_next_tx_buf() {
struct command_message_t *buf = robot_msg_buffer_ptr;
// Increment the buffer pointer
robot_msg_buffer_ptr++;
if (robot_msg_buffer_ptr > &robot_msg_buffer[ROBOT_MSG_BUFFER_SIZE-1]) {
robot_msg_buffer_ptr = robot_msg_buffer;
}
return buf;
}
int robot_notify_msg(struct command_message_t* msg) {
k_msgq_put(&robot_msgq, &msg, K_NO_WAIT);
return 0;
}
servo_controller/src/robot.h
+13
View File
@@ -0,0 +1,13 @@
#ifndef ROBOT_H
#define ROBOT_H
#include "command_message.h"
int robot_init();
struct command_message_t* robot_get_next_tx_buf();
int robot_notify_msg(struct command_message_t* msg);
#endif // ROBOT_H
servo_controller/src/usb.c
+25 -10
View File
@@ -1,6 +1,7 @@
#include "usb.h"
#include "usb_conf.h"
#include "command_handler.h"
#include "robot.h"
#include <zephyr/logging/log.h>
#include <zephyr/device.h>
@@ -90,8 +91,7 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) {
ARG_UNUSED(p1);
ARG_UNUSED(p2);
ARG_UNUSED(p3);
struct command_message_t msg;
command_message_init(&msg);
struct command_message_t *msg;
LOG_INF("USB command processing thread started");
@@ -110,17 +110,26 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) {
len = ring_buf_get(&ringbuf, buf_header, (COMMAND_HEADER_SIZE - 1));
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];
uint8_t is_move = 0;
if (buf_header[2] > MOVE_COMMAND_START && buf_header[2] < MOVE_COMMAND_END) {
msg = robot_get_next_tx_buf();
is_move = 1;
}
else {
msg = cmd_get_next_rx_buf();
}
command_message_init(msg);
msg->length = buf_header[0];
msg->command = buf_header[2];
// Ignore the tick for now
msg.crc = buf_header[COMMAND_HEADER_SIZE - 2];
msg->crc = buf_header[COMMAND_HEADER_SIZE - 2];
if (msg.length) {
len = ring_buf_get(&ringbuf, msg.data, msg.length);
if (msg->length) {
len = ring_buf_get(&ringbuf, msg->data, msg->length);
}
uint8_t calculated_crc = command_calculate_crc(&msg);
if (calculated_crc != msg.crc) {
uint8_t calculated_crc = command_calculate_crc(msg);
if (calculated_crc != msg->crc) {
if (RETURN_ACK) {
// Send NACK
nack.tick = k_uptime_get();
@@ -130,7 +139,13 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) {
continue;
}
int ret = command_handler_rx(&msg);
int ret;
if (is_move) {
ret = robot_notify_msg(msg);
}
else {
ret = command_handler_rx(msg);
}
if (ret == 0) {
if (RETURN_ACK) {
// Send ACK