195 lines
8.3 KiB
C
195 lines
8.3 KiB
C
#include "app.h"
|
||
#include "cmsis_os2.h"
|
||
#include <string.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 1" — 使能电机 1
|
||
* "motor enable 2" — 使能电机 2
|
||
* "motor disable" — 关闭全部电机（惰行 coast）
|
||
* "motor disable 1" — 关闭电机 1
|
||
* "motor disable 2" — 关闭电机 2
|
||
* "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);
|
||
}
|
||
|
||
void callback_task(void)
|
||
{
|
||
char cmd_buf[MAX_CMD_LEN];
|
||
|
||
g_cmd_parser.state = USER_CMD_IDLE;
|
||
g_cmd_parser.index = 0;
|
||
g_cmd_parser.cmd_queue = g_cmd_queue;
|
||
|
||
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 [1|2] ==> enable motor(s), default both\r\n"
|
||
" motor disable [1|2] ==> 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 <motor> <p> <v> ==> set PID param (v÷10=实际值, motor disabled)\r\n"
|
||
" motor: 1|2, 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, "1") == 0) {
|
||
gimbal_motor_enable_id(GIMBAL_MOTOR_YAW);
|
||
log_printf("[cmd] motor 1 enabled\r\n");
|
||
} else if (strcmp(p, "2") == 0) {
|
||
gimbal_motor_enable_id(GIMBAL_MOTOR_PITCH);
|
||
log_printf("[cmd] motor 2 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, "1") == 0) {
|
||
gimbal_motor_disable_id(GIMBAL_MOTOR_YAW);
|
||
log_printf("[cmd] motor 1 disabled\r\n");
|
||
} else if (strcmp(p, "2") == 0) {
|
||
gimbal_motor_disable_id(GIMBAL_MOTOR_PITCH);
|
||
log_printf("[cmd] motor 2 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] Motor 1: 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] Motor 2: 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 <1|2> <kp|ki|kd> <value> （value ÷ 10 = 实际系数） */
|
||
char param[8];
|
||
int motor_id, int_val;
|
||
if (sscanf(cmd_buf, "pid %d %7s %d", &motor_id, param, &int_val) == 3) {
|
||
if (motor_id < 1 || motor_id > 2) {
|
||
log_printf("[cmd] usage: pid <1|2> <kp|ki|kd> <value>\r\n");
|
||
} else if (gimbal_is_motor_enabled()) {
|
||
log_printf("[cmd] ERROR: disable motor first (motor disable)\r\n");
|
||
} else {
|
||
const char *axis = (motor_id == 1) ? "roll" : "pitch";
|
||
float value = (float)int_val / 10.0f;
|
||
if (gimbal_set_pid(axis, param, value) == 0) {
|
||
log_printf("[cmd] motor %d.%s = %d\r\n", motor_id, param, int_val);
|
||
} else {
|
||
log_printf("[cmd] usage: pid <1|2> <kp|ki|kd> <value>\r\n");
|
||
}
|
||
}
|
||
} else {
|
||
log_printf("[cmd] usage: pid <1|2> <kp|ki|kd> <value>\r\n");
|
||
}
|
||
} else {
|
||
log_printf("[cmd] unknown: %s\r\n", cmd_buf);
|
||
}
|
||
}
|
||
}
|