BetaFlight深入传感设计之十：传感器物理特性方向对齐

AHRS(Attitude and Heading Reference Systems)算法主要需要考虑：加速度、方向角、磁力计、GPS等传感器物理特性对飞机姿态以及机头指向的计算和错误估算方法。

鉴于电子器件在实际使用时，受安装、干扰和各种因素(PCB layout、机架传感器安装位置、打印件结构等)影响，其传感物理特性的指向方向与机体坐标并非完全一致。因此在算法之初需要对传感器的各个物理特性进行对齐，确保数据和姿态的一致性。

注1：当我们说到BetaFlight固件机体坐标系的时候，请使用xEast-yNorth-zUp(RH)坐标系统，与通常我们所说的机体坐标系有一定的差异，请特别注意！！！
注2：在系统OSD显示以及GPS COG指向时，要注意角度与机体坐标系的yaw进行反向，以便确保+/-号的一致性。

1. 对齐定义

以下是BetaFlight代码给出的对齐方式。

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;

2. 常见对齐方式

• CW0_DEG: 默认芯片坐标系
• CW90_DEG: 芯片坐标系右手系旋转90度
• CW180_DEG: 芯片坐标系右手系旋转180度
• CW270_DEG: 芯片坐标系右手系旋转270度
• CW0_DEG_FLIP: 芯片沿着yNorth指向不变，坐标系翻转(-)
• CW90_DEG_FLIP: 芯片沿着yNorth指向不变，坐标系翻转(-); 然后按照右手系旋转90度
• CW180_DEG_FLIP: 芯片沿着yNorth指向不变，坐标系翻转(-); 然后按照右手系旋转180度
• CW270_DEG_FLIP: 芯片沿着yNorth指向不变，坐标系翻转(-); 然后按照右手系旋转270度

注：FLIP的意思可以认为，保持yNorth不变，进行芯片翻转。

void alignSensorViaRotation(float *dest, uint8_t rotation)
{
    const float x = dest[X];
    const float y = dest[Y];
    const float z = dest[Z];

    switch (rotation) {
    default:
    case CW0_DEG:
        dest[X] = x;
        dest[Y] = y;
        dest[Z] = z;
        break;
    case CW90_DEG:
        dest[X] = y;
        dest[Y] = -x;
        dest[Z] = z;
        break;
    case CW180_DEG:
        dest[X] = -x;
        dest[Y] = -y;
        dest[Z] = z;
        break;
    case CW270_DEG:
        dest[X] = -y;
        dest[Y] = x;
        dest[Z] = z;
        break;
    case CW0_DEG_FLIP:
        dest[X] = -x;
        dest[Y] = y;
        dest[Z] = -z;
        break;
    case CW90_DEG_FLIP:
        dest[X] = y;
        dest[Y] = x;
        dest[Z] = -z;
        break;
    case CW180_DEG_FLIP:
        dest[X] = x;
        dest[Y] = -y;
        dest[Z] = -z;
        break;
    case CW270_DEG_FLIP:
        dest[X] = -y;
        dest[Y] = -x;
        dest[Z] = -z;
        break;
    }

    if (!standardBoardAlignment) {
        alignBoard(dest);
    }
}

3. 自定义对齐方式

• ALIGN_CUSTOM

这里尤其要注意这个自定义对齐方式，有一些GPS模块由于芯片在PCB上是45度安装的，当需要绑在机架上的时候，往往会出现需要旋转45度或者135度的情况。此时，无法使用常见的对齐方式，需要采用自定义的方式。

请查阅芯片手册，并确认物理芯片上的一个小点，依据上述两个信息来明确芯片的物理特性指向，并将芯片坐标系旋转到的机体xEast-yNorth-zUp(RH)坐标系统。

void buildRotationMatrixFromAlignment(const sensorAlignment_t* sensorAlignment, fp_rotationMatrix_t* rm)
{
    fp_angles_t rotationAngles;
    rotationAngles.angles.roll  = DECIDEGREES_TO_RADIANS(sensorAlignment->roll);
    rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(sensorAlignment->pitch);
    rotationAngles.angles.yaw   = DECIDEGREES_TO_RADIANS(sensorAlignment->yaw);

    buildRotationMatrix(&rotationAngles, rm);
}

void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation)
{
    float cosx, sinx, cosy, siny, cosz, sinz;
    float coszcosx, sinzcosx, coszsinx, sinzsinx;

    cosx = cos_approx(delta->angles.roll);
    sinx = sin_approx(delta->angles.roll);
    cosy = cos_approx(delta->angles.pitch);
    siny = sin_approx(delta->angles.pitch);
    cosz = cos_approx(delta->angles.yaw);
    sinz = sin_approx(delta->angles.yaw);

    coszcosx = cosz * cosx;
    sinzcosx = sinz * cosx;
    coszsinx = sinx * cosz;
    sinzsinx = sinx * sinz;

    rotation->m[0][X] = cosz * cosy;
    rotation->m[0][Y] = -cosy * sinz;
    rotation->m[0][Z] = siny;
    rotation->m[1][X] = sinzcosx + (coszsinx * siny);
    rotation->m[1][Y] = coszcosx - (sinzsinx * siny);
    rotation->m[1][Z] = -sinx * cosy;
    rotation->m[2][X] = (sinzsinx) - (coszcosx * siny);
    rotation->m[2][Y] = (coszsinx) + (sinzcosx * siny);
    rotation->m[2][Z] = cosy * cosx;
}

4. 总结

关于传感器物理特性方向对齐相当于给出了上下左右前后的方向，确保飞控系统能够正确的响应传感数据。

以下一些重要信息大家可以查阅之前的资料：

1. 旋转矩阵：【7】
2. 坐标系统：【9】
3. 物理特性：【2】、【3】、【4】、【5】

5. 参考资料

【1】BetaFlight深入传感设计：传感模块设计框架
【2】BetaFlight深入传感设计之一：Baro传感模块
【3】BetaFlight深入传感设计之二：Mag传感模块
【4】BetaFlight深入传感设计之三：IMU传感模块
【5】BetaFlight深入传感设计之四：GPS传感模块
【6】BetaFlight深入传感设计之五：MahonyAHRS & 方向余弦矩阵理论
【7】BetaFlight深入传感设计之六：四元数计算方法
【8】BetaFlight深入传感设计之七：GPS&Baro高度数据融合
【9】BetaFlight深入传感设计之八：坐标系
【10】BetaFlight深入传感设计之九：传感坐标系/机体坐标系/导航坐标系/经纬度坐标系

6. 补充：gyro + mag对齐方式

align_mag = CUSTOM
Allowed values: DEFAULT, CW0, CW90, CW180, CW270, CW0FLIP, CW90FLIP, CW180FLIP, CW270FLIP, CUSTOM
Default value: DEFAULT

mag_align_roll = 0
Allowed range: -3600 - 3600

mag_align_pitch = 1800
Allowed range: -3600 - 3600
Default value: 0

mag_align_yaw = 1350
Allowed range: -3600 - 3600
Default value: 0

gyro_1_sensor_align = CW270
Allowed values: DEFAULT, CW0, CW90, CW180, CW270, CW0FLIP, CW90FLIP, CW180FLIP, CW270FLIP, CUSTOM
Default value: CW0FLIP

gyro_1_align_roll = 0
Allowed range: -3600 - 3600

gyro_1_align_pitch = 0
Allowed range: -3600 - 3600
Default value: 1800

gyro_1_align_yaw = 2700
Allowed range: -3600 - 3600
Default value: 0