diff --git a/User/device/mrobot.c b/User/device/mrobot.c index 09cfa31..65cfe1b 100644 --- a/User/device/mrobot.c +++ b/User/device/mrobot.c @@ -96,6 +96,7 @@ 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_va(char *buf, size_t size, const char *fmt, va_list args); /* CLI 命令定义表 */ static const CLI_Command_Definition_t builtin_commands[] = { @@ -650,21 +651,13 @@ const MRobot_Device_t *MRobot_FindDevice(const char *name) { return NULL; } -MRobot_Error_t MRobot_Print(const char *str) { - if (str == NULL) return MROBOT_ERR_NULL_PTR; - if (!ctx.initialized) return MROBOT_ERR_NOT_INIT; - - send_string(str); - return MROBOT_OK; -} - int MRobot_Printf(const char *fmt, ...) { if (fmt == NULL || !ctx.initialized) return -1; char buffer[MROBOT_OUTPUT_BUFFER_SIZE]; va_list args; va_start(args, fmt); - int len = vsnprintf(buffer, sizeof(buffer), fmt, args); + int len = format_float_va(buffer, sizeof(buffer), fmt, args); va_end(args); if (len > 0) { @@ -673,6 +666,132 @@ int MRobot_Printf(const char *fmt, ...) { return len; } +/** + * @brief 内部函数:格式化浮点数到缓冲区(va_list 版本) + */ +static int format_float_va(char *buf, size_t size, const char *fmt, va_list args) { + if (buf == NULL || size == 0 || fmt == NULL) return 0; + + char *buf_ptr = buf; + size_t buf_remain = size - 1; + + const char *p = fmt; + while (*p && buf_remain > 0) { + if (*p != '%') { + *buf_ptr++ = *p++; + buf_remain--; + continue; + } + + p++; /* 跳过 '%' */ + + /* 处理 %% */ + if (*p == '%') { + *buf_ptr++ = '%'; + buf_remain--; + p++; + continue; + } + + /* 解析精度 %.Nf */ + int precision = 2; /* 默认精度 */ + if (*p == '.') { + p++; + precision = 0; + while (*p >= '0' && *p <= '9') { + precision = precision * 10 + (*p - '0'); + p++; + } + if (precision > 6) precision = 6; + } + + int written = 0; + switch (*p) { + case 'f': { /* 浮点数 */ + double val = va_arg(args, double); + written = MRobot_FormatFloat(buf_ptr, buf_remain, (float)val, precision); + break; + } + case 'd': + case 'i': { + int val = va_arg(args, int); + written = snprintf(buf_ptr, buf_remain, "%d", val); + break; + } + case 'u': { + unsigned int val = va_arg(args, unsigned int); + written = snprintf(buf_ptr, buf_remain, "%u", val); + break; + } + case 'x': { + unsigned int val = va_arg(args, unsigned int); + written = snprintf(buf_ptr, buf_remain, "%x", val); + break; + } + case 'X': { + unsigned int val = va_arg(args, unsigned int); + written = snprintf(buf_ptr, buf_remain, "%X", val); + break; + } + case 's': { + const char *str = va_arg(args, const char *); + if (str == NULL) str = "(null)"; + written = snprintf(buf_ptr, buf_remain, "%s", str); + break; + } + case 'c': { + int ch = va_arg(args, int); + *buf_ptr = (char)ch; + written = 1; + break; + } + case 'l': { + p++; + if (*p == 'd' || *p == 'i') { + long val = va_arg(args, long); + written = snprintf(buf_ptr, buf_remain, "%ld", val); + } else if (*p == 'u') { + unsigned long val = va_arg(args, unsigned long); + written = snprintf(buf_ptr, buf_remain, "%lu", val); + } else if (*p == 'x' || *p == 'X') { + unsigned long val = va_arg(args, unsigned long); + written = snprintf(buf_ptr, buf_remain, *p == 'x' ? "%lx" : "%lX", val); + } else { + p--; + } + break; + } + default: { + *buf_ptr++ = '%'; + buf_remain--; + if (buf_remain > 0) { + *buf_ptr++ = *p; + buf_remain--; + } + written = 0; + break; + } + } + + if (written > 0) { + buf_ptr += written; + buf_remain -= (size_t)written; + } + p++; + } + + *buf_ptr = '\0'; + return (int)(buf_ptr - buf); +} + +int MRobot_Snprintf(char *buf, size_t size, const char *fmt, ...) { + va_list args; + va_start(args, fmt); + int len = format_float_va(buf, size, fmt, args); + va_end(args); + return len; +} + int MRobot_FormatFloat(char *buf, size_t size, float val, int precision) { if (buf == NULL || size == 0) return 0; diff --git a/User/device/mrobot.h b/User/device/mrobot.h index 3461bf2..99c1164 100644 --- a/User/device/mrobot.h +++ b/User/device/mrobot.h @@ -124,7 +124,7 @@ extern "C" { /* 可在编译时通过 -D 选项覆盖这些默认值 */ #ifndef MROBOT_MAX_DEVICES -#define MROBOT_MAX_DEVICES 32 /* 最大设备数 */ +#define MROBOT_MAX_DEVICES 64 /* 最大设备数 */ #endif #ifndef MROBOT_MAX_CUSTOM_COMMANDS @@ -156,11 +156,11 @@ extern "C" { #endif #ifndef MROBOT_USER_NAME -#define MROBOT_USER_NAME "Robofish" /* CLI 用户名 */ +#define MROBOT_USER_NAME "root" /* CLI 用户名 */ #endif #ifndef MROBOT_HOST_NAME -#define MROBOT_HOST_NAME "balance_infantry" /* CLI 主机名 */ +#define MROBOT_HOST_NAME "MRobot" /* CLI 主机名 */ #endif /* Error codes -------------------------------------------------------------- */ @@ -272,20 +272,37 @@ const MRobot_Device_t *MRobot_FindDevice(const char *name); void MRobot_Run(void); /** - * @brief 发送字符串到 CLI 终端(线程安全) - * @param str 要发送的字符串 - * @return MRobot_Error_t 错误码 - */ -MRobot_Error_t MRobot_Print(const char *str); - -/** - * @brief 格式化输出到 CLI 终端(线程安全) + * @brief 格式化输出到 CLI 终端(线程安全,支持浮点数) * @param fmt 格式字符串 * @param ... 可变参数 * @return 实际输出的字符数,失败返回负数 + * + * @note 支持的格式说明符: + * - %d, %i, %u, %x, %X, %ld, %lu, %lx : 整数 + * - %s, %c : 字符串/字符 + * - %f : 浮点数 (默认2位小数) + * - %.Nf : 浮点数 (N位小数, N=0-6) + * - %% : 百分号 + * + * @example + * MRobot_Printf("Euler: R=%.2f P=%.2f Y=%.2f\\r\\n", roll, pitch, yaw); */ int MRobot_Printf(const char *fmt, ...); +/** + * @brief 格式化到缓冲区(用于回调函数,支持浮点数) + * @note 格式说明符同 MRobot_Printf + * + * @example + * static int print_imu(const void *data, char *buf, size_t size) { + * const BMI088_t *imu = (const BMI088_t *)data; + * return MRobot_Snprintf(buf, size, + * " Accel: X=%.3f Y=%.3f Z=%.3f\\r\\n", + * imu->accl.x, imu->accl.y, imu->accl.z); + * } + */ +int MRobot_Snprintf(char *buf, size_t size, const char *fmt, ...); + /* Utility functions -------------------------------------------------------- */ /** diff --git a/User/task/atti_esit.c b/User/task/atti_esit.c index af7dad7..66e4487 100644 --- a/User/task/atti_esit.c +++ b/User/task/atti_esit.c @@ -21,22 +21,11 @@ /* 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; -MyIMU_Data_t imu_device; /* 使用自定义结构 */ #ifndef USE_QEKF AHRS_t gimbal_ahrs; AHRS_Magn_t magn; @@ -64,53 +53,23 @@ static const KPID_Params_t imu_temp_ctrl_pid_param = { /* Private function --------------------------------------------------------- */ /** - * @brief IMU 设备打印回调函数(用户自定义格式) + * @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; - - /* 弧度转角度 */ + const BMI088_t *imu = (const BMI088_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, + return MRobot_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", + " Accel : X=%.3f Y=%.3f Z=%.3f (m/s^2)\r\n" + " Gyro : X=%.3f Y=%.3f Z=%.3f (rad/s)\r\n" + " Euler : R=%.2f P=%.2f Y=%.2f (deg)\r\n" + " Temp : %.1f C\r\n", imu->header.online ? "Online" : "Offline", - ax, ay, az, - gx, gy, gz, - roll, pitch, yaw, - q0, q1, q2, q3, - temp); + imu->accl.x, imu->accl.y, imu->accl.z, + imu->gyro.x, imu->gyro.y, imu->gyro.z, + eulr_to_send.rol * RAD2DEG, eulr_to_send.pit * RAD2DEG, eulr_to_send.yaw * RAD2DEG, + imu->temp); } /* Exported functions ------------------------------------------------------- */ @@ -131,8 +90,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_RegisterDevice("bmi088", &imu_device, print_imu_callback); + /* 注册 IMU 设备到 MRobot */ + MRobot_RegisterDevice("bmi088", &bmi088, print_imu_callback); /* USER CODE INIT END */ while (1) { @@ -183,20 +142,6 @@ 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 */