robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

功能化mrobot

Browse Source
This commit is contained in:
Robofish 2026-02-04 14:32:14 +08:00
parent 268a132283
commit e056d5ecc7
4 changed files with 228 additions and 237 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

205
User/device/mrobot.c
View File

@ -5,8 +5,6 @@
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
#include "device/mrobot.h" #include "device/mrobot.h"
#include "device/device.h"
#include "device/motor.h"
#include "component/freertos_cli.h" #include "component/freertos_cli.h"
#include "bsp/uart.h" #include "bsp/uart.h"
#include <string.h> #include <string.h>
@ -34,9 +32,6 @@ static const char *const CLI_WELCOME_MESSAGE =
#define ANSI_CURSOR_HOME "\033[H" #define ANSI_CURSOR_HOME "\033[H"
#define ANSI_BACKSPACE "\b \b" #define ANSI_BACKSPACE "\b \b"
/* 弧度转角度常量 */
#define RAD_TO_DEG 57.2957795131f
/* Private types ------------------------------------------------------------ */ /* Private types ------------------------------------------------------------ */
/* CLI 上下文结构体 - 封装所有状态 */ /* CLI 上下文结构体 - 封装所有状态 */
typedef struct { 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_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_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_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_tx_callback(void);
static void uart_rx_callback(void); static void uart_rx_callback(void);
static void send_string(const char *str); static void send_string(const char *str);
static void send_prompt(void); 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 命令定义表 */ /* CLI 命令定义表 */
static const CLI_Command_Definition_t builtin_commands[] = { static const CLI_Command_Definition_t builtin_commands[] = {
@ -111,7 +102,6 @@ static const CLI_Command_Definition_t builtin_commands[] = {
{ "cd", "cd <path>: 切换目录\r\n", cmd_cd, 1 }, { "cd", "cd <path>: 切换目录\r\n", cmd_cd, 1 },
{ "ls", "ls: 列出当前目录内容\r\n", cmd_ls, 0 }, { "ls", "ls: 列出当前目录内容\r\n", cmd_ls, 0 },
{ "show", "show [device] [count]: 显示设备信息\r\n", cmd_show, -1 }, { "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])) #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（阻塞等待完成） * @brief 发送字符串到 UART（阻塞等待完成）
*/ */
@ -213,90 +171,6 @@ static void uart_rx_callback(void) {
BSP_UART_Receive(MROBOT_UART_PORT, &ctx.uart_rx_char, 1, false); 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 命令实现 */ /* CLI 命令实现 */
/* ========================================================================== */ /* ========================================================================== */
@ -342,15 +216,6 @@ static BaseType_t cmd_htop(char *pcWriteBuffer, size_t xWriteBufferLen, const ch
return pdFALSE; 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 命令 - 切换目录 * @brief cd 命令 - 切换目录
*/ */
@ -409,24 +274,12 @@ static BaseType_t cmd_ls(char *pcWriteBuffer, size_t xWriteBufferLen, const char
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
" (No devices)\r\n"); " (No devices)\r\n");
} else { } else {
/* 按类型分组显示 */ /* 直接列出所有设备 */
static const char *type_names[] = { "IMU", "Motor", "Sensor", "Custom" }; for (uint8_t i = 0; i < ctx.device_count && offset < (int)xWriteBufferLen - 50; i++) {
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, offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
" - %s\r\n", ctx.devices[i].name); " - %s\r\n", ctx.devices[i].name);
} }
} }
}
}
} else if (strcmp(ctx.current_path, "/modules") == 0) { } else if (strcmp(ctx.current_path, "/modules") == 0) {
snprintf(pcWriteBuffer, xWriteBufferLen, snprintf(pcWriteBuffer, xWriteBufferLen,
"Module functions not yet implemented\r\n"); "Module functions not yet implemented\r\n");
@ -685,8 +538,7 @@ MRobot_State_t MRobot_GetState(void) {
return ctx.state; return ctx.state;
} }
MRobot_Error_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type, MRobot_Error_t MRobot_RegisterDevice(const char *name, void *data, MRobot_PrintCallback_t print_cb) {
void *data, MRobot_PrintCallback_t print_cb) {
if (name == NULL || data == NULL) { if (name == NULL || data == NULL) {
return MROBOT_ERR_NULL_PTR; 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; 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++) { for (uint8_t i = 0; i < ctx.device_count; i++) {
if (strcmp(ctx.devices[i].name, name) == 0) { 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); 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].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].data = data;
ctx.devices[ctx.device_count].print_cb = print_cb; ctx.devices[ctx.device_count].print_cb = print_cb;
ctx.device_count++; ctx.device_count++;
@ -755,14 +602,6 @@ MRobot_Error_t MRobot_UnregisterDevice(const char *name) {
return MROBOT_ERR_NOT_FOUND; 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_Error_t MRobot_RegisterCommand(const char *command, const char *help_text,
MRobot_CommandCallback_t callback, int8_t param_count) { MRobot_CommandCallback_t callback, int8_t param_count) {
if (command == NULL || help_text == NULL || callback == NULL) { if (command == NULL || help_text == NULL || callback == NULL) {
@ -832,6 +671,44 @@ int MRobot_Printf(const char *fmt, ...) {
return len; 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) { void MRobot_Run(void) {
if (!ctx.initialized) return; if (!ctx.initialized) return;

187
User/device/mrobot.h
View File

@ -8,6 +8,104 @@
* - htop 风格的任务监控 * - 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 <gyro|accel|save>\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 <gyro|accel|save>: IMU calibration\r\n", cmd_cali, -1);
* @endcode
*/ */
#pragma once #pragma once
@ -20,8 +118,6 @@ extern "C" {
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include <stddef.h> #include <stddef.h>
#include "component/ahrs.h"
#include "device/device.h"
#include "bsp/uart.h" #include "bsp/uart.h"
/* Configuration ------------------------------------------------------------ */ /* Configuration ------------------------------------------------------------ */
@ -71,15 +167,6 @@ typedef enum {
MROBOT_ERR_NOT_INIT = -7, /* 未初始化 */ MROBOT_ERR_NOT_INIT = -7, /* 未初始化 */
} MRobot_Error_t; } 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 运行状态 */ /* CLI 运行状态 */
typedef enum { typedef enum {
MROBOT_STATE_IDLE, /* 空闲状态，等待输入 */ MROBOT_STATE_IDLE, /* 空闲状态，等待输入 */
@ -91,10 +178,11 @@ typedef enum {
/** /**
* @brief 设备打印回调函数类型 * @brief 设备打印回调函数类型
* @param device_data 设备数据指针 * @param device_data 用户注册时传入的设备数据指针
* @param buffer 输出缓冲区 * @param buffer 输出缓冲区
* @param buffer_size 缓冲区大小 * @param buffer_size 缓冲区大小
* @return 实际写入的字节数 * @return 实际写入的字节数
* @note 用户需要自行实现此函数来格式化设备数据
*/ */
typedef int (*MRobot_PrintCallback_t)(const void *device_data, char *buffer, size_t buffer_size); 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 --------------------------------------------------------- */ /* Device structure --------------------------------------------------------- */
typedef struct { typedef struct {
char name[MROBOT_DEVICE_NAME_LEN]; /* 设备名称 */ char name[MROBOT_DEVICE_NAME_LEN]; /* 设备名称 */
MRobot_DeviceType_t type; /* 设备类型 */ void *data; /* 用户设备数据指针 */
void *data; /* 设备数据指针 */ MRobot_PrintCallback_t print_cb; /* 用户打印回调函数 */
MRobot_PrintCallback_t print_cb; /* 打印回调函数 */
} MRobot_Device_t; } 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 --------------------------------------------------------------- */ /* Public API --------------------------------------------------------------- */
/** /**
@ -166,13 +220,11 @@ MRobot_State_t MRobot_GetState(void);
/** /**
* @brief 注册设备到 MRobot 系统 * @brief 注册设备到 MRobot 系统
* @param name 设备名称（会被截断到 MROBOT_DEVICE_NAME_LEN-1） * @param name 设备名称（会被截断到 MROBOT_DEVICE_NAME_LEN-1）
* @param type 设备类型
* @param data 设备数据指针（不能为 NULL） * @param data 设备数据指针（不能为 NULL）
* @param print_cb 打印回调函数（可为 NULL，但无法用 show 命令查看） * @param print_cb 打印回调函数（可为 NULL，但无法用 show 命令查看）
* @return MRobot_Error_t 错误码 * @return MRobot_Error_t 错误码
*/ */
MRobot_Error_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type, MRobot_Error_t MRobot_RegisterDevice(const char *name, void *data, MRobot_PrintCallback_t print_cb);
void *data, MRobot_PrintCallback_t print_cb);
/** /**
* @brief 注销设备 * @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); 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 注册自定义命令 * @brief 注册自定义命令
* @param command 命令名称 * @param command 命令名称
@ -242,6 +278,23 @@ MRobot_Error_t MRobot_Print(const char *str);
*/ */
int MRobot_Printf(const char *fmt, ...); 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 #ifdef __cplusplus
} }
#endif #endif

68
User/task/atti_esit.c
View File

@ -17,15 +17,26 @@
#include "device/device.h" #include "device/device.h"
#include "module/balance_chassis.h" #include "module/balance_chassis.h"
#include "task/user_task.h" #include "task/user_task.h"
#include <stdio.h>
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* 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 define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
BMI088_t bmi088; BMI088_t bmi088;
DEVICE_IMU_t imu_device; MyIMU_Data_t imu_device; /* 使用自定义结构 */
#ifndef USE_QEKF #ifndef USE_QEKF
AHRS_t gimbal_ahrs; AHRS_t gimbal_ahrs;
AHRS_Magn_t magn; AHRS_Magn_t magn;
@ -51,6 +62,57 @@ static const KPID_Params_t imu_temp_ctrl_pid_param = {
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* 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 ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
void Task_atti_esit(void *argument) { void Task_atti_esit(void *argument) {
(void)argument; /* 未使用argument，消除警告 */ (void)argument; /* 未使用argument，消除警告 */
@ -69,8 +131,8 @@ void Task_atti_esit(void *argument) {
1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param); 1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
BSP_PWM_Start(BSP_PWM_IMU_HEAT); BSP_PWM_Start(BSP_PWM_IMU_HEAT);
/* 注册 IMU 设备到 MRobot（使用通用打印函数） */ /* 注册 IMU 设备到 MRobot（只需名称、数据、打印回调） */
MRobot_RegisterIMU("bmi088", &imu_device); MRobot_RegisterDevice("bmi088", &imu_device, print_imu_callback);
/* USER CODE INIT END */ /* USER CODE INIT END */
while (1) { while (1) {

1
User/task/ctrl_shoot.c
View File

@ -24,7 +24,6 @@ Shoot_CMD_t shoot_cmd;
void Task_ctrl_shoot(void *argument) { void Task_ctrl_shoot(void *argument) {
(void)argument; /* 未使用argument，消除警告 */ (void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */ /* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_SHOOT_FREQ; const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_SHOOT_FREQ;