robofish/MRobot
1
0
Fork 0
mirror of https://github.com/goldenfishs/MRobot.git synced 2026-02-04 18:00:19 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

添加卡尔曼和mrobot

Browse Source
This commit is contained in:
Robofish 2026-02-04 15:36:21 +08:00
parent 8a031012fa
commit c5acabdc49
8 changed files with 2002 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
assets/User_code/component/dependencies.csv
View File

@ -6,3 +6,4 @@ pid,component/filter
filter,component/ahrs
mixer,component/user_math.h
ui,component/user_math.h
kalman_filter,arm_math.h
1 ahrs component/user_math.h
6 filter component/ahrs
7 mixer component/user_math.h
8 ui component/user_math.h
9 kalman_filter arm_math.h

1
assets/User_code/component/describe.csv
View File

@ -12,3 +12,4 @@ mixer,混控器
ui,用户交互
user_math,用户自定义数学函数
pid,PID控制器
kalman_filter,卡尔曼滤波器
1 pid 好用的
12 ui 用户交互
13 user_math 用户自定义数学函数
14 pid PID控制器
15 kalman_filter 卡尔曼滤波器

591
assets/User_code/component/kalman_filter/kalman_filter.c Normal file
View File

@ -0,0 +1,591 @@
/*
卡尔曼滤波器 modified from wang hongxi
支持动态量测调整，使用ARM CMSIS DSP优化矩阵运算
主要特性：
- 基于量测有效性的 H、R、K 矩阵动态调整
- 支持不同传感器采样频率
- 矩阵 P 防过度收敛机制
- ARM CMSIS DSP 优化的矩阵运算
- 可扩展架构，支持用户自定义函数（EKF/UKF/ESKF）
使用说明：
1. 矩阵初始化：P、F、Q 使用标准初始化方式
H、R 在使用自动调整时需要配置量测映射
2. 自动调整模式 (use_auto_adjustment = 1)：
- 提供 measurement_map：每个量测对应的状态索引
- 提供 measurement_degree：H 矩阵元素值
- 提供 mat_r_diagonal_elements：量测噪声方差
3. 固定模式 (use_auto_adjustment = 0)：
- 像初始化 P 矩阵那样正常初始化 z、H、R
4. 量测更新：
- 在传感器回调函数中更新 measured_vector
- 值为 0 表示量测无效
- 向量在每次 KF 更新后会被重置为 0
5. 防过度收敛：
- 设置 state_min_variance 防止 P 矩阵过度收敛
- 帮助滤波器适应缓慢变化的状态
使用示例：高度估计
状态向量 x =
| 高度 |
| 速度 |
| 加速度 |
KF_t Height_KF;
void INS_Task_Init(void)
{
// 初始协方差矩阵 P
static float P_Init[9] =
{
10, 0, 0,
0, 30, 0,
0, 0, 10,
};
// 状态转移矩阵 F（根据运动学模型）
static float F_Init[9] =
{
1, dt, 0.5*dt*dt,
0, 1, dt,
0, 0, 1,
};
// 过程噪声协方差矩阵 Q
static float Q_Init[9] =
{
0.25*dt*dt*dt*dt, 0.5*dt*dt*dt, 0.5*dt*dt,
0.5*dt*dt*dt, dt*dt, dt,
0.5*dt*dt, dt, 1,
};
// 设置状态最小方差（防止过度收敛）
static float state_min_variance[3] = {0.03, 0.005, 0.1};
// 开启自动调整
Height_KF.use_auto_adjustment = 1;
// 量测映射：[气压高度对应状态1, GPS高度对应状态1, 加速度计对应状态3]
static uint8_t measurement_reference[3] = {1, 1, 3};
// 量测系数（H矩阵元素值）
static float measurement_degree[3] = {1, 1, 1};
// 根据 measurement_reference 与 measurement_degree 生成 H 矩阵如下
// （在当前周期全部量测数据有效的情况下）
// |1 0 0|
// |1 0 0|
// |0 0 1|
// 量测噪声方差（R矩阵对角元素）
static float mat_r_diagonal_elements[3] = {30, 25, 35};
// 根据 mat_r_diagonal_elements 生成 R 矩阵如下
// （在当前周期全部量测数据有效的情况下）
// |30 0 0|
// | 0 25 0|
// | 0 0 35|
// 初始化卡尔曼滤波器（状态维数3，控制维数0，量测维数3）
KF_Init(&Height_KF, 3, 0, 3);
// 设置矩阵初值
memcpy(Height_KF.P_data, P_Init, sizeof(P_Init));
memcpy(Height_KF.F_data, F_Init, sizeof(F_Init));
memcpy(Height_KF.Q_data, Q_Init, sizeof(Q_Init));
memcpy(Height_KF.measurement_map, measurement_reference,
sizeof(measurement_reference));
memcpy(Height_KF.measurement_degree, measurement_degree,
sizeof(measurement_degree));
memcpy(Height_KF.mat_r_diagonal_elements, mat_r_diagonal_elements,
sizeof(mat_r_diagonal_elements));
memcpy(Height_KF.state_min_variance, state_min_variance,
sizeof(state_min_variance));
}
void INS_Task(void const *pvParameters)
{
// 循环更新卡尔曼滤波器
KF_Update(&Height_KF);
vTaskDelay(ts);
}
// 传感器回调函数示例：在数据就绪时更新 measured_vector
void Barometer_Read_Over(void)
{
......
INS_KF.measured_vector[0] = baro_height; // 气压计高度
}
void GPS_Read_Over(void)
{
......
INS_KF.measured_vector[1] = GPS_height; // GPS高度
}
void Acc_Data_Process(void)
{
......
INS_KF.measured_vector[2] = acc.z; // Z轴加速度
}
*/
#include "kalman_filter.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 私有函数声明 */
static void KF_AdjustHKR(KF_t *kf);
/**
* @brief 初始化卡尔曼滤波器并分配矩阵内存
*
* @param kf 卡尔曼滤波器结构体指针
* @param xhat_size 状态向量维度
* @param u_size 控制向量维度（无控制输入时设为0）
* @param z_size 量测向量维度
* @return int8_t 0对应没有错误
*/
int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size) {
if (kf == NULL) return -1;
kf->xhat_size = xhat_size;
kf->u_size = u_size;
kf->z_size = z_size;
kf->measurement_valid_num = 0;
/* 量测标志分配 */
kf->measurement_map = (uint8_t *)user_malloc(sizeof(uint8_t) * z_size);
memset(kf->measurement_map, 0, sizeof(uint8_t) * z_size);
kf->measurement_degree = (float *)user_malloc(sizeof(float) * z_size);
memset(kf->measurement_degree, 0, sizeof(float) * z_size);
kf->mat_r_diagonal_elements = (float *)user_malloc(sizeof(float) * z_size);
memset(kf->mat_r_diagonal_elements, 0, sizeof(float) * z_size);
kf->state_min_variance = (float *)user_malloc(sizeof(float) * xhat_size);
memset(kf->state_min_variance, 0, sizeof(float) * xhat_size);
kf->temp = (uint8_t *)user_malloc(sizeof(uint8_t) * z_size);
memset(kf->temp, 0, sizeof(uint8_t) * z_size);
/* 滤波数据分配 */
kf->filtered_value = (float *)user_malloc(sizeof(float) * xhat_size);
memset(kf->filtered_value, 0, sizeof(float) * xhat_size);
kf->measured_vector = (float *)user_malloc(sizeof(float) * z_size);
memset(kf->measured_vector, 0, sizeof(float) * z_size);
kf->control_vector = (float *)user_malloc(sizeof(float) * u_size);
memset(kf->control_vector, 0, sizeof(float) * u_size);
/* 状态估计 xhat x(k|k) */
kf->xhat_data = (float *)user_malloc(sizeof(float) * xhat_size);
memset(kf->xhat_data, 0, sizeof(float) * xhat_size);
KF_MatInit(&kf->xhat, kf->xhat_size, 1, kf->xhat_data);
/* 先验状态估计 xhatminus x(k|k-1) */
kf->xhatminus_data = (float *)user_malloc(sizeof(float) * xhat_size);
memset(kf->xhatminus_data, 0, sizeof(float) * xhat_size);
KF_MatInit(&kf->xhatminus, kf->xhat_size, 1, kf->xhatminus_data);
/* 控制向量 u */
if (u_size != 0) {
kf->u_data = (float *)user_malloc(sizeof(float) * u_size);
memset(kf->u_data, 0, sizeof(float) * u_size);
KF_MatInit(&kf->u, kf->u_size, 1, kf->u_data);
}
/* 量测向量 z */
kf->z_data = (float *)user_malloc(sizeof(float) * z_size);
memset(kf->z_data, 0, sizeof(float) * z_size);
KF_MatInit(&kf->z, kf->z_size, 1, kf->z_data);
/* 协方差矩阵 P(k|k) */
kf->P_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
memset(kf->P_data, 0, sizeof(float) * xhat_size * xhat_size);
KF_MatInit(&kf->P, kf->xhat_size, kf->xhat_size, kf->P_data);
/* 先验协方差矩阵 P(k|k-1) */
kf->Pminus_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
memset(kf->Pminus_data, 0, sizeof(float) * xhat_size * xhat_size);
KF_MatInit(&kf->Pminus, kf->xhat_size, kf->xhat_size, kf->Pminus_data);
/* 状态转移矩阵 F 及其转置 FT */
kf->F_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
kf->FT_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
memset(kf->F_data, 0, sizeof(float) * xhat_size * xhat_size);
memset(kf->FT_data, 0, sizeof(float) * xhat_size * xhat_size);
KF_MatInit(&kf->F, kf->xhat_size, kf->xhat_size, kf->F_data);
KF_MatInit(&kf->FT, kf->xhat_size, kf->xhat_size, kf->FT_data);
/* 控制矩阵 B */
if (u_size != 0) {
kf->B_data = (float *)user_malloc(sizeof(float) * xhat_size * u_size);
memset(kf->B_data, 0, sizeof(float) * xhat_size * u_size);
KF_MatInit(&kf->B, kf->xhat_size, kf->u_size, kf->B_data);
}
/* 量测矩阵 H 及其转置 HT */
kf->H_data = (float *)user_malloc(sizeof(float) * z_size * xhat_size);
kf->HT_data = (float *)user_malloc(sizeof(float) * xhat_size * z_size);
memset(kf->H_data, 0, sizeof(float) * z_size * xhat_size);
memset(kf->HT_data, 0, sizeof(float) * xhat_size * z_size);
KF_MatInit(&kf->H, kf->z_size, kf->xhat_size, kf->H_data);
KF_MatInit(&kf->HT, kf->xhat_size, kf->z_size, kf->HT_data);
/* 过程噪声协方差矩阵 Q */
kf->Q_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
memset(kf->Q_data, 0, sizeof(float) * xhat_size * xhat_size);
KF_MatInit(&kf->Q, kf->xhat_size, kf->xhat_size, kf->Q_data);
/* 量测噪声协方差矩阵 R */
kf->R_data = (float *)user_malloc(sizeof(float) * z_size * z_size);
memset(kf->R_data, 0, sizeof(float) * z_size * z_size);
KF_MatInit(&kf->R, kf->z_size, kf->z_size, kf->R_data);
/* 卡尔曼增益 K */
kf->K_data = (float *)user_malloc(sizeof(float) * xhat_size * z_size);
memset(kf->K_data, 0, sizeof(float) * xhat_size * z_size);
KF_MatInit(&kf->K, kf->xhat_size, kf->z_size, kf->K_data);
/* 临时矩阵分配 */
kf->S_data = (float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
kf->temp_matrix_data =
(float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
kf->temp_matrix_data1 =
(float *)user_malloc(sizeof(float) * xhat_size * xhat_size);
kf->temp_vector_data = (float *)user_malloc(sizeof(float) * xhat_size);
kf->temp_vector_data1 = (float *)user_malloc(sizeof(float) * xhat_size);
KF_MatInit(&kf->S, kf->xhat_size, kf->xhat_size, kf->S_data);
KF_MatInit(&kf->temp_matrix, kf->xhat_size, kf->xhat_size,
kf->temp_matrix_data);
KF_MatInit(&kf->temp_matrix1, kf->xhat_size, kf->xhat_size,
kf->temp_matrix_data1);
KF_MatInit(&kf->temp_vector, kf->xhat_size, 1, kf->temp_vector_data);
KF_MatInit(&kf->temp_vector1, kf->xhat_size, 1, kf->temp_vector_data1);
/* 初始化跳过标志 */
kf->skip_eq1 = 0;
kf->skip_eq2 = 0;
kf->skip_eq3 = 0;
kf->skip_eq4 = 0;
kf->skip_eq5 = 0;
/* 初始化用户函数指针 */
kf->User_Func0_f = NULL;
kf->User_Func1_f = NULL;
kf->User_Func2_f = NULL;
kf->User_Func3_f = NULL;
kf->User_Func4_f = NULL;
kf->User_Func5_f = NULL;
kf->User_Func6_f = NULL;
return 0;
}
/**
* @brief 获取量测并在启用自动调整时调整矩阵
*
* @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误
*/
int8_t KF_Measure(KF_t *kf) {
if (kf == NULL) return -1;
/* 根据量测有效性自动调整 H, K, R 矩阵 */
if (kf->use_auto_adjustment != 0) {
KF_AdjustHKR(kf);
} else {
memcpy(kf->z_data, kf->measured_vector, sizeof(float) * kf->z_size);
memset(kf->measured_vector, 0, sizeof(float) * kf->z_size);
}
memcpy(kf->u_data, kf->control_vector, sizeof(float) * kf->u_size);
return 0;
}
/**
* @brief 步骤1：先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u
*
* @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误
*/
int8_t KF_PredictState(KF_t *kf) {
if (kf == NULL) return -1;
if (!kf->skip_eq1) {
if (kf->u_size > 0) {
/* 有控制输入: xhat'(k) = F·xhat(k-1) + B·u */
kf->temp_vector.numRows = kf->xhat_size;
kf->temp_vector.numCols = 1;
kf->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->temp_vector);
kf->temp_vector1.numRows = kf->xhat_size;
kf->temp_vector1.numCols = 1;
kf->mat_status = KF_MatMult(&kf->B, &kf->u, &kf->temp_vector1);
kf->mat_status =
KF_MatAdd(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus);
} else {
/* 无控制输入: xhat'(k) = F·xhat(k-1) */
kf->mat_status = KF_MatMult(&kf->F, &kf->xhat, &kf->xhatminus);
}
}
return 0;
}
/**
* @brief 步骤2：先验协方差 - P'(k) = F·P(k-1)·F^T + Q
*
* @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误
*/
int8_t KF_PredictCovariance(KF_t *kf) {
if (kf == NULL) return -1;
if (!kf->skip_eq2) {
kf->mat_status = KF_MatTrans(&kf->F, &kf->FT);
kf->mat_status = KF_MatMult(&kf->F, &kf->P, &kf->Pminus);
kf->temp_matrix.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->FT.numCols;
/* F·P(k-1)·F^T */
kf->mat_status = KF_MatMult(&kf->Pminus, &kf->FT, &kf->temp_matrix);
kf->mat_status = KF_MatAdd(&kf->temp_matrix, &kf->Q, &kf->Pminus);
}
return 0;
}
/**
* @brief 步骤3：卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R)
*
* @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误
*/
int8_t KF_CalcGain(KF_t *kf) {
if (kf == NULL) return -1;
if (!kf->skip_eq3) {
kf->mat_status = KF_MatTrans(&kf->H, &kf->HT);
kf->temp_matrix.numRows = kf->H.numRows;
kf->temp_matrix.numCols = kf->Pminus.numCols;
/* H·P'(k) */
kf->mat_status = KF_MatMult(&kf->H, &kf->Pminus, &kf->temp_matrix);
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->HT.numCols;
/* H·P'(k)·H^T */
kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1);
kf->S.numRows = kf->R.numRows;
kf->S.numCols = kf->R.numCols;
/* S = H·P'(k)·H^T + R */
kf->mat_status = KF_MatAdd(&kf->temp_matrix1, &kf->R, &kf->S);
/* S^-1 */
kf->mat_status = KF_MatInv(&kf->S, &kf->temp_matrix1);
kf->temp_matrix.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->HT.numCols;
/* P'(k)·H^T */
kf->mat_status = KF_MatMult(&kf->Pminus, &kf->HT, &kf->temp_matrix);
/* K = P'(k)·H^T·S^-1 */
kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
}
return 0;
}
/**
* @brief 步骤4：状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k))
*
* @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误
*/
int8_t KF_UpdateState(KF_t *kf) {
if (kf == NULL) return -1;
if (!kf->skip_eq4) {
kf->temp_vector.numRows = kf->H.numRows;
kf->temp_vector.numCols = 1;
/* H·xhat'(k) */
kf->mat_status = KF_MatMult(&kf->H, &kf->xhatminus, &kf->temp_vector);
kf->temp_vector1.numRows = kf->z.numRows;
kf->temp_vector1.numCols = 1;
/* 新息: z - H·xhat'(k) */
kf->mat_status = KF_MatSub(&kf->z, &kf->temp_vector, &kf->temp_vector1);
kf->temp_vector.numRows = kf->K.numRows;
kf->temp_vector.numCols = 1;
/* K·新息 */
kf->mat_status = KF_MatMult(&kf->K, &kf->temp_vector1, &kf->temp_vector);
/* xhat = xhat' + K·新息 */
kf->mat_status = KF_MatAdd(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
}
return 0;
}
/**
* @brief 步骤5：协方差更新 - P(k) = P'(k) - K·H·P'(k)
*
* @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误
*/
int8_t KF_UpdateCovariance(KF_t *kf) {
if (kf == NULL) return -1;
if (!kf->skip_eq5) {
kf->temp_matrix.numRows = kf->K.numRows;
kf->temp_matrix.numCols = kf->H.numCols;
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->Pminus.numCols;
/* K·H */
kf->mat_status = KF_MatMult(&kf->K, &kf->H, &kf->temp_matrix);
/* K·H·P'(k) */
kf->mat_status = KF_MatMult(&kf->temp_matrix, &kf->Pminus, &kf->temp_matrix1);
/* P = P' - K·H·P' */
kf->mat_status = KF_MatSub(&kf->Pminus, &kf->temp_matrix1, &kf->P);
}
return 0;
}
/**
* @brief 执行完整的卡尔曼滤波周期（五大方程）
*
* 实现标准KF，并支持用户自定义函数钩子用于扩展（EKF/UKF/ESKF/AUKF）。
* 每个步骤都可以通过 User_Func 回调函数替换。
*
* @param kf 卡尔曼滤波器结构体指针
* @return float* 滤波后的状态估计值指针
*/
float *KF_Update(KF_t *kf) {
if (kf == NULL) return NULL;
/* 步骤0: 量测获取和矩阵调整 */
KF_Measure(kf);
if (kf->User_Func0_f != NULL) kf->User_Func0_f(kf);
/* 步骤1: 先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u */
KF_PredictState(kf);
if (kf->User_Func1_f != NULL) kf->User_Func1_f(kf);
/* 步骤2: 先验协方差 - P'(k) = F·P(k-1)·F^T + Q */
KF_PredictCovariance(kf);
if (kf->User_Func2_f != NULL) kf->User_Func2_f(kf);
/* 量测更新（仅在存在有效量测时执行） */
if (kf->measurement_valid_num != 0 || kf->use_auto_adjustment == 0) {
/* 步骤3: 卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R) */
KF_CalcGain(kf);
if (kf->User_Func3_f != NULL) kf->User_Func3_f(kf);
/* 步骤4: 状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k)) */
KF_UpdateState(kf);
if (kf->User_Func4_f != NULL) kf->User_Func4_f(kf);
/* 步骤5: 协方差更新 - P(k) = P'(k) - K·H·P'(k) */
KF_UpdateCovariance(kf);
} else {
/* 无有效量测 - 仅预测 */
memcpy(kf->xhat_data, kf->xhatminus_data, sizeof(float) * kf->xhat_size);
memcpy(kf->P_data, kf->Pminus_data,
sizeof(float) * kf->xhat_size * kf->xhat_size);
}
/* 后处理钩子 */
if (kf->User_Func5_f != NULL) kf->User_Func5_f(kf);
/* 防过度收敛：强制最小方差 */
for (uint8_t i = 0; i < kf->xhat_size; i++) {
if (kf->P_data[i * kf->xhat_size + i] < kf->state_min_variance[i])
kf->P_data[i * kf->xhat_size + i] = kf->state_min_variance[i];
}
/* 复制结果到输出 */
memcpy(kf->filtered_value, kf->xhat_data, sizeof(float) * kf->xhat_size);
/* 附加后处理钩子 */
if (kf->User_Func6_f != NULL) kf->User_Func6_f(kf);
return kf->filtered_value;
}
/**
* @brief 重置卡尔曼滤波器状态
*
* @param kf 卡尔曼滤波器结构体指针
*/
void KF_Reset(KF_t *kf) {
if (kf == NULL) return;
memset(kf->xhat_data, 0, sizeof(float) * kf->xhat_size);
memset(kf->xhatminus_data, 0, sizeof(float) * kf->xhat_size);
memset(kf->filtered_value, 0, sizeof(float) * kf->xhat_size);
kf->measurement_valid_num = 0;
}
/**
* @brief 根据量测有效性动态调整 H, R, K 矩阵
*
* 该函数根据当前周期中哪些量测有效（非零）来重建量测相关矩阵。
* 支持具有不同采样率的异步传感器。
*
* @param kf 卡尔曼滤波器结构体指针
*/
static void KF_AdjustHKR(KF_t *kf) {
kf->measurement_valid_num = 0;
/* 复制并重置量测向量 */
memcpy(kf->z_data, kf->measured_vector, sizeof(float) * kf->z_size);
memset(kf->measured_vector, 0, sizeof(float) * kf->z_size);
/* 清空 H 和 R 矩阵 */
memset(kf->R_data, 0, sizeof(float) * kf->z_size * kf->z_size);
memset(kf->H_data, 0, sizeof(float) * kf->xhat_size * kf->z_size);
/* 识别有效量测并重建 z, H */
for (uint8_t i = 0; i < kf->z_size; i++) {
if (kf->z_data[i] != 0) { /* 非零表示有效量测 */
/* 将有效量测压缩到 z */
kf->z_data[kf->measurement_valid_num] = kf->z_data[i];
kf->temp[kf->measurement_valid_num] = i;
/* 重建 H 矩阵行 */
kf->H_data[kf->xhat_size * kf->measurement_valid_num +
kf->measurement_map[i] - 1] = kf->measurement_degree[i];
kf->measurement_valid_num++;
}
}
/* 用有效量测方差重建 R 矩阵 */
for (uint8_t i = 0; i < kf->measurement_valid_num; i++) {
kf->R_data[i * kf->measurement_valid_num + i] =
kf->mat_r_diagonal_elements[kf->temp[i]];
}
/* 调整矩阵维度以匹配有效量测数量 */
kf->H.numRows = kf->measurement_valid_num;
kf->H.numCols = kf->xhat_size;
kf->HT.numRows = kf->xhat_size;
kf->HT.numCols = kf->measurement_valid_num;
kf->R.numRows = kf->measurement_valid_num;
kf->R.numCols = kf->measurement_valid_num;
kf->K.numRows = kf->xhat_size;
kf->K.numCols = kf->measurement_valid_num;
kf->z.numRows = kf->measurement_valid_num;
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

199
assets/User_code/component/kalman_filter/kalman_filter.h Normal file
View File

@ -0,0 +1,199 @@
/*
卡尔曼滤波器
支持动态量测调整，使用ARM CMSIS DSP优化矩阵运算
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "arm_math.h"
#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 内存分配配置 */
#ifndef user_malloc
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc /* FreeRTOS堆分配 */
#else
#define user_malloc malloc /* 标准C库分配 */
#endif
#endif
/* ARM CMSIS DSP 矩阵运算别名 */
#define KF_Mat arm_matrix_instance_f32
#define KF_MatInit arm_mat_init_f32
#define KF_MatAdd arm_mat_add_f32
#define KF_MatSub arm_mat_sub_f32
#define KF_MatMult arm_mat_mult_f32
#define KF_MatTrans arm_mat_trans_f32
#define KF_MatInv arm_mat_inverse_f32
/* 卡尔曼滤波器主结构体 */
typedef struct KF_s {
/* 输出和输入向量 */
float *filtered_value; /* 滤波后的状态估计输出 */
float *measured_vector; /* 量测输入向量 */
float *control_vector; /* 控制输入向量 */
/* 维度 */
uint8_t xhat_size; /* 状态向量维度 */
uint8_t u_size; /* 控制向量维度 */
uint8_t z_size; /* 量测向量维度 */
/* 自动调整参数 */
uint8_t use_auto_adjustment; /* 启用动态 H/R/K 调整 */
uint8_t measurement_valid_num; /* 有效量测数量 */
/* 量测配置 */
uint8_t *measurement_map; /* 量测到状态的映射 */
float *measurement_degree; /* 每个量测的H矩阵元素值 */
float *mat_r_diagonal_elements; /* 量测噪声方差（R对角线） */
float *state_min_variance; /* 最小状态方差（防过度收敛） */
uint8_t *temp; /* 临时缓冲区 */
/* 方程跳过标志（用于自定义用户函数） */
uint8_t skip_eq1, skip_eq2, skip_eq3, skip_eq4, skip_eq5;
/* 卡尔曼滤波器矩阵 */
KF_Mat xhat; /* 状态估计 x(k|k) */
KF_Mat xhatminus; /* 先验状态估计 x(k|k-1) */
KF_Mat u; /* 控制向量 */
KF_Mat z; /* 量测向量 */
KF_Mat P; /* 后验误差协方差 P(k|k) */
KF_Mat Pminus; /* 先验误差协方差 P(k|k-1) */
KF_Mat F, FT; /* 状态转移矩阵及其转置 */
KF_Mat B; /* 控制矩阵 */
KF_Mat H, HT; /* 量测矩阵及其转置 */
KF_Mat Q; /* 过程噪声协方差 */
KF_Mat R; /* 量测噪声协方差 */
KF_Mat K; /* 卡尔曼增益 */
KF_Mat S; /* 临时矩阵 S */
KF_Mat temp_matrix, temp_matrix1; /* 临时矩阵 */
KF_Mat temp_vector, temp_vector1; /* 临时向量 */
int8_t mat_status; /* 矩阵运算状态 */
/* 用户自定义函数指针（用于EKF/UKF/ESKF扩展） */
void (*User_Func0_f)(struct KF_s *kf); /* 自定义量测处理 */
void (*User_Func1_f)(struct KF_s *kf); /* 自定义状态预测 */
void (*User_Func2_f)(struct KF_s *kf); /* 自定义协方差预测 */
void (*User_Func3_f)(struct KF_s *kf); /* 自定义卡尔曼增益计算 */
void (*User_Func4_f)(struct KF_s *kf); /* 自定义状态更新 */
void (*User_Func5_f)(struct KF_s *kf); /* 自定义后处理 */
void (*User_Func6_f)(struct KF_s *kf); /* 附加自定义函数 */
/* 矩阵数据存储指针 */
float *xhat_data, *xhatminus_data;
float *u_data;
float *z_data;
float *P_data, *Pminus_data;
float *F_data, *FT_data;
float *B_data;
float *H_data, *HT_data;
float *Q_data;
float *R_data;
float *K_data;
float *S_data;
float *temp_matrix_data, *temp_matrix_data1;
float *temp_vector_data, *temp_vector_data1;
} KF_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/**
* @brief 初始化卡尔曼滤波器并分配矩阵内存
*
* @param kf 卡尔曼滤波器结构体指针
* @param xhat_size 状态向量维度
* @param u_size 控制向量维度（无控制输入时设为0）
* @param z_size 量测向量维度
* @return int8_t 0对应没有错误
*/
int8_t KF_Init(KF_t *kf, uint8_t xhat_size, uint8_t u_size, uint8_t z_size);
/**
* @brief 获取量测并在启用自动调整时调整矩阵
*
* @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误
*/
int8_t KF_Measure(KF_t *kf);
/**
* @brief 步骤1：先验状态估计 - xhat'(k) = F·xhat(k-1) + B·u
*
* @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误
*/
int8_t KF_PredictState(KF_t *kf);
/**
* @brief 步骤2：先验协方差 - P'(k) = F·P(k-1)·F^T + Q
*
* @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误
*/
int8_t KF_PredictCovariance(KF_t *kf);
/**
* @brief 步骤3：卡尔曼增益 - K = P'(k)·H^T / (H·P'(k)·H^T + R)
*
* @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误
*/
int8_t KF_CalcGain(KF_t *kf);
/**
* @brief 步骤4：状态更新 - xhat(k) = xhat'(k) + K·(z - H·xhat'(k))
*
* @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误
*/
int8_t KF_UpdateState(KF_t *kf);
/**
* @brief 步骤5：协方差更新 - P(k) = P'(k) - K·H·P'(k)
*
* @param kf 卡尔曼滤波器结构体指针
* @return int8_t 0对应没有错误
*/
int8_t KF_UpdateCovariance(KF_t *kf);
/**
* @brief 执行完整的卡尔曼滤波周期（五大方程）
*
* @param kf 卡尔曼滤波器结构体指针
* @return float* 滤波后的状态估计值指针
*/
float *KF_Update(KF_t *kf);
/**
* @brief 重置卡尔曼滤波器状态
*
* @param kf 卡尔曼滤波器结构体指针
*/
void KF_Reset(KF_t *kf);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

2
assets/User_code/config.csv
View File

@ -1,4 +1,4 @@
bsp,can,fdcan,dwt,gpio,i2c,mm,spi,uart,pwm,time,flash
component,ahrs,capacity,cmd,crc8,crc16,error_detect,filter,FreeRTOS_CLI,limiter,mixer,pid,ui,user_math
device,dr16,bmi088,ist8310,motor,motor_rm,motor_dm,motor_vesc,motor_lk,motor_lz,motor_odrive,dm_imu,rc_can,servo,buzzer,led,ws2812,vofa,ops9,oid,lcd_driver
device,dr16,bmi088,ist8310,motor,motor_rm,motor_dm,motor_vesc,motor_lk,motor_lz,motor_odrive,dm_imu,rc_can,servo,buzzer,led,ws2812,vofa,ops9,oid,lcd_driver,mrobot
module,
1 bsp,can,fdcan,dwt,gpio,i2c,mm,spi,uart,pwm,time,flash
2 component,ahrs,capacity,cmd,crc8,crc16,error_detect,filter,FreeRTOS_CLI,limiter,mixer,pid,ui,user_math
3 device,dr16,bmi088,ist8310,motor,motor_rm,motor_dm,motor_vesc,motor_lk,motor_lz,motor_odrive,dm_imu,rc_can,servo,buzzer,led,ws2812,vofa,ops9,oid,lcd_driver device,dr16,bmi088,ist8310,motor,motor_rm,motor_dm,motor_vesc,motor_lk,motor_lz,motor_odrive,dm_imu,rc_can,servo,buzzer,led,ws2812,vofa,ops9,oid,lcd_driver,mrobot
4 module,

15
assets/User_code/device/config.yaml
View File

@ -260,3 +260,18 @@ devices:
files:
header: "lcd.h"
source: "lcd.c"
mrobot:
name: "MRobot CLI"
description: "基于 FreeRTOS CLI 的嵌入式调试命令行系统，支持设备注册与监控、类 Unix 文件系统命令、htop 风格任务监控等"
dependencies:
bsp: ["uart", "mm"]
component: ["freertos_cli"]
bsp_requirements:
- type: "uart"
var_name: "BSP_UART_MROBOT"
description: "用于 MRobot CLI 命令行交互"
thread_signals: []
files:
header: "mrobot.h"
source: "mrobot.c"

874
assets/User_code/device/mrobot/mrobot.c Normal file
View File

@ -0,0 +1,874 @@
/**
* @file mrobot.c
* @brief MRobot CLI 实现
*/
/* Includes ----------------------------------------------------------------- */
#include "device/mrobot.h"
#include "component/freertos_cli.h"
#include "bsp/uart.h"
#include "bsp/mm.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <FreeRTOS.h>
#include <task.h>
#include <semphr.h>
#include <cmsis_os2.h>
/* Private constants -------------------------------------------------------- */
static const char *const CLI_WELCOME_MESSAGE =
"\r\n"
" __ __ _____ _ _ \r\n"
" | \\/ | __ \\ | | | | \r\n"
" | \\ / | |__) |___ | |__ ___ | |_ \r\n"
" | |\\/| | _ // _ \\| '_ \\ / _ \\| __|\r\n"
" | | | | | \\ \\ (_) | |_) | (_) | |_ \r\n"
" |_| |_|_| \\_\\___/|_.__/ \\___/ \\__|\r\n"
" ------------------------------------\r\n"
" Welcome to use MRobot CLI. Type 'help' to view a list of registered commands.\r\n"
"\r\n";
/* ANSI 转义序列 */
#define ANSI_CLEAR_SCREEN "\033[2J\033[H"
#define ANSI_CURSOR_HOME "\033[H"
#define ANSI_BACKSPACE "\b \b"
/* Private types ------------------------------------------------------------ */
/* CLI 上下文结构体 - 封装所有状态 */
typedef struct {
/* 设备管理 */
MRobot_Device_t devices[MROBOT_MAX_DEVICES];
uint8_t device_count;
/* 自定义命令 */
CLI_Command_Definition_t *custom_cmds[MROBOT_MAX_CUSTOM_COMMANDS];
uint8_t custom_cmd_count;
/* CLI 状态 */
MRobot_State_t state;
char current_path[MROBOT_PATH_MAX_LEN];
/* 命令缓冲区 */
uint8_t cmd_buffer[MROBOT_CMD_BUFFER_SIZE];
volatile uint8_t cmd_index;
volatile bool cmd_ready;
/* UART 相关 */
uint8_t uart_rx_char;
volatile bool tx_complete;
volatile bool htop_exit;
/* 输出缓冲区 */
char output_buffer[MROBOT_OUTPUT_BUFFER_SIZE];
/* 初始化标志 */
bool initialized;
/* 互斥锁 */
SemaphoreHandle_t mutex;
} MRobot_Context_t;
/* Private variables -------------------------------------------------------- */
static MRobot_Context_t ctx = {
.device_count = 0,
.custom_cmd_count = 0,
.state = MROBOT_STATE_IDLE,
.current_path = "/",
.cmd_index = 0,
.cmd_ready = false,
.tx_complete = true,
.htop_exit = false,
.initialized = false,
.mutex = NULL
};
/* Private function prototypes ---------------------------------------------- */
/* 命令处理函数 */
static BaseType_t cmd_help(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
static BaseType_t cmd_htop(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_show(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_va(char *buf, size_t size, const char *fmt, va_list args);
/* CLI 命令定义表 */
static const CLI_Command_Definition_t builtin_commands[] = {
{ "help", " --help: 显示所有可用命令\r\n", cmd_help, 0 },
{ "htop", " --htop: 动态显示 FreeRTOS 任务状态 (按 'q' 退出)\r\n", cmd_htop, 0 },
{ "cd", " --cd <path>: 切换目录\r\n", cmd_cd, 1 },
{ "ls", " --ls: 列出当前目录内容\r\n", cmd_ls, 0 },
{ "show", " --show [device] [count]: 显示设备信息\r\n", cmd_show, -1 },
};
#define BUILTIN_CMD_COUNT (sizeof(builtin_commands) / sizeof(builtin_commands[0]))
/* ========================================================================== */
/* 辅助函数实现 */
/* ========================================================================== */
/**
* @brief 发送字符串到 UART（阻塞等待完成）
*/
static void send_string(const char *str) {
if (str == NULL || *str == '\0') return;
ctx.tx_complete = false;
BSP_UART_Transmit(MROBOT_UART_PORT, (uint8_t *)str, strlen(str), true);
while (!ctx.tx_complete) { osDelay(1); }
}
/**
* @brief 发送命令提示符
*/
static void send_prompt(void) {
char prompt[MROBOT_PATH_MAX_LEN + 32];
snprintf(prompt, sizeof(prompt), MROBOT_USER_NAME "@" MROBOT_HOST_NAME ":%s$ ", ctx.current_path);
send_string(prompt);
}
/**
* @brief UART 发送完成回调
*/
static void uart_tx_callback(void) {
ctx.tx_complete = true;
}
/**
* @brief UART 接收回调
*/
static void uart_rx_callback(void) {
uint8_t ch = ctx.uart_rx_char;
/* htop 模式下检查退出键 */
if (ctx.state == MROBOT_STATE_HTOP) {
if (ch == 'q' || ch == 'Q' || ch == 27) {
ctx.htop_exit = true;
}
BSP_UART_Receive(MROBOT_UART_PORT, &ctx.uart_rx_char, 1, false);
return;
}
/* 正常命令输入处理 */
if (ch == '\r' || ch == '\n') {
if (ctx.cmd_index > 0) {
ctx.cmd_buffer[ctx.cmd_index] = '\0';
ctx.cmd_ready = true;
BSP_UART_Transmit(MROBOT_UART_PORT, (uint8_t *)"\r\n", 2, false);
}
} else if (ch == 127 || ch == 8) { /* 退格键 */
if (ctx.cmd_index > 0) {
ctx.cmd_index--;
BSP_UART_Transmit(MROBOT_UART_PORT, (uint8_t *)ANSI_BACKSPACE, 3, false);
}
} else if (ch >= 32 && ch < 127 && ctx.cmd_index < sizeof(ctx.cmd_buffer) - 1) {
ctx.cmd_buffer[ctx.cmd_index++] = ch;
BSP_UART_Transmit(MROBOT_UART_PORT, &ch, 1, false);
}
BSP_UART_Receive(MROBOT_UART_PORT, &ctx.uart_rx_char, 1, false);
}
/* ========================================================================== */
/* CLI 命令实现 */
/* ========================================================================== */
/**
* @brief help 命令 - 显示帮助信息
*/
static BaseType_t cmd_help(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
(void)pcCommandString;
int offset = snprintf(pcWriteBuffer, xWriteBufferLen,
"MRobot CLI v2.0\r\n"
"================\r\n"
"Built-in Commands:\r\n");
for (size_t i = 0; i < BUILTIN_CMD_COUNT && offset < (int)xWriteBufferLen - 50; i++) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
" %s", builtin_commands[i].pcHelpString);
}
if (ctx.custom_cmd_count > 0) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
"\r\nCustom Commands:\r\n");
for (uint8_t i = 0; i < ctx.custom_cmd_count && offset < (int)xWriteBufferLen - 50; i++) {
if (ctx.custom_cmds[i] != NULL) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
" %s", ctx.custom_cmds[i]->pcHelpString);
}
}
}
return pdFALSE;
}
/**
* @brief htop 命令 - 设置 htop 模式标志
*/
static BaseType_t cmd_htop(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
(void)pcCommandString;
(void)pcWriteBuffer;
(void)xWriteBufferLen;
/* htop 模式在 MRobot_Run 中处理 */
return pdFALSE;
}
/**
* @brief cd 命令 - 切换目录
*/
static BaseType_t cmd_cd(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
const char *param;
BaseType_t param_len;
param = FreeRTOS_CLIGetParameter(pcCommandString, 1, &param_len);
if (param == NULL) {
/* 无参数时切换到根目录 */
strcpy(ctx.current_path, "/");
snprintf(pcWriteBuffer, xWriteBufferLen, "Changed to: %s\r\n", ctx.current_path);
return pdFALSE;
}
/* 安全复制路径参数 */
char path[MROBOT_PATH_MAX_LEN];
size_t copy_len = (size_t)param_len < sizeof(path) - 1 ? (size_t)param_len : sizeof(path) - 1;
strncpy(path, param, copy_len);
path[copy_len] = '\0';
/* 路径解析 */
if (strcmp(path, "/") == 0 || strcmp(path, "..") == 0 || strcmp(path, "~") == 0) {
strcpy(ctx.current_path, "/");
} else if (strcmp(path, "dev") == 0 || strcmp(path, "/dev") == 0) {
strcpy(ctx.current_path, "/dev");
} else if (strcmp(path, "modules") == 0 || strcmp(path, "/modules") == 0) {
strcpy(ctx.current_path, "/modules");
} else {
snprintf(pcWriteBuffer, xWriteBufferLen, "Error: Directory '%s' not found\r\n", path);
return pdFALSE;
}
snprintf(pcWriteBuffer, xWriteBufferLen, "Changed to: %s\r\n", ctx.current_path);
return pdFALSE;
}
/**
* @brief ls 命令 - 列出目录内容
*/
static BaseType_t cmd_ls(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
(void)pcCommandString;
int offset = 0;
if (strcmp(ctx.current_path, "/") == 0) {
snprintf(pcWriteBuffer, xWriteBufferLen,
"dev/\r\n"
"modules/\r\n");
} else if (strcmp(ctx.current_path, "/dev") == 0) {
offset = snprintf(pcWriteBuffer, xWriteBufferLen,
"Device List (%d devices)\r\n\r\n",
ctx.device_count);
if (ctx.device_count == 0) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
" (No devices)\r\n");
} else {
/* 直接列出所有设备 */
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) {
snprintf(pcWriteBuffer, xWriteBufferLen,
"Module functions not yet implemented\r\n");
}
return pdFALSE;
}
/**
* @brief show 命令 - 显示设备信息
*/
static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
const char *param;
const char *count_param;
BaseType_t param_len, count_param_len;
/* 使用局部静态变量跟踪多次打印状态 */
static uint32_t print_count = 0;
static uint32_t current_iter = 0;
static char target_device[MROBOT_DEVICE_NAME_LEN] = {0};
/* 首次调用时解析参数 */
if (current_iter == 0) {
/* 检查是否在 /dev 目录 */
if (strcmp(ctx.current_path, "/dev") != 0) {
snprintf(pcWriteBuffer, xWriteBufferLen,
"Error: 'show' command only works in /dev directory\r\n"
"Hint: Use 'cd /dev' to switch to device directory\r\n");
return pdFALSE;
}
param = FreeRTOS_CLIGetParameter(pcCommandString, 1, &param_len);
count_param = FreeRTOS_CLIGetParameter(pcCommandString, 2, &count_param_len);
/* 解析打印次数 */
print_count = 1;
if (count_param != NULL) {
char count_str[16];
size_t copy_len = (size_t)count_param_len < sizeof(count_str) - 1 ?
(size_t)count_param_len : sizeof(count_str) - 1;
strncpy(count_str, count_param, copy_len);
count_str[copy_len] = '\0';
int parsed = atoi(count_str);
if (parsed > 0 && parsed <= 1000) {
print_count = (uint32_t)parsed;
}
}
/* 保存目标设备名称 */
if (param != NULL) {
size_t copy_len = (size_t)param_len < sizeof(target_device) - 1 ?
(size_t)param_len : sizeof(target_device) - 1;
strncpy(target_device, param, copy_len);
target_device[copy_len] = '\0';
} else {
target_device[0] = '\0';
}
}
int offset = 0;
/* 连续打印模式：清屏 */
if (print_count > 1) {
offset = snprintf(pcWriteBuffer, xWriteBufferLen, "%s[%lu/%lu]\r\n",
ANSI_CLEAR_SCREEN,
(unsigned long)(current_iter + 1),
(unsigned long)print_count);
}
if (target_device[0] == '\0') {
/* 显示所有设备 */
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
"=== All Devices ===\r\n\r\n");
for (uint8_t i = 0; i < ctx.device_count && offset < (int)xWriteBufferLen - 100; i++) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
"--- %s ---\r\n", ctx.devices[i].name);
if (ctx.devices[i].print_cb != NULL) {
int written = ctx.devices[i].print_cb(ctx.devices[i].data,
pcWriteBuffer + offset,
xWriteBufferLen - offset);
offset += (written > 0) ? written : 0;
} else {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
" (No print function)\r\n");
}
}
if (ctx.device_count == 0) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
" (No devices registered)\r\n");
}
} else {
/* 显示指定设备 */
const MRobot_Device_t *dev = MRobot_FindDevice(target_device);
if (dev == NULL) {
snprintf(pcWriteBuffer, xWriteBufferLen,
"Error: Device '%s' not found\r\n",
target_device);
current_iter = 0;
return pdFALSE;
}
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
"=== %s ===\r\n", dev->name);
if (dev->print_cb != NULL) {
dev->print_cb(dev->data, pcWriteBuffer + offset, xWriteBufferLen - offset);
} else {
snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
" (No print function)\r\n");
}
}
/* 判断是否继续打印 */
current_iter++;
if (current_iter < print_count) {
osDelay(MROBOT_HTOP_REFRESH_MS);
return pdTRUE;
} else {
current_iter = 0;
return pdFALSE;
}
}
/* ============================================================================
* htop 模式处理
* ========================================================================== */
static void handle_htop_mode(void) {
send_string(ANSI_CLEAR_SCREEN);
send_string("=== MRobot Task Monitor (Press 'q' to exit) ===\r\n\r\n");
/* 获取任务列表 */
char task_buffer[1024];
char display_line[128];
vTaskList(task_buffer);
/* 表头 */
send_string("Task Name State Prio Stack Num\r\n");
send_string("------------------------------------------------\r\n");
/* 解析并格式化任务列表 */
char *line = strtok(task_buffer, "\r\n");
while (line != NULL) {
char name[17] = {0};
char state_char = '?';
int prio = 0, stack = 0, num = 0;
if (sscanf(line, "%16s %c %d %d %d", name, &state_char, &prio, &stack, &num) == 5) {
const char *state_str;
switch (state_char) {
case 'R': state_str = "Running"; break;
case 'B': state_str = "Blocked"; break;
case 'S': state_str = "Suspend"; break;
case 'D': state_str = "Deleted"; break;
case 'X': state_str = "Ready "; break;
default: state_str = "Unknown"; break;
}
snprintf(display_line, sizeof(display_line),
"%-16s %-8s %-4d %-8d %-4d\r\n",
name, state_str, prio, stack, num);
send_string(display_line);
}
line = strtok(NULL, "\r\n");
}
/* 显示系统信息 */
snprintf(display_line, sizeof(display_line),
"------------------------------------------------\r\n"
"System Tick: %lu | Free Heap: %lu bytes\r\n",
(unsigned long)xTaskGetTickCount(),
(unsigned long)xPortGetFreeHeapSize());
send_string(display_line);
/* 检查退出 */
if (ctx.htop_exit) {
ctx.state = MROBOT_STATE_IDLE;
ctx.htop_exit = false;
send_string(ANSI_CLEAR_SCREEN);
send_prompt();
}
osDelay(MROBOT_HTOP_REFRESH_MS);
}
/* ========================================================================== */
/* 公共 API 实现 */
/* ========================================================================== */
void MRobot_Init(void) {
if (ctx.initialized) return;
/* 创建互斥锁 */
ctx.mutex = xSemaphoreCreateMutex();
/* 初始化状态 */
memset(ctx.devices, 0, sizeof(ctx.devices));
ctx.device_count = 0;
ctx.custom_cmd_count = 0;
ctx.state = MROBOT_STATE_IDLE;
strcpy(ctx.current_path, "/");
ctx.cmd_index = 0;
ctx.cmd_ready = false;
ctx.tx_complete = true;
ctx.htop_exit = false;
/* 注册内置命令 */
for (size_t i = 0; i < BUILTIN_CMD_COUNT; i++) {
FreeRTOS_CLIRegisterCommand(&builtin_commands[i]);
}
/* 注册 UART 回调 */
BSP_UART_RegisterCallback(MROBOT_UART_PORT, BSP_UART_RX_CPLT_CB, uart_rx_callback);
BSP_UART_RegisterCallback(MROBOT_UART_PORT, BSP_UART_TX_CPLT_CB, uart_tx_callback);
/* 启动 UART 接收 */
BSP_UART_Receive(MROBOT_UART_PORT, &ctx.uart_rx_char, 1, false);
/* 等待用户按下回车 */
while (ctx.uart_rx_char != '\r' && ctx.uart_rx_char != '\n') {
osDelay(10);
}
/* 发送欢迎消息和提示符 */
send_string(CLI_WELCOME_MESSAGE);
send_prompt();
ctx.initialized = true;
}
void MRobot_DeInit(void) {
if (!ctx.initialized) return;
/* 释放自定义命令内存 */
for (uint8_t i = 0; i < ctx.custom_cmd_count; i++) {
if (ctx.custom_cmds[i] != NULL) {
BSP_Free(ctx.custom_cmds[i]);
ctx.custom_cmds[i] = NULL;
}
}
/* 删除互斥锁 */
if (ctx.mutex != NULL) {
vSemaphoreDelete(ctx.mutex);
ctx.mutex = NULL;
}
ctx.initialized = false;
}
MRobot_State_t MRobot_GetState(void) {
return ctx.state;
}
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;
}
if (ctx.device_count >= MROBOT_MAX_DEVICES) {
return MROBOT_ERR_FULL;
}
/* 检查重名 */
for (uint8_t i = 0; i < ctx.device_count; i++) {
if (strcmp(ctx.devices[i].name, name) == 0) {
return MROBOT_ERR_INVALID_ARG; /* 设备名已存在 */
}
}
/* 线程安全写入 */
if (ctx.mutex != NULL) {
xSemaphoreTake(ctx.mutex, portMAX_DELAY);
}
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].data = data;
ctx.devices[ctx.device_count].print_cb = print_cb;
ctx.device_count++;
if (ctx.mutex != NULL) {
xSemaphoreGive(ctx.mutex);
}
return MROBOT_OK;
}
MRobot_Error_t MRobot_UnregisterDevice(const char *name) {
if (name == NULL) {
return MROBOT_ERR_NULL_PTR;
}
if (ctx.mutex != NULL) {
xSemaphoreTake(ctx.mutex, portMAX_DELAY);
}
for (uint8_t i = 0; i < ctx.device_count; i++) {
if (strcmp(ctx.devices[i].name, name) == 0) {
/* 移动后续设备 */
for (uint8_t j = i; j < ctx.device_count - 1; j++) {
ctx.devices[j] = ctx.devices[j + 1];
}
ctx.device_count--;
if (ctx.mutex != NULL) {
xSemaphoreGive(ctx.mutex);
}
return MROBOT_OK;
}
}
if (ctx.mutex != NULL) {
xSemaphoreGive(ctx.mutex);
}
return MROBOT_ERR_NOT_FOUND;
}
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) {
return MROBOT_ERR_NULL_PTR;
}
if (ctx.custom_cmd_count >= MROBOT_MAX_CUSTOM_COMMANDS) {
return MROBOT_ERR_FULL;
}
/* 动态分配命令结构体 */
CLI_Command_Definition_t *cmd_def = BSP_Malloc(sizeof(CLI_Command_Definition_t));
if (cmd_def == NULL) {
return MROBOT_ERR_ALLOC;
}
/* 初始化命令定义 */
*(const char **)&cmd_def->pcCommand = command;
*(const char **)&cmd_def->pcHelpString = help_text;
*(pdCOMMAND_LINE_CALLBACK *)&cmd_def->pxCommandInterpreter = (pdCOMMAND_LINE_CALLBACK)callback;
cmd_def->cExpectedNumberOfParameters = param_count;
/* 注册到 FreeRTOS CLI */
FreeRTOS_CLIRegisterCommand(cmd_def);
ctx.custom_cmds[ctx.custom_cmd_count] = cmd_def;
ctx.custom_cmd_count++;
return MROBOT_OK;
}
uint8_t MRobot_GetDeviceCount(void) {
return ctx.device_count;
}
const MRobot_Device_t *MRobot_FindDevice(const char *name) {
if (name == NULL) return NULL;
for (uint8_t i = 0; i < ctx.device_count; i++) {
if (strcmp(ctx.devices[i].name, name) == 0) {
return &ctx.devices[i];
}
}
return NULL;
}
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 = format_float_va(buffer, sizeof(buffer), fmt, args);
va_end(args);
if (len > 0) {
send_string(buffer);
}
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;
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;
/* htop 模式 */
if (ctx.state == MROBOT_STATE_HTOP) {
handle_htop_mode();
return;
}
/* 处理命令 */
if (ctx.cmd_ready) {
ctx.state = MROBOT_STATE_PROCESSING;
/* 检查是否是 htop 命令 */
if (strcmp((char *)ctx.cmd_buffer, "htop") == 0) {
ctx.state = MROBOT_STATE_HTOP;
ctx.htop_exit = false;
} else {
/* 处理其他命令 */
BaseType_t more;
do {
ctx.output_buffer[0] = '\0';
more = FreeRTOS_CLIProcessCommand((char *)ctx.cmd_buffer,
ctx.output_buffer,
sizeof(ctx.output_buffer));
if (ctx.output_buffer[0] != '\0') {
send_string(ctx.output_buffer);
}
} while (more != pdFALSE);
send_prompt();
ctx.state = MROBOT_STATE_IDLE;
}
ctx.cmd_index = 0;
ctx.cmd_ready = false;
}
osDelay(10);
}

