From 61763544b6ee12071ff203d6ab39f1439b5cbc8c Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 16 Apr 2020 08:01:13 +0200 Subject: [PATCH] Ekf: centralize baro fusion startup --- EKF/control.cpp | 44 ++++++-------------------------------------- EKF/ekf.h | 2 ++ EKF/ekf_helper.cpp | 19 ++++++++++++++++++- 3 files changed, 26 insertions(+), 39 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 8e1517ffde..7f36005807 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -848,7 +848,7 @@ void Ekf::controlHeightSensorTimeouts() _gps_hgt_intermittent); if (reset_to_baro) { - setControlBaroHeight(); + startBaroHgtFusion(); request_height_reset = true; ECL_WARN_TIMESTAMPED("gps hgt timeout - reset to baro"); @@ -865,7 +865,7 @@ void Ekf::controlHeightSensorTimeouts() ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to rng hgt"); } else if (!_baro_hgt_faulty) { - setControlBaroHeight(); + startBaroHgtFusion(); request_height_reset = true; ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to baro"); @@ -881,7 +881,7 @@ void Ekf::controlHeightSensorTimeouts() ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to ev hgt"); } else if (!_baro_hgt_faulty) { - setControlBaroHeight(); + startBaroHgtFusion(); request_height_reset = true; ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to baro"); @@ -961,23 +961,9 @@ void Ekf::controlHeightFusion() } } else if (!do_range_aid && _baro_data_ready && !_baro_hgt_faulty) { - setControlBaroHeight(); + startBaroHgtFusion(); fuse_height = true; - // we have just switched to using baro height, we don't need to set a height sensor offset - // since we track a separate _baro_hgt_offset - if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) { - _hgt_sensor_offset = 0.0f; - } - - // Turn off ground effect compensation if it times out - if (_control_status.flags.gnd_effect) { - if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { - - _control_status.flags.gnd_effect = false; - } - } - } else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) { // switch to gps if there was a reset to gps fuse_height = true; @@ -1012,14 +998,8 @@ void Ekf::controlHeightFusion() } } else if (_baro_data_ready && !_baro_hgt_faulty) { - setControlBaroHeight(); + startBaroHgtFusion(); fuse_height = true; - - // we have just switched to using baro height, we don't need to set a height sensor offset - // since we track a separate _baro_hgt_offset - if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) { - _hgt_sensor_offset = 0.0f; - } } break; @@ -1050,12 +1030,6 @@ void Ekf::controlHeightFusion() } else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { // switch to baro if there was a reset to baro fuse_height = true; - - // we have just switched to using baro height, we don't need to set a height sensor offset - // since we track a separate _baro_hgt_offset - if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) { - _hgt_sensor_offset = 0.0f; - } } break; @@ -1072,12 +1046,6 @@ void Ekf::controlHeightFusion() if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { // switch to baro if there was a reset to baro fuse_height = true; - - // we have just switched to using baro height, we don't need to set a height sensor offset - // since we track a separate _baro_hgt_offset - if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) { - _hgt_sensor_offset = 0.0f; - } } // determine if we should use the vertical position observation @@ -1119,7 +1087,7 @@ void Ekf::controlHeightFusion() Vector3f baro_hgt_obs_var; // vertical position innovation - baro measurement has opposite sign to earth z axis - _baro_hgt_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset; + _baro_hgt_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset; // observation variance - user parameter defined baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f)); // innovation gate size diff --git a/EKF/ekf.h b/EKF/ekf.h index cc25170f6c..1a26250278 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -758,6 +758,8 @@ private: void startMagHdgFusion(); void startMag3DFusion(); + void startBaroHgtFusion(); + // calculate the measurement variance for the optical flow sensor float calcOptFlowMeasVar(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 7e5d5ca8e8..85dc517fc9 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -252,7 +252,7 @@ void Ekf::resetHeight() const baroSample &baro_newest = _baro_buffer.get_newest(); if (!_baro_hgt_faulty) { - _state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset; + _state.pos(2) = -baro_newest.hgt + _baro_hgt_offset; // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise)); @@ -1470,6 +1470,23 @@ void Ekf::startMag3DFusion() } } +void Ekf::startBaroHgtFusion() +{ + setControlBaroHeight(); + + // We don't need to set a height sensor offset + // since we track a separate _baro_hgt_offset + _hgt_sensor_offset = 0.0f; + + // Turn off ground effect compensation if it times out + if (_control_status.flags.gnd_effect) { + if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { + + _control_status.flags.gnd_effect = false; + } + } +} + // update the rotation matrix which rotates EV measurements into the EKF's navigation frame void Ekf::calcExtVisRotMat() {