05-PID、按键舵机电机与 GL_Math：执行层与算法层代码精读

PID、按键舵机电机与 GL_Math：执行层与算法层代码精读

本篇串起三块底层：Agorithm.c 的 PID/限幅/快开方，Common_Peripherals.c 的蜂鸣器/按键/舵机/无刷，GL_Math.c 的三角与 atan2 查表。 说明它们如何支撑 GPS 与视觉两条业务线。 源码仓库：https://github.com/shuifanyu/TC264-GPS-Vision-Car

目录

1. 层位置

业务：GPS Follow_track / Image steering
│ 误差 e、目标 duty
▼
算法：Agorithm.c PID、限幅
│ 输出 u / duty / Nomal 用法
▼
执行：Common_Peripherals.c
Steer_set / BLDC_ctrl / Key / Buzzer
│
▼
库：pwm/gpio + GL_Math（方位角等用到的数学）

2. pid_param_t：一个结构体两种算法

typedef struct
{
float kp, ki, kd;
float imax; // 积分限幅（意图）

float out_p, out_i, out_d, out;
float integrator; // 位置式积分累加
float last_error;
float last_derivative; // 增量式里存 e(k)-e(k-1)
unsigned long last_t;
} pid_param_t;

extern pid_param_t I_PID; // 倾向惯性/视觉类
extern pid_param_t G_PID; // 倾向 GPS 类

双实例：GPS 与惯性/其它模式可各用一套参数，避免互相污染。

3. PidLocCtrl 逐行问题意识

真实代码要点：

float PidLocCtrl(pid_param_t * pid, float error)
{
G_PID.kp = 1.1; // ①
G_PID.kd = 5;
I_PID.kp = 2;
I_PID.kd = 1;

pid->integrator += error;
constrain_float(pid->integrator, –pid->imax, pid->imax); // ②

pid->out_p = pid->kp * error;
pid->out_i = pid->ki * pid->integrator;
pid->out_d = pid->kd * (error – pid->last_error);

pid->last_error = error;
pid->out = pid->out_p + pid->out_i + pid->out_d;
return pid->out;
}

① 参数被函数内写死

无论菜单改成多少，每次进入 PID 都会把 G_PID/I_PID 的 kp/kd 盖掉。 结果：

• 在线调参无效
• 只能改源码重编译
• 两套 PID 实例在函数里被交叉赋值，语义混乱

正确做法：PidLocCtrl 只读 pid->kp/ki/kd，初始化/菜单/Flash 负责写参数。

② 积分限幅未写回

constrain_float(pid->integrator, –pid->imax, pid->imax);

函数返回限幅后的值，但这里 丢弃返回值，integrator 仍可能无限增长。

应写成：

pid->integrator = constrain_float(pid->integrator, –pid->imax, pid->imax);

或改函数原型为就地限幅。

③ 未用 last_t 做 dt

结构体有 last_t，实现却按 固定周期、dt 并入系数 的简化形式。 若未来周期可变，应显式用 dt 或重新标定 kp/ki。

④ 微分为裸差分

kd * (e – last) 无滤波，e 噪声大时会抖；可在 d 路径加一阶低通。

4. PidIncCtrl_L/R：增量式实现

float PidIncCtrl_L(pid_param_t * pid, float error)
{
pid->out_p = pid->kp * (error – pid->last_error);
pid->out_i = pid->ki * error;
pid->out_d = pid->kd * ((error – pid->last_error) – pid->last_derivative);

pid->last_derivative = error – pid->last_error;
pid->last_error = error;

pid->out += pid->out_p + pid->out_i + pid->out_d;
return pid->out;
}

PidIncCtrl_R 与 L 逻辑相同（当前未体现左右不同参数；可留作左右轮增益差异扩展点）。

与公式对应

[ \\Delta u=K_p(e-e_{k-1})+K_i e+K_d\\big((e-e_{k-1})-(e_{k-1}-e_{k-2})\\big) ]

last_derivative 存的是上一次的一阶差分，从而差分得到近似二阶差分。

缺点（现状）

• out 无上下限（应在累加后 out = constrain_float(…)）
• 无抗积分饱和专项（增量式相对好一些，但仍需输出限幅）
• L/R 重复代码，可合并为一个函数

