APM/PX4/betaflight/inav开源飞控之IMU方向

1. 源由

飞控硬件厂商在设计飞控的时候，在开发、测试阶段已经将IMU方向配置在硬件定义文件中了。

目前，相对来说，国内比较主流的飞控代码可以分为两大类：

• 航模：betaflight/inav
• 无人机：APM/PX4

两者坐标系有差别，尤其在硬件配置文件中定义的角度，更是出现非常离奇的情况。这里做一个整体应用级别的分析，希望能够帮助后续定位底层差异。

2. 坐标系

2.1 APM/PX4：机体坐标 + 右手系规则

2.2 betaflight/inav：xEast-yNorth-zUp + yaw反向 + 右手系规则

3. 转向定义

3.1 APM/PX4

enum Rotation : uint8_t {
    ROTATION_NONE                = 0,
    ROTATION_YAW_45              = 1,
    ROTATION_YAW_90              = 2,
    ROTATION_YAW_135             = 3,
    ROTATION_YAW_180             = 4,
    ROTATION_YAW_225             = 5,
    ROTATION_YAW_270             = 6,
    ROTATION_YAW_315             = 7,
    ROTATION_ROLL_180            = 8,
    ROTATION_ROLL_180_YAW_45     = 9,
    ROTATION_ROLL_180_YAW_90     = 10,
    ROTATION_ROLL_180_YAW_135    = 11,
    ROTATION_PITCH_180           = 12,
    ROTATION_ROLL_180_YAW_225    = 13,
    ROTATION_ROLL_180_YAW_270    = 14,
    ROTATION_ROLL_180_YAW_315    = 15,
    ROTATION_ROLL_90             = 16,
    ROTATION_ROLL_90_YAW_45      = 17,
    ROTATION_ROLL_90_YAW_90      = 18,
    ROTATION_ROLL_90_YAW_135     = 19,
    ROTATION_ROLL_270            = 20,
    ROTATION_ROLL_270_YAW_45     = 21,
    ROTATION_ROLL_270_YAW_90     = 22,
    ROTATION_ROLL_270_YAW_135    = 23,
    ROTATION_PITCH_90            = 24,
    ROTATION_PITCH_270           = 25,
    ROTATION_PITCH_180_YAW_90    = 26, // same as ROTATION_ROLL_180_YAW_270
    ROTATION_PITCH_180_YAW_270   = 27, // same as ROTATION_ROLL_180_YAW_90
    ROTATION_ROLL_90_PITCH_90    = 28,
    ROTATION_ROLL_180_PITCH_90   = 29,
    ROTATION_ROLL_270_PITCH_90   = 30,
    ROTATION_ROLL_90_PITCH_180   = 31,
    ROTATION_ROLL_270_PITCH_180  = 32,
    ROTATION_ROLL_90_PITCH_270   = 33,
    ROTATION_ROLL_180_PITCH_270  = 34,
    ROTATION_ROLL_270_PITCH_270  = 35,
    ROTATION_ROLL_90_PITCH_180_YAW_90 = 36,
    ROTATION_ROLL_90_YAW_270     = 37,
    ROTATION_ROLL_90_PITCH_68_YAW_293 = 38, // this is actually, roll 90, pitch 68.8, yaw 293.3
    ROTATION_PITCH_315           = 39,
    ROTATION_ROLL_90_PITCH_315   = 40,
    ROTATION_PITCH_7             = 41,
    ROTATION_ROLL_45             = 42,
    ROTATION_ROLL_315            = 43,
    ///
    // Do not add more rotations without checking that there is not a conflict
    // with the MAVLink spec. MAV_SENSOR_ORIENTATION is expected to match our
    // list of rotations here. If a new rotation is added it needs to be added
    // to the MAVLink messages as well.
    ///
    ROTATION_MAX,
    ROTATION_CUSTOM_OLD          = 100,
    ROTATION_CUSTOM_1            = 101,
    ROTATION_CUSTOM_2            = 102,
    ROTATION_CUSTOM_END,
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57

3.2 betaflight/inav

typedef enum {
    ALIGN_DEFAULT = 0, // driver-provided alignment

    // the order of these 8 values also correlate to corresponding code in ALIGNMENT_TO_BITMASK.

                            // R, P, Y
    CW0_DEG = 1,            // 00,00,00
    CW90_DEG = 2,           // 00,00,01
    CW180_DEG = 3,          // 00,00,10
    CW270_DEG = 4,          // 00,00,11
    CW0_DEG_FLIP = 5,       // 00,10,00 // _FLIP = 2x90 degree PITCH rotations
    CW90_DEG_FLIP = 6,      // 00,10,01
    CW180_DEG_FLIP = 7,     // 00,10,10
    CW270_DEG_FLIP = 8,     // 00,10,11

    ALIGN_CUSTOM = 9,    // arbitrary sensor angles, e.g. for external sensors
} sensor_align_e;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

4. 实例

注：始终采用Y轴方向朝前进行变换。相关映射，后续会补充，其主要原因，会随着底层分析深入，给出答案！

4.1 $ICM42688P$

正常旋转获得结果，XYZ轴重叠。

• APM/PX4：ROTATION_ROLL_180_YAW_90
• betaflight/inav：CW90_DEG

4.2 $MPU6000^{\ast }$

非正常旋转，Y变成X、X变成Y、Z反向，需要驱动底层代码配合，以保持物理方向与应用的一致性。

• APM/PX4：ROTATION_NONE
• betaflight/inav：CW90_DEG

5. 参考资料

【1】BetaFlight深入传感设计之九：传感坐标系/机体坐标系/导航坐标系/经纬度坐标系
【2】BetaFlight深入传感设计之八：坐标系
【3】BetaFlight深入传感设计之五：MahonyAHRS & 方向余弦矩阵理论
【4】PX4-Flight Controller/Sensor Orientation
【5】betaflight-Flight-Controller-Orientation
【6】GeoGebra - 数学教学软件

6. 补充资料-常见bf-apm映射

# betaflight sensor alignment is a mysterious art
# some of these might be right but you should always check
alignment = {
    "CW0" : "ROTATION_YAW_270",
    "CW90" : "ROTATION_NONE",
    "CW180" : "ROTATION_YAW_90",
    "CW270" : "ROTATION_YAW_180",
    "CW0FLIP" : "ROTATION_ROLL_180_YAW_270",
    "CW90FLIP" : "ROTATION_ROLL_180",
    "CW180FLIP" : "ROTATION_ROLL_180_YAW_90",
    "CW270FLIP" : "ROTATION_PITCH_180",
    "DEFAULT" : "ROTATION_NONE",
}
1
2
3
4
5
6
7
8
9
10
11
12
13