mirror of
https://github.com/goldenfishs/MRobot.git
synced 2026-02-04 18:00:19 +08:00
添加卡尔曼和mrobot
This commit is contained in:
parent
8a031012fa
commit
c5acabdc49
@ -5,4 +5,5 @@ error_detect,bsp/mm
|
|||||||
pid,component/filter
|
pid,component/filter
|
||||||
filter,component/ahrs
|
filter,component/ahrs
|
||||||
mixer,component/user_math.h
|
mixer,component/user_math.h
|
||||||
ui,component/user_math.h
|
ui,component/user_math.h
|
||||||
|
kalman_filter,arm_math.h
|
||||||
|
@ -11,4 +11,5 @@ limiter,限幅器
|
|||||||
mixer,混控器
|
mixer,混控器
|
||||||
ui,用户交互
|
ui,用户交互
|
||||||
user_math,用户自定义数学函数
|
user_math,用户自定义数学函数
|
||||||
pid,PID控制器
|
pid,PID控制器
|
||||||
|
kalman_filter,卡尔曼滤波器
|
||||||
|
591
assets/User_code/component/kalman_filter/kalman_filter.c
Normal file
591
assets/User_code/component/kalman_filter/kalman_filter.c
Normal 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
199
assets/User_code/component/kalman_filter/kalman_filter.h
Normal 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
|
||||||
@ -1,4 +1,4 @@
|
|||||||
bsp,can,fdcan,dwt,gpio,i2c,mm,spi,uart,pwm,time,flash
|
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
|
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,
|
module,
|
||||||
|
@ -259,4 +259,19 @@ devices:
|
|||||||
thread_signals: []
|
thread_signals: []
|
||||||
files:
|
files:
|
||||||
header: "lcd.h"
|
header: "lcd.h"
|
||||||
source: "lcd.c"
|
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
874
assets/User_code/device/mrobot/mrobot.c
Normal 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, ¶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);
|
||||||
|
}
|
||||||
317
assets/User_code/device/mrobot/mrobot.h
Normal file
317
assets/User_code/device/mrobot/mrobot.h
Normal 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
|
||||||
Loading…
Reference in New Issue
Block a user