SimpleBGC 代码解读

太好了！我已经检查并标注出仓库中与 MPU6050、传感器融合、PID 和电机控制相关的模块（evvgcCF.c、MargAHRS.c、pid.c、computeMotorCommands.c、drv_pwmMotors.c、mpu6050Calibration.c 等）。下一步我会把实现流程、必需的配置和调优步骤写成一份可执行的指南并给出具体在本工程中应修改或检查的位置。下面是详细指南（含文件定位和实操建议）⬇️

---

## 简要流程概述 ✅

要把 MPU6050 用于让机器“保持水平”，常见的控制流程是：

1. 读取 MPU6050（加速度计 + 陀螺仪）数据，并进行偏置/温度补偿与量程换算。  
2. 用传感器融合算法（互补滤波 / Madgwick / Mahony / Kalman）估计姿态（滚转/俯仰，pitch/roll）。  
3. 将期望姿态（通常水平为 0°）与当前姿态求差，送入 PID 控制器产生控制量（输出为电机角度/相位）。  
4. 把 PID 输出转换为电机驱动 PWM/相位，输出给电机驱动器。  
5. 做充分的校准、滤波和安全检查（限幅、死区、软停止）与调参（PID 调优）。

下面把每一步映射到你工程中的具体位置并补充实操细节。

---

## 在本工程里的对应位置（我已确认）🔎

- 传感器读取与量化 / 协处理：
  - mpu6050.c, mpu6050.h —— 读取原始值 (rawAccel/rawGyro)、定义尺度常数（ACCEL_SCALE_FACTOR、GYRO_SCALE_FACTOR）、温度补偿与偏置变量（accelTCBias、gyroTCBias）和校准标志。
  - 校准：mpu6050Calibration.c，CLI 命令 'b' 会触发 `mpu6050Calibration()`（见 cliSensor.c）。
- 姿态估计（融合）：
  - 简单互补实现：evvgcCF.c（函数 `getOrientation(...)`，用 accel 的 atan2f 与陀螺积分做互补滤波）。
  - 更完整的 AHRS：MargAHRS.c（Madgwick 或 Mahony 实现），并把结果写到 `sensors.margAttitude500Hz`。
- 控制器（PID）：
  - pid.c（`updatePID()`，包括 I/D 滤波、积分限幅、角度类型处理等）。
  - PID 参数保存在 `eepromConfig.PID[...]`（可通过 CLI 或 EEPROM 修改）。
- 计算并下发电机命令：
  - computeMotorCommands.c：把目标角（pointingCmd）和 `sensors.margAttitude500Hz[]` 当作 state，调用 `updatePID()`，限制变速（rateLimit），最终调用 `setRollMotor()`, `setPitchMotor()`, `setYawMotor()`。
  - 电机底层驱动：drv_pwmMotors.c（实现 PWM/sine table、`setRollMotor()` 等具体上电机相位与激励）。
- 调试与 CLI：
  - cliSensor.c、cli.c：阅读传感器/姿态、触发校准等。

---

## 具体实施步骤（实操指引）🔧

1. 硬件与驱动确认
   - 确保 MPU6050 的 I2C 地址、供电、接地和 PCB 姿态（安装方向）正确。
   - 确认证明 `initMPU6050()` 被调用（drv_system.c 中 init）且 I2C 没有报错。

2. 校准（必须）
   - 在静止平衡位置（云台平稳放置、尽量水平）通过 CLI 触发温度校准和零速偏校准：
     - 进入 CLI（串口/USB），使用菜单中的 'b'（`mpu6050Calibration()`）来校准。
   - 校准完成后，检查 `accelTCBias[]` / `gyroRTBias[]` 的值是否合理。
   - 反复做多次（不同温度下注意温漂），必要时保存到 EEPROM。

3. 确认姿态估计工作正常
   - 短板点：
     - `getOrientation()` (evvgcCF) 是个简单互补滤波，短期靠 gyro、长期靠 accel（书面注释里 0.0002f 的融合权重）。
     - `MargAHRS` 提供完整的四元数滤波与磁校正，通常语义更稳定。
   - 操作：
     - 在 CLI 中打印 `sensors.margAttitude500Hz` 或 `sensors.evvgcCFAttitude500Hz`（文件中已见打印位置），确认角度读数随实际姿态变化且无明显漂移。
     - 检查采样频率 dt（工程内 `frame_500Hz` 给出了 500Hz 的控制循环）——保证 dt 稳定很重要。

