BetaFlight深入传感设计之九：传感坐标系/机体坐标系/导航坐标系/经纬度坐标系

最近几周，在这个CF/BF/iNav代码上对理解BetaFlight深入传感设计之五：MahonyAHRS & 方向余弦矩阵理论折腾了好久，包括各类信息咨询、收集，总体来看总是差那么几分+/-问题。

1. 问题症结

由于以下问题导致代码实现与理论的出入：

1. 缺乏设计文档
2. 关键位置缺乏解释
3. 解释标注不够明朗

总体来说，开源代码还是专业的(相较于时下国内的一些公司有过之无不及)，经过这几周的折腾，基本也理清了一些头绪。

【1】BetaFlight深入传感设计之六：四元数计算方法
【2】BetaFlight深入传感设计之八：坐标系

2. 入手分析

为了更好的理解CF/BF/iNav代码与实际理论之间的差异，没有一手体验+编程基础+理论逻辑，要一下子搞得明明白白还是有点头大的，估计这也是为什么很少有这种代码理论结合的分析。

这里我们澄清一下概念：

1. 传感坐标系
2. 机体坐标系
3. 导航坐标系
4. 经纬坐标系

2.1 传感坐标系

这里指出的是传感器的坐标系(XYZ)，通常可以在芯片规格书中找到。比如：MPU6000，HMC5883L

注：详见芯片手册。

2.2 机体坐标系

这里指与欧拉角相关的坐标系(XYZ- α β γ \alpha \beta \gamma αβγ)，这个很关键，因为这里就是折腾的主要原因。

注：下图旋转方向满足右手系，其中Z（垂直方向），已经进行了反向旋转。

xEast-yNorth-zUp + yaw反向

1. xEast-yNorth-zUp做矢量运算，符合右手法则
2. α β γ \alpha \beta \gamma αβγ + yaw( β \beta β)反向 ==》CF/BF/iNav机体坐标系的欧拉角方向

定义XYZ (已经yaw反向)
X：指向机头，绕X旋转 γ \gamma γ
Y：指向左侧，绕Y旋转 α \alpha α
Z：指向上方，绕Z旋转 β \beta β

实测视频：BetaFlight Mark4 + 善贤地铁站口 + 城北公园
X：roll-right(+); roll-left(-)
Y：pitch-forward(+); pitch-backward(-)
Z：yaw-right(+); yaw-left(-)
注：采用右手坐标系法则，请看视频的时候注意方框中的数字。

2.3 导航坐标系

这里理解为xNorth-yEast-zDown(右手系)或者xEast-yNorth-zUp(右手系)都可以，其实代码实现角度并没有太多这方面的概念。

2.4 经纬坐标系

关于经纬度不再过多解释，详见：BetaFlight深入传感设计之八：坐标系。

补充一个关键信息：COG，Course Over Groud 地面航向角(水平航向角)

航向角：是质心沿速度方向在水平面上的投影与预定轨迹的切线方向之间的夹角。
偏航角：是质心沿机头方向在水平面上的投影与预定轨迹的切线方向之间的夹角。

因此，偏航角是考虑航行方向并纠正航行阻力所采取的一种机体姿态。GPS模块提供的就是航向角，而飞机姿态提供的是偏航角。

3. CF/BF/iNav代码

3.1 机体坐标系(xEast-yNorth-zUp + yaw反向)

3.1.1 BF/CF代码

3.1.2 iNav代码

3.2 航向角误差

鉴于以下两个现实问题：

1. 加速度制造工艺导致，实际加速度方向与受力方向相反
2. yaw方向与COG方向正好相反

3.2.1 BF/CF代码

3.2.2 iNav代码

4. 参考资料

【1】BetaFlight深入传感设计：传感模块设计框架
【2】BetaFlight深入传感设计之一：Baro传感模块
【3】BetaFlight深入传感设计之二：Mag传感模块
【4】BetaFlight深入传感设计之三：IMU传感模块
【5】BetaFlight深入传感设计之四：GPS传感模块