diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 6b20d6ce44..962155a02c 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -87,7 +87,7 @@ void Ekf::controlFusionModes() if (_baro_buffer) { // check for intermittent data - _baro_hgt_faulty = !isRecent(_time_last_baro, 2 * BARO_MAX_INTERVAL); + _baro_hgt_intermittent = !isRecent(_time_last_baro, 2 * BARO_MAX_INTERVAL); const uint64_t baro_time_prev = _baro_sample_delayed.time_us; _baro_data_ready = _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed); @@ -626,25 +626,16 @@ void Ekf::controlHeightSensorTimeouts() if (hgt_fusion_timeout || continuous_bad_accel_hgt) { - bool request_height_reset = false; const char *failing_height_source = nullptr; const char *new_height_source = nullptr; if (_control_status.flags.baro_hgt) { - // check if GPS height is available - bool gps_hgt_accurate = false; - - if (_gps_buffer) { - const gpsSample &gps_init = _gps_buffer->get_newest(); - gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); - } - - // check for inertial sensing errors in the last BADACC_PROBATION seconds - const bool prev_bad_vert_accel = isRecent(_time_bad_vert_accel, BADACC_PROBATION); + bool reset_to_gps = false; // reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data - const bool reset_to_gps = !_gps_intermittent && - ((gps_hgt_accurate && !prev_bad_vert_accel) || _baro_hgt_faulty); + if (!_gps_intermittent) { + reset_to_gps = (_gps_checks_passed && !_fault_status.flags.bad_acc_vertical) || _baro_hgt_faulty || _baro_hgt_intermittent; + } if (reset_to_gps) { // set height sensor health @@ -652,47 +643,33 @@ void Ekf::controlHeightSensorTimeouts() startGpsHgtFusion(); - request_height_reset = true; failing_height_source = "baro"; new_height_source = "gps"; - } else if (!_baro_hgt_faulty) { - request_height_reset = true; + } else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) { + resetHeightToBaro(); + failing_height_source = "baro"; new_height_source = "baro"; } } else if (_control_status.flags.gps_hgt) { - // check if GPS height is available - bool gps_hgt_accurate = false; + bool reset_to_baro = false; - if (_gps_buffer) { - const gpsSample &gps_init = _gps_buffer->get_newest(); - gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); + // if baro data is available and GPS data is inaccurate and the timeout cannot be blamed on IMU data, reset height to baro + if (!_baro_hgt_faulty && !_baro_hgt_intermittent) { + reset_to_baro = (!_fault_status.flags.bad_acc_vertical && !_gps_checks_passed) || _gps_intermittent; } - // check the baro height source for consistency and freshness - bool baro_data_consistent = false; - - if (_baro_buffer) { - const baroSample &baro_init = _baro_buffer->get_newest(); - const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); - baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9, 9)) * sq(_params.baro_innov_gate); - } - - // if baro data is acceptable and GPS data is inaccurate, reset height to baro - const bool reset_to_baro = !_baro_hgt_faulty && - ((baro_data_consistent && !gps_hgt_accurate) || _gps_intermittent); - if (reset_to_baro) { startBaroHgtFusion(); - request_height_reset = true; failing_height_source = "gps"; new_height_source = "baro"; } else if (!_gps_intermittent) { - request_height_reset = true; + resetHeightToGps(); + failing_height_source = "gps"; new_height_source = "gps"; } @@ -700,14 +677,14 @@ void Ekf::controlHeightSensorTimeouts() } else if (_control_status.flags.rng_hgt) { if (_range_sensor.isHealthy()) { - request_height_reset = true; + resetHeightToRng(); + failing_height_source = "rng"; new_height_source = "rng"; - } else if (!_baro_hgt_faulty) { + } else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) { startBaroHgtFusion(); - request_height_reset = true; failing_height_source = "rng"; new_height_source = "baro"; } @@ -722,21 +699,21 @@ void Ekf::controlHeightSensorTimeouts() } if (ev_data_available) { - request_height_reset = true; + resetHeightToEv(); + failing_height_source = "ev"; new_height_source = "ev"; } else if (_range_sensor.isHealthy()) { // Fallback to rangefinder data if available startRngHgtFusion(); - request_height_reset = true; + failing_height_source = "ev"; new_height_source = "rng"; - } else if (!_baro_hgt_faulty) { + } else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) { startBaroHgtFusion(); - request_height_reset = true; failing_height_source = "ev"; new_height_source = "baro"; } @@ -747,9 +724,12 @@ void Ekf::controlHeightSensorTimeouts() ECL_WARN("%s hgt timeout - reset to %s", failing_height_source, new_height_source); } - // Reset vertical position and velocity states to the last measurement - if (request_height_reset) { - resetHeight(); + // Also reset the vertical velocity + if (_control_status.flags.gps && !_gps_intermittent && _gps_checks_passed) { + resetVerticalVelocityToGps(); + + } else { + resetVerticalVelocityToZero(); } } } @@ -835,8 +815,14 @@ void Ekf::controlHeightFusion() } } else { - if (!_control_status.flags.baro_hgt && !_baro_hgt_faulty) { - startBaroHgtFusion(); + if (!_control_status.flags.baro_hgt) { + if (!_baro_hgt_faulty && !_baro_hgt_intermittent) { + startBaroHgtFusion(); + + } else if (!_control_status.flags.gps_hgt && !_gps_intermittent && _gps_checks_passed) { + // Use GPS as a fallback + startGpsHgtFusion(); + } } } @@ -880,7 +866,7 @@ void Ekf::controlHeightFusion() // In fallback mode and GPS has recovered so start using it startGpsHgtFusion(); - } else if (!_control_status.flags.baro_hgt && !_baro_hgt_faulty) { + } else if (!_control_status.flags.baro_hgt && !_baro_hgt_faulty && !_baro_hgt_intermittent) { // Use baro as a fallback startBaroHgtFusion(); } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 386db11826..718c62b14f 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -542,8 +542,9 @@ private: terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground // height sensor status - bool _baro_hgt_faulty{true}; ///< true if valid baro data is unavailable for use - bool _gps_intermittent{true}; ///< true if into the buffer is intermittent + bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags + bool _baro_hgt_intermittent{true}; ///< true if data into the buffer is intermittent + bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent // imu fault status uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec) @@ -656,7 +657,13 @@ private: void resetVerticalPositionTo(float new_vert_pos); - void resetHeight(); + void resetHeightToBaro(); + void resetHeightToGps(); + void resetHeightToRng(); + void resetHeightToEv(); + + void resetVerticalVelocityToGps(); + void resetVerticalVelocityToZero(); // fuse optical flow line of sight rate measurements void fuseOptFlow(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 01156f15af..b59c47a06c 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -157,9 +157,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel) void Ekf::resetHorizontalPosition() { - // let the next odometry update know that the previous value of states cannot be used to calculate the change in position - _hpos_prev_available = false; - if (_control_status.flags.gps) { // this reset is only called if we have new gps data at the fusion time horizon resetHorizontalPositionToGps(); @@ -212,6 +209,9 @@ void Ekf::resetHorizontalPositionToVision() resetHorizontalPositionTo(Vector2f(_ev_pos)); P.uncorrelateCovarianceSetVariance<2>(7, _ev_sample_delayed.posVar.slice<2, 1>(0, 0)); + + // let the next odometry update know that the previous value of states cannot be used to calculate the change in position + _hpos_prev_available = false; } void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos) @@ -250,104 +250,82 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos) // add the reset amount to the output observer vertical position state _output_vert_new.vert_vel_integ = _state.pos(2); + + // Reset the timout timer + _time_last_hgt_fuse = _time_last_imu; } -// Reset height state using the last height measurement -void Ekf::resetHeight() +void Ekf::resetHeightToBaro() { - // reset the vertical position - if (_control_status.flags.rng_hgt) { - float dist_bottom; - - if (_control_status.flags.in_air) { - dist_bottom = _range_sensor.getDistBottom(); - - } else { - // use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar) - dist_bottom = _params.rng_gnd_clearance; - } + resetVerticalPositionTo(-_baro_sample_delayed.hgt + _baro_hgt_offset); - // update the state and associated variance - resetVerticalPositionTo(-dist_bottom + _hgt_sensor_offset); + // the state variance is the same as the observation + P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise)); +} - // the state variance is the same as the observation - P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise)); +void Ekf::resetHeightToGps() +{ + const float z_pos_before_reset = _state.pos(2); + resetVerticalPositionTo(-_gps_sample_delayed.hgt + _gps_alt_ref); - // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup - if (_baro_buffer) { - const baroSample &baro_newest = _baro_buffer->get_newest(); - _baro_hgt_offset = baro_newest.hgt + _state.pos(2); - } + // the state variance is the same as the observation + P.uncorrelateCovarianceSetVariance<1>(9, getGpsHeightVariance()); - } else if (_control_status.flags.baro_hgt) { - // initialize vertical position with newest baro measurement - if (!_baro_hgt_faulty) { - if (_baro_buffer) { - const baroSample &baro_newest = _baro_buffer->get_newest(); - resetVerticalPositionTo(-baro_newest.hgt + _baro_hgt_offset); - } + // adjust the baro offset + _baro_hgt_offset += _state.pos(2) - z_pos_before_reset; +} - // the state variance is the same as the observation - P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise)); +void Ekf::resetHeightToRng() +{ + float dist_bottom; - } else { - // TODO: reset to last known baro based estimate - } + if (_control_status.flags.in_air) { + dist_bottom = _range_sensor.getDistBottom(); - } else if (_control_status.flags.gps_hgt) { - // initialize vertical position and velocity with newest gps measurement - if (!_gps_intermittent && _gps_buffer) { - const gpsSample &gps_newest = _gps_buffer->get_newest(); - resetVerticalPositionTo(_hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref); + } else { + // use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar) + dist_bottom = _params.rng_gnd_clearance; + } - // the state variance is the same as the observation - P.uncorrelateCovarianceSetVariance<1>(9, sq(gps_newest.vacc)); + // update the state and associated variance + const float z_pos_before_reset = _state.pos(2); + resetVerticalPositionTo(-dist_bottom + _hgt_sensor_offset); - // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup - if (_baro_buffer) { - const baroSample &baro_newest = _baro_buffer->get_newest(); - _baro_hgt_offset = baro_newest.hgt + _state.pos(2); - } + // the state variance is the same as the observation + P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise)); - } else { - // TODO: reset to last known gps based estimate - } - - } else if (_control_status.flags.ev_hgt) { - // initialize vertical position with newest measurement - if (_ext_vision_buffer) { - const extVisionSample &ev_newest = _ext_vision_buffer->get_newest(); + // adjust the baro offset + _baro_hgt_offset += _state.pos(2) - z_pos_before_reset; +} - // use the most recent data if it's time offset from the fusion time horizon is smaller - if (ev_newest.time_us >= _ev_sample_delayed.time_us) { - resetVerticalPositionTo(ev_newest.pos(2)); +void Ekf::resetHeightToEv() +{ + const float z_pos_before_reset = _state.pos(2); + resetVerticalPositionTo(_ev_sample_delayed.pos(2)); - } else { - resetVerticalPositionTo(_ev_sample_delayed.pos(2)); - } - } - } + // the state variance is the same as the observation + P.uncorrelateCovarianceSetVariance<1>(9, fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f))); - // reset the vertical velocity state - if (_control_status.flags.gps && !_gps_intermittent && _gps_buffer) { - // If we are using GPS, then use it to reset the vertical velocity - const gpsSample &gps_newest = _gps_buffer->get_newest(); - resetVerticalVelocityTo(gps_newest.vel(2)); + // adjust the baro offset + _baro_hgt_offset += _state.pos(2) - z_pos_before_reset; +} - // the state variance is the same as the observation - P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_newest.sacc)); +void Ekf::resetVerticalVelocityToGps() +{ + resetVerticalVelocityTo(_gps_sample_delayed.vel(2)); - } else { - // we don't know what the vertical velocity is, so set it to zero - resetVerticalVelocityTo(0.0f); + // the state variance is the same as the observation + P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * _gps_sample_delayed.sacc)); +} - // Set the variance to a value large enough to allow the state to converge quickly - // that does not destabilise the filter - P.uncorrelateCovarianceSetVariance<1>(6, 10.0f); - } +void Ekf::resetVerticalVelocityToZero() +{ + // we don't know what the vertical velocity is, so set it to zero + resetVerticalVelocityTo(0.0f); - // Reset the timout timer - _time_last_hgt_fuse = _time_last_imu; + // Set the variance to a value large enough to allow the state to converge quickly + // that does not destabilise the filter + P.uncorrelateCovarianceSetVariance<1>(6, 10.0f); } // align output filter states to match EKF states at the fusion time horizon @@ -1271,21 +1249,34 @@ void Ekf::startMag3DFusion() void Ekf::startBaroHgtFusion() { - setControlBaroHeight(); + if (!_control_status.flags.baro_hgt) { + if (!_control_status.flags.rng_hgt) { + resetHeightToBaro(); + } - // We don't need to set a height sensor offset - // since we track a separate _baro_hgt_offset - _hgt_sensor_offset = 0.0f; + setControlBaroHeight(); + + // We don't need to set a height sensor offset + // since we track a separate _baro_hgt_offset + _hgt_sensor_offset = 0.0f; + } } void Ekf::startGpsHgtFusion() { if (!_control_status.flags.gps_hgt) { - setControlGPSHeight(); + if (_control_status.flags.rng_hgt) { + // swith out of range aid + // calculate height sensor offset such that current + // measurement matches our current height estimate + _hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2); - // calculate height sensor offset such that current - // measurement matches our current height estimate - _hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2); + } else { + _hgt_sensor_offset = 0.f; + resetHeightToGps(); + } + + setControlGPSHeight(); } } @@ -1300,7 +1291,7 @@ void Ekf::startRngHgtFusion() if (!_control_status_prev.flags.ev_hgt) { // EV and range finders are using the same height datum - resetHeight(); + resetHeightToRng(); } } } @@ -1323,7 +1314,7 @@ void Ekf::startEvHgtFusion() if (!_control_status_prev.flags.rng_hgt) { // EV and range finders are using the same height datum - resetHeight(); + resetHeightToEv(); } } } diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index 9abb3bed1a..c451ee2c5b 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -101,11 +101,6 @@ bool Ekf::collect_gps(const gps_message &gps) _gps_origin_eph = gps.eph; _gps_origin_epv = gps.epv; - // if the user has selected GPS as the primary height source, switch across to using it - if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) { - startGpsHgtFusion(); - } - _information_events.flags.gps_checks_passed = true; ECL_INFO("GPS checks passed");