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

## 问题描述

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

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

## 根本原因

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

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

### 符号定义分析

1. **内部计算**（motion_control.c）：
   - 使用数学坐标系（东=0°，北=90°，逆时针为正）
   - `yaw_rate_cmd > 0` → 需要逆时针转（CCW）
   - `yaw_rate_cmd < 0` → 需要顺时针转（CW）

2. **PWM映射**（motion_control_task.c）：
   - 注释说明："正值=左转，负值=右转"
   - 左转 = 逆时针（罗盘角减少）
   - 右转 = 顺时针（罗盘角增加）

3. **符号链路**：
   - **不取反**：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`**

```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值总是显示为正数，无法区分正负：

```c
// 修改前：
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（左转）
   - **预期结果**：车辆逆时针转 ✓

2. **动态测试**：
   - 车辆应该平稳地从308°逆时针转向141°
   - 不应出现震荡或反向转动

## 相关文件

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

## 总结

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

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