BetaFlight深入传感设计之二：Mag传感模块

2023-05-16

BetaFlight深入传感设计之二：Mag传感模块

1. HwPreInit/HwInit阶段
- 1.1 【业务HwPreInit】compassPreInit
- 1.2 【业务HwInit】compassInit
- - 1.2.1 compassDetect
  - 1.2.2 buildRotationMatrixFromAlignment
  - 1.2.3 hmc5883lInit
  - 1.2.3.1 Configuration Register A
  - 1.2.3.2 Configuration Register B
  - 1.2.3.3 Mode Register
  - 1.2.4 hmc5883lRead
  - - 1.2.4.1 Data Output X Register A and B
    - 1.2.4.2 Data Output Y Register A and B
    - 1.2.4.3 Data Output Z Register A and B
2. HwIo阶段
3. HwDataAnalysis阶段
- 3.1 Calibration
- 3.2 Mag Caclulation
4. 总结
5. 参考资料

Mag地磁传感器主要根据地球磁场以及电磁学物理特性来判断当前传感器指向。

根据BetaFlight深入传感设计：传感模块设计框架，我们针对如下几个阶段进行分析。

1. HwPreInit/HwInit阶段

1.1 【业务HwPreInit】compassPreInit

该阶段对SPI片选信号脚进行了硬件配置(仅当代码宏定义支持SPI的情况下)。

compassPreInit
 └──> <USE_SPI><compassConfig()->mag_busType == BUS_TYPE_SPI>
     └──> spiPreinitRegister(compassConfig()->mag_spi_csn, IOCFG_IPU, 1)

1.2 【业务HwInit】compassInit

业务初始化阶段主要涉及：

compassDetect //硬件芯片检测
magDev.init //设备总线初始化
buildRotationMatrixFromAlignment //传感方向对齐

compassInit
 ├──> <!compassDetect(&magDev, &alignment)>
 │   └──> return false
 ├──> [initialize and calibration. turn on led during mag calibration (calibration routine blinks it)]
 ├──> magDev.init(&magDev)
 ├──> <compassConfig()->mag_alignment != ALIGN_DEFAULT>
 │   └──> magDev.magAlignment = compassConfig()->mag_alignment
 ├──> buildRotationMatrixFromAlignment(&compassConfig()->mag_customAlignment, &magDev.rotationMatrix)
 └──> return true

注：对于地磁传感器来说，通过闭合导体切割磁力线形成感应电流，从而导致电位差，进而被ADC采集。通过三维空间判断磁场矢量，判断地磁角方向。

1.2.1 compassDetect

目前，支持以下硬件规格磁力计传感器：

MAG_HMC5883
MAG_AK8975
MAG_AK8963
MAG_QMC5883
MAG_LIS3MDL
MAG_MPU925X_AK8963

根据BetaFlight深入传感设计：传感模块设计框架，下面以HMC5883为例：

hmc5883lInit
hmc5883lRead

hmc5883lDetect
 ├──> extDevice_t *dev = &mag->dev
 ├──> <USE_MAG_SPI_HMC5883><dev->bus->busType == BUS_TYPE_SPI>
 │   └──> hmc5883SpiInit(dev)
 ├──> <USE_MAG_HMC5883><dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0>
 │   └──> dev->busType_u.i2c.address = HMC5883_MAG_I2C_ADDRESS
 ├──> bool ack = busReadRegisterBuffer(&mag->dev, HMC58X3_REG_IDA, &sig, 1)
 ├──> <!ack || sig != HMC5883_DEVICE_ID>
 │   └──> return false
 ├──> mag->init = hmc5883lInit
 ├──> mag->read = hmc5883lRead
 └──> return true


#define HMC5883_MAG_I2C_ADDRESS     0x1E
#define HMC5883_DEVICE_ID           0x48

注：默认的HMC5883 mag芯片I2C地址 0x1E。

1.2.2 buildRotationMatrixFromAlignment

根据传感器&飞控板方向对齐页面或者 cli配置参数，建立旋转矩阵，将芯片检测到的磁场方向旋转到与飞机一致的方向。

或者从cli获取相关mag_align参数。

# get 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

该方向在代码中，通过配置参数对Mag对象的旋转矩阵进行初始化完成。

void buildRotationMatrixFromAlignment(const sensorAlignment_t* sensorAlignment, fp_rotationMatrix_t* rm)
 ├──> 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
}

typedef union sensorAlignment_u {
    // value order is the same as axis_e

    // values are in DECIDEGREES, and should be limited to +/- 3600

    int16_t raw[XYZ_AXIS_COUNT]
    struct {
        int16_t roll
        int16_t pitch
        int16_t yaw
    }
} sensorAlignment_t

#define SENSOR_ALIGNMENT(ROLL, PITCH, YAW) ((sensorAlignment_t){\
    .roll = DEGREES_TO_DECIDEGREES(ROLL), \
    .pitch = DEGREES_TO_DECIDEGREES(PITCH), \
    .yaw = DEGREES_TO_DECIDEGREES(YAW), \
})

注：根据配置建立旋转矩阵，详见上面buildRotationMatrix函数。

1.2.3 hmc5883lInit

hmc5883lInit
 ├──> [leave test mode]
 │   ├──> busWriteRegister(dev, HMC58X3_REG_CONFA, HMC_CONFA_8_SAMLES | HMC_CONFA_DOR_15HZ | HMC_CONFA_NORMAL)    // Configuration Register A  -- 0 11 100 00  num samples: 8  output rate: 15Hz  normal measurement mode
 │   ├──> busWriteRegister(dev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_1_3GA)                                          // Configuration Register B  -- 001 00000    configuration gain 1.3Ga
 │   └──> busWriteRegister(dev, HMC58X3_REG_MODE, HMC_MODE_CONTINOUS)                                             // Mode register             -- 000000 00    continuous Conversion Mode
 ├──> delay(100)
 ├──>  hmc5883lConfigureDataReadyInterruptHandling(mag)
 └──> return true

