refactor(callback_task): replace hardcoded dispatch with cmd_parser module
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>
This commit is contained in:
robinson
@@ -1,6 +1,8 @@
|
||||
#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"
|
||||
@@ -82,110 +84,179 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
|
||||
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 (strcmp(cmd_buf, "help") == 0) {
|
||||
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");
|
||||
} else if (strncmp(cmd_buf, "motor enable", 12) == 0) {
|
||||
const char *p = cmd_buf + 12;
|
||||
while (*p == ' ') p++;
|
||||
if (*p == '\0') {
|
||||
gimbal_motor_enable();
|
||||
log_printf("[cmd] both motors enabled\r\n");
|
||||
} else if (strcmp(p, "yaw") == 0) {
|
||||
gimbal_motor_enable_id(GIMBAL_MOTOR_YAW);
|
||||
log_printf("[cmd] yaw motor enabled\r\n");
|
||||
} else if (strcmp(p, "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", p);
|
||||
}
|
||||
} else if (strncmp(cmd_buf, "motor disable", 13) == 0) {
|
||||
const char *p = cmd_buf + 13;
|
||||
while (*p == ' ') p++;
|
||||
if (*p == '\0') {
|
||||
gimbal_motor_disable();
|
||||
log_printf("[cmd] both motors disabled\r\n");
|
||||
} else if (strcmp(p, "yaw") == 0) {
|
||||
gimbal_motor_disable_id(GIMBAL_MOTOR_YAW);
|
||||
log_printf("[cmd] yaw motor disabled\r\n");
|
||||
} else if (strcmp(p, "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", p);
|
||||
}
|
||||
} else if (strcmp(cmd_buf, "sensor enable") == 0) {
|
||||
sensor_enable();
|
||||
log_printf("[cmd] sensor enabled\r\n");
|
||||
} else if (strcmp(cmd_buf, "sensor disable") == 0) {
|
||||
sensor_disable();
|
||||
log_printf("[cmd] sensor disabled\r\n");
|
||||
} else if (strcmp(cmd_buf, "sensorcali acc enable") == 0) {
|
||||
sensor_acc_cali_enable();
|
||||
log_printf("[cmd] acc calibration started\r\n");
|
||||
} else if (strcmp(cmd_buf, "sensorcali acc disable") == 0) {
|
||||
sensor_acc_cali_disable();
|
||||
log_printf("[cmd] acc calibration stopped\r\n");
|
||||
} else if (strcmp(cmd_buf, "sensorcali mag enable") == 0) {
|
||||
sensor_mag_cali_enable();
|
||||
log_printf("[cmd] mag calibration started\r\n");
|
||||
} else if (strcmp(cmd_buf, "sensorcali mag disable") == 0) {
|
||||
sensor_mag_cali_disable();
|
||||
log_printf("[cmd] mag calibration stopped\r\n");
|
||||
} else if (strcmp(cmd_buf, "pid") == 0) {
|
||||
/* 查看当前 PID 系数（整数显示，值 = 实际系数 × 10） */
|
||||
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));
|
||||
} else if (strncmp(cmd_buf, "pid ", 4) == 0) {
|
||||
/* 调参：pid <roll|pitch> <kp|ki|kd> <value> （value ÷ 10 = 实际系数） */
|
||||
char axis[8], param[8];
|
||||
int int_val;
|
||||
if (sscanf(cmd_buf, "pid %7s %7s %d", axis, param, &int_val) == 3) {
|
||||
if (gimbal_is_motor_enabled()) {
|
||||
log_printf("[cmd] ERROR: disable motor first (motor disable)\r\n");
|
||||
} else {
|
||||
float value = (float)int_val / 10.0f;
|
||||
if (gimbal_set_pid(axis, param, value) == 0) {
|
||||
log_printf("[cmd] pid.%s.%s = %d\r\n", axis, param, int_val);
|
||||
} else {
|
||||
log_printf("[cmd] usage: pid <roll|pitch> <kp|ki|kd> <value>\r\n");
|
||||
}
|
||||
}
|
||||
} else {
|
||||
log_printf("[cmd] usage: pid <roll|pitch> <kp|ki|kd> <value>\r\n");
|
||||
}
|
||||
} else {
|
||||
log_printf("[cmd] unknown: %s\r\n", cmd_buf);
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user