mrobot好了
This commit is contained in:
parent
8468c53092
commit
6422b1e512
@ -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;
|
||||
|
||||
|
||||
@ -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 -------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
|
||||
@ -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 */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user