航向控制方向反转问题修复

问题描述

实车测试时，车辆在原地震荡，不断左右转向，无法正常跟踪目标航向。

症状

• 当前位置：(1.89, -2.81) ENU
• 目标位置：(5.40, -7.17) ENU
• 当前航向：308°（西北）
• 目标航向：141°（东南）
• 预期行为：应该逆时针转167°到达目标
• 实际行为：车辆顺时针和逆时针反复震荡

根本原因

STM32H743/FML/motion_control.c 第548行有一个错误的取反操作：

_out->turn_rate = -_out->turn_rate;  // 错误：导致转向方向反转

符号定义分析

1. 内部计算（motion_control.c）：

• 使用数学坐标系（东=0°，北=90°，逆时针为正）
• yaw_rate_cmd > 0 → 需要逆时针转（CCW）
• yaw_rate_cmd < 0 → 需要顺时针转（CW）

1. PWM映射（motion_control_task.c）：

• 注释说明："正值=左转，负值=右转"
• 左转 = 逆时针（罗盘角减少）
• 右转 = 顺时针（罗盘角增加）

1. 符号链路：

• 不取反：CCW(+) → turn_rate(+) → 左转(逆时针) ✓ 正确
• 取反后：CCW(+) → turn_rate(-) → 右转(顺时针) ✗ 方向反了！

IMU符号定义确认

根据实际测试：
- 车身从上往下看**顺时针旋转**（右转）
- 从北转向东（0° → 90°）
- 此时**IMU GyroZ为正值**

这与代码中的定义一致：
c /* GyroZ符号定义：顺时针正，逆时针负 */ _state->yaw_rate_rad = -(_gpimu->m_fGyroZ * DEG2RAD); // 转换为数学坐标系

修复方案

1. 去掉motion_control.c第548行的取反

文件：STM32H743/FML/motion_control.c

// 修改前：
_out->turn_rate = -_out->turn_rate;

// 修改后：
/* 不再取反：yaw_rate_cmd（数学坐标系，正=CCW）直接作为turn_rate输出
 * motion_control_task.c中，正值=左转(逆时针)，负值=右转(顺时针)，与数学坐标系一致 */
// _out->turn_rate = -_out->turn_rate;  // 已注释：此取反导致方向错误

2. 修复log输出格式

文件：STM32H743/APL/motion_control_task.c

原来的log格式使用fabsf导致turn值总是显示为正数，无法区分正负：

// 修改前：
int turn_int = (int)output.turn_rate;  // -0.8 → 0
int turn_frac = (int)(fabsf(output.turn_rate - turn_int) * 100);  // fabs(-0.8-0)*100 = 80
// 显示：turn=0.80（看起来是正值，实际是-0.80）

// 修改后：
float turn_abs = fabsf(output.turn_rate);
int turn_int = (int)turn_abs;
int turn_frac = (int)((turn_abs - (float)turn_int) * 100.0f);
char turn_sign = (output.turn_rate < 0.0f) ? '-' : '+';
// 显示：turn=-0.80（明确显示符号）

测试验证

修复后，重新编译并测试：

1. 静态验证：

• 当前308°，目标141°
• 航向误差：+2.91 rad（需要逆时针转）
• yaw_rate_cmd：+0.80 rad/s（CCW）
• turn_rate：+0.80 rad/s（不取反）
• PWM：< 1500（左转）
• 预期结果：车辆逆时针转 ✓

1. 动态测试：

• 车辆应该平稳地从308°逆时针转向141°
• 不应出现震荡或反向转动

相关文件

• STM32H743/FML/motion_control.c - 核心修复
• STM32H743/APL/motion_control_task.c - Log格式修复
• python/tools/debug_heading_control.py - 符号分析工具

总结

这是一个经典的符号定义错误，由一个多余的取反操作引起。修复方法很简单：去掉第548行的取反，让数学坐标系的符号定义直接传递到PWM映射层，保持一致性。

关键教训：在多层坐标系转换中，必须在每一层明确注释符号定义，并保持一致性。