ELCT/Simba-OmniInfantry
9
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor(modules): 按角色重排 modules/ 目录,消除命名不一致

Browse Source 
原 modules/ 下目录大小写混用(BMI088/BUZZER/DR16/IST8310/LED/PID 为大写,
attitude/c_comm/filter/gimbal/ins/motor/referee/shoot 为小写),且把第三方
移植代码(QuaternionEKF/kalman_filter)与本工程封装(ins)混在同一目录。

新结构(全部小写,按角色分层,第三方代码单独隔离):

  modules/
    algorithm/   pid.c/h  filter.c/h          通用算法
    imu/         attitude.c/h  ins.c/h        姿态与惯导封装
    imu/qekf/    QuaternionEKF.c/h  kalman_filter.c/h
                                              (移植自 Wang Hongxi INS 例程)
    sensor/      bmi088.c/h  ist8310.c/h      板载传感器驱动
    comms/       dr16.c/h  c_comm.c/h  referee.c/h
    motor/       dji_motor.c/h                电机抽象
    control/     gimbal.c/h  shoot.c/h        整车控制逻辑
                 (底盘为 chassis.c/h)
    dev/         led.c/h  buzzer.c/h          板级指示/发声

共移动 58 个文件(每板 29 个),旧目录全部删除。

工程文件同步(每个工程各 2 个):
- .uvprojx: Modules 分组 16 个 <FilePath> 改为新路径;
  2 个非空 <IncludePath> 元素:15 个旧模块路径收敛为 8 个新目录,
  并顺带删除陈旧项 ../modules/AHRS(目录根本不存在,legacy 时代残留)
  与冗余项 ..\bsp(与 ../Bsp/Inc 重复)
- .uvoptx: 16 个 <PathWithFileName> 同步

关于"为什么不用改源码": Keil 的 include 靠 IncludePath 解析,本工程所有
#include 都是纯文件名形式(无路径限定),所以移动目录只需改工程配置。
另外 .h 完全不登记在工程里(只有 16 个 .c 被登记),故头文件只需移动。

回归验证(无法在此环境编译,用等价判据):
- 两个工程引用的 98 / 95 个 <FilePath> 全部真实存在
- 磁盘上 47 / 45 个 .c 全部仍被工程引用 —— 即没有任何文件掉出编译
- 2 个 IncludePath 元素均含全部 8 个新模块目录,无旧路径残留
- 所有 IncludePath 指向的目录均真实存在
- .uvprojx/.uvoptx XML 良构
- 两棵树结构一致(chassis/gimbal 等板级差异符合预期)

