chore: 导入 Simba-OmniInfantry 基线(云台/底盘双工程)

- 云台(ROBOT_TYPE_GIMBAL)与底盘(ROBOT_TYPE_CHASSIS)完整固件源码 - 新增根级 .gitignore,排除 Keil MDK-ARM 构建产物(.o/.d/.crf/.axf/.lnp/.map/.htm/.sct/.dep/.iex/.lst 等) 与 *.uvguix.* / DebugConfig/(与机器绑定的 UI 布局) - core.autocrlf=false,避免固件源码在检出时被改动换行符
2026-09-12 10:28:21 +08:00
commit a4da5ad17c
517 changed files with 503795 additions and 0 deletions
@@ -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);
+}
@@ -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