BetaFlight深入传感设计之一:Baro传感模块

BetaFlight深入传感设计之一:Baro传感模块

  • 1. HwPreInit/HwInit阶段
    • 1.1 【业务HwPreInit】baroPreInit
    • 1.2 【业务HwInit】baroDetect
      • 1.2.1 bmp280StartUP
      • 1.2.2 bmp280ReadUP
      • 1.2.3 bmp280GetUP
      • 1.2.4 bmp280Calculate
      • 1.2.4.1 bmp280CompensateTemperature
      • 1.2.4.2 bmp280CompensatePressure
  • 2. HwIo阶段
  • 3. HwDataAnalysis阶段
    • 3.1 Calibration
    • 3.2 Altitude Caclulation
  • 4. 总结
  • 5. 参考资料

Baro高度计传感器主要根据气压、温度与海拔高度之间的关系来计算当前离地(出发地)高度。

通常气压与高度之间的关系如下图所示:

BetaFlight深入传感设计之一:Baro传感模块_第1张图片

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

1. HwPreInit/HwInit阶段

1.1 【业务HwPreInit】baroPreInit

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

baroPreInit
 └──> baro_busType == BUS_TYPE_SPI>
     └──> spiPreinitRegister(barometerConfig()->baro_spi_csn, IOCFG_IPU, 1);

1.2 【业务HwInit】baroDetect

目前,支持以下硬件规格气压计传感器:

  1. BARO_BMP085
  2. BARO_MS5611
  3. BARO_BMP280
  4. BARO_LPS
  5. BARO_QMP6988
  6. BARO_BMP388
  7. BARO_DPS310

注:目前代码中的气压计检测高度的计算公式来自BMP085芯片手册。

根据BetaFlight深入传感设计:传感模块设计框架,下面以BMP280为例:

  • dummy函数:bmp280StartUT / bmp280GetUT / bmp280ReadUT
  • 其中与芯片相关的重要函数是:bmp280StartUP / bmp280GetUP / bmp280ReadUP / bmp280Calculate

注:BMP280芯片温度和压力都是在压力采集阶段完成,所以温度采集例程是dummy。

bmp280Detect
 ├──> delay(20) // 不知道为什么一上来就来个delay???
 ├──> bool defaultAddressApplied = false 
 ├──> bmp280BusInit(dev) // 设备总线初始化
 ├──> <(dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)>
 │   ├──> dev->busType_u.i2c.address = BMP280_I2C_ADDR  // Default address for BMP280
 │   └──> defaultAddressApplied = true
 ├──> busReadRegisterBuffer(dev, BMP280_CHIP_ID_REG, &bmp280_chip_id, 1)  /* read Chip Id */
 ├──> <(bmp280_chip_id != BMP280_DEFAULT_CHIP_ID) && (bmp280_chip_id != BME280_DEFAULT_CHIP_ID)>
 │   ├──> bmp280BusDeinit(dev)
 │   ├──> 
 │   │   └──> dev->busType_u.i2c.address = 0
 │   └──> return false
 ├──> busDeviceRegister(dev)
 ├──> busReadRegisterBuffer(dev, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, (uint8_t *)&bmp280_cal, sizeof(bmp280_calib_param_t))  // read calibration
 ├──> busWriteRegister(dev, BMP280_CTRL_MEAS_REG, BMP280_MODE) // set oversampling + power mode (forced), and start sampling
 ├──> baro->combined_read = true
 ├──> baro->ut_delay = 0
 ├──> baro->start_ut = bmp280StartUT  //dummy as temperature is measured as part of pressure
 ├──> baro->get_ut = bmp280GetUT  //dummy as temperature is measured as part of pressure
 ├──> baro->read_ut = bmp280ReadUT  //dummy as temperature is measured as part of pressure
 ├──> baro->start_up = bmp280StartUP  //gets both temperature and pressure
 ├──> baro->get_up = bmp280GetUP  //gets both temperature and pressure
 ├──> baro->read_up = bmp280ReadUP  //gets both temperature and pressure
 ├──> baro->up_delay = ((T_INIT_MAX + T_MEASURE_PER_OSRS_MAX * (((1 << BMP280_TEMPERATURE_OSR) >> 1) + ((1 << BMP280_PRESSURE_OSR) >> 1)) + (BMP280_PRESSURE_OSR ? T_SETUP_PRESSURE_MAX : 0) + 15) / 16) * 1000
 ├──> baro->calculate = bmp280Calculate 
 └──> return true

1.2.1 bmp280StartUP

触发一次压力数据采集。

static void bmp280StartUP(baroDev_t *baro)
{
    // start measurement
    // set oversampling + power mode (forced), and start sampling
    busWriteRegisterStart(&baro->dev, BMP280_CTRL_MEAS_REG, BMP280_MODE);
}

#define BMP280_CTRL_MEAS_REG                 (0xF4)  /* Ctrl Measure Register */