⚠️ 仍需在 Keil 里点一次编译确认(见 README 的构建章节)。
This commit is contained in:
LSMushui
2026-09-12 11:51:41 +08:00
parent 3e5e5e3f95
commit e3d432f3b5
62 changed files with 62 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
底盘/modules/algorithm/filter.c
+118
View File
@@ -0,0 +1,118 @@
/* =============================================================================
* filter.c - 滤波与数学工具集(自 legacy user_lib / WaveFiltering 移植)
* 内容:
* kalmanFilter() 一维卡尔曼(R 越大越平滑),用于姿态角平滑
* ramp_calc() 斜波(缓启动)
* first_order_filter 一阶低通
* invSqrt/abs_limit/fp32_constrain/loop_fp32_constrain/theta_format
* rad_format() 弧度归一化到 -PI~PI(角度环误差计算用)
* ========================================================================== */
#include "filter.h"
#include <math.h>
void kalman_filter_init(KFP * kfp, float q, float r)
{
kfp->LastP = 0.02f;
kfp->Now_P = 0.0f;
kfp->out = 0.0f;
kfp->Kg = 0.0f;
kfp->Q = q;
kfp->R = r;
}
float kalmanFilter(KFP * kfp, float input)
{
kfp->Now_P = kfp->LastP + kfp->Q; // 预测协方差
kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R); // 卡尔曼增益
kfp->out += kfp->Kg * (input - kfp->out); // 更新最优值
kfp->LastP = (1 - kfp->Kg) * kfp->Now_P; // 更新协方差
return kfp->out;
}
void ramp_init(ramp_t * ramp, float frame_period, float max, float min)
{
ramp->frame_period = frame_period;
ramp->max_value = max;
ramp->min_value = min;
ramp->input = 0.0f;
ramp->out = 0.0f;
}
void ramp_calc(ramp_t * ramp, float input)
{
ramp->input = input;
ramp->out += ramp->input * ramp->frame_period;
if (ramp->out > ramp->max_value)
ramp->out = ramp->max_value;
else if (ramp->out < ramp->min_value)
ramp->out = ramp->min_value;
}
void first_order_filter_init(first_order_filter_t * f, float frame_period, const float num[1])
{
f->frame_period = frame_period;
f->num[0] = num[0];
f->input = 0.0f;
f->out = 0.0f;
}
void first_order_filter_cali(first_order_filter_t * f, float input)
{
f->input = input;
f->out = f->num[0] / (f->num[0] + f->frame_period) * f->out +
f->frame_period / (f->num[0] + f->frame_period) * f->input;
}
float invSqrt(float num)
{
return 1 / sqrtf(num);
}
void abs_limit(float * num, float limit)
{
if (*num > limit)
*num = limit;
else if (*num < -limit)
*num = -limit;
}
float fp32_constrain(float value, float min_value, float max_value)
{
if (value > max_value)
return max_value;
if (value < min_value)
return min_value;
return value;
}
int16_t int16_constrain(int16_t value, int16_t min_value, int16_t max_value)
{
if (value > max_value)
return max_value;
if (value < min_value)
return min_value;
return value;
}
float loop_fp32_constrain(float input, float min_value, float max_value)
{
if (max_value < min_value)
return input;
if (input > max_value)
{
float len = max_value - min_value;
return input - len * ((int)((input - min_value) / len) + 1);
}
else if (input < min_value)
{
float len = max_value - min_value;
return input + len * ((int)((min_value - input) / len) + 1);
}
return input;
}
float theta_format(float angle)
{
return loop_fp32_constrain(angle, -180.0f, 180.0f);
}
底盘/modules/algorithm/filter.h
+61
View File
@@ -0,0 +1,61 @@
#ifndef FILTER_H
#define FILTER_H
#include <stdint.h>
/* 滤波/数学工具集,自 legacy 的 user_lib 与 WaveFiltering_Kalman_Filtering 移植 */
#define PI 3.14159265358979f
#define RAD_TO_ANGLE 57.295779513082320876798154814105f
/* 一维卡尔曼滤波器 */
typedef struct
{
float LastP; // 上次估算协方差 初始化值为0.02
float Now_P; // 当前估算协方差 初始化值为0
float out; // 卡尔曼滤波器输出 初始化值为0
float Kg; // 卡尔曼增益 初始化值为0
float Q; // 过程噪声协方差 初始化值为0.001
float R; // 观测噪声协方差 初始化值为0.543
} KFP;
void kalman_filter_init(KFP * kfp, float q, float r);
float kalmanFilter(KFP * kfp, float input);
/* 斜波函数 */
typedef struct
{
float input;
float out;
float min_value;
float max_value;
float frame_period;
} ramp_t;
void ramp_init(ramp_t * ramp, float frame_period, float max, float min);
void ramp_calc(ramp_t * ramp, float input);
/* 一阶低通滤波 */
typedef struct
{
float input;
float out;
float num[1];
float frame_period;
} first_order_filter_t;
void first_order_filter_init(first_order_filter_t * f, float frame_period, const float num[1]);
void first_order_filter_cali(first_order_filter_t * f, float input);
/* 快速开方 */
float invSqrt(float num);
/* 限幅/死区/格式化 */
void abs_limit(float * num, float limit);
float fp32_constrain(float value, float min_value, float max_value);
int16_t int16_constrain(int16_t value, int16_t min_value, int16_t max_value);
float loop_fp32_constrain(float input, float min_value, float max_value);
float theta_format(float angle); /* 角度格式化为 -180~180 */
#define rad_format(Ang) loop_fp32_constrain((Ang), -PI, PI)
#endif
底盘/modules/algorithm/pid.c
+126
View File
@@ -0,0 +1,126 @@
/* =============================================================================
* pid.c - 位置式/增量式 PID(自 legacy Control/PID 原样移植)
* 用法:
* PID_PositionStructureInit(pid, need) 初始化
* PID_PositionSetParameter(pid, kp, ki, kd) 设置增益
* PID_PositionSetEkRange(pid, lo, hi) 误差死区(|ek|在区间内视为0)
* PID_PositionSetOUTRange(pid, lo, hi) 输出限幅
* PID_PositionCalc(pid, now) 每周期计算,结果在 pid->OUT
* PID_PositionClean(pid) 清误差与积分(模式切换时)
* 注意: 积分无独立限幅,靠输出限幅+死区约束;所有云台/底盘 PID 参数集中在
* robot_param.h,控制周期 2ms(与 legacy 调参时一致,勿随意改周期)
* ========================================================================== */
#include "pid.h"
void PID_PositionStructureInit(PID_PositionTypeDef * pid, float need_value)
{
pid->Need_Value = need_value;
pid->Ek = 0;
pid->Sum_Ek = 0;
pid->Ek_low = 0;
pid->Ek_up = 0;
pid->Kp = 0;
pid->Ki = 0;
pid->Kd = 0;
pid->OUT_low = -1e10;
pid->OUT_up = 1e10;
}
void PID_PositionSetParameter(PID_PositionTypeDef * pid, float kp, float ki, float kd)
{
pid->Kp = kp;
pid->Ki = ki;
pid->Kd = kd;
}
void PID_PositionSetEkRange(PID_PositionTypeDef * pid, float ek_low, float ek_up)
{
pid->Ek_low = ek_low;
pid->Ek_up = ek_up;
}
void PID_PositionSetOUTRange(PID_PositionTypeDef * pid, float out_low, float out_up)
{
pid->OUT_low = out_low;
pid->OUT_up = out_up;
}
void PID_PositionClean(PID_PositionTypeDef * pid)
{
pid->Ek = 0;
pid->Sum_Ek = 0;
}
void PID_PositionCalc(PID_PositionTypeDef * pid, float now_value)
{
pid->Now_Value = now_value;
pid->Ek_1 = pid->Ek;
pid->Ek = pid->Need_Value - pid->Now_Value;
if (pid->Ek_low < pid->Ek && pid->Ek < pid->Ek_up) // 误差为0检测
pid->Ek = 0;
pid->Sum_Ek += pid->Ek;
pid->Del_Ek = pid->Ek - pid->Ek_1;
pid->P_OUT = pid->Kp * pid->Ek;
pid->I_OUT = pid->Ki * pid->Sum_Ek;
pid->D_OUT = pid->Kd * pid->Del_Ek;
pid->OUT = pid->P_OUT + pid->I_OUT + pid->D_OUT;
if (pid->OUT < pid->OUT_low) // 输出限幅
pid->OUT = pid->OUT_low;
if (pid->OUT > pid->OUT_up)
pid->OUT = pid->OUT_up;
}
void PID_IncrementalStructureInit(PID_IncrementalTypeDef * pid, float need_value)
{
pid->Need_Value = need_value;
pid->Ek = 0;
pid->Ek_1 = 0;
pid->Ek_low = 0;
pid->Ek_up = 0;
pid->Kp = 0;
pid->Ki = 0;
pid->Kd = 0;
pid->OUT_low = -1e10;
pid->OUT_up = 1e10;
}
void PID_IncrementalSetParameter(PID_IncrementalTypeDef * pid, float kp, float ki, float kd)
{
pid->Kp = kp;
pid->Ki = ki;
pid->Kd = kd;
}
void PID_IncrementalSetEkRange(PID_IncrementalTypeDef * pid, float ek_low, float ek_up)
{
pid->Ek_low = ek_low;
pid->Ek_up = ek_up;
}
void PID_IncrementalSetOUTRange(PID_IncrementalTypeDef * pid, float out_low, float out_up)
{
pid->OUT_low = out_low;
pid->OUT_up = out_up;
}
void PID_IncrementalCalc(PID_IncrementalTypeDef * pid, float now_value)
{
pid->Now_Value = now_value;
pid->Ek_2 = pid->Ek_1;
pid->Ek_1 = pid->Ek;
pid->Ek = pid->Need_Value - pid->Now_Value;
if (pid->Ek_low < pid->Ek && pid->Ek < pid->Ek_up) // 误差为0检测
pid->Ek = 0;
pid->P_OUT = pid->Kp * (pid->Ek - pid->Ek_1);
pid->I_OUT = pid->Ki * pid->Ek;
pid->D_OUT = pid->Kd * (pid->Ek - 2 * pid->Ek_1 + pid->Ek_2);
pid->OUT = pid->P_OUT + pid->I_OUT + pid->D_OUT;
if (pid->OUT < pid->OUT_low) // 输出限幅
pid->OUT = pid->OUT_low;
if (pid->OUT > pid->OUT_up)
pid->OUT = pid->OUT_up;
}
底盘/modules/algorithm/pid.h
+66
View File
@@ -0,0 +1,66 @@
#ifndef PID_H
#define PID_H
/* 位置式/增量式 PID,自 legacy 工程 Control/PID 原样移植 */
typedef struct
{
float Need_Value; // 预期值
float Now_Value; // 当前值
float Ek; // 本次误差
float Ek_1; // 上一次误差
float Sum_Ek; // 误差积分
float Del_Ek; // 误差差分
float Ek_low; // 误差为0阈值下限
float Ek_up; // 误差为0阈值上限
float Kp; // Kp
float Ki; // Ki
float Kd; // Kd
float P_OUT; // 比例输出
float I_OUT; // 积分输出
float D_OUT; // 微分输出
float OUT; // 总输出
float OUT_low; // 输出限幅下限
float OUT_up; // 输出限幅上限
} PID_PositionTypeDef; // 位置式PID参数结构体
typedef struct
{
float Need_Value; // 预期值
float Now_Value; // 当前值
float Ek; // 本次误差
float Ek_1; // 上一次误差
float Ek_2; // 上两次误差
float Ek_low; // 误差为0阈值下限
float Ek_up; // 误差为0阈值上限
float Kp; // Kp
float Ki; // Ki
float Kd; // Kd
float P_OUT; // 比例输出
float I_OUT; // 积分输出
float D_OUT; // 微分输出
float OUT; // 总输出
float OUT_low; // 输出限幅下限
float OUT_up; // 输出限幅上限
} PID_IncrementalTypeDef; // 增量式PID参数结构体
void PID_PositionStructureInit(PID_PositionTypeDef * pid, float need_value);
void PID_PositionSetParameter(PID_PositionTypeDef * pid, float kp, float ki, float kd);
void PID_PositionSetEkRange(PID_PositionTypeDef * pid, float ek_low, float ek_up);
void PID_PositionSetOUTRange(PID_PositionTypeDef * pid, float out_low, float out_up);
void PID_PositionClean(PID_PositionTypeDef * pid);
void PID_PositionCalc(PID_PositionTypeDef * pid, float now_value);
void PID_IncrementalStructureInit(PID_IncrementalTypeDef * pid, float need_value);
void PID_IncrementalSetParameter(PID_IncrementalTypeDef * pid, float kp, float ki, float kd);
void PID_IncrementalSetEkRange(PID_IncrementalTypeDef * pid, float ek_low, float ek_up);
void PID_IncrementalSetOUTRange(PID_IncrementalTypeDef * pid, float out_low, float out_up);
void PID_IncrementalCalc(PID_IncrementalTypeDef * pid, float now_value);
#endif