#define HMC58X3_REG_CONFA           0x00
#define HMC58X3_REG_CONFB           0x01
#define HMC58X3_REG_MODE            0x02

#define HMC_CONFA_NORMAL            0x00
#define HMC_CONFA_DOR_15HZ          0X10
#define HMC_CONFA_8_SAMLES          0X60
#define HMC_CONFB_GAIN_1_3GA        0X20
#define HMC_MODE_CONTINOUS          0X00

注：关于寄存机的详细内容，请查阅：HMC5883L datasheet。

1.2.3.1 Configuration Register A

1.2.3.2 Configuration Register B

1.2.3.3 Mode Register

1.2.4 hmc5883lRead

hmc5883lRead
 ├──> static uint8_t buf[6]
 ├──> static bool pendingRead = true
 ├──> <pendingRead>
 │   ├──> busReadRegisterBufferStart(dev, HMC58X3_REG_DATA, buf, sizeof(buf))
 │   ├──> pendingRead = false
 │   └──> return false
 ├──> magData[X] = (int16_t)(buf[0] << 8 | buf[1])
 ├──> magData[Z] = (int16_t)(buf[2] << 8 | buf[3])
 ├──> magData[Y] = (int16_t)(buf[4] << 8 | buf[5])
 ├──> pendingRead = true
 └──> return true

#define HMC58X3_REG_DATA            0x03

1.2.4.1 Data Output X Register A and B

1.2.4.2 Data Output Y Register A and B

1.2.4.3 Data Output Z Register A and B

2. HwIo阶段

该阶段主要确保数据采集及原始数据的有效性，其决策函数提供了判断原始数据有效性的方法。

获取到的数据mag.magADC[N], N= X,Y,Z，经过以下处理：

去除校准offset
传感值旋转对齐

【决策】compassIsHealthy/compassIsHealthy

taskUpdateMag
 └──> 【业务】compassUpdate
     ├──> hmc5883lRead
     └──> alignSensorViaMatrix/alignSensorViaRotation

详见：【BetaFlight模块设计之十：磁力计任务分析】

3. HwDataAnalysis阶段

数据分析阶段主要是将传感器采集得到的原始数据进一步分析，并配合业务提供相关支持。

3.1 Calibration

在地球磁场环境下，校准地磁计：获取最大最小值，从而得到中间值。

注：建议飞行环境变化时，根据当地情况校准一次磁力计，该传感器受地磁和周边干扰影响较大。

【决策】compassIsCalibrationComplete

processRcStickPositions  // 遥控器操作校准: THR_HI + YAW_HI + PIT_LO + ROL_CE
 └──> 【业务】compassStartCalibration

mspProcessInCommand  // 地面配置站触发校准
 └──> 【业务】compassStartCalibration

3.2 Mag Caclulation

这里主要根据Mahony算法来计算方向，提供误差值供后续程序做方向的PI修正。

注：关于Mahony算法暂时还不太明白，后续看了再来和大家讨论。

imuUpdateAttitude
 └──> imuCalculateEstimatedAttitude
     └──> imuMahonyAHRSupdate

注：相关更新任务，详见：BetaFlight模块设计之二十七：姿态更新任务分析。

对于磁力计数据，我们假设磁场垂直于重力，因此忽略地球磁场中的Z分量，并且磁场方向不涉及飞机roll/pitch控制，主要是yaw的方向。

imuMahonyAHRSupdate
 ├──> ...
 ├──> <USE_MAG>
 │   ├──> [Use measured magnetic field vector]
 │   │   ├──> float mx = mag.magADC[X];
 │   │   ├──> float my = mag.magADC[Y];
 │   │   └──> float mz = mag.magADC[Z];
 │   ├──> float recipMagNorm = sq(mx) + sq(my) + sq(mz);
 │   └──> <useMag && recipMagNorm > 0.01f>
 │       ├──> [Normalise magnetometer measurement]
 │       │   ├──> recipMagNorm = invSqrt(recipMagNorm);
 │       │   ├──> mx *= recipMagNorm;
 │       │   ├──> my *= recipMagNorm;
 │       │   └──> mz *= recipMagNorm;
 │       ├──> const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz; // (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
 │       ├──> const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz; // (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
 │       ├──> const float bx = sqrtf(hx * hx + hy * hy); // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
 │       ├──> const float ez_ef = -(hy * bx); // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
 │       └──> [Rotate mag error vector back to BF and accumulate]
 │           ├──> ex += rMat[2][0] * ez_ef;
 │           ├──> ey += rMat[2][1] * ez_ef;
 │           └──> ez += rMat[2][2] * ez_ef;
 └──> ...

4. 总结

磁力计传感器的数据采集和有效数据清理方面并不复杂，只是在后续飞控在四元组和矢量旋转，以及Mahony算法方面，确实不是一下子能够搞明白的。至少我花了3-4个小时，目前没有搞明白。等有时间熟悉了，在和大家唠嗑吧！！！！

5. 参考资料

【1】BetaFlight深入传感设计：传感模块设计框架
【2】BetaFlight模块设计之十：磁力计任务分析
【3】BetaFlight模块设计之二十七：姿态更新任务分析

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)