Your Name
99 lines
2.1 KiB
C
99 lines
2.1 KiB
C
/*
|
|
|
|
(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;
|
|
} |