feat: 修正红外循迹方向符号，解决循迹偏差越修越偏问题

- 反转了循迹P控制输出的角速度修正方向，保证检测到黑线时车辆向正确方向修正
- 同步修正丢线回找时的转向方向
- 相关代码在 track_ir_algo.c
- 编译通过，功能已现场验证

Core/Bsp/track_ir_algo.c

@@ -0,0 +1,109 @@
+#include "track_ir_algo.h"
+
+#define TRACK_IR_P_GAIN 0.45f
+#define TRACK_IR_MAX_YAW_CORR 18.0f
+#define TRACK_IR_LOST_TURN_RPM 10.0f
+#define TRACK_IR_LOST_SPEED_SCALE 35U
+
+static int16_t s_last_deviation = 0;
+
+static int16_t track_ir_calc_deviation(uint8_t line_mask)
+{
+    int16_t weighted_sum = 0;
+    uint8_t count = 0;
+
+    if ((line_mask & TRACK_IR_H4_BIT) != 0U) {
+        weighted_sum += -30;
+        count++;
+    }
+    if ((line_mask & TRACK_IR_H3_BIT) != 0U) {
+        weighted_sum += -10;
+        count++;
+    }
+    if ((line_mask & TRACK_IR_H2_BIT) != 0U) {
+        weighted_sum += 10;
+        count++;
+    }
+    if ((line_mask & TRACK_IR_H1_BIT) != 0U) {
+        weighted_sum += 30;
+        count++;
+    }
+
+    if (count == 0U) {
+        return 0;
+    }
+
+    return (int16_t)(weighted_sum / (int16_t)count);
+}
+
+void track_ir_algo_init(void)
+{
+    s_last_deviation = 0;
+}
+
+void track_ir_algo_reset(void)
+{
+    s_last_deviation = 0;
+}
+
+void track_ir_algo_step(uint8_t line_mask, track_ir_ctrl_output_t *out)
+{
+    track_ir_state_t state;
+    int16_t deviation = 0;
+    float yaw = 0.0f;
+    uint8_t speed_scale = 100U;
+
+    if (out == NULL) {
+        return;
+    }
+
+    if (line_mask == (TRACK_IR_H1_BIT | TRACK_IR_H2_BIT | TRACK_IR_H3_BIT | TRACK_IR_H4_BIT)) {
+        state = TRACK_IR_STATE_LOST;
+    } else if (line_mask == 0U) {
+        state = TRACK_IR_STATE_CENTER;
+    } else {
+        uint8_t left_mask = line_mask & (TRACK_IR_H4_BIT | TRACK_IR_H3_BIT);
+        uint8_t right_mask = line_mask & (TRACK_IR_H2_BIT | TRACK_IR_H1_BIT);
+
+        if ((left_mask != 0U) && (right_mask == 0U)) {
+            state = TRACK_IR_STATE_LEFT;
+        } else if ((right_mask != 0U) && (left_mask == 0U)) {
+            state = TRACK_IR_STATE_RIGHT;
+        } else {
+            state = TRACK_IR_STATE_CROSS;
+        }
+    }
+
+    deviation = track_ir_calc_deviation(line_mask);
+
+    if (state == TRACK_IR_STATE_LOST) {
+        speed_scale = TRACK_IR_LOST_SPEED_SCALE;
+        if (s_last_deviation < 0) {
+            yaw = TRACK_IR_LOST_TURN_RPM;
+        } else if (s_last_deviation > 0) {
+            yaw = -TRACK_IR_LOST_TURN_RPM;
+        } else {
+            yaw = 0.0f;
+        }
+    } else if (state == TRACK_IR_STATE_CROSS) {
+        yaw = 0.0f; // 十字路口默认直行
+    } else {
+        /* 车体旋转正方向与传感器偏差方向相反，因此取负号做闭环修正。 */
+        yaw = -TRACK_IR_P_GAIN * (float)deviation;
+        if (yaw > TRACK_IR_MAX_YAW_CORR) {
+            yaw = TRACK_IR_MAX_YAW_CORR;
+        }
+        if (yaw < -TRACK_IR_MAX_YAW_CORR) {
+            yaw = -TRACK_IR_MAX_YAW_CORR;
+        }
+    }
+
+    if (deviation != 0) {
+        s_last_deviation = deviation;
+    }
+
+    out->yaw_correction_rpm = yaw;
+    out->speed_scale_percent = speed_scale;
+    out->deviation = deviation;
+    out->state = state;
+}