ArduPilot开源飞控之AP_Baro
ArduPilot开源飞控之AP_Baro
- 1. 源由
- 2，框架设计
-
- 2.1 启动代码
- 2.2 任务代码 AP_Baro::update
- 2.3 任务代码 AP_Baro::accumulate
- 2.4 任务代码 AP_TempCalibration::update
- 3. 重要例程
-
- 3.1 AP_Baro::calibrate
- 3.2 AP_TempCalibration
-
- 3.2.1 AP_TempCalibration::setup_learning
- 3.2.2 AP_TempCalibration::learn_calibration
- 3.2.3 AP_TempCalibration::calculate_calibration
- 3.2.4 AP_TempCalibration::apply_calibration
- 3.2.5 AP_TempCalibration::calculate_correction
- 4. 总结
- 5. 参考资料
- 6. 补充资料 - DroneCAN/ExternalAHRS/MSP/SITL
-
- 6.1 AP_Baro_MSP
- 6.2 AP_Baro_ExternalAHRS
- 6.3 AP_Baro_DroneCAN
- 6.4 AP_Baro_SITL
1. 源由
AP_Baro气压计主要用气压来计算相对高度。
关于Ardupilot的所谓高度有多种定义：
- ASL: Altitude above (mean) Sea Level, 飞机相对于海平面高度
- AGL: Altitude above Ground Level, 飞机相对于地面高度
- Relative ALT: 飞机相对于起飞点高度
- Terrain ALT: 地面相对于海平面的高度
- Estimated ALT: 飞控估计相对于起飞点的高度
- Target ALT: 飞控期望相对于起飞点的高度
而这里所主要关注的是是Relative ALT。
2，框架设计
启动代码：
Copter::init_ardupilot
├──> barometer.init
├──> barometer.set_log_baro_bit(MASK_LOG_IMU)
└──> barometer.calibrate(true) // 启动校准地面气压，后续高度都是相对于这个起飞点的高度。
任务代码：
SCHED_TASK(update_altitude, 10, 100, 42),
└──> Copter::update_altitude
└──> Copter::read_barometer
└──> barometer.update
SCHED_TASK_CLASS(AP_Baro, &copter.barometer, accumulate, 50, 90, 63),
└──> AP_Baro::accumulate
└──> AP_Baro_Backend::accumulate
SCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, update, 10, 100, 135),
└──> AP_TempCalibration::update
2.1 启动代码
启动过程分三步：
- 前处理
- 驱动设备检测
- 后处理
/*
initialise the barometer object, loading backend drivers
*/
AP_Baro::init
├──> init_done = true
│
│ /********************************************************************************
│ * Application pre-initialization *
│ ********************************************************************************/
│ // always set field elvation to zero on reboot in the case user fails to update.
│ // TBD automate sanity checking error bounds on previously saved value at new location etc.
├──>
│ ├──> _field_elevation.set_and_save(0.0f)
│ └──> _field_elevation.notify()
├──> // zero bus IDs before probing
│ └──> sensors[i].bus_id.set(0)
│
│
│ /********************************************************************************
│ * Add backend baro drivers *
│ ********************************************************************************/
│ // AP_Baro_SITL
├──>
│ ├──> SITL::SIM *sitl = AP::sitl()
│ ├──>
│ │ └──>AP_HAL::panic("No SITL pointer")
│ └──> baro_count i++)>
│ └──> ADD_BACKEND(new AP_Baro_SITL(*this)) // use dronecan instances instead of SITL instances
│
│ // AP_Baro_DroneCAN
├──>
│ └──> ADD_BACKEND(AP_Baro_DroneCAN::probe(*this))// Detect UAVCAN Modules, try as many times as there are driver slots
│
│ // AP_Baro_ExternalAHRS
├──>
│ ├──> const int8_t serial_port = AP::externalAHRS().get_port(AP_ExternalAHRS::AvailableSensor::BARO)
│ └──> = 0>
│ └──> ADD_BACKEND(new AP_Baro_ExternalAHRS(*this, serial_port))
│
│ // AP_Baro_MS56XX
├──>
│ ├──>
│ │ └──> ADD_BACKEND(AP_Baro_MS56XX::probe(*this,std::move(GET_I2C_DEVICE(_ext_bus, HAL_BARO_MS5611_I2C_ADDR))))
│ └──> return // do not probe for other drivers when using simulation:
│
│ // 板级自定义气压计，目前通常配置以这种方式来缩减代码开销。
├──>
│ └──> HAL_BARO_PROBE_LIST // probe list from BARO lines in hwdef.dat
│
│
│ //AP_Baro_MS56XX/AP_Baro_ICM20789
├──>
│ ├──>
│ │ ├──> ADD_BACKEND(AP_Baro_MS56XX::probe
│ │ └──> break
│ ├──>
│ │ ├──> ADD_BACKEND(AP_Baro_MS56XX::probe
│ │ └──> break
│ ├──>
│ │ ├──> ADD_BACKEND(AP_Baro_MS56XX::probe
│ │ ├──> ADD_BACKEND(AP_Baro_MS56XX::probe
│ │ └──> break
│ ├──>
│ │ ├──> ADD_BACKEND(AP_Baro_MS56XX::probe
│ │ └──> break
│ ├──>
│ │ ├──> ADD_BACKEND(AP_Baro_MS56XX::probe
│ │ └──> break
│ ├──>
│ │ ├──>
│ │ │ ├──> ADD_BACKEND(AP_Baro_MS56XX::probe
│ │ │ └──> ADD_BACKEND(AP_Baro_MS56XX::probe
│ │ ├──>
│ │ │ └──> ADD_BACKEND(AP_Baro_MS56XX::probe
│ │ └──> break
│ ├──>
│ │ ├──> ADD_BACKEND(AP_Baro_MS56XX::probe
│ │ └──> break
│ ├──>
│ │ ├──> ADD_BACKEND(AP_Baro_ICM20789::probe
│ │ └──> break
│ └──>
│ ├──> ADD_BACKEND(AP_Baro_MS56XX::probe
│ └──> break
│
│
│ // AP_Baro_LPS2XH/AP_Baro_ICM20789
├──>
│ └──> ADD_BACKEND(AP_Baro_LPS2XH::probe_InvensenseIMU
├──>
│ └──> ADD_BACKEND(AP_Baro_ICM20789::probe
├──>
│ └──> ADD_BACKEND(AP_Baro_ICM20789::probe
│
│
│ // AP_Baro_MS56XX/AP_Baro_KellerLD
├──> <_ext_bus >= 0> // can optionally have baro on I2C too
│ ├──>
│ │ ├──> ADD_BACKEND(AP_Baro_MS56XX::probe
│ │ └──> ADD_BACKEND(AP_Baro_KellerLD::probe
│ └──>
│ └──> ADD_BACKEND(AP_Baro_MS56XX::probe
│
│
│ // AP_Baro_BMP085/AP_Baro_BMP280/AP_Baro_SPL06/AP_Baro_BMP388/AP_Baro_MS56XX/AP_Baro_FBM320/AP_Baro_DPS280/AP_Baro_LPS2XH/AP_Baro_KellerLD
├──>
│ └──> _probe_i2c_barometers()
│
│
│ // AP_Baro_MSP
├──>
│ ├──> <(_baro_probe_ext.get() & PROBE_MSP) && msp_instance_mask == 0>
│ │ └──> msp_instance_mask |= 1 // allow for late addition of MSP sensor
│ └──>
│ └──> ADD_BACKEND(new AP_Baro_MSP(*this, i))
│
│
│ /********************************************************************************
│ * Application post-initialization *
│ ********************************************************************************/
├──> // most boards requires external baro
│ ├──> baro_count == 0>
│ │ └──> return
│ └──> <_num_drivers == 0 || _num_sensors == 0 || drivers[0] == nullptr>
│ └──> AP_BoardConfig::config_error("Baro: unable to initialise driver")
│
│
└──>
│ // AP_Periph always is set calibrated. We only want the pressure,
│ // so ground calibration is unnecessary
└──>
├──> sensors[i].calibrated = true
└──> sensors[i].alt_ok = true
2.2 任务代码 AP_Baro::update
定时轮训，更新相对高度数据。
注：_alt_offset从当前代码看，始终初始化为0，且没有其他赋值的地方。因此_alt_offset_active也为0。
BARO_ALT_OFFSET: altitude offset
Note: This parameter is for advanced usersaltitude offset in meters added to barometric altitude. This is used to allow for automatic adjustment of
the base barometric altitude by a ground station equipped with a barometer. The value is added to the
barometric altitude read by the aircraft. It is automatically reset to 0 when the barometer is calibrated
on each reboot or when a preflight calibration is performed.
AP_Baro::update
├──> WITH_SEMAPHORE(_rsem)
│
│ /********************************************************************************
│ * _alt_offset_active update *
│ ********************************************************************************/
├──> 0.01f>
│ │ // If there's more than 1cm difference then slowly slew to it via LPF.
│ │ // The EKF does not like step inputs so this keeps it happy.
│ └──> _alt_offset_active = (0.95f*_alt_offset_active) + (0.05f*_alt_offset)
├──>
│ └──> _alt_offset_active = _alt_offset
│
│
├──>
│ └──> bool old_primary_healthy = sensors[_primary].healthy
│
│
│ /********************************************************************************
│ * sensor driver update *
│ ********************************************************************************/
├──>
│ └──> drivers[i]->backend_update(i)
│
│ /********************************************************************************
│ * update altitude calculation *
│ ********************************************************************************/
├──>
│ ├──> float ground_pressure = sensors[i].ground_pressure
│ ├──>
│ │ └──> sensors[i].ground_pressure.set(sensors[i].pressure)
│ ├──> float altitude = sensors[i].altitude
│ ├──> float corrected_pressure = sensors[i].pressure + sensors[i].p_correction
│ │
│ │
│ ├──> //BARO_TYPE_AIR
│ │ ├──>
│ │ │ └──> corrected_pressure -= wind_pressure_correction(i)
│ │ └──> altitude = get_altitude_difference(sensors[i].ground_pressure, corrected_pressure)
│ │
│ │
│ │ //101325Pa is sea level air pressure, 9800 Pascal/ m depth in water.
│ │ //No temperature or depth compensation for density of water.
│ ├──> //BARO_TYPE_WATER
│ │ └──> altitude = (sensors[i].ground_pressure - corrected_pressure) / 9800.0f / _specific_gravity
│ │
│ │
│ │ // sanity check altitude
│ ├──>
│ └──> sensors[i].altitude = altitude + _alt_offset_active
│
│ /********************************************************************************
│ * ensure the climb rate filter is updated *
│ ********************************************************************************/
├──>
│ └──> _climb_rate_filter.update(get_altitude(), get_last_update())
│
│ /********************************************************************************
│ * choose primary sensor *
│ ********************************************************************************/
├──> <_primary_baro >= 0 && _primary_baro < _num_sensors && healthy(_primary_baro)>
│ └──> _primary = _primary_baro
│ // not primary sensor
├──>
│ ├──> _primary = 0
│ └──>
│ ├──> _primary = i
│ └──> break
│
│
├──>
│ └──> update_field_elevation()
└──>
├──> Write_Baro()
└──> // log sensor healthy state change
├──> const LogErrorCode code = sensors[_primary].healthy ? LogErrorCode::ERROR_RESOLVED : LogErrorCode::UNHEALTHY
└──> AP::logger().Write_Error(LogErrorSubsystem::BARO, code)
2.3 任务代码 AP_Baro::accumulate
/*
call accumulate on all drivers
*/
AP_Baro::accumulate
└──>
└──> drivers[i]->accumulate()
注：目前accumulate方法在·AP_Baro_XXX中未实现，相当于间接使用AP_Baro_Backend::accumulate`空函数。
2.4 任务代码 AP_TempCalibration::update
定时轮训，根据策略选择修正气压方式。
/*
called at 10Hz from the main thread. This is called both when armed
and disarmed. It only does learning while disarmed, but needs to
supply the corrections to the sensor libraries at all times
*/
AP_TempCalibration::update
├──>
│ └──> break
├──>
│ ├──> learn_calibration()
│ └──> FALLTHROUGH
└──>
├──> apply_calibration()
└──> break
3. 重要例程
3.1 AP_Baro::calibrate
这个流程主要是在上电时，对起飞点气压进行检测，最为相对高度的一个基点。因此，从气压计校准和安全的角度，请将飞机置于起飞点后，再上电。
注：关于气压计计算方面可参考：
【1】BetaFlight深入传感设计之一：Baro传感模块
【2】Delta between betaFlight and National Oceanic and Atmospheric Administration (NOAA) formula #11870
// calibrate the barometer. This must be called at least once before
// the altitude() or climb_rate() interfaces can be used
AP_Baro::calibrate
├──> [start by assuming all sensors are calibrated (for healthy() test)]
│ └──>
│ ├──> sensors[i].calibrated = true
│ └──> sensors[i].alt_ok = true
├──> was_watchdog_reset()>
│ ├──> BARO_SEND_TEXT(MAV_SEVERITY_INFO, "Baro: skipping calibration after WDG reset")
│ └──> return
├──> baro_count == 0>
│ └──> return
├──>
│ ├──> BARO_SEND_TEXT(MAV_SEVERITY_INFO, "Baro: no sensors found, skipping calibration")
│ └──> return
│
│ /********************************************************************************
│ * Calibrating *
│ ********************************************************************************/
├──> BARO_SEND_TEXT(MAV_SEVERITY_INFO, "Calibrating barometer")
│
│ // reset the altitude offset when we calibrate. The altitude
│ // offset is supposed to be for within a flight
├──> _alt_offset.set_and_save(0)
│
│ // let the barometer settle for a full second after startup
│ // the MS5611 reads quite a long way off for the first second,
│ // leading to about 1m of error if we don't wait
├──>
│ ├──> uint32_t tstart = AP_HAL::millis()
│ ├──>
│ │ ├──> update()
│ │ ├──> 500>
│ │ │ └──> AP_BoardConfig::config_error("Baro: unable to calibrate")
│ │ └──> hal.scheduler->delay(10)
│ └──> hal.scheduler->delay(100)
│
│ // now average over 5 values for the ground pressure settings
├──> float sum_pressure[BARO_MAX_INSTANCES] = {0}
├──> uint8_t count[BARO_MAX_INSTANCES] = {0}
├──> const uint8_t num_samples = 5
├──>
│ ├──> uint32_t tstart = AP_HAL::millis()
│ ├──>
│ │ ├──> update()
│ │ └──> 500>
│ │ └──> AP_BoardConfig::config_error("Baro: unable to calibrate")
│ ├──>
│ │ └──>
│ │ ├──> sum_pressure[i] += sensors[i].pressure
│ │ └──> count[i] += 1
│ └──> hal.scheduler->delay(100)
├──>
│ ├──>
│ │ └──> sensors[i].calibrated = false
│ └──>
│ ├──> float p0_sealevel = get_sealevel_pressure(sum_pressure[i] / count[i])
│ └──> sensors[i].f.set_and_save(p0_sealevel)
├──> _guessed_ground_temperature = get_external_temperature()
│
│ /********************************************************************************
│ * Calibrating *
│ ********************************************************************************/
│ // panic if all sensors are not calibrated
├──> uint8_t num_calibrated = 0
├──>
│ ├──> BARO_SEND_TEXT(MAV_SEVERITY_INFO, "Barometer %u calibration complete", i+1)
│ └──> num_calibrated++
├──>
│ └──> return
└──> AP_BoardConfig::config_error("Baro: all sensors uncalibrated")
3.2 AP_TempCalibration
该类主要用于温度对气压的校准，默认情况下为TC_DISABLED，仅使用第一个baro数据。
注：当前代码情况看，似乎未使用TC_ENABLE_USE/TC_ENABLE_LEARN
enum {
TC_DISABLED = 0,
TC_ENABLE_USE = 1,
TC_ENABLE_LEARN = 2,
}
3.2.1 AP_TempCalibration::setup_learning
TC_ENABLE_LEARN初始化过程
AP_TempCalibration::setup_learning
├──> learn_temp_start = AP::baro().get_temperature()
├──> learn_temp_step = 0.25
├──> learn_count = 200
├──> learn_i = 0
├──> delete [] learn_values
├──> learn_values = new float[learn_count]
└──>
└──> return
3.2.2 AP_TempCalibration::learn_calibration
TC_ENABLE_LEARN学习/更新校准曲线
AP_TempCalibration::learn_calibration
├──> const AP_Baro &baro = AP::baro()
├──> get_soft_armed() || baro.get_temperature(0) < Tzero>
│ └──> return
├──>
│ ├──> debug("learn reset\n") // if we have any movement then we reset learning
│ ├──> setup_learning()
│ └──>
│ └──> return
│
│ /********************************************************************************
│ * Learning curve *
│ ********************************************************************************/
├──> float temp = baro.get_temperature(0)
├──> float P = baro.get_pressure(0)
├──> uint16_t idx = (temp - learn_temp_start) / learn_temp_step
├──> = learn_count>
│ └──> return // could change learn_temp_step here
├──>
│ ├──> learn_values[idx] = P
│ └──> debug("learning %u %.2f at %.2f\n", idx, learn_values[idx], temp)
├──>
│ └──> learn_values[idx] = 0.9 * learn_values[idx] + 0.1 * P // filter in new value
├──> learn_i = MAX(learn_i, idx)
│
│ /********************************************************************************
│ * Calculate calibration *
│ ********************************************************************************/
├──> uint32_t now = AP_HAL::millis()
└──> 100 && idx*learn_temp_step > min_learn_temp_range && temp - learn_temp_start > temp_max - temp_min>
├──>last_learn_ms = now
└──> calculate_calibration() // run estimation and update parameters
3.2.3 AP_TempCalibration::calculate_calibration
TC_ENABLE_LEARN更新校准曲线
AP_TempCalibration::calculate_calibration
├──> float current_err = calculate_p_range(baro_exponent)
├──> float test_exponent = baro_exponent + learn_delta
├──> float test_err = calculate_p_range(test_exponent)
├──> = current_err>
│ ├──> test_exponent = baro_exponent - learn_delta
│ └──> test_err = calculate_p_range(test_exponent)
└──> = exp_limit_min && test_err < current_err>
├──> debug("CAL: %.2f\n", test_exponent)
│ └──>
│ └──> baro_exponent.set_and_save(test_exponent)
├──> temp_min.set_and_save_ifchanged(learn_temp_start)
└──> temp_max.set_and_save_ifchanged(learn_temp_start + learn_i*learn_temp_step)
3.2.4 AP_TempCalibration::apply_calibration
AP_TempCalibration::apply_calibration
├──> AP_Baro &baro = AP::baro()
├──>
│ └──> return
├──> float temp = baro.get_temperature(0)
├──> float correction = calculate_correction(temp, baro_exponent)
└──> baro.set_pressure_correction(0, correction)
3.2.5 AP_TempCalibration::calculate_correction
float powf( float base, float exponent ); C99规范标准C接口。
/*
calculate the correction given an exponent and a temperature
This one parameter correction is deliberately chosen to be very
robust for extrapolation. It fits the characteristics of the
ICM-20789 barometer nicely.
*/
AP_TempCalibration::calculate_correction
└──> return powf(MAX(temp - Tzero, 0), exponent)
4. 总结
总体来看，AP_Baro代码整体看主要是：
- 与起飞点相对高度的计算；
- 默认起飞点高度为0，满足航模起飞降落同一地点场景；
- 若起飞降落不在同一位置，可以考虑输入参数：
BARO_ALT_OFFSET； - 同时，
AP_TempCalibration校准功能似乎并未使用（因为，并未发现设置校准类型的代码或者参数）； - 气压计芯片主要采用I2C总线，也支持串行/CAN总线；
I2C芯片
KellerLD
MS56XX
ICM20789
LPS2XH
BMP085
BMP280
SPL06
BMP388
FBM320
DPS280
DPS310
AP_Baro_MSP
AP_Baro_ExternalAHRS
AP_Baro_DroneCAN
AP_Baro_SITL
5. 参考资料
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
6. 补充资料 - DroneCAN/ExternalAHRS/MSP/SITL
非I2C总线驱动启动的方式与I2C总线的probe方式略有不同，这里补充说明下。
AP_Baro_DroneCAN
AP_Baro_ExternalAHRS
AP_Baro_MSP
AP_Baro_SITL
6.1 AP_Baro_MSP
AP_Baro_MSP::AP_Baro_MSP(AP_Baro &baro, uint8_t _msp_instance) :
AP_Baro_Backend(baro)
{
msp_instance = _msp_instance;
instance = _frontend.register_sensor();
set_bus_id(instance, AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_MSP,0,msp_instance,0));
}
AP_Vehicle::setup
└──> AP_MSP::init
└──> AP_MSP::loop
└──> AP_MSP_Telem_Backend::process_incoming_data
└──> AP_MSP_Telem_Backend::msp_process_received_command
└──> AP_MSP_Telem_Backend::msp_process_command
└──> AP_MSP_Telem_Backend::msp_process_sensor_command
└──> AP_MSP_Telem_Backend::msp_handle_baro
└──> AP_Baro::handle_msp
└──> AP_Baro_MSP::handle_msp
6.2 AP_Baro_ExternalAHRS
AP_Baro_ExternalAHRS::AP_Baro_ExternalAHRS(AP_Baro &baro, uint8_t port) :
AP_Baro_Backend(baro)
{
instance = _frontend.register_sensor();
set_bus_id(instance, AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL,port,0,0));
}
AP_Vehicle::setup
└──> AP_ExternalAHRS::init
└──> AP_ExternalAHRS_VectorNav::AP_ExternalAHRS_VectorNav
└──> AP_ExternalAHRS_VectorNav::update_thread
└──> AP_ExternalAHRS_VectorNav::check_uart
└──> AP_ExternalAHRS_VectorNav::process_packet1
└──> AP_Baro::handle_external
└──> AP_Baro_ExternalAHRS::handle_external
6.3 AP_Baro_DroneCAN
CAN总线后续单独介绍，待定.
6.4 AP_Baro_SITL
SITL模拟后续单独介绍，待定.