Added robot move commands its own message buffer

servo_controller/scripts/send_cmd.py

@@ -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

@@ -12,20 +12,21 @@
 LOG_MODULE_REGISTER(command_handler, LOG_LEVEL_INF);
 
 
-// BUFFER (add ack and nack at the and as static)
+// BUFFER
-#define CMD_MSG_BUFFER_SIZE 10
+#define CMD_TX_MSG_BUFFER_SIZE 10
-struct command_message_t cmd_msg_buffer[CMD_MSG_BUFFER_SIZE + 2];
+struct command_message_t cmd_tx_msg_buffer[CMD_TX_MSG_BUFFER_SIZE];
-struct command_message_t *cmd_msg_buffer_ptr;
+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));
+	memset(cmd_tx_msg_buffer, 0, sizeof(cmd_tx_msg_buffer));
-	cmd_msg_buffer_ptr = cmd_msg_buffer;
+	cmd_tx_msg_buffer_ptr = cmd_tx_msg_buffer;
-
+	memset(cmd_rx_msg_buffer, 0, sizeof(cmd_rx_msg_buffer));
-	// CREATE ACK/NACK
+	cmd_rx_msg_buffer_ptr = cmd_rx_msg_buffer;
-	command_create_ack(&cmd_msg_buffer[CMD_MSG_BUFFER_SIZE]);
-	command_create_nack(&cmd_msg_buffer[CMD_MSG_BUFFER_SIZE + 1]);
 
 	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]) {
+	if (cmd_tx_msg_buffer_ptr > &cmd_tx_msg_buffer[CMD_TX_MSG_BUFFER_SIZE-1]) {
-		cmd_msg_buffer_ptr = cmd_msg_buffer;
+		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

@@ -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

@@ -27,8 +27,12 @@ typedef enum {
 	POWER_MONITOR_SET_READ,
 	POWER_MONITOR_READ,
 
-	// Keep last
+	MOVE_COMMAND_START,
-	NUM_COMMANDS,
+	// 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

@@ -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

@@ -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

@@ -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

@@ -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;
+	struct command_message_t *msg;
-	command_message_init(&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];
+					uint8_t is_move = 0;
-					msg.command = buf_header[2];
+					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) {
+					if (msg->length) {
-						len = ring_buf_get(&ringbuf, msg.data, msg.length);
+						len = ring_buf_get(&ringbuf, msg->data, msg->length);
 					}
 
-					uint8_t calculated_crc = command_calculate_crc(&msg);
+					uint8_t calculated_crc = command_calculate_crc(msg);
-					if (calculated_crc != msg.crc) {
+					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