// configure pressure and temperature oversampling, forced sampling mode
#define BMP280_PRESSURE_OSR              (BMP280_OVERSAMP_8X)
#define BMP280_TEMPERATURE_OSR           (BMP280_OVERSAMP_1X)
#define BMP280_MODE                      (BMP280_PRESSURE_OSR << 2 | BMP280_TEMPERATURE_OSR << 5 | BMP280_FORCED_MODE)

#define BMP280_OVERSAMP_SKIPPED          (0x00)
#define BMP280_OVERSAMP_1X               (0x01)
#define BMP280_OVERSAMP_2X               (0x02)
#define BMP280_OVERSAMP_4X               (0x03)
#define BMP280_OVERSAMP_8X               (0x04)
#define BMP280_OVERSAMP_16X              (0x05)

#define BMP280_FORCED_MODE                   (0x01)

BetaFlight深入传感设计之一:Baro传感模块_第2张图片

BetaFlight深入传感设计之一:Baro传感模块_第3张图片

BetaFlight深入传感设计之一:Baro传感模块_第4张图片

BetaFlight深入传感设计之一:Baro传感模块_第5张图片

1.2.2 bmp280ReadUP

从传感芯片获取一次压力数据。

static bool bmp280ReadUP(baroDev_t *baro)
{
    if (busBusy(&baro->dev, NULL)) {
        return false;
    }

    // read data from sensor
    busReadRegisterBufferStart(&baro->dev, BMP280_PRESSURE_MSB_REG, sensor_data, BMP280_DATA_FRAME_SIZE);

    return true;
}

#define BMP280_PRESSURE_MSB_REG              (0xF7)  /* Pressure MSB Register */
#define BMP280_DATA_FRAME_SIZE               (6)

BetaFlight深入传感设计之一:Baro传感模块_第6张图片
BetaFlight深入传感设计之一:Baro传感模块_第7张图片

1.2.3 bmp280GetUP

根据1.2.2数据格式,解析一次温度和压力原始数据。

static bool bmp280GetUP(baroDev_t *baro)
{
    if (busBusy(&baro->dev, NULL)) {
        return false;
    }

    bmp280_up = (int32_t)(sensor_data[0] << 12 | sensor_data[1] << 4 | sensor_data[2] >> 4);
    bmp280_ut = (int32_t)(sensor_data[3] << 12 | sensor_data[4] << 4 | sensor_data[5] >> 4);

    return true;
}

1.2.4 bmp280Calculate

对温度和压力进行补偿

  • bmp280CompensateTemperature
  • bmp280CompensatePressure

详细请见:bmp280 data sheet, page 44, section 8.2 compensation formula in 32 bit fixed point。

STATIC_UNIT_TESTED void bmp280Calculate(int32_t *pressure, int32_t *temperature)
{
    // calculate
    int32_t t;
    uint32_t p;
    t = bmp280CompensateTemperature(bmp280_ut);
    p = bmp280CompensatePressure(bmp280_up);

    if (pressure)
        *pressure = (int32_t)(p / 256);
    if (temperature)
        *temperature = t;
}

1.2.4.1 bmp280CompensateTemperature

// Returns temperature in DegC, resolution is 0.01 DegC. Output value of "5123" equals 51.23 DegC
// t_fine carries fine temperature as global value
static int32_t bmp280CompensateTemperature(int32_t adc_T)
{
    int32_t var1, var2, T;

    var1 = ((((adc_T >> 3) - ((int32_t)bmp280_cal.dig_T1 << 1))) * ((int32_t)bmp280_cal.dig_T2)) >> 11;
    var2  = (((((adc_T >> 4) - ((int32_t)bmp280_cal.dig_T1)) * ((adc_T >> 4) - ((int32_t)bmp280_cal.dig_T1))) >> 12) * ((int32_t)bmp280_cal.dig_T3)) >> 14;
    t_fine = var1 + var2;
    T = (t_fine * 5 + 128) >> 8;

    return T;
}

1.2.4.2 bmp280CompensatePressure

// Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8 fractional bits).
// Output value of "24674867" represents 24674867/256 = 96386.2 Pa = 963.862 hPa
static uint32_t bmp280CompensatePressure(int32_t adc_P)
{
    int64_t var1, var2, p;
    var1 = ((int64_t)t_fine) - 128000;
    var2 = var1 * var1 * (int64_t)bmp280_cal.dig_P6;
    var2 = var2 + ((var1*(int64_t)bmp280_cal.dig_P5) << 17);
    var2 = var2 + (((int64_t)bmp280_cal.dig_P4) << 35);
    var1 = ((var1 * var1 * (int64_t)bmp280_cal.dig_P3) >> 8) + ((var1 * (int64_t)bmp280_cal.dig_P2) << 12);
    var1 = (((((int64_t)1) << 47) + var1)) * ((int64_t)bmp280_cal.dig_P1) >> 33;
    if (var1 == 0)
        return 0;
    p = 1048576 - adc_P;
    p = (((p << 31) - var2) * 3125) / var1;
    var1 = (((int64_t)bmp280_cal.dig_P9) * (p >> 13) * (p >> 13)) >> 25;
    var2 = (((int64_t)bmp280_cal.dig_P8) * p) >> 19;
    p = ((p + var1 + var2) >> 8) + (((int64_t)bmp280_cal.dig_P7) << 4);
    return (uint32_t)p;
}

