bbe8bd06bc
Separate command handlers into dedicated static functions registered with cmd_parser, replacing the strcmp/strncmp chain. Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
263 lines
8.8 KiB
C
263 lines
8.8 KiB
C
#include "app.h"
|
||
#include "cmd_parser.h"
|
||
#include "cmsis_os2.h"
|
||
#include <string.h>
|
||
#include <stdlib.h>
|
||
#include "FreeRTOS.h"
|
||
#include "queue.h"
|
||
#include "stm32f4xx_hal.h"
|
||
|
||
typedef enum {
|
||
USER_CMD_IDLE = 0,
|
||
USER_CMD_RECEIVE,
|
||
} user_cmd_state;
|
||
|
||
static struct {
|
||
user_cmd_state state;
|
||
char buffer[MAX_CMD_LEN];
|
||
uint8_t index;
|
||
osMessageQueueId_t cmd_queue;
|
||
} g_cmd_parser;
|
||
|
||
/*
|
||
* 支持的命令（格式：@command\n）:
|
||
* "motor enable" — 使能全部电机（零速启动）
|
||
* "motor enable yaw" — 使能 Yaw 电机
|
||
* "motor enable pitch" — 使能 Pitch 电机
|
||
* "motor disable" — 关闭全部电机（惰行 coast）
|
||
* "motor disable yaw" — 关闭 Yaw 电机
|
||
* "motor disable pitch" — 关闭 Pitch 电机
|
||
* "sensor enable" — 恢复 IMU 读取
|
||
* "sensor disable" — 暂停 IMU 读取
|
||
* "sensorcali acc enable" — 开始加速度计标定
|
||
* "sensorcali acc disable" — 停止加速度计标定
|
||
* "sensorcali mag enable" — 开始磁场标定
|
||
* "sensorcali mag disable" — 停止磁场标定
|
||
*/
|
||
|
||
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
|
||
{
|
||
if (huart->Instance != USART1) return;
|
||
|
||
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
||
uint8_t rx = g_uart_rx_byte;
|
||
|
||
switch (g_cmd_parser.state) {
|
||
case USER_CMD_IDLE:
|
||
if (rx == '@') {
|
||
g_cmd_parser.state = USER_CMD_RECEIVE;
|
||
g_cmd_parser.index = 0;
|
||
}
|
||
break;
|
||
|
||
case USER_CMD_RECEIVE:
|
||
if (rx == '\n') {
|
||
/* 命令结束 —— null-terminate 并入队 */
|
||
if (g_cmd_parser.cmd_queue != NULL) {
|
||
g_cmd_parser.buffer[g_cmd_parser.index] = '\0';
|
||
xQueueSendFromISR(g_cmd_parser.cmd_queue,
|
||
g_cmd_parser.buffer,
|
||
&xHigherPriorityTaskWoken);
|
||
}
|
||
g_cmd_parser.state = USER_CMD_IDLE;
|
||
g_cmd_parser.index = 0;
|
||
} else if (rx == '\r') {
|
||
/* 忽略回车符（兼容终端发送 \r\n） */
|
||
} else if (rx == '@') {
|
||
/* 重新开始 —— 取消当前输入 */
|
||
g_cmd_parser.index = 0;
|
||
} else {
|
||
if (g_cmd_parser.index < MAX_CMD_LEN - 1) {
|
||
g_cmd_parser.buffer[g_cmd_parser.index++] = (char)rx;
|
||
} else {
|
||
/* 缓冲区溢出 — 丢弃并等待下一个 @ */
|
||
g_cmd_parser.state = USER_CMD_IDLE;
|
||
g_cmd_parser.index = 0;
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
|
||
/* 重新使能 UART 接收中断 */
|
||
HAL_UART_Receive_IT(huart, &g_uart_rx_byte, 1);
|
||
|
||
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
||
}
|
||
|
||
/* ============================================================================
|
||
* Command handlers (registered with cmd_parser)
|
||
* ============================================================================ */
|
||
|
||
static int cmd_help(void *ctx, int argc, char **argv)
|
||
{
|
||
(void)ctx; (void)argc; (void)argv;
|
||
log_printf("[cmd] available commands:\r\n"
|
||
" help ==> show this message\r\n"
|
||
" motor enable [yaw|pitch] ==> enable motor(s), default both\r\n"
|
||
" motor disable [yaw|pitch] ==> disable motor(s), default both\r\n"
|
||
" sensor enable ==> resume IMU data publishing\r\n"
|
||
" sensor disable ==> pause IMU data publishing\r\n"
|
||
" sensorcali acc enable ==> start accelerometer calibration\r\n"
|
||
" sensorcali acc disable ==> stop accelerometer calibration\r\n"
|
||
" sensorcali mag enable ==> start magnetometer calibration\r\n"
|
||
" sensorcali mag disable ==> stop magnetometer calibration\r\n"
|
||
" pid ==> show all PID coefficients\r\n"
|
||
" pid <axis> <p> <v> ==> set PID param (v÷10=实际值, motor disabled)\r\n"
|
||
" axis: roll|pitch, p: kp|ki|kd, v: float\r\n");
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_motor(void *ctx, int argc, char **argv)
|
||
{
|
||
(void)ctx;
|
||
if (argc < 2) return -1;
|
||
|
||
if (strcmp(argv[1], "enable") == 0) {
|
||
if (argc == 2) {
|
||
gimbal_motor_enable();
|
||
log_printf("[cmd] both motors enabled\r\n");
|
||
} else if (strcmp(argv[2], "yaw") == 0) {
|
||
gimbal_motor_enable_id(GIMBAL_MOTOR_YAW);
|
||
log_printf("[cmd] yaw motor enabled\r\n");
|
||
} else if (strcmp(argv[2], "pitch") == 0) {
|
||
gimbal_motor_enable_id(GIMBAL_MOTOR_PITCH);
|
||
log_printf("[cmd] pitch motor enabled\r\n");
|
||
} else {
|
||
log_printf("[cmd] unknown motor: %s\r\n", argv[2]);
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
if (strcmp(argv[1], "disable") == 0) {
|
||
if (argc == 2) {
|
||
gimbal_motor_disable();
|
||
log_printf("[cmd] both motors disabled\r\n");
|
||
} else if (strcmp(argv[2], "yaw") == 0) {
|
||
gimbal_motor_disable_id(GIMBAL_MOTOR_YAW);
|
||
log_printf("[cmd] yaw motor disabled\r\n");
|
||
} else if (strcmp(argv[2], "pitch") == 0) {
|
||
gimbal_motor_disable_id(GIMBAL_MOTOR_PITCH);
|
||
log_printf("[cmd] pitch motor disabled\r\n");
|
||
} else {
|
||
log_printf("[cmd] unknown motor: %s\r\n", argv[2]);
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
return -1;
|
||
}
|
||
|
||
static int cmd_sensor(void *ctx, int argc, char **argv)
|
||
{
|
||
(void)ctx;
|
||
if (argc < 2) return -1;
|
||
|
||
if (strcmp(argv[1], "enable") == 0) {
|
||
sensor_enable();
|
||
log_printf("[cmd] sensor enabled\r\n");
|
||
} else if (strcmp(argv[1], "disable") == 0) {
|
||
sensor_disable();
|
||
log_printf("[cmd] sensor disabled\r\n");
|
||
} else {
|
||
return -1;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_sensorcali(void *ctx, int argc, char **argv)
|
||
{
|
||
(void)ctx;
|
||
if (argc < 3) return -1;
|
||
|
||
if (strcmp(argv[1], "acc") == 0) {
|
||
if (strcmp(argv[2], "enable") == 0) {
|
||
sensor_acc_cali_enable();
|
||
log_printf("[cmd] acc calibration started\r\n");
|
||
} else if (strcmp(argv[2], "disable") == 0) {
|
||
sensor_acc_cali_disable();
|
||
log_printf("[cmd] acc calibration stopped\r\n");
|
||
} else {
|
||
return -1;
|
||
}
|
||
} else if (strcmp(argv[1], "mag") == 0) {
|
||
if (strcmp(argv[2], "enable") == 0) {
|
||
sensor_mag_cali_enable();
|
||
log_printf("[cmd] mag calibration started\r\n");
|
||
} else if (strcmp(argv[2], "disable") == 0) {
|
||
sensor_mag_cali_disable();
|
||
log_printf("[cmd] mag calibration stopped\r\n");
|
||
} else {
|
||
return -1;
|
||
}
|
||
} else {
|
||
return -1;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_pid(void *ctx, int argc, char **argv)
|
||
{
|
||
(void)ctx;
|
||
if (argc == 1) {
|
||
float kp, ki, kd;
|
||
gimbal_get_pid("roll", &kp, &ki, &kd);
|
||
log_printf("[cmd] Roll: Kp=%d Ki=%d Kd=%d\r\n",
|
||
(int)(kp * 10.0f), (int)(ki * 10.0f), (int)(kd * 10.0f));
|
||
gimbal_get_pid("pitch", &kp, &ki, &kd);
|
||
log_printf("[cmd] Pitch: Kp=%d Ki=%d Kd=%d\r\n",
|
||
(int)(kp * 10.0f), (int)(ki * 10.0f), (int)(kd * 10.0f));
|
||
return 0;
|
||
}
|
||
|
||
if (argc == 4) {
|
||
if (gimbal_is_motor_enabled()) {
|
||
log_printf("[cmd] ERROR: disable motor first (motor disable)\r\n");
|
||
return 0;
|
||
}
|
||
char *end;
|
||
long int_val = strtol(argv[3], &end, 10);
|
||
if (*end != '\0' || end == argv[3]) {
|
||
log_printf("[cmd] usage: pid <roll|pitch> <kp|ki|kd> <value>\r\n");
|
||
return 0;
|
||
}
|
||
float value = (float)int_val / 10.0f;
|
||
if (gimbal_set_pid(argv[1], argv[2], value) == 0) {
|
||
log_printf("[cmd] pid.%s.%s = %ld\r\n", argv[1], argv[2], int_val);
|
||
} else {
|
||
log_printf("[cmd] usage: pid <roll|pitch> <kp|ki|kd> <value>\r\n");
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
return -1;
|
||
}
|
||
|
||
void callback_task(void)
|
||
{
|
||
char cmd_buf[MAX_CMD_LEN];
|
||
char disp_buf[MAX_CMD_LEN];
|
||
cmd_parser_t parser;
|
||
|
||
g_cmd_parser.state = USER_CMD_IDLE;
|
||
g_cmd_parser.index = 0;
|
||
g_cmd_parser.cmd_queue = g_cmd_queue;
|
||
|
||
cmd_parser_init(&parser);
|
||
cmd_parser_register(&parser, "help", cmd_help);
|
||
cmd_parser_register(&parser, "motor", cmd_motor);
|
||
cmd_parser_register(&parser, "sensor", cmd_sensor);
|
||
cmd_parser_register(&parser, "sensorcali", cmd_sensorcali);
|
||
cmd_parser_register(&parser, "pid", cmd_pid);
|
||
|
||
while (1) {
|
||
osStatus_t qstat = osMessageQueueGet(g_cmd_queue, cmd_buf, NULL, osWaitForever);
|
||
if (qstat != osOK) continue;
|
||
if (cmd_buf[0] == '\0') continue;
|
||
|
||
memcpy(disp_buf, cmd_buf, MAX_CMD_LEN);
|
||
if (cmd_parser_dispatch(&parser, cmd_buf, NULL) != 0) {
|
||
log_printf("[cmd] unknown: %s\r\n", disp_buf);
|
||
}
|
||
}
|
||
}
|