dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: Correct airspeed used by EKF for calibration errors

This commit is contained in:
Paul Riseborough 2021-01-06 11:22:53 +11:00 committed by GitHub
parent b94e346488
commit 64ed96bd95
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 30 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
src/modules/ekf2/EKF2.cpp
View File

@ -160,11 +160,6 @@ EKF2::EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool
_param_ekf2_mag_check(_params->check_mag_strength),
_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
{
// initialise parameter cache
updateParams();
_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
if (_multi_mode) {
// advertise immediately to ensure consistent uORB instance numbering
_attitude_pub.advertise();
@ -234,6 +229,25 @@ void EKF2::Run()
return;
}
// check for parameter updates
if (_parameter_update_sub.updated() || !_callback_registered) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
updateParams();
_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
// The airspeed scale factor correcton is only available via parameter as used by the airspeed module
param_t param_aspd_scale = param_find("ASPD_SCALE");
if (param_aspd_scale != PARAM_INVALID) {
param_get(param_aspd_scale, &_airspeed_scale_factor);
}
}
if (!_callback_registered) {
if (_multi_mode) {
_callback_registered = _vehicle_imu_sub.registerCallback();
@ -249,16 +263,6 @@ void EKF2::Run()
}
}
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
updateParams();
}
bool imu_updated = false;
imuSample imu_sample_new {};
@ -1151,12 +1155,19 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
airspeed_s airspeed;
if (_airspeed_sub.update(&airspeed)) {
// The airspeed measurement received via the airspeed.msg topic has not been corrected
// for scale favtor errors and requires the ASPD_SCALE correction to be applied.
// This could be avoided if true_airspeed_m_s from the airspeed-validated.msg topic
// was used instead, however this would introduce a potential circular dependency
// via the wind estimator that uses EKF velocity estimates.
const float true_airspeed_m_s = airspeed.true_airspeed_m_s * _airspeed_scale_factor;
// only set airspeed data if condition for airspeed fusion are met
if ((_param_ekf2_arsp_thr.get() > FLT_EPSILON) && (airspeed.true_airspeed_m_s > _param_ekf2_arsp_thr.get())) {
if ((_param_ekf2_arsp_thr.get() > FLT_EPSILON) && (true_airspeed_m_s > _param_ekf2_arsp_thr.get())) {
airspeedSample airspeed_sample {
.time_us = airspeed.timestamp,
.true_airspeed = airspeed.true_airspeed_m_s,
.true_airspeed = true_airspeed_m_s,
.eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s,
};
_ekf.setAirspeedData(airspeed_sample);

2
src/modules/ekf2/EKF2.hpp
View File

@ -207,6 +207,8 @@ private:
Vector3f _last_gyro_bias{};
Vector3f _last_mag_bias{};
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
uORB::Subscription _distance_sensor_sub{ORB_ID(distance_sensor)};