From 929c5c2b37af8966aa6542a9af3dbe678c80a96a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 19 Oct 2017 18:21:47 +1100 Subject: [PATCH] EKF: enable gps fusion flag to be false while fusing air data --- EKF/common.h | 1 + EKF/control.cpp | 37 +++++++++++++++++++++++++------------ EKF/ekf_helper.cpp | 21 +++++++++++++++------ EKF/estimator_interface.h | 2 +- 4 files changed, 42 insertions(+), 19 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 70cb67cf49..a998987e1d 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -416,6 +416,7 @@ union filter_control_status_u { uint32_t update_mag_states_only : 1; ///< 16 - true when only the magnetometer states are updated by the magnetometer uint32_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle uint32_t mag_fault : 1; ///< 18 - true when the magnetomer has been declared faulty and is no longer being used + uint32_t fuse_aspd : 1; ///< 19 - true when airspedd measurements are being fused } flags; uint32_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 3321d08ad5..a962adacc1 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -494,14 +494,18 @@ void Ekf::controlGpsFusion() } else { // handle the case where we do not have GPS and have not been using it for an extended period, but are still relying on it bool lost_gps = _control_status.flags.gps && (_time_last_imu - _time_last_gps > 10e6); - bool no_velpos_aiding = !_control_status.flags.opt_flow && !_control_status.flags.ev_pos; - bool no_airdata_aiding = (_time_last_imu - _time_last_arsp_fuse > 10e6) || (_time_last_imu - _time_last_beta_fuse > 10e6); // If we don't have a source of aiding to constrain attitude drift, then we need to switch // to the non-aiding mode, zero the velocity states and set the position to the current estimate. // Air data aiding prevents loss of attitude reference, constrains velocity drift and can assist // a FW vehicle to recover to the launch area. - if (lost_gps && no_velpos_aiding && no_airdata_aiding) { + if (lost_gps) { _control_status.flags.gps = false; + ECL_WARN("EKF GPS data stopped"); + + } + bool no_velpos_aiding = !_control_status.flags.opt_flow && !_control_status.flags.ev_pos; + bool no_airdata_aiding = (_time_last_imu - _time_last_arsp_fuse > 10e6) || (_time_last_imu - _time_last_beta_fuse > 10e6); + if (!_control_status.flags.gps && no_velpos_aiding && no_airdata_aiding) { _last_known_posNE(0) = _state.pos(0); _last_known_posNE(1) = _state.pos(1); _state.vel.setZero(); @@ -1010,9 +1014,18 @@ void Ekf::controlAirDataFusion() } - // Always try to fuse airspeed data if available and we are in flight and the filter is operating in a normal aiding mode - bool is_aiding = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos; - if (_tas_data_ready && _control_status.flags.in_air && is_aiding) { + if (_control_status.flags.fuse_aspd && airspeed_timed_out) { + _control_status.flags.fuse_aspd = false; + + } + + // Always try to fuse airspeed data if available and we are in flight + if (_tas_data_ready && _control_status.flags.in_air) { + // always fuse airsped data if we are flying and data is present + if (!_control_status.flags.fuse_aspd) { + _control_status.flags.fuse_aspd = true; + } + // If starting wind state estimation, reset the wind states and covariances before fusing any data if (!_control_status.flags.wind) { // activate the wind states @@ -1047,10 +1060,7 @@ void Ekf::controlBetaFusion() // Suffient time has lapsed sice the last fusion bool beta_fusion_time_triggered = _time_last_imu - _time_last_beta_fuse > _params.beta_avg_ft_us; - // The filter is operating in a mode where velocity states can be used - bool vel_states_active = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos; - - if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air && vel_states_active) { + if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) { // If starting wind state estimation, reset the wind states and covariances before fusing any data if (!_control_status.flags.wind) { // activate the wind states @@ -1267,8 +1277,11 @@ void Ekf::controlVelPosFusion() { // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz - if (!_control_status.flags.gps && !_control_status.flags.opt_flow && !_control_status.flags.ev_pos - && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { + if (!_control_status.flags.gps && + !_control_status.flags.opt_flow && + !_control_status.flags.ev_pos && + !(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta) && + ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { _fuse_pos = true; _time_last_fake_gps = _time_last_imu; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 32a51a79df..466b48a8fb 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -902,7 +902,10 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko // TODO - allow for baro drift in vertical position error float hpos_err; float vpos_err; - bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos); + bool vel_pos_aiding = (_control_status.flags.gps || + _control_status.flags.opt_flow || + _control_status.flags.ev_pos || + (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)); if (vel_pos_aiding && _NED_origin_initialised) { hpos_err = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph)); @@ -933,7 +936,10 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko // TODO - allow for baro drift in vertical position error float hpos_err; float vpos_err; - bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos); + bool vel_pos_aiding = (_control_status.flags.gps || + _control_status.flags.opt_flow || + _control_status.flags.ev_pos || + (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)); if (vel_pos_aiding && _NED_origin_initialised) { hpos_err = sqrtf(P[7][7] + P[8][8]); @@ -963,7 +969,10 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon { float hvel_err; float vvel_err; - bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos); + bool vel_pos_aiding = (_control_status.flags.gps || + _control_status.flags.opt_flow || + _control_status.flags.ev_pos || + (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)); if (vel_pos_aiding && _NED_origin_initialised) { hvel_err = sqrtf(P[4][4] + P[5][5]); @@ -1058,9 +1067,9 @@ void Ekf::get_ekf_soln_status(uint16_t *status) { ekf_solution_status soln_status{}; soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0); - soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); + soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0); - soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); + soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow ) && (_fault_status.value == 0); soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert; soln_status.flags.pos_vert_agl = get_terrain_valid(); @@ -1146,7 +1155,7 @@ bool Ekf::inertial_dead_reckoning() bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos) && ((_time_last_imu - _time_last_pos_fuse <= 1E6) || (_time_last_imu - _time_last_vel_fuse <= 1E6)); bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= 1E6); - bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= 1E6); + bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= 1E6) && (_time_last_imu - _time_last_beta_fuse <= 1E6); return !velPosAiding && !optFlowAiding && !airDataAiding; } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 07c0fec6c5..91497618f0 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -199,7 +199,7 @@ public: void set_is_fixed_wing(bool is_fixed_wing) {_control_status.flags.fixed_wing = is_fixed_wing;} // set flag if synthetic sideslip measurement should be fused - void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = fuse_beta;} + void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air);} // set flag if only only mag states should be updated by the magnetometer void set_update_mag_states_only_flag(bool update_mag_states_only) {_control_status.flags.update_mag_states_only = update_mag_states_only;}