4. 启用控制轴与初始参数
   - 在配置 `eepromConfig` 中设置 `rollEnabled`, `pitchEnabled` 等为 true（或通过 CLI 使能）。
   - 将 `pointingCmd[ROLL]` 和 `pointingCmd[PITCH]` 设为 0（代表目标水平）。
   - 初始 PID 参数建议设置保守值（比如小 P、I=0、适量 D），快速限制电机“力度”或 `rollPower/pitchPower` 限制在很小的值以便测试。

5. 测试（安全优先）
   - 先在不接电机（或断开转子）或用限流供电下，观察 PID 输出值（可通过 CLI 打印或在代码里添加临时打印）。
   - 观察姿态时，轻推机器查看响应，确保没有突发大输出或振荡。
   - 慢慢提高 `rollPower`/`pitchPower`，再进行有电机、低速空载测试。

6. PID 调参建议（迭代）
   - 常见流程（Ziegler–Nichols 的简易版 + 经验法）：
     1. 只开 P（I=0，D=0），逐步加 P，直到出现小幅振荡（略有过冲）——记录临界 P*。
     2. 加 D 来抑制振荡并提升阻尼（避免高 D 引入噪声问题）。D 需要与滤波器参数配合（参见 pid.c 中的 D 一阶低通滤波系数 F_CUT）。
     3. 最后微调 I 以消除稳态误差（注意积分风up，`windupGuard` 必须合理）。
     4. 测试不同负载与外界扰动（抖动、碰撞等情景）。
   - 注意：
     - 高频噪声会放大 D 项导致震荡，必要时增加陀螺低通或减小 D。
     - 如果系统在低频下漂移，加 I；但 I 太大易导致慢速振荡。
     - 对云台系统，短时间重心偏移或外部冲击时，要更偏向于较高的 D 和适度 P。

7. 额外调优与改进建议
   - 使用 `MargAHRS`（Madgwick）替代或配合 `evvgcCF`（已存在两者）并比较效果：Madgwick 在动态运动下通常更好。
   - 如果 MPU6050 支持 DMP，可利用 DMP 提供的滤波角度（注意 DMP 的配置和延时）。
   - 加强加速度低通滤波（`firstOrderFilter` 已支持配置）。
   - 增加安全限幅（检测电流/温度/异常角度，触发失能）。

---

## 在本仓库里快速验证的实操命令（建议）🎯

- 校准 MPU6050（进入 CLI 后按 'b'）
- 打印姿态（CLI里对应命令里会有 attitude 输出，参见 cli.c）
- 暂时把 `eepromConfig.rollPower`/`pitchPower` 减小到 10%-20% 进行测试
- 观察 `computeMotorCommands()` 中 PID 输出，必要时在 computeMotorCommands.c 里临时添加调试打印（小心影响实时性）

---

## 安全与调试要点 ⚠️

- 初始测试务必在低功率下进行，或将电机固定（无转子/断开）观察响应。
- 避免让设备有强烈振动或碰撞——IMU 的 accel 受振动影响大。
- 始终有“kill switch”（断电或进入安全模式）的方法。
- 在每次重大参数改动后都复测校准与 PID。

---

## 我可以帮你做的下一步（选项）🛠️

- 我可以帮你：  
  - 在代码里添加一个“测试模式”CLI 命令：自动 set pointingCmd=0、限幅 power、并打印 PID 输出（便于快速迭代调参）。（需要我改代码吗？我可以实现）  
  - 或者添加额外的态度/PID实时日志（CLI 或串口 CSV），方便做参数扫描和绘图调试。  
  - 或替换互补滤波为 Madgwick（MargAHRS.c 已实现；可与现有替换或比较）。

如果你希望我动手修改代码并添加这些功能，请回复你想要哪个（例如：“请添加 CLI 的 test-mode”），我会开始实现并用 TODO 跟踪（并提供补丁/演示如何测试）。

---

拼接总结（WRAP UP）📋  
我已经检查了仓库并确认 MPU6050 读取、姿态估计（互补/Madgwick）、PID 控制和电机驱动都已实现：你只要完成校准（CLI 的 'b'）、在 `eepromConfig` 中启用轴并把 target/pointing = 0，就可以用 PID 来把机器调整到水平；我也给出了一套安全的调参步骤与改进建议。下一步你想让我直接修改代码并添加“test-mode”还是只需要更详细的调参范例（包含示例 PID 值）？请告诉我你的偏好。

# 参考文档
[simple BGC源码](https://github.com/zhaojiahai/simpleBGC32)
[开源三轴云台EVVGC（simple BGC）分析](https://www.cnblogs.com/yueze/p/7107235.html)

[开源云台OpenSAM](http://opensamofficial.github.io/OpenSAM/)
[OpenSAM_Manual](http://opensamofficial.github.io/OpenSAM/OpenSAM_Manual.pdf)