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深入传感设计：传感模块设计框架，我们针对如下几个阶段进行分析。
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
目前，支持以下硬件规格气压计传感器：
- BARO_BMP085
- BARO_MS5611
- BARO_BMP280
- BARO_LPS
- BARO_QMP6988
- BARO_BMP388
- 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)
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)
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
该问题涉及：
- GPS高度精度问题导致的偏差
- GPS与气压计高度测量在斜率上不一致的问题
- 两种测量方式混合叠加后，以何种方式提高精度
- BF4.4及后续RTH (GPS Rescure)在自动降落功能拓展依赖高精度高度测量
【2】baro temperature OSD display #11874
该问题涉及：
- #11870 DEBUG_ALTITUDE尚未记录baro温度，因此缺少温度数据对气压的支持
- 飞行过程尤其穿云，能够有周边温度显示是一个很炫的功能，而core temperature/ESC temperature不能代表实际气温
5. 参考资料
【1】BetaFlight深入传感设计：传感模块设计框架
【2】BetaFlight模块设计之九：气压计任务分析
【3】BetaFlight模块设计之十四：高度计算任务分析
【4】Wikipedia, Pressure_altitude
【5】Relationship Between Altitude and Pressure