317
assets/User_code/device/mrobot/mrobot.h Normal file
View File

@ -0,0 +1,317 @@
/**
* @file mrobot.h
* @brief MRobot CLI - 基于 FreeRTOS CLI 的嵌入式调试命令行系统
*
* 功能特性:
* - 设备注册与监控（IMU、电机、传感器等）
* - 类 Unix 文件系统命令（cd, ls, pwd）
* - 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;
* return MRobot_Snprintf(buf, size,
* " Status: %s\r\n"
* " Accel : X=%.3f Y=%.3f Z=%.3f\r\n"
* " Euler : R=%.2f P=%.2f Y=%.2f\r\n"
* " Temp : %.1f C\r\n",
* imu->online ? "Online" : "Offline",
* imu->accl[0], imu->accl[1], imu->accl[2],
* imu->euler[0], imu->euler[1], imu->euler[2],
* imu->temp);
* }
*
* // 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;
* return MRobot_Snprintf(buf, size,
* " Status : %s\r\n"
* " Angle : %.2f deg\r\n"
* " Speed : %.1f RPM\r\n"
* " Current: %.3f A\r\n",
* m->online ? "Online" : "Offline",
* m->angle, m->speed, m->current);
* }
*
* // 注册 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 MRobot_Snprintf(buf, size, "Usage: cali <gyro|accel|save>\r\n");
* }
* if (strncmp(param, "gyro", 4) == 0) {
* // 采集 1000 次陀螺仪数据求平均
* MRobot_Snprintf(buf, size, "Calibrating gyro... keep IMU still!\r\n");
* // ... 校准逻辑 ...
* return 0;
* }
* if (strncmp(param, "save", 4) == 0) {
* // 保存到 Flash
* MRobot_Snprintf(buf, size, "Calibration saved to flash.\r\n");
* return 0;
* }
* return MRobot_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
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include "bsp/uart.h"
/* Configuration ------------------------------------------------------------ */
/* 可在编译时通过 -D 选项覆盖这些默认值 */
#ifndef MROBOT_MAX_DEVICES
#define MROBOT_MAX_DEVICES 64 /* 最大设备数 */
#endif
#ifndef MROBOT_MAX_CUSTOM_COMMANDS
#define MROBOT_MAX_CUSTOM_COMMANDS 16 /* 最大自定义命令数 */
#endif
#ifndef MROBOT_CMD_BUFFER_SIZE
#define MROBOT_CMD_BUFFER_SIZE 128 /* 命令缓冲区大小 */
#endif
#ifndef MROBOT_OUTPUT_BUFFER_SIZE
#define MROBOT_OUTPUT_BUFFER_SIZE 512 /* 输出缓冲区大小 */
#endif
#ifndef MROBOT_DEVICE_NAME_LEN
#define MROBOT_DEVICE_NAME_LEN 32 /* 设备名最大长度 */
#endif
#ifndef MROBOT_PATH_MAX_LEN
#define MROBOT_PATH_MAX_LEN 64 /* 路径最大长度 */
#endif
#ifndef MROBOT_HTOP_REFRESH_MS
#define MROBOT_HTOP_REFRESH_MS 200 /* htop 刷新间隔 (ms) */
#endif
#ifndef MROBOT_UART_PORT
#define MROBOT_UART_PORT BSP_UART_VOFA /* 默认 UART 端口 */
#endif
#ifndef MROBOT_USER_NAME
#define MROBOT_USER_NAME "root" /* CLI 用户名 */
#endif
#ifndef MROBOT_HOST_NAME
#define MROBOT_HOST_NAME "MRobot" /* CLI 主机名 */
#endif
/* Error codes -------------------------------------------------------------- */
typedef enum {
MROBOT_OK = 0, /* 成功 */
MROBOT_ERR_FULL = -1, /* 容量已满 */
MROBOT_ERR_NULL_PTR = -2, /* 空指针 */
MROBOT_ERR_INVALID_ARG = -3, /* 无效参数 */
MROBOT_ERR_NOT_FOUND = -4, /* 未找到 */
MROBOT_ERR_ALLOC = -5, /* 内存分配失败 */
MROBOT_ERR_BUSY = -6, /* 设备忙 */
MROBOT_ERR_NOT_INIT = -7, /* 未初始化 */
} MRobot_Error_t;
/* CLI 运行状态 */
typedef enum {
MROBOT_STATE_IDLE, /* 空闲状态，等待输入 */
MROBOT_STATE_HTOP, /* htop 模式 */
MROBOT_STATE_PROCESSING, /* 正在处理命令 */
} MRobot_State_t;
/* Callback types ----------------------------------------------------------- */
/**
* @brief 设备打印回调函数类型
* @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);
/**
* @brief 命令处理回调函数类型（与 FreeRTOS CLI 兼容）
*/
typedef long (*MRobot_CommandCallback_t)(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
/* Device structure --------------------------------------------------------- */
typedef struct {
char name[MROBOT_DEVICE_NAME_LEN]; /* 设备名称 */
void *data; /* 用户设备数据指针 */
MRobot_PrintCallback_t print_cb; /* 用户打印回调函数 */
} MRobot_Device_t;
/* Public API --------------------------------------------------------------- */
/**
* @brief 初始化 MRobot CLI 系统
* @note 必须在 FreeRTOS 调度器启动后调用
*/
void MRobot_Init(void);
/**
* @brief 反初始化 MRobot CLI 系统，释放资源
*/
void MRobot_DeInit(void);
/**
* @brief 获取当前 CLI 状态
* @return MRobot_State_t 当前状态
*/
MRobot_State_t MRobot_GetState(void);
/**
* @brief 注册设备到 MRobot 系统
* @param name 设备名称（会被截断到 MROBOT_DEVICE_NAME_LEN-1）
* @param data 设备数据指针（不能为 NULL）
* @param print_cb 打印回调函数（可为 NULL，但无法用 show 命令查看）
* @return MRobot_Error_t 错误码
*/
MRobot_Error_t MRobot_RegisterDevice(const char *name, void *data, MRobot_PrintCallback_t print_cb);
/**
* @brief 注销设备
* @param name 设备名称
* @return MRobot_Error_t 错误码
*/
MRobot_Error_t MRobot_UnregisterDevice(const char *name);
/**
* @brief 注册自定义命令
* @param command 命令名称
* @param help_text 帮助文本
* @param callback 命令回调函数
* @param param_count 参数个数（-1 表示可变参数）
* @return MRobot_Error_t 错误码
*/
MRobot_Error_t MRobot_RegisterCommand(const char *command, const char *help_text,
MRobot_CommandCallback_t callback, int8_t param_count);
/**
* @brief 获取已注册设备数量
* @return 设备数量
*/
uint8_t MRobot_GetDeviceCount(void);
/**
* @brief 根据名称查找设备
* @param name 设备名称
* @return 设备指针，未找到返回 NULL
*/
const MRobot_Device_t *MRobot_FindDevice(const char *name);
/**
* @brief MRobot 主循环，在 CLI 任务中周期性调用
* @note 建议调用周期 10ms
*/
void MRobot_Run(void);
/**
* @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 -------------------------------------------------------- */
/**
* @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