2. HwIo阶段

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

【决策】isBaroReady 

taskUpdateBaro
 └──> 【业务】baroUpdate
     └──> recalculateBarometerTotal

注:关于该任务的介绍,详见:【BetaFlight模块设计之九:气压计任务分析】

3. HwDataAnalysis阶段

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

3.1 Calibration

地面水平校准,通常RC模型起飞和落地都是同一地点,因此我们不必像航空那样,到了将落地设置当地的地面气压。

【决策】baroIsCalibrationComplete 

processRcStickPositions
 └──> 【业务】baroSetGroundLevel  // 耗时250ms
     └──> baroSetCalibrationCycles

cmsCalibrateBaro
 └──> baroStartCalibration
     └──> baroSetCalibrationCycles

init
 └──> 【业务】baroStartCalibration  // 耗时5秒 + 10秒(extra) = 15秒
     └──> baroSetCalibrationCycles

taskCalculateAltitude
 └──> calculateEstimatedAltitude
     └──> 【业务】performBaroCalibrationCycle

#define CALIBRATING_BARO_CYCLES 200 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
#define SET_GROUND_LEVEL_BARO_CYCLES 10 // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)

渐变算术平均校准地面气压,然后是一个气压校准公式(与pressureToAltitude函数重复):

baroGroundAltitude = (1.0f - pow_approx((baroGroundPressure / 8) / 101325.0f, 0.190259f)) * 4433000.0f

void performBaroCalibrationCycle(void)
{
    static int32_t savedGroundPressure = 0;

    baroGroundPressure -= baroGroundPressure / 8;
    baroGroundPressure += baroPressureSum / barometerConfig()->baro_sample_count;
    baroGroundAltitude = (1.0f - pow_approx((baroGroundPressure / 8) / 101325.0f, 0.190259f)) * 4433000.0f;

    if (baroGroundPressure == savedGroundPressure) {
        calibratingB = 0;
    } else {
        calibratingB--;
        savedGroundPressure = baroGroundPressure;
    }
}

static float pressureToAltitude(const float pressure)
{
    return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f;
}

3.2 Altitude Caclulation

飞机地面高度计算:当前气压显示高度 - 地面气压显示高度

注:有高度不等于当前飞机距离地面的高度,而是当前高度在垂直方向距离出发点(起飞点)的高度。因此需要有Minimum safe altitudes(最低安全高度)的概念,通常飞机尤其固定翼飞机是需要飞在最低高度以上的,这样才能进行盲飞。

【决策】getEstimatedAltitudeCm

taskCalculateAltitude
 └──> calculateEstimatedAltitude
     └──> 【业务】baroCalculateAltitude
int32_t baroCalculateAltitude(void)
{
    int32_t BaroAlt_tmp;

    // calculates height from ground via baro readings
    if (baroIsCalibrationComplete()) {
        BaroAlt_tmp = lrintf(pressureToAltitude((float)(baroPressureSum / barometerConfig()->baro_sample_count)));
        BaroAlt_tmp -= baroGroundAltitude;
        baro.BaroAlt = lrintf((float)baro.BaroAlt * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_noise_lpf) + (float)BaroAlt_tmp * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_noise_lpf))); // additional LPF to reduce baro noise
    }
    else {
        baro.BaroAlt = 0;
    }
    return baro.BaroAlt;
}

4. 总结

鉴于研读这些代码,从远航以及使用的角度与社区展开讨论:

【1】Delta between betaFlight and National Oceanic and Atmospheric Administration (NOAA) formula #11870

该问题涉及:

  1. GPS高度精度问题导致的偏差
  2. GPS与气压计高度测量在斜率上不一致的问题
  3. 两种测量方式混合叠加后,以何种方式提高精度
  4. BF4.4及后续RTH (GPS Rescure)在自动降落功能拓展依赖高精度高度测量

【2】baro temperature OSD display #11874

该问题涉及:

  1. #11870 DEBUG_ALTITUDE尚未记录baro温度,因此缺少温度数据对气压的支持
  2. 飞行过程尤其穿云,能够有周边温度显示是一个很炫的功能,而core temperature/ESC temperature不能代表实际气温

5. 参考资料

【1】BetaFlight深入传感设计:传感模块设计框架
【2】BetaFlight模块设计之九:气压计任务分析
【3】BetaFlight模块设计之十四:高度计算任务分析
【4】Wikipedia, Pressure_altitude
【5】Relationship Between Altitude and Pressure

你可能感兴趣的:(xFlight,嵌入式硬件,stm32,betaflight)