5. constrain_float 与 Sqrt_Fast

constrain_float

float constrain_float(float amt, float low, float high)
{
return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}

标准夹逼。关键在 调用方是否使用返回值。

Sqrt_Fast（快速逆平方根风格）

float Sqrt_Fast(float x)
{
float halfx = 0.5f * x;
float y = x;
long i = *(long *) &y;
i = 0x5f3759df – (i >> 1);
y = *(float *) &i;
y = y * (1.5f – (halfx * y * y));
return y; // 注意：近似 1/sqrt(x)，不是 sqrt(x)
}

GPS 距离若用 (d=\\sqrt{\\Delta x^2+\\Delta y^2})，需 1/Sqrt_Fast 或直接 func_sqrt/库 sqrt。

6. 蜂鸣器与分层自检

void Buzzer_init(void) {
gpio_init(BUZZER_PIN, GPO, 0, GPO_PUSH_PULL);
}
void Buzzer_check(int time2) {
gpio_set_level(BUZZER_PIN, 1);
system_delay_ms(time2);
gpio_set_level(BUZZER_PIN, 0);
}

配合 core0_main：

init 外设 → 短响 50ms
配置 PIT → 长响 300ms

以及菜单 keyN_clear 里短响 50ms：操作反馈。

system_delay_ms 在 ISR 里应避免；菜单在主循环 clear 时蜂鸣是同步的，会短暂卡主循环（50ms），一般可接受。

7. 按键扫描与标志位

uint8 key1_flag, key2_flag, key3_flag, key4_flag;
int Key_close_flag = 0;

void key_scan(void) {
key1_state_last = key1_state;
key1_state = gpio_get_level(KEY1);
// …
if (key1_state && !key1_state_last) key1_flag = 1;
}

初始化

gpio_init(KEY1, GPI, GPIO_HIGH, GPI_PULL_UP); // 上拉，按下常见为低

与注释的差异（要心里有数）

注释写“检测到按键按下”，实现是 state && !last，即 高电平沿。 若原理图是 低电平有效，条件应改为 (!state && last)。

现场以万用表/按键实测为准；方向错了会出现“抬手触发/没反应”。

开关量

switch1_state 等已定义，便于拨码选科目/模式（进一步可做模式输入）。

8. Steer_set：角度域限幅再进 PWM

void Steer_init(void) {
pwm_init(SERVO_MOTOR_PWM, SERVO_MOTOR_FREQ,
(uint32)SERVO_MOTOR_DUTY(SERVO_MOTOR_MID));
}

void Steer_set(int angle)
{
if (angle > SERVO_MOTOR_LMAX) angle = SERVO_MOTOR_LMAX;
if (angle < SERVO_MOTOR_RMAX) angle = SERVO_MOTOR_RMAX;
pwm_set_duty(SERVO_MOTOR_PWM, (uint32)SERVO_MOTOR_DUTY(angle));
}

Steer_text：键 1/2 ±10，键 3/4 直接到左右极限——标定中值与极限的工程函数。

视觉 duty 与 Steer_set 的量纲

image_steering_duty 来自 96 – (mid/188)*40，范围约 56～96，更像 脉宽/占空比域。 Steer_set(int angle) 是角度语义。

对接时要统一：

若 duty 就是 pwm_set_duty 参数 → 应调 pwm_set_duty 而非 Steer_set(angle)
或：把视觉输出映射到 angle，再 Steer_set

量纲不一致是联调时最常见坑之一。

9. BLDC_ctrl：符号表方向

void BLDC_init(void)
{
pwm_init(PWM_CH1, 1000, 0);
gpio_init(DIR_CH1, GPO, 1, GPO_PUSH_PULL);
}

void BLDC_ctrl(int16 Motor_SPEED)
{
if (Motor_SPEED >= 0) {
pwm_set_duty(PWM_CH1, Motor_SPEED);
gpio_set_level(DIR_CH1, 1);
} else {
pwm_set_duty(PWM_CH1, –Motor_SPEED);
gpio_set_level(DIR_CH1, 0);
}
}

