diff --git a/assets/User_code/component/dependencies.csv b/assets/User_code/component/dependencies.csv index c3f1e6d..b8e03ae 100644 --- a/assets/User_code/component/dependencies.csv +++ b/assets/User_code/component/dependencies.csv @@ -5,4 +5,5 @@ error_detect,bsp/mm pid,component/filter filter,component/ahrs mixer,component/user_math.h -ui,component/user_math.h \ No newline at end of file +ui,component/user_math.h +kalman_filter,arm_math.h \ No newline at end of file diff --git a/assets/User_code/component/describe.csv b/assets/User_code/component/describe.csv index 2752291..457125d 100644 --- a/assets/User_code/component/describe.csv +++ b/assets/User_code/component/describe.csv @@ -11,4 +11,5 @@ limiter,限幅器 mixer,混控器 ui,用户交互 user_math,用户自定义数学函数 -pid,PID控制器 \ No newline at end of file +pid,PID控制器 +kalman_filter,卡尔曼滤波器 \ No newline at end of file diff --git a/assets/User_code/component/kalman_filter/kalman_filter.c b/assets/User_code/component/kalman_filter/kalman_filter.c new file mode 100644 index 0000000..e5463a4 --- /dev/null +++ b/assets/User_code/component/kalman_filter/kalman_filter.c @@ -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 */ diff --git a/assets/User_code/component/kalman_filter/kalman_filter.h b/assets/User_code/component/kalman_filter/kalman_filter.h new file mode 100644 index 0000000..7e6b7d1 --- /dev/null +++ b/assets/User_code/component/kalman_filter/kalman_filter.h @@ -0,0 +1,199 @@ +/* + ˲ + ֶ֧̬ʹARM CMSIS DSPŻ +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "arm_math.h" + +#include +#include +#include +#include + +/* 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) = Fxhat(k-1) + Bu + * + * @param kf ˲ṹָ + * @return int8_t 0Ӧûд + */ +int8_t KF_PredictState(KF_t *kf); + +/** + * @brief 2Э - P'(k) = FP(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 / (HP'(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 - Hxhat'(k)) + * + * @param kf ˲ṹָ + * @return int8_t 0Ӧûд + */ +int8_t KF_UpdateState(KF_t *kf); + +/** + * @brief 5Э - P(k) = P'(k) - KHP'(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 diff --git a/assets/User_code/config.csv b/assets/User_code/config.csv index 1eb9b34..3f0ecc5 100644 --- a/assets/User_code/config.csv +++ b/assets/User_code/config.csv @@ -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, \ No newline at end of file diff --git a/assets/User_code/device/config.yaml b/assets/User_code/device/config.yaml index f02ad18..60e73f8 100644 --- a/assets/User_code/device/config.yaml +++ b/assets/User_code/device/config.yaml @@ -259,4 +259,19 @@ devices: thread_signals: [] files: header: "lcd.h" - source: "lcd.c" \ No newline at end of file + 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" \ No newline at end of file diff --git a/assets/User_code/device/mrobot/mrobot.c b/assets/User_code/device/mrobot/mrobot.c new file mode 100644 index 0000000..e96471a --- /dev/null +++ b/assets/User_code/device/mrobot/mrobot.c @@ -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 +#include +#include +#include +#include +#include +#include +#include + +/* 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 : 切换目录\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, ¶m_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, ¶m_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); +} diff --git a/assets/User_code/device/mrobot/mrobot.h b/assets/User_code/device/mrobot/mrobot.h new file mode 100644 index 0000000..74c0875 --- /dev/null +++ b/assets/User_code/device/mrobot/mrobot.h @@ -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 \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 : IMU calibration\r\n", cmd_cali, -1); + * @endcode + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include +#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 \ No newline at end of file