[feat] 添加二轴云台增稳控制和 Soft I2C 模块

主要变更:
- 二轴云台增稳控制 (control_task.c): 保持 Roll=0, Pitch=0
  - 双 PID 控制器补偿基座倾斜
  - Yaw 电机补偿 Roll 轴，Pitch 电机补偿 Pitch 轴
  - 100Hz 控制循环

- Soft I2C 模块 (modules/bus/soft_i2c/):
  - 基于 GPIO 位时序的 I2C 实现
  - 支持多实例，可适配到 i2c_if 接口
  - 适配层用于 WIT 传感器 (sensor_i2c_port.c)

- MF4010V2 驱动增强:
  - 添加 mf4010v2_set_angle() 位置控制函数
  - 完善命令生成函数文档

Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>

app/control_task.c

@@ -0,0 +1,162 @@
+/**
+ * @file    control_task.c
+ * @brief   二轴云台增稳控制任务
+ * @note    目标：保持 Roll=0, Pitch=0，Yaw 用于方向控制
+ *
+ * 控制架构：
+ *   IMU 数据 → PID 控制器 → 电机补偿角度 → 抵消基座倾斜
+ */
+
+#include "app.h"
+#include "cmsis_os2.h"
+#include "mf4010v2.h"
+#include <stdio.h>
+
+/* ============================================================================
+ * 外部变量 (来自 sensor_task)
+ * ============================================================================ */
+extern float fAcc[3], fGyro[3], fAngle[3];  /* fAngle[0]=Roll, fAngle[1]=Pitch, fAngle[2]=Yaw */
+
+/* ============================================================================
+ * 云台配置
+ * ============================================================================ */
+
+/* 目标角度：保持水平 */
+static float g_target_roll = 0.0f;
+static float g_target_pitch = 0.0f;
+static float g_target_yaw = 0.0f;  /* Yaw 目标可通过外部修改 */
+
+/* 电机实例 */
+static mf4010v2_t motor_yaw;    /* Yaw 轴电机（外框） */
+static mf4010v2_t motor_pitch;  /* Pitch 轴电机（内框） */
+
+/* PID 控制器 */
+typedef struct {
+    float Kp, Ki, Kd;
+    float integral;
+    float prev_error;
+    float output_max;
+} PID_t;
+
+static PID_t g_roll_pid = {0};
+static PID_t g_pitch_pid = {0};
+
+/* ============================================================================
+ * PID 控制函数
+ * ============================================================================ */
+
+void PID_Init(PID_t *pid, float kp, float ki, float kd, float max)
+{
+    pid->Kp = kp;
+    pid->Ki = ki;
+    pid->Kd = kd;
+    pid->output_max = max;
+    pid->integral = 0;
+    pid->prev_error = 0;
+}
+
+float PID_Compute(PID_t *pid, float target, float actual)
+{
+    float error = target - actual;
+    float output;
+
+    /* 比例项 */
+    output = pid->Kp * error;
+
+    /* 积分项（带抗饱和） */
+    pid->integral += error;
+    if (pid->integral > pid->output_max) {
+        pid->integral = pid->output_max;
+    } else if (pid->integral < -pid->output_max) {
+        pid->integral = -pid->output_max;
+    }
+    output += pid->Ki * pid->integral;
+
+    /* 微分项 */
+    float derivative = error - pid->prev_error;
+    output += pid->Kd * derivative;
+    pid->prev_error = error;
+
+    /* 输出限幅 */
+    if (output > pid->output_max) {
+        output = pid->output_max;
+    } else if (output < -pid->output_max) {
+        output = -pid->output_max;
+    }
+
+    return output;
+}
+
+/* ============================================================================
+ * 控制任务
+ * ============================================================================ */
+
+void control_task(void)
+{
+    int32_t yaw_cmd, pitch_cmd;
+    float roll_comp, pitch_comp;
+
+    printf("[gimbal] starting stabilization control\n");
+
+    /* 1. 初始化 PID 参数 */
+    /* Roll 轴 PID：控制 Yaw 电机来补偿 Roll 倾斜 */
+    PID_Init(&g_roll_pid, 0.5f, 0.01f, 0.1f, 18000.0f);  /* 最大补偿 180 度 */
+
+    /* Pitch 轴 PID：控制 Pitch 电机补偿 Pitch 倾斜 */
+    PID_Init(&g_pitch_pid, 0.5f, 0.01f, 0.1f, 9000.0f);  /* 最大补偿 90 度 */
+
+    /* 2. 初始化电机 */
+    mf4010v2_init(&motor_yaw, 0x141, 1);    /* Yaw 电机 ID: 0x141, CAN2 */
+    mf4010v2_init(&motor_pitch, 0x142, 1);  /* Pitch 电机 ID: 0x142, CAN2 */
+
+    /* 3. 电机关机状态启动 */
+    mf4010v2_close(&motor_yaw);
+    mf4010v2_close(&motor_pitch);
+
+    /* 等待 IMU 稳定 */
+    osDelay(pdMS_TO_TICKS(1000));
+
+    /* 4. 使能电机 */
+    mf4010v2_run(&motor_yaw);
+    mf4010v2_run(&motor_pitch);
+
+    printf("[gimbal] stabilization enabled\n");
+
+    /* 主控制循环 (100Hz) */
+    while (1) {
+        /* 读取 IMU 数据 */
+        float current_roll = fAngle[0];   /* Roll 角 */
+        float current_pitch = fAngle[1];  /* Pitch 角 */
+
+        /* PID 计算补偿量 */
+        /* 目标：Roll=0, Pitch=0 */
+        roll_comp = PID_Compute(&g_roll_pid, g_target_roll, current_roll);
+        pitch_comp = PID_Compute(&g_pitch_pid, g_target_pitch, current_pitch);
+
+        /* 运动学映射：补偿量 -> 电机角度 */
+        /* 对于 Pitch-Yaw 云台：
+         * - Pitch 倾斜 → Pitch 电机反向补偿
+         * - Roll 倾斜 → Yaw 电机 + Pitch 电机联合补偿（简化为 Yaw 电机）
+         */
+        yaw_cmd = (int32_t)(roll_comp * 100.0f);      /* 转为 0.01 度 */
+        pitch_cmd = (int32_t)(pitch_comp * 100.0f);
+
+        /* 发送位置指令到电机 */
+        /* 使用增量位置控制，直接叠加补偿量 */
+        mf4010v2_set_angle(&motor_yaw, motor_yaw.current_angle + yaw_cmd);
+        mf4010v2_set_angle(&motor_pitch, motor_pitch.current_angle + pitch_cmd);
+
+        /* 100Hz 控制循环 */
+        osDelay(pdMS_TO_TICKS(10));
+    }
+}
+
+/* ============================================================================
+ * 外部 API
+ * ============================================================================ */
+
+/* 设置 Yaw 目标方向 */
+void gimbal_set_yaw_target(float yaw_deg)
+{
+    g_target_yaw = yaw_deg;
+}