Follow_track 里直接查表调用（2000/4500/0），开环速度档；不是编码器闭环。

Motor_text：±100/±1000 手动摸电机，用于方向与占空比标定。

安全

• 无显式 PWM 上限 clamp（4500 等需确认与 PWM 分辨率匹配）
• 堵转仅靠机械/电池
• 比赛前应加软件上限

10. 为什么需要 GL_Math

GPS 方位角、距离、IMU 相关运算会用到 sin/cos/atan2/sqrt。

11. func_sin / func_sqrt / fast_atan2

func_sqrt（牛顿迭代）

double func_sqrt(double x)
{
double j = 0.0, k = x / 2;
while (j != k) { // ① 浮点 == 比较
j = k;
k = (j + x / j) / 2; // 牛顿迭代
}
return j;
}

py_sqrt / k_sqrt

递归版牛顿，k_sqrt 用 py_fabs(x1-x0)>=10e-15 停止，比 != 稳妥。

func_sin（泰勒）

// 奇次项累加，|temp|>1e-15 时继续
result = x – x^3/3! + x^5/5! – ...

注意：输入应为 弧度；大角度需先归约到 ([-π,π]) 再算，否则阶乘/幂误差与循环次数都会出问题。代码里 func_cos 用 func_sin(π/2-x)。

func_asin

级数展开，factorial(2n) 在 n 增大时 long 溢出 风险高，竞赛里小范围角度可用，生产代码应换查表/库。

fast_atan2（查表 + 线性插值）

// 1) 把 |y|/|x| 折到 0~45° 相关的 z
// 2) z 小 → angle≈z
// 3) 否则表索引 + 插值：
// alpha = z * 256 – 0.5
// base = table[index] + (table[index+1]-table[index])*alpha
// 4) 按象限翻转符号 / 用 π±base

| 优点 | 比泰勒 atan 快且内存可控（257 个 double） | | 精度 | 与表分辨率、插值有关，导航够用 | | 用途 | 方位角 atan2(Δy,Δx) |

与 Agorithm.c 的 Sqrt_Fast 对比：一个是三角查表，一个是开方快速近似，服务不同热点。

12. 与 GPS/视觉的衔接

GPS: Nomal_Error → PidLocCtrl → (理想) Steer_set
Distance → 到点；N → BLDC_ctrl 档位

视觉: mid_avg → duty 公式 → 低通 → pwm/Steer

菜单: 改 flag、测 Steer_text/Motor_text、蜂鸣反馈

13. 改法建议

13.1 PID

float PidLocCtrl(pid_param_t * pid, float error)
{
pid->integrator += error;
pid->integrator = constrain_float(pid->integrator, –pid->imax, pid->imax);
pid->out_p = pid->kp * error;
pid->out_i = pid->ki * pid->integrator;
pid->out_d = pid->kd * (error – pid->last_error);
pid->last_error = error;
pid->out = constrain_float(pid->out_p + pid->out_i + pid->out_d,
–OUT_LIMIT, OUT_LIMIT);
return pid->out;
}

参数初始化放 pid_init(&G_PID, kp, ki, kd, imax)。

13.2 增量式

累加后：

pid->out = constrain_float(pid->out, u_min, u_max);

合并 L/R：

float PidIncCtrl(pid_param_t *pid, float e) { ... }

13.3 执行层

• BLDC_ctrl 内部软件限幅
• 明确 image_steering_duty 与 Steer_set 量纲
• 按键有效电平与原理图一致

13.4 GL_Math

• sqrt 停止条件用 epsilon
• 大角先归约再泰勒
• 导航路径优先 fast_atan2，调试可用库函数对照

14. 小结

读竞赛代码不要只背“有 PID”：

参数从哪来？
限幅有没有写回？
输出量纲和执行器是否一致？
自写数学用在哪条链路？

把这四问写进精读，文章就有实质，而不是目录。

作者：shuifanyu 标签：PID 嵌入式 智能车 TC264 GL_Math 代码精读

源码：code/Agorithm.c/h、code/Common_Peripherals.c、code/GL_Math.c 仓库：https://github.com/shuifanyu/TC264-GPS-Vision-Car