第1步：MPU6000 FIFO原始字节
SPI读到14字节（大端序）：
偏移: 0 1 2 3 4 5 6 7 8 9 10 11 12 13
内容: 00 80 00 10 FF 9C 1A 7D 00 2D 00 03 00 05
│accel Y│accel X│accel Z│ temp │gyro Y│gyro X│gyro Z│
第2步：int16_val 拼字节
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
┌─────────┬─────────────┬────────────────┬─────────────────┐
│ 字段 │ 字节 │ 计算 │ 原始值(int16_t) │
├─────────┼─────────────┼────────────────┼─────────────────┤
│ accel Y │ data[0:1] │ 0x00<<8 | 0x80 │ 128 │
├─────────┼─────────────┼────────────────┼─────────────────┤
│ accel X │ data[2:3] │ 0x00<<8 | 0x10 │ 16 │
├─────────┼─────────────┼────────────────┼─────────────────┤
│ accel Z │ data[4:5] │ 0xFF<<8 | 0x9C │ -100 │
├─────────┼─────────────┼────────────────┼─────────────────┤
│ temp │ data[6:7] │ 0x1A<<8 | 0x7D │ 6781 │
├─────────┼─────────────┼────────────────┼─────────────────┤
│ gyro Y │ data[8:9] │ 0x00<<8 | 0x2D │ 45 │
├─────────┼─────────────┼────────────────┼─────────────────┤
│ gyro X │ data[10:11] │ 0x00<<8 | 0x03 │ 3 │
├─────────┼─────────────┼────────────────┼─────────────────┤
│ gyro Z │ data[12:13] │ 0x00<<8 | 0x05 │ 5 │
└─────────┴─────────────┴────────────────┴─────────────────┘
第3步：构造向量 + Z轴取负
accel = Vector3f(int16_val(data,1), int16_val(data,0), -int16_val(data,2));
gyro = Vector3f(int16_val(data,5), int16_val(data,4), -int16_val(data,6));
accel = (16, 128, 100) // Z取负：-(-100) = 100
gyro = (3, 45, -5) // Z取负：-(5) = -5
Z取负是因为芯片Z轴朝上，飞控用NED坐标系Z朝下。
第4步：乘缩放因子 → 物理量
MPU6000 Rev C（Pixhawk 2.4.8 常见版本）：
_accel_scale = 9.80665 / 4096.0 = 0.002393 m/s² per LSB // ±8g量程
_gyro_scale = radians(1) / 16.4 = 0.001065 rad/s per LSB // ±2000°/s量程
加速度计：
accel.x = 16 × 0.002393 = 0.0383 m/s²
accel.y = 128 × 0.002393 = 0.3063 m/s²
accel.z = 100 × 0.002393 = 0.2393 m/s²
嗯，这些值太小了。静止时Z轴应该接近 1g = 9.8 m/s²。重新假设一组更真实的FIFO数据：
重新来，用真实静止场景
飞机水平静止，加速度计只感受到重力（Z轴约1g），陀螺仪接近零。
FIFO原始字节
accel X: 0x00 0x20 → 32
accel Y: 0xFF 0xE0 → -32
accel Z: 0x0F 0xA0 → 4000
temp: 0x1A 0x7D → 6781 (约25°C)
gyro X: 0xFF 0xFE → -2
gyro Y: 0x00 0x01 → 1
gyro Z: 0xFF 0xFF → -1
构造向量（Z取负）
accel = (32, -32, -4000) // Z取负: -(4000) = -4000
gyro = (-2, 1, 1) // Z取负: -(-1) = 1
乘缩放因子
accel_scale = 9.80665 / 4096 = 0.002393 m/s²/LSB
gyro_scale = 0.0174533 / 16.4 = 0.001065 rad/s/LSB
加速度计：
accel.x = 32 × 0.002393 = 0.0766 m/s² (接近0，水平无加速度)
accel.y = -32 × 0.002393 = -0.0766 m/s² (接近0)
accel.z = -4000 × 0.002393 = -9.5720 m/s² (接近-9.8，重力向上，NED朝下为正)
│ 等等，Z是-9.57？因为代码里先取负Z，所以 -4000 × scale = -9.57。但静止时NED系Z应该是+9.8。
让我重新看代码顺序：Z取负是在乘scale之前。所以：
原始Z = 4000 (芯片Z朝上，1g ≈ 4096 LSB)
构造向量时: accel.z = -int16_val(data,2) = -4000
乘scale后: accel.z = -4000 × 0.002393 = -9.572 m/s²
然后进入 _rotate_and_correct_accel：
旋转修正
Pixhawk 2.4.8 的板级方向是 ROTATION_NONE（芯片朝上，和飞控坐标系一致，不需要旋转）。但实际Pixhawk用的是 ROTATION_ROLL_180_YAW_90
或类似旋转，具体看hwdef。
假设板级旋转是 ROTATION_ROLL_180（芯片Y朝前，飞控X朝前）：
旋转后 accel = (-0.0766, 0.0766, 9.572) // Z变成正值=重力朝下=NED的D方向
减零偏
accel -= _accel_offset[instance];
gyro -= _gyro_offset[instance];
零偏是开机时标定的，典型值很小。假设：
accel_offset = (0.02, -0.01, 0.05) m/s²
gyro_offset = (0.001, -0.002, 0.001) rad/s
accel = (-0.0766-0.02, 0.0766-(-0.01), 9.572-0.05) = (-0.097, 0.087, 9.522) m/s²
gyro = (-2×0.001065-0.001, 1×0.001065-(-0.002), 1×0.001065-0.001)
= (-0.003, 0.003, 0.0001) rad/s
陀螺仪换算成直观单位
-2 LSB × 0.001065 rad/s/LSB = -0.00213 rad/s = -0.122 °/s
1 LSB × 0.001065 rad/s/LSB = 0.00107 rad/s = 0.061 °/s
-1 LSB × 0.001065 rad/s/LSB = -0.00107 rad/s = -0.061 °/s
静止时陀螺仪读数应该在 ±0.1°/s 以内，这组数据正常。
最终结果（给上层用）
┌──────────────────────┬────────────────────────────────────────┬──────────────────────┐
│ 接口 │ 值 │ 说明 │
├──────────────────────┼────────────────────────────────────────┼──────────────────────┤
│ get_accel() │ (-0.097, 0.087, 9.522) m/s² │ XY接近0，Z接近9.8 │
├──────────────────────┼────────────────────────────────────────┼──────────────────────┤
│ get_gyro() │ (-0.003, 0.003, 0.0001) rad/s │ 接近零，飞机静止 │
├──────────────────────┼────────────────────────────────────────┼──────────────────────┤
│ get_delta_angle() │ (-0.0000075, 0.0000075, 0.0000003) rad │ 陀螺×dt(1kHz=0.001s) │
├──────────────────────┼────────────────────────────────────────┼──────────────────────┤
│ get_delta_velocity() │ (-0.000097, 0.000087, 0.009522) m/s │ 加速度×dt │
└──────────────────────┴────────────────────────────────────────┴──────────────────────┘
数值范围参考
┌──────────┬──────────────────┬────────────────────────┐
│ 场景 │ 加速度计 │ 陀螺仪 │
├──────────┼──────────────────┼────────────────────────┤
│ 静止 │ Z ≈ ±9.8 m/s² │ ≈ 0 rad/s │
├──────────┼──────────────────┼────────────────────────┤
│ 悬停 │ Z ≈ 9.8, XY ≈ ±2 │ ≈ 0.1-0.5 rad/s │
├──────────┼──────────────────┼────────────────────────┤
│ 快速机动 │ 全轴 ±20-80 m/s² │ ±5-10 rad/s │
├──────────┼──────────────────┼────────────────────────┤
│ 满量程 │ ±78.5 m/s² (±8g) │ ±34.9 rad/s (±2000°/s) │
└──────────┴──────────────────┴────────────────────────┘