From e056d5ecc7009df38d3a148e6310cd4f977532bc Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Wed, 4 Feb 2026 14:32:14 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=9F=E8=83=BD=E5=8C=96mrobot?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/device/mrobot.c | 209 +++++++++-------------------------------- User/device/mrobot.h | 187 +++++++++++++++++++++++------------- User/task/atti_esit.c | 68 +++++++++++++- User/task/ctrl_shoot.c | 1 - 4 files changed, 228 insertions(+), 237 deletions(-) diff --git a/User/device/mrobot.c b/User/device/mrobot.c index 76210f2..03c1a13 100644 --- a/User/device/mrobot.c +++ b/User/device/mrobot.c @@ -5,8 +5,6 @@ /* Includes ----------------------------------------------------------------- */ #include "device/mrobot.h" -#include "device/device.h" -#include "device/motor.h" #include "component/freertos_cli.h" #include "bsp/uart.h" #include @@ -34,9 +32,6 @@ static const char *const CLI_WELCOME_MESSAGE = #define ANSI_CURSOR_HOME "\033[H" #define ANSI_BACKSPACE "\b \b" -/* 弧度转角度常量 */ -#define RAD_TO_DEG 57.2957795131f - /* Private types ------------------------------------------------------------ */ /* CLI 上下文结构体 - 封装所有状态 */ typedef struct { @@ -93,16 +88,12 @@ static BaseType_t cmd_htop(char *pcWriteBuffer, size_t xWriteBufferLen, const ch static BaseType_t cmd_cd(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString); static BaseType_t cmd_ls(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString); static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString); -static BaseType_t cmd_pwd(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString); /* 内部辅助函数 */ static void uart_tx_callback(void); static void uart_rx_callback(void); static void send_string(const char *str); static void send_prompt(void); -static int format_float(char *buf, size_t size, float val, int precision); -static int print_imu_device(const void *device_data, char *buffer, size_t buffer_size); -static int print_motor_device(const void *device_data, char *buffer, size_t buffer_size); /* CLI 命令定义表 */ static const CLI_Command_Definition_t builtin_commands[] = { @@ -111,7 +102,6 @@ static const CLI_Command_Definition_t builtin_commands[] = { { "cd", "cd : 切换目录\r\n", cmd_cd, 1 }, { "ls", "ls: 列出当前目录内容\r\n", cmd_ls, 0 }, { "show", "show [device] [count]: 显示设备信息\r\n", cmd_show, -1 }, - { "pwd", "pwd: 显示当前目录\r\n", cmd_pwd, 0 }, }; #define BUILTIN_CMD_COUNT (sizeof(builtin_commands) / sizeof(builtin_commands[0])) @@ -119,38 +109,6 @@ static const CLI_Command_Definition_t builtin_commands[] = { /* 辅助函数实现 */ /* ========================================================================== */ -/** - * @brief 格式化浮点数为字符串（避免嵌入式 printf 浮点支持问题） - */ -static int format_float(char *buf, size_t size, float val, int precision) { - if (buf == NULL || size == 0) return 0; - - int offset = 0; - - /* 处理负数 */ - if (val < 0) { - if (offset < (int)size - 1) buf[offset++] = '-'; - val = -val; - } - - /* 计算乘数 */ - int multiplier = 1; - for (int i = 0; i < precision; i++) multiplier *= 10; - - int int_part = (int)val; - int frac_part = (int)((val - int_part) * multiplier + 0.5f); - - /* 处理进位 */ - if (frac_part >= multiplier) { - int_part++; - frac_part -= multiplier; - } - - /* 格式化输出 */ - int written = snprintf(buf + offset, size - offset, "%d.%0*d", int_part, precision, frac_part); - return (written > 0) ? (offset + written) : offset; -} - /** * @brief 发送字符串到 UART（阻塞等待完成） */ @@ -213,90 +171,6 @@ static void uart_rx_callback(void) { BSP_UART_Receive(MROBOT_UART_PORT, &ctx.uart_rx_char, 1, false); } -/* ========================================================================== */ -/* 设备打印函数实现 */ -/* ========================================================================== */ - -/** - * @brief IMU 设备打印函数 - */ -static int print_imu_device(const void *device_data, char *buffer, size_t buffer_size) { - if (device_data == NULL || buffer == NULL || buffer_size == 0) return 0; - - const DEVICE_IMU_t *imu = (const DEVICE_IMU_t *)device_data; - - char ax[16], ay[16], az[16]; - char gx[16], gy[16], gz[16]; - char temp[16]; - char roll[16], pitch[16], yaw[16]; - char q0[16], q1[16], q2[16], q3[16]; - - /* 格式化加速度 */ - format_float(ax, sizeof(ax), imu->accl.x, 3); - format_float(ay, sizeof(ay), imu->accl.y, 3); - format_float(az, sizeof(az), imu->accl.z, 3); - - /* 格式化陀螺仪 */ - format_float(gx, sizeof(gx), imu->gyro.x, 3); - format_float(gy, sizeof(gy), imu->gyro.y, 3); - format_float(gz, sizeof(gz), imu->gyro.z, 3); - - /* 格式化温度 */ - format_float(temp, sizeof(temp), imu->temp, 2); - - /* 格式化欧拉角（转换为角度） */ - format_float(roll, sizeof(roll), imu->euler.rol * RAD_TO_DEG, 2); - format_float(pitch, sizeof(pitch), imu->euler.pit * RAD_TO_DEG, 2); - format_float(yaw, sizeof(yaw), imu->euler.yaw * RAD_TO_DEG, 2); - - /* 格式化四元数 */ - format_float(q0, sizeof(q0), imu->quat.q0, 4); - format_float(q1, sizeof(q1), imu->quat.q1, 4); - format_float(q2, sizeof(q2), imu->quat.q2, 4); - format_float(q3, sizeof(q3), imu->quat.q3, 4); - - return snprintf(buffer, buffer_size, - " Status : %s\r\n" - " Accel : X=%-10s Y=%-10s Z=%-10s (m/s^2)\r\n" - " Gyro : X=%-10s Y=%-10s Z=%-10s (rad/s)\r\n" - " Euler : Roll=%-8s Pitch=%-8s Yaw=%-8s (deg)\r\n" - " Quaternion: q0=%-10s q1=%-10s q2=%-10s q3=%-10s\r\n" - " Temp : %-10s C\r\n", - imu->header.online ? "Online" : "Offline", - ax, ay, az, - gx, gy, gz, - roll, pitch, yaw, - q0, q1, q2, q3, - temp); -} - -/** - * @brief 电机设备打印函数 - */ -static int print_motor_device(const void *device_data, char *buffer, size_t buffer_size) { - if (device_data == NULL || buffer == NULL || buffer_size == 0) return 0; - - const MOTOR_t *motor = (const MOTOR_t *)device_data; - - char angle[16], speed[16], current[16], temp[16]; - - format_float(angle, sizeof(angle), motor->feedback.rotor_abs_angle, 2); - format_float(speed, sizeof(speed), motor->feedback.rotor_speed, 2); - format_float(current, sizeof(current), motor->feedback.torque_current, 2); - format_float(temp, sizeof(temp), motor->feedback.temp, 1); - - return snprintf(buffer, buffer_size, - " Status : %s\r\n" - " Reverse : %s\r\n" - " Angle : %-15s deg\r\n" - " Speed : %-15s RPM\r\n" - " Current : %-15s A\r\n" - " Temp : %-15s C\r\n", - motor->header.online ? "Online" : "Offline", - motor->reverse ? "Yes" : "No", - angle, speed, current, temp); -} - /* ========================================================================== */ /* CLI 命令实现 */ /* ========================================================================== */ @@ -342,15 +216,6 @@ static BaseType_t cmd_htop(char *pcWriteBuffer, size_t xWriteBufferLen, const ch return pdFALSE; } -/** - * @brief pwd 命令 - 显示当前路径 - */ -static BaseType_t cmd_pwd(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) { - (void)pcCommandString; - snprintf(pcWriteBuffer, xWriteBufferLen, "%s\r\n", ctx.current_path); - return pdFALSE; -} - /** * @brief cd 命令 - 切换目录 */ @@ -409,22 +274,10 @@ static BaseType_t cmd_ls(char *pcWriteBuffer, size_t xWriteBufferLen, const char offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, " (No devices)\r\n"); } else { - /* 按类型分组显示 */ - static const char *type_names[] = { "IMU", "Motor", "Sensor", "Custom" }; - - for (uint8_t t = 0; t < MROBOT_DEVICE_TYPE_NUM && offset < (int)xWriteBufferLen - 50; t++) { - bool has_type = false; - for (uint8_t i = 0; i < ctx.device_count; i++) { - if (ctx.devices[i].type == t) { - if (!has_type) { - offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, - "\r\n[%s]\r\n", type_names[t]); - has_type = true; - } - offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, - " - %s\r\n", ctx.devices[i].name); - } - } + /* 直接列出所有设备 */ + for (uint8_t i = 0; i < ctx.device_count && offset < (int)xWriteBufferLen - 50; i++) { + offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, + " - %s\r\n", ctx.devices[i].name); } } } else if (strcmp(ctx.current_path, "/modules") == 0) { @@ -685,8 +538,7 @@ MRobot_State_t MRobot_GetState(void) { return ctx.state; } -MRobot_Error_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type, - void *data, MRobot_PrintCallback_t print_cb) { +MRobot_Error_t MRobot_RegisterDevice(const char *name, void *data, MRobot_PrintCallback_t print_cb) { if (name == NULL || data == NULL) { return MROBOT_ERR_NULL_PTR; } @@ -695,10 +547,6 @@ MRobot_Error_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type, return MROBOT_ERR_FULL; } - if (type >= MROBOT_DEVICE_TYPE_NUM) { - return MROBOT_ERR_INVALID_ARG; - } - /* 检查重名 */ for (uint8_t i = 0; i < ctx.device_count; i++) { if (strcmp(ctx.devices[i].name, name) == 0) { @@ -713,7 +561,6 @@ MRobot_Error_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type, strncpy(ctx.devices[ctx.device_count].name, name, MROBOT_DEVICE_NAME_LEN - 1); ctx.devices[ctx.device_count].name[MROBOT_DEVICE_NAME_LEN - 1] = '\0'; - ctx.devices[ctx.device_count].type = type; ctx.devices[ctx.device_count].data = data; ctx.devices[ctx.device_count].print_cb = print_cb; ctx.device_count++; @@ -755,14 +602,6 @@ MRobot_Error_t MRobot_UnregisterDevice(const char *name) { return MROBOT_ERR_NOT_FOUND; } -MRobot_Error_t MRobot_RegisterIMU(const char *name, DEVICE_IMU_t *imu_device) { - return MRobot_RegisterDevice(name, MROBOT_DEVICE_TYPE_IMU, imu_device, print_imu_device); -} - -MRobot_Error_t MRobot_RegisterMotor(const char *name, void *motor) { - return MRobot_RegisterDevice(name, MROBOT_DEVICE_TYPE_MOTOR, motor, print_motor_device); -} - MRobot_Error_t MRobot_RegisterCommand(const char *command, const char *help_text, MRobot_CommandCallback_t callback, int8_t param_count) { if (command == NULL || help_text == NULL || callback == NULL) { @@ -832,6 +671,44 @@ int MRobot_Printf(const char *fmt, ...) { return len; } +int MRobot_FormatFloat(char *buf, size_t size, float val, int precision) { + if (buf == NULL || size == 0) return 0; + + int offset = 0; + + /* 处理负数 */ + if (val < 0) { + if (offset < (int)size - 1) buf[offset++] = '-'; + val = -val; + } + + /* 限制精度范围 */ + if (precision < 0) precision = 0; + if (precision > 6) precision = 6; + + /* 计算乘数 */ + int multiplier = 1; + for (int i = 0; i < precision; i++) multiplier *= 10; + + int int_part = (int)val; + int frac_part = (int)((val - int_part) * multiplier + 0.5f); + + /* 处理进位 */ + if (frac_part >= multiplier) { + int_part++; + frac_part -= multiplier; + } + + /* 格式化输出 */ + int written; + if (precision > 0) { + written = snprintf(buf + offset, size - offset, "%d.%0*d", int_part, precision, frac_part); + } else { + written = snprintf(buf + offset, size - offset, "%d", int_part); + } + return (written > 0) ? (offset + written) : offset; +} + void MRobot_Run(void) { if (!ctx.initialized) return; diff --git a/User/device/mrobot.h b/User/device/mrobot.h index fba4adb..e4c8c10 100644 --- a/User/device/mrobot.h +++ b/User/device/mrobot.h @@ -8,6 +8,104 @@ * - htop 风格的任务监控 * - 自定义命令扩展 * - 线程安全设计 + * + * @example IMU 设备注册示例 + * @code + * // 1. 定义 IMU 数据结构 + * typedef struct { + * bool online; + * float accl[3]; + * float gyro[3]; + * float euler[3]; // roll, pitch, yaw (deg) + * float temp; + * } MyIMU_t; + * + * MyIMU_t my_imu; + * + * // 2. 实现打印回调 + * static int print_imu(const void *data, char *buf, size_t size) { + * const MyIMU_t *imu = (const MyIMU_t *)data; + * char ax[16], ay[16], az[16], r[16], p[16], y[16], t[16]; + * MRobot_FormatFloat(ax, 16, imu->accl[0], 3); + * MRobot_FormatFloat(ay, 16, imu->accl[1], 3); + * MRobot_FormatFloat(az, 16, imu->accl[2], 3); + * MRobot_FormatFloat(r, 16, imu->euler[0], 2); + * MRobot_FormatFloat(p, 16, imu->euler[1], 2); + * MRobot_FormatFloat(y, 16, imu->euler[2], 2); + * MRobot_FormatFloat(t, 16, imu->temp, 1); + * return snprintf(buf, size, + * " Status: %s\r\n" + * " Accel : X=%s Y=%s Z=%s\r\n" + * " Euler : R=%s P=%s Y=%s\r\n" + * " Temp : %s C\r\n", + * imu->online ? "Online" : "Offline", ax, ay, az, r, p, y, t); + * } + * + * // 3. 注册设备 + * MRobot_RegisterDevice("imu", &my_imu, print_imu); + * @endcode + * + * @example 电机设备注册示例 + * @code + * typedef struct { + * bool online; + * float angle; // deg + * float speed; // RPM + * float current; // A + * } MyMotor_t; + * + * MyMotor_t motor[4]; + * + * static int print_motor(const void *data, char *buf, size_t size) { + * const MyMotor_t *m = (const MyMotor_t *)data; + * char ang[16], spd[16], cur[16]; + * MRobot_FormatFloat(ang, 16, m->angle, 2); + * MRobot_FormatFloat(spd, 16, m->speed, 1); + * MRobot_FormatFloat(cur, 16, m->current, 3); + * return snprintf(buf, size, + * " Status : %s\r\n" + * " Angle : %s deg\r\n" + * " Speed : %s RPM\r\n" + * " Current: %s A\r\n", + * m->online ? "Online" : "Offline", ang, spd, cur); + * } + * + * // 注册 4 个电机 + * MRobot_RegisterDevice("motor0", &motor[0], print_motor); + * MRobot_RegisterDevice("motor1", &motor[1], print_motor); + * MRobot_RegisterDevice("motor2", &motor[2], print_motor); + * MRobot_RegisterDevice("motor3", &motor[3], print_motor); + * @endcode + * + * @example 自定义校准命令示例 + * @code + * // 校准数据 + * static float gyro_offset[3] = {0}; + * + * // 命令回调: cali gyro / cali accel / cali save + * static long cmd_cali(char *buf, size_t size, const char *cmd) { + * const char *param = FreeRTOS_CLIGetParameter(cmd, 1, NULL); + * + * if (param == NULL) { + * return snprintf(buf, size, "Usage: cali \r\n"); + * } + * if (strncmp(param, "gyro", 4) == 0) { + * // 采集 1000 次陀螺仪数据求平均 + * snprintf(buf, size, "Calibrating gyro... keep IMU still!\r\n"); + * // ... 校准逻辑 ... + * return 0; + * } + * if (strncmp(param, "save", 4) == 0) { + * // 保存到 Flash + * snprintf(buf, size, "Calibration saved to flash.\r\n"); + * return 0; + * } + * return snprintf(buf, size, "Unknown: %s\r\n", param); + * } + * + * // 注册命令 + * MRobot_RegisterCommand("cali", "cali : IMU calibration\r\n", cmd_cali, -1); + * @endcode */ #pragma once @@ -20,8 +118,6 @@ extern "C" { #include #include #include -#include "component/ahrs.h" -#include "device/device.h" #include "bsp/uart.h" /* Configuration ------------------------------------------------------------ */ @@ -71,15 +167,6 @@ typedef enum { MROBOT_ERR_NOT_INIT = -7, /* 未初始化 */ } MRobot_Error_t; -/* Device types ------------------------------------------------------------- */ -typedef enum { - MROBOT_DEVICE_TYPE_IMU, /* IMU 设备 */ - MROBOT_DEVICE_TYPE_MOTOR, /* 电机设备 */ - MROBOT_DEVICE_TYPE_SENSOR, /* 传感器设备 */ - MROBOT_DEVICE_TYPE_CUSTOM, /* 自定义设备 */ - MROBOT_DEVICE_TYPE_NUM /* 设备类型总数 */ -} MRobot_DeviceType_t; - /* CLI 运行状态 */ typedef enum { MROBOT_STATE_IDLE, /* 空闲状态，等待输入 */ @@ -91,10 +178,11 @@ typedef enum { /** * @brief 设备打印回调函数类型 - * @param device_data 设备数据指针 + * @param device_data 用户注册时传入的设备数据指针 * @param buffer 输出缓冲区 * @param buffer_size 缓冲区大小 * @return 实际写入的字节数 + * @note 用户需要自行实现此函数来格式化设备数据 */ typedef int (*MRobot_PrintCallback_t)(const void *device_data, char *buffer, size_t buffer_size); @@ -106,44 +194,10 @@ typedef long (*MRobot_CommandCallback_t)(char *pcWriteBuffer, size_t xWriteBuffe /* Device structure --------------------------------------------------------- */ typedef struct { char name[MROBOT_DEVICE_NAME_LEN]; /* 设备名称 */ - MRobot_DeviceType_t type; /* 设备类型 */ - void *data; /* 设备数据指针 */ - MRobot_PrintCallback_t print_cb; /* 打印回调函数 */ + void *data; /* 用户设备数据指针 */ + MRobot_PrintCallback_t print_cb; /* 用户打印回调函数 */ } MRobot_Device_t; -/* 标准设备数据结构 ---------------------------------------------------------- */ - -/** - * @brief IMU 设备数据结构（标准模板） - */ -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; - -/** - * @brief 电机反馈数据结构 - */ -typedef struct { - float rotor_abs_angle; /* 转子绝对角度 */ - float rotor_speed; /* 实际转子转速 */ - float torque_current; /* 转矩电流 */ - float temp; /* 温度 */ -} DEVICE_MOTOR_Feedback_t; - -/** - * @brief 电机设备数据结构（标准模板） - */ -typedef struct { - DEVICE_Header_t header; - bool reverse; /* 是否反装 */ - DEVICE_MOTOR_Feedback_t feedback; -} DEVICE_MOTOR_t; - /* Public API --------------------------------------------------------------- */ /** @@ -166,13 +220,11 @@ MRobot_State_t MRobot_GetState(void); /** * @brief 注册设备到 MRobot 系统 * @param name 设备名称（会被截断到 MROBOT_DEVICE_NAME_LEN-1） - * @param type 设备类型 * @param data 设备数据指针（不能为 NULL） * @param print_cb 打印回调函数（可为 NULL，但无法用 show 命令查看） * @return MRobot_Error_t 错误码 */ -MRobot_Error_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type, - void *data, MRobot_PrintCallback_t print_cb); +MRobot_Error_t MRobot_RegisterDevice(const char *name, void *data, MRobot_PrintCallback_t print_cb); /** * @brief 注销设备 @@ -181,22 +233,6 @@ MRobot_Error_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type, */ MRobot_Error_t MRobot_UnregisterDevice(const char *name); -/** - * @brief 注册 IMU 设备（使用内置打印函数） - * @param name 设备名称 - * @param imu_device 指向 DEVICE_IMU_t 结构的指针 - * @return MRobot_Error_t 错误码 - */ -MRobot_Error_t MRobot_RegisterIMU(const char *name, DEVICE_IMU_t *imu_device); - -/** - * @brief 注册电机设备（使用内置打印函数） - * @param name 设备名称 - * @param motor 指向 MOTOR_t 或兼容结构的指针 - * @return MRobot_Error_t 错误码 - */ -MRobot_Error_t MRobot_RegisterMotor(const char *name, void *motor); - /** * @brief 注册自定义命令 * @param command 命令名称 @@ -242,6 +278,23 @@ MRobot_Error_t MRobot_Print(const char *str); */ int MRobot_Printf(const char *fmt, ...); +/* Utility functions -------------------------------------------------------- */ + +/** + * @brief 格式化浮点数为字符串（嵌入式环境不支持 %f） + * @param buf 输出缓冲区 + * @param size 缓冲区大小 + * @param val 要格式化的浮点数 + * @param precision 小数位数 (0-6) + * @return 写入的字符数 + * + * @example + * char buf[16]; + * MRobot_FormatFloat(buf, sizeof(buf), 3.14159f, 2); // "3.14" + * MRobot_FormatFloat(buf, sizeof(buf), -0.001f, 4); // "-0.0010" + */ +int MRobot_FormatFloat(char *buf, size_t size, float val, int precision); + #ifdef __cplusplus } #endif \ No newline at end of file diff --git a/User/task/atti_esit.c b/User/task/atti_esit.c index 2bbb228..af7dad7 100644 --- a/User/task/atti_esit.c +++ b/User/task/atti_esit.c @@ -17,15 +17,26 @@ #include "device/device.h" #include "module/balance_chassis.h" #include "task/user_task.h" +#include /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ + +/* 自定义 IMU 数据结构 */ +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 */ +} MyIMU_Data_t; /* Private define ----------------------------------------------------------- */ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ BMI088_t bmi088; -DEVICE_IMU_t imu_device; +MyIMU_Data_t imu_device; /* 使用自定义结构 */ #ifndef USE_QEKF AHRS_t gimbal_ahrs; AHRS_Magn_t magn; @@ -51,6 +62,57 @@ static const KPID_Params_t imu_temp_ctrl_pid_param = { /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ + +/** + * @brief IMU 设备打印回调函数（用户自定义格式） + */ +static int print_imu_callback(const void *data, char *buffer, size_t size) { + const MyIMU_Data_t *imu = (const MyIMU_Data_t *)data; + + /* 弧度转角度 */ + #define RAD2DEG 57.2957795131f + + /* 使用 MRobot 提供的工具函数格式化浮点数 */ + char ax[16], ay[16], az[16]; + char gx[16], gy[16], gz[16]; + char roll[16], pitch[16], yaw[16]; + char q0[16], q1[16], q2[16], q3[16]; + char temp[16]; + + MRobot_FormatFloat(ax, sizeof(ax), imu->accl.x, 3); + MRobot_FormatFloat(ay, sizeof(ay), imu->accl.y, 3); + MRobot_FormatFloat(az, sizeof(az), imu->accl.z, 3); + + MRobot_FormatFloat(gx, sizeof(gx), imu->gyro.x, 3); + MRobot_FormatFloat(gy, sizeof(gy), imu->gyro.y, 3); + MRobot_FormatFloat(gz, sizeof(gz), imu->gyro.z, 3); + + MRobot_FormatFloat(roll, sizeof(roll), imu->euler.rol * RAD2DEG, 2); + MRobot_FormatFloat(pitch, sizeof(pitch), imu->euler.pit * RAD2DEG, 2); + MRobot_FormatFloat(yaw, sizeof(yaw), imu->euler.yaw * RAD2DEG, 2); + + MRobot_FormatFloat(q0, sizeof(q0), imu->quat.q0, 4); + MRobot_FormatFloat(q1, sizeof(q1), imu->quat.q1, 4); + MRobot_FormatFloat(q2, sizeof(q2), imu->quat.q2, 4); + MRobot_FormatFloat(q3, sizeof(q3), imu->quat.q3, 4); + + MRobot_FormatFloat(temp, sizeof(temp), imu->temp, 1); + + return snprintf(buffer, size, + " Status : %s\r\n" + " Accel : X=%s Y=%s Z=%s (m/s^2)\r\n" + " Gyro : X=%s Y=%s Z=%s (rad/s)\r\n" + " Euler : R=%s P=%s Y=%s (deg)\r\n" + " Quat : q0=%s q1=%s q2=%s q3=%s\r\n" + " Temp : %s C\r\n", + imu->header.online ? "Online" : "Offline", + ax, ay, az, + gx, gy, gz, + roll, pitch, yaw, + q0, q1, q2, q3, + temp); +} + /* Exported functions ------------------------------------------------------- */ void Task_atti_esit(void *argument) { (void)argument; /* 未使用argument，消除警告 */ @@ -69,8 +131,8 @@ void Task_atti_esit(void *argument) { 1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param); BSP_PWM_Start(BSP_PWM_IMU_HEAT); - /* 注册 IMU 设备到 MRobot（使用通用打印函数） */ - MRobot_RegisterIMU("bmi088", &imu_device); + /* 注册 IMU 设备到 MRobot（只需名称、数据、打印回调） */ + MRobot_RegisterDevice("bmi088", &imu_device, print_imu_callback); /* USER CODE INIT END */ while (1) { diff --git a/User/task/ctrl_shoot.c b/User/task/ctrl_shoot.c index c1c387f..ded9daf 100644 --- a/User/task/ctrl_shoot.c +++ b/User/task/ctrl_shoot.c @@ -23,7 +23,6 @@ Shoot_CMD_t shoot_cmd; /* Exported functions ------------------------------------------------------- */ void Task_ctrl_shoot(void *argument) { (void)argument; /* 未使用argument，消除警告 */ - /* 计算任务运行到指定频率需要等待的tick数 */ const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_SHOOT_FREQ;