From 5f624a51b9920b1ccfb50256e409506939ff86cb Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Wed, 4 Feb 2026 04:38:24 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=AF=E4=BB=A5=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/device/mrobot.c | 202 ++++++++++++++++++++++++++++++++++++------ User/device/mrobot.h | 41 +++++++++ User/task/atti_esit.c | 77 ++++------------ 3 files changed, 232 insertions(+), 88 deletions(-) diff --git a/User/device/mrobot.c b/User/device/mrobot.c index 9d74bef..2538807 100644 --- a/User/device/mrobot.c +++ b/User/device/mrobot.c @@ -1,11 +1,15 @@ /* Includes ----------------------------------------------------------------- */ #include "device/mrobot.h" +#include "device/device.h" +#include "device/motor.h" #include "component/freertos_cli.h" #include "bsp/uart.h" #include #include +#include #include #include +#include /* Private variables -------------------------------------------------------- */ static MRobot_Device_t devices[MROBOT_MAX_DEVICES]; @@ -61,7 +65,7 @@ static const CLI_Command_Definition_t cmd_def_ls = { static const CLI_Command_Definition_t cmd_def_show = { "show", - "show [device]: 显示设备信息\r\n", + "show [device] [count]: 显示设备信息，count 默认为 1\r\n", cmd_show, -1 /* 可变参数 */ }; @@ -75,6 +79,83 @@ static const CLI_Command_Definition_t cmd_def_pwd = { /* Private functions -------------------------------------------------------- */ +/* 通用 IMU 设备打印函数 */ +static void mrobot_print_imu(void *device_data, char *buffer, uint16_t buffer_size) { + DEVICE_IMU_t *imu = (DEVICE_IMU_t *)device_data; + + /* 将浮点数转换为整数和小数部分，避免 printf 浮点数支持问题 */ + int accl_x_int = (int)(imu->accl.x); + int accl_x_frac = (int)((imu->accl.x - accl_x_int) * 1000); + int accl_y_int = (int)(imu->accl.y); + int accl_y_frac = (int)((imu->accl.y - accl_y_int) * 1000); + int accl_z_int = (int)(imu->accl.z); + int accl_z_frac = (int)((imu->accl.z - accl_z_int) * 1000); + + int gyro_x_int = (int)(imu->gyro.x); + int gyro_x_frac = (int)((imu->gyro.x - gyro_x_int) * 1000); + int gyro_y_int = (int)(imu->gyro.y); + int gyro_y_frac = (int)((imu->gyro.y - gyro_y_int) * 1000); + int gyro_z_int = (int)(imu->gyro.z); + int gyro_z_frac = (int)((imu->gyro.z - gyro_z_int) * 1000); + + int temp_int = (int)(imu->temp); + int temp_frac = (int)((imu->temp - temp_int) * 100); + + float roll_deg = imu->euler.rol * 57.2958f; + float pitch_deg = imu->euler.pit * 57.2958f; + float yaw_deg = imu->euler.yaw * 57.2958f; + + int roll_int = (int)roll_deg; + int roll_frac = (int)((roll_deg - roll_int) * 100); + int pitch_int = (int)pitch_deg; + int pitch_frac = (int)((pitch_deg - pitch_int) * 100); + int yaw_int = (int)yaw_deg; + int yaw_frac = (int)((yaw_deg - yaw_int) * 100); + + snprintf(buffer, buffer_size, + "状态: %s\r\n" + "加速度计: X=%d.%03d Y=%d.%03d Z=%d.%03d m/s²\r\n" + "陀螺仪: X=%d.%03d Y=%d.%03d Z=%d.%03d rad/s\r\n" + "温度: %d.%02d °C\r\n" + "欧拉角: Roll=%d.%02d Pitch=%d.%02d Yaw=%d.%02d °\r\n", + imu->header.online ? "在线" : "离线", + accl_x_int, abs(accl_x_frac), accl_y_int, abs(accl_y_frac), accl_z_int, abs(accl_z_frac), + gyro_x_int, abs(gyro_x_frac), gyro_y_int, abs(gyro_y_frac), gyro_z_int, abs(gyro_z_frac), + temp_int, abs(temp_frac), + roll_int, abs(roll_frac), pitch_int, abs(pitch_frac), yaw_int, abs(yaw_frac)); +} + +/* 通用电机设备打印函数 */ +static void mrobot_print_motor(void *device_data, char *buffer, uint16_t buffer_size) { + MOTOR_t *motor = (MOTOR_t *)device_data; + + int angle_int = (int)(motor->feedback.rotor_abs_angle); + int angle_frac = (int)((motor->feedback.rotor_abs_angle - angle_int) * 100); + + int speed_int = (int)(motor->feedback.rotor_speed); + int speed_frac = (int)((motor->feedback.rotor_speed - speed_int) * 100); + + int current_int = (int)(motor->feedback.torque_current); + int current_frac = (int)((motor->feedback.torque_current - current_int) * 100); + + int temp_int = (int)(motor->feedback.temp); + int temp_frac = (int)((motor->feedback.temp - temp_int) * 10); + + snprintf(buffer, buffer_size, + "状态: %s\r\n" + "反装: %s\r\n" + "角度: %d.%02d °\r\n" + "转速: %d.%02d RPM\r\n" + "电流: %d.%02d A\r\n" + "温度: %d.%01d °C\r\n", + motor->header.online ? "在线" : "离线", + motor->reverse ? "是" : "否", + angle_int, abs(angle_frac), + speed_int, abs(speed_frac), + current_int, abs(current_frac), + temp_int, abs(temp_frac)); +} + /* help 命令实现 */ static BaseType_t cmd_help(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) { (void)pcCommandString; @@ -164,18 +245,59 @@ static BaseType_t cmd_ls(char *pcWriteBuffer, size_t xWriteBufferLen, const char /* show 命令实现 */ static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) { const char *param; - BaseType_t param_len; + const char *count_param; + BaseType_t param_len, count_param_len; + static uint32_t print_count = 0; + static uint32_t current_iteration = 0; + static char saved_device_name[32] = {0}; - param = FreeRTOS_CLIGetParameter(pcCommandString, 1, ¶m_len); - - if (strcmp(current_path, "/dev") != 0) { - snprintf(pcWriteBuffer, xWriteBufferLen, "错误: show 命令仅在 /dev 目录下可用\r\n"); - return pdFALSE; + /* 首次调用时解析参数 */ + if (current_iteration == 0) { + param = FreeRTOS_CLIGetParameter(pcCommandString, 1, ¶m_len); + count_param = FreeRTOS_CLIGetParameter(pcCommandString, 2, &count_param_len); + + if (strcmp(current_path, "/dev") != 0) { + snprintf(pcWriteBuffer, xWriteBufferLen, "错误: show 命令仅在 /dev 目录下可用\r\n"); + return pdFALSE; + } + + /* 解析打印次数，默认为 1 */ + print_count = 1; + if (count_param != NULL) { + char count_str[16]; + strncpy(count_str, count_param, count_param_len < 15 ? count_param_len : 15); + count_str[count_param_len < 15 ? count_param_len : 15] = '\0'; + int parsed_count = atoi(count_str); + if (parsed_count > 0 && parsed_count <= 100) { + print_count = parsed_count; + } + } + + /* 保存设备名称 */ + if (param != NULL) { + strncpy(saved_device_name, param, param_len); + saved_device_name[param_len] = '\0'; + } else { + saved_device_name[0] = '\0'; + } } - if (param == NULL) { + /* 执行打印 */ + int offset = 0; + + /* 多次打印时清屏（像htop一样） */ + if (print_count > 1) { + const char *clear = "\033[2J\033[H"; /* ANSI清屏 + 光标归位 */ + offset = snprintf(pcWriteBuffer, xWriteBufferLen, "%s", clear); + } + + if (saved_device_name[0] == '\0') { /* 显示所有设备 */ - int offset = snprintf(pcWriteBuffer, xWriteBufferLen, "所有设备信息:\r\n"); + if (print_count > 1) { + offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, + "[第 %u/%u 次]\r\n", current_iteration + 1, print_count); + } + offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, "所有设备信息:\r\n"); for (uint8_t i = 0; i < device_count && offset < (int)xWriteBufferLen; i++) { offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, "\r\n=== %s ===\r\n", devices[i].name); @@ -188,16 +310,19 @@ static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const ch } } else { /* 显示特定设备 */ - char device_name[32]; - strncpy(device_name, param, param_len); - device_name[param_len] = '\0'; - bool found = false; for (uint8_t i = 0; i < device_count; i++) { - if (strcmp(devices[i].name, device_name) == 0) { + if (strcmp(devices[i].name, saved_device_name) == 0) { found = true; - int offset = snprintf(pcWriteBuffer, xWriteBufferLen, + int offset = 0; + if (print_count > 1) { + offset = snprintf(pcWriteBuffer, xWriteBufferLen, + "[第 %u/%u 次] === %s ===\r\n", + current_iteration + 1, print_count, devices[i].name); + } else { + offset = snprintf(pcWriteBuffer, xWriteBufferLen, "=== %s ===\r\n", devices[i].name); + } if (devices[i].print_callback != NULL) { devices[i].print_callback(devices[i].data, pcWriteBuffer + offset, @@ -210,12 +335,22 @@ static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const ch } } - if (!found) { - snprintf(pcWriteBuffer, xWriteBufferLen, "错误: 设备 '%s' 未找到\r\n", device_name); + if (!found && current_iteration == 0) { + snprintf(pcWriteBuffer, xWriteBufferLen, "错误: 设备 '%s' 未找到\r\n", saved_device_name); + current_iteration = 0; + return pdFALSE; } } - return pdFALSE; + /* 判断是否需要继续打印 */ + current_iteration++; + if (current_iteration < print_count) { + osDelay(200); /* 延时 200ms 再打印下一次 */ + return pdTRUE; /* 返回 pdTRUE 表示还有更多输出 */ + } else { + current_iteration = 0; /* 重置计数器 */ + return pdFALSE; + } } /* UART 发送完成回调 */ @@ -315,6 +450,14 @@ int8_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type, return 0; } +int8_t MRobot_RegisterIMU(const char *name, void *imu_device) { + return MRobot_RegisterDevice(name, MROBOT_DEVICE_TYPE_IMU, imu_device, mrobot_print_imu); +} + +int8_t MRobot_RegisterMotor(const char *name, void *motor) { + return MRobot_RegisterDevice(name, MROBOT_DEVICE_TYPE_MOTOR, motor, mrobot_print_motor); +} + void MRobot_Run(void) { /* 动态 htop 模式 */ if (htop_mode) { @@ -411,20 +554,21 @@ void MRobot_Run(void) { } else { /* 处理其他命令 */ BaseType_t result; - int offset = 0; + output_buffer[0] = '\0'; /* 清空输出缓冲区 */ do { result = FreeRTOS_CLIProcessCommand((char *)cmd_buffer, - output_buffer + offset, - sizeof(output_buffer) - offset); - offset = strlen(output_buffer); - } while (result != pdFALSE && offset < (int)sizeof(output_buffer)); - - if (strlen(output_buffer) > 0) { - tx_complete = false; - BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)output_buffer, strlen(output_buffer), true); - while (!tx_complete) { osDelay(1); } /* 等待发送完成 */ - } + output_buffer, + sizeof(output_buffer)); + + /* 立即发送输出 */ + if (strlen(output_buffer) > 0) { + tx_complete = false; + BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)output_buffer, strlen(output_buffer), true); + while (!tx_complete) { osDelay(1); } + output_buffer[0] = '\0'; /* 清空缓冲区准备下一次 */ + } + } while (result != pdFALSE); /* 发送提示符 */ char prompt[128]; diff --git a/User/device/mrobot.h b/User/device/mrobot.h index ba67148..f907037 100644 --- a/User/device/mrobot.h +++ b/User/device/mrobot.h @@ -31,6 +31,8 @@ extern "C" { /* Includes ----------------------------------------------------------------- */ #include #include +#include "component/ahrs.h" +#include "device/device.h" /* Exported constants ------------------------------------------------------- */ #define MROBOT_MAX_DEVICES 32 @@ -57,6 +59,29 @@ typedef struct { MRobot_PrintCallback_t print_callback; /* 打印设备信息的回调函数 */ } MRobot_Device_t; +/* 设备模板结构体 - 使用 AHRS 标准结构 */ +typedef struct { + DEVICE_Header_t header; + AHRS_Accl_t accl; /* 加速度 m/s² */ + AHRS_Gyro_t gyro; /* 角速度 rad/s */ + AHRS_Eulr_t euler; /* 欧拉角 rad */ + AHRS_Quaternion_t quat; /* 四元数 */ + float temp; /* 温度 °C */ +} DEVICE_IMU_t; + +typedef struct { + float rotor_abs_angle; /* 转子绝对角度 */ + float rotor_speed; /* 实际转子转速 */ + float torque_current; /* 转矩电流 */ + float temp; /* 温度 */ +} DEVICE_MOTOR_Feedback_t; + +typedef struct { + DEVICE_Header_t header; + bool reverse; /* 是否反装 true表示反装 */ + DEVICE_MOTOR_Feedback_t feedback; +} DEVICE_MOTOR_t; + /* Exported functions ------------------------------------------------------- */ /** @@ -75,6 +100,22 @@ void MRobot_Init(void); int8_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type, void *data, MRobot_PrintCallback_t print_callback); +/** + * @brief 注册 IMU 设备到 MRobot（使用通用打印函数） + * @param name 设备名称 + * @param imu_device 指向 DEVICE_IMU_t 结构的指针 + * @retval 0 成功, -1 失败 + */ +int8_t MRobot_RegisterIMU(const char *name, void *imu_device); + +/** + * @brief 注册电机设备到 MRobot（使用通用打印函数） + * @param name 设备名称 + * @param motor 指向 MOTOR_t 结构的指针 + * @retval 0 成功, -1 失败 + */ +int8_t MRobot_RegisterMotor(const char *name, void *motor); + /** * @brief MRobot 主循环，在 CLI 任务中调用 */ diff --git a/User/task/atti_esit.c b/User/task/atti_esit.c index a57caf6..2bbb228 100644 --- a/User/task/atti_esit.c +++ b/User/task/atti_esit.c @@ -14,9 +14,9 @@ #include "component/pid.h" #include "device/bmi088.h" #include "device/mrobot.h" +#include "device/device.h" #include "module/balance_chassis.h" #include "task/user_task.h" -#include /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -25,7 +25,7 @@ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ BMI088_t bmi088; - +DEVICE_IMU_t imu_device; #ifndef USE_QEKF AHRS_t gimbal_ahrs; AHRS_Magn_t magn; @@ -34,17 +34,6 @@ AHRS_Eulr_t eulr_to_send; Chassis_IMU_t imu_for_chassis; KPID_t imu_temp_ctrl_pid; -/* 用于 MRobot 显示的 IMU 数据结构 */ -typedef struct { - BMI088_t *bmi088; - AHRS_Eulr_t *euler; -} IMU_Display_Data_t; - -static IMU_Display_Data_t imu_display_data = { - .bmi088 = &bmi088, - .euler = &eulr_to_send, -}; - /*默认校准参数*/ BMI088_Cali_t cali_bmi088 = { .gyro_offset = {-0.00147764047f, -0.00273479894f, 0.00154074503f}, @@ -62,50 +51,6 @@ static const KPID_Params_t imu_temp_ctrl_pid_param = { /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ -/* BMI088 设备信息打印回调 */ -static void bmi088_print_callback(void *device_data, char *buffer, uint16_t buffer_size) { - IMU_Display_Data_t *data = (IMU_Display_Data_t *)device_data; - - /* 将浮点数转换为整数和小数部分，避免 printf 浮点数支持问题 */ - int accl_x_int = (int)(data->bmi088->accl.x); - int accl_x_frac = (int)((data->bmi088->accl.x - accl_x_int) * 1000); - int accl_y_int = (int)(data->bmi088->accl.y); - int accl_y_frac = (int)((data->bmi088->accl.y - accl_y_int) * 1000); - int accl_z_int = (int)(data->bmi088->accl.z); - int accl_z_frac = (int)((data->bmi088->accl.z - accl_z_int) * 1000); - - int gyro_x_int = (int)(data->bmi088->gyro.x); - int gyro_x_frac = (int)((data->bmi088->gyro.x - gyro_x_int) * 1000); - int gyro_y_int = (int)(data->bmi088->gyro.y); - int gyro_y_frac = (int)((data->bmi088->gyro.y - gyro_y_int) * 1000); - int gyro_z_int = (int)(data->bmi088->gyro.z); - int gyro_z_frac = (int)((data->bmi088->gyro.z - gyro_z_int) * 1000); - - int temp_int = (int)(data->bmi088->temp); - int temp_frac = (int)((data->bmi088->temp - temp_int) * 100); - - float roll_deg = data->euler->rol * 57.2958f; - float pitch_deg = data->euler->pit * 57.2958f; - float yaw_deg = data->euler->yaw * 57.2958f; - - int roll_int = (int)roll_deg; - int roll_frac = (int)((roll_deg - roll_int) * 100); - int pitch_int = (int)pitch_deg; - int pitch_frac = (int)((pitch_deg - pitch_int) * 100); - int yaw_int = (int)yaw_deg; - int yaw_frac = (int)((yaw_deg - yaw_int) * 100); - - snprintf(buffer, buffer_size, - "加速度计: X=%d.%03d Y=%d.%03d Z=%d.%03d m/s²\r\n" - "陀螺仪: X=%d.%03d Y=%d.%03d Z=%d.%03d rad/s\r\n" - "温度: %d.%02d °C\r\n" - "欧拉角: Roll=%d.%02d Pitch=%d.%02d Yaw=%d.%02d °\r\n", - accl_x_int, abs(accl_x_frac), accl_y_int, abs(accl_y_frac), accl_z_int, abs(accl_z_frac), - gyro_x_int, abs(gyro_x_frac), gyro_y_int, abs(gyro_y_frac), gyro_z_int, abs(gyro_z_frac), - temp_int, abs(temp_frac), - roll_int, abs(roll_frac), pitch_int, abs(pitch_frac), yaw_int, abs(yaw_frac)); -} - /* Exported functions ------------------------------------------------------- */ void Task_atti_esit(void *argument) { (void)argument; /* 未使用argument，消除警告 */ @@ -124,8 +69,8 @@ void Task_atti_esit(void *argument) { 1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param); BSP_PWM_Start(BSP_PWM_IMU_HEAT); - /* 注册 BMI088 设备到 MRobot */ - MRobot_RegisterDevice("bmi088", MROBOT_DEVICE_TYPE_IMU, &imu_display_data, bmi088_print_callback); + /* 注册 IMU 设备到 MRobot（使用通用打印函数） */ + MRobot_RegisterIMU("bmi088", &imu_device); /* USER CODE INIT END */ while (1) { @@ -176,6 +121,20 @@ void Task_atti_esit(void *argument) { 0); // 非阻塞发送底盘IMU数据 BSP_PWM_SetComp(BSP_PWM_IMU_HEAT, PID_Calc(&imu_temp_ctrl_pid, 40.5f, bmi088.temp, 0.0f, 0.0f)); + /* 直接赋值 AHRS 结构体 - 完美兼容 */ + imu_device.accl = bmi088.accl; + imu_device.gyro = bmi088.gyro; + imu_device.euler = eulr_to_send; +#ifdef USE_QEKF + imu_device.quat.q0 = QEKF_INS.q[0]; + imu_device.quat.q1 = QEKF_INS.q[1]; + imu_device.quat.q2 = QEKF_INS.q[2]; + imu_device.quat.q3 = QEKF_INS.q[3]; +#else + imu_device.quat = gimbal_ahrs.quat; +#endif + imu_device.temp = bmi088.temp; + imu_device.header = bmi088.header; osKernelUnlock(); /* USER CODE END */