diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 31b2946f19..71afa252b7 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -54,6 +54,7 @@ px4_add_module( EKF/EKFGSF_yaw.cpp EKF/estimator_interface.cpp EKF/gps_checks.cpp + EKF/gps_control.cpp EKF/gps_fusion.cpp EKF/gps_yaw_fusion.cpp EKF/imu_down_sampler.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 8b11a0dcb1..63b8a43212 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -42,6 +42,7 @@ add_library(ecl_EKF EKFGSF_yaw.cpp estimator_interface.cpp gps_checks.cpp + gps_control.cpp gps_fusion.cpp gps_yaw_fusion.cpp imu_down_sampler.cpp diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index d50c547527..1bb9e9df3d 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -519,196 +519,6 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks() _time_good_motion_us = _imu_sample_delayed.time_us; } -void Ekf::controlGpsFusion() -{ - if (!(_params.fusion_mode & MASK_USE_GPS)) { - stopGpsFusion(); - return; - } - - // Check for new GPS data that has fallen behind the fusion time horizon - if (_gps_data_ready) { - - const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6); - const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6); - - controlGpsYawFusion(gps_checks_passing, gps_checks_failing); - - // Determine if we should use GPS aiding for velocity and horizontal position - // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently - const bool mandatory_conditions_passing = _control_status.flags.tilt_align - && _control_status.flags.yaw_align - && _NED_origin_initialised; - const bool continuing_conditions_passing = mandatory_conditions_passing - && !gps_checks_failing; - const bool starting_conditions_passing = continuing_conditions_passing - && gps_checks_passing; - - if (_control_status.flags.gps) { - if (mandatory_conditions_passing) { - if (continuing_conditions_passing - || !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { - fuseGpsVelPos(); - - // We are relying on aiding to constrain drift so after a specified time - // with no aiding we need to do something - const bool do_vel_pos_reset = hasHorizontalAidingTimedOut() - || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max); - - /* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of - navigation casued by a bad yaw estimate. - - A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity - innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff, - different test criteria are used that take longer to trigger and reduce false positives. A reset is - not performed if the fault condition was present before flight to prevent triggering due to GPS glitches - or other sensor errors. - - The yaw reset to the EKF-GSF estimate can be requested externally at any time during flight. - - The total number of resets allowed per boot cycle is limited. - - The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time - to improve its estimate if the previous reset was not successful. - - A reset is not performed when getting GPS back after a significant period of no data because the timeout - could have been caused by bad GPS. - */ - const bool is_recent_takeoff_nav_failure = isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) - && (_time_last_hor_vel_fuse > _time_last_on_ground_us) - && isRecentTakeoff(); - - - const bool is_inflight_nav_failure = _control_status.flags.in_air && - do_vel_pos_reset && - (_time_last_hor_vel_fuse > _time_last_on_ground_us) && - (_time_last_hor_pos_fuse > _time_last_on_ground_us); - - if (is_recent_takeoff_nav_failure || is_inflight_nav_failure){ - const bool is_yaw_failure = !isVelStateAlignedWithObs(); - const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000); - - if (is_yaw_failure - && !was_gps_signal_lost - && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit) { - - _do_ekfgsf_yaw_reset = true; - _ekfgsf_yaw_reset_count++; - - } else { - - // use GPS velocity data to check and correct yaw angle if a FW vehicle - if (_control_status.flags.fixed_wing && _control_status.flags.in_air) { - // if flying a fixed wing aircraft, do a complete reset that includes yaw - _control_status.flags.mag_aligned_in_flight = realignYawGPS(); - } - - _warning_events.flags.gps_fusion_timout = true; - ECL_WARN("GPS fusion timeout - resetting"); - _velpos_reset_request = true; - } - } - - } else { - stopGpsFusion(); - _warning_events.flags.gps_quality_poor = true; - ECL_WARN("GPS quality poor - stopping use"); - } - - } else { // mandatory conditions are not passing - stopGpsFusion(); - } - - } else { - if (starting_conditions_passing) { - // If the heading is not aligned, reset the yaw and magnetic field states - // Do not use external vision for yaw if using GPS because yaw needs to be - // defined relative to an NED reference frame - if (_control_status.flags.ev_yaw - || _mag_inhibit_yaw_reset_req - || _mag_yaw_reset_req) { - - _mag_yaw_reset_req = true; - - // Stop the vision for yaw fusion and do not allow it to start again - stopEvYawFusion(); - _inhibit_ev_yaw_use = true; - - } else { - startGpsFusion(); - } - } - } - - const bool align_yaw_using_gsf = !_control_status.flags.yaw_align - && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE); - _do_ekfgsf_yaw_reset = _do_ekfgsf_yaw_reset || align_yaw_using_gsf; - - // Process yaw estimator reset request - if (_do_ekfgsf_yaw_reset - && isTimedOut(_ekfgsf_yaw_reset_time, 5000000)){ - if (resetYawToEKFGSF()) { - _ekfgsf_yaw_reset_time = _time_last_imu; - _time_last_hor_pos_fuse = _time_last_imu; - _time_last_hor_vel_fuse = _time_last_imu; - - _do_ekfgsf_yaw_reset = false; - _velpos_reset_request = false; // included in yaw reset - } - } - - if (_velpos_reset_request) { - resetVelocity(); - resetHorizontalPosition(); - _velpos_reset_request = false; - - // Reset the timeout counters - _time_last_hor_pos_fuse = _time_last_imu; - _time_last_hor_vel_fuse = _time_last_imu; - } - - } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { - stopGpsFusion(); - _warning_events.flags.gps_data_stopped = true; - ECL_WARN("GPS data stopped"); - - } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) - && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { - // Handle the case where we are fusing another position source along GPS, - // stop waiting for GPS after 1 s of lost signal - stopGpsFusion(); - _warning_events.flags.gps_data_stopped_using_alternate = true; - ECL_WARN("GPS data stopped, using only EV, OF or air data"); - } - //TODO: move that resetHorizontalPosition to EV - /* // Handle the case where we are using GPS and another source of aiding and GPS is failing checks */ - /* if (_control_status.flags.gps && gps_checks_failing && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { */ - /* stopGpsFusion(); */ - - /* // Reset position state to external vision if we are going to use absolute values */ - /* if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) { */ - /* resetHorizontalPosition(); */ - /* } */ - - /* _warning_events.flags.gps_quality_poor = true; */ - /* ECL_WARN("GPS quality poor - stopping use"); */ - /* } */ -} - -bool Ekf::hasHorizontalAidingTimedOut() const -{ - return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max); -} - -bool Ekf::isRecentTakeoff() const -{ - return _control_status.flags.in_air - && !isTimedOut(_time_last_on_ground_us, 30000000); -} - void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) { if (!(_params.fusion_mode & MASK_USE_GPSYAW) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 9c479164d7..aa6c7ce0a6 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -662,7 +662,6 @@ private: Vector3f &innov_var, Vector2f &test_ratio); void fuseGpsVelPos(); - bool isVelStateAlignedWithObs() const; // calculate optical flow body angular rate compensation // returns false if bias corrected body rate data is unavailable @@ -805,10 +804,13 @@ private: // control fusion of GPS observations void controlGpsFusion(); - void controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing); - + bool shouldResetGpsFusion() const; bool hasHorizontalAidingTimedOut() const; - bool isRecentTakeoff() const; + bool isVelStateAlignedWithObs() const; + void processYawEstimatorResetRequest(); + void processVelPosResetRequest(); + + void controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing); // control fusion of magnetometer observations void controlMagFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 8081389df4..35c1d3326d 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1515,9 +1515,14 @@ void Ekf::startGpsFusion() void Ekf::stopGpsFusion() { - stopGpsPosFusion(); - stopGpsVelFusion(); - stopGpsYawFusion(); + if (_control_status.flags.gps) { + stopGpsPosFusion(); + stopGpsVelFusion(); + } + + if (_control_status.flags.gps_yaw) { + stopGpsYawFusion(); + } // We do not need to know the true North anymore // EV yaw can start again diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp new file mode 100644 index 0000000000..d846037444 --- /dev/null +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -0,0 +1,255 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gps_control.cpp + * Control functions for ekf GNSS fusion + */ + +#include "ekf.h" +#include + +void Ekf::controlGpsFusion() +{ + if (!(_params.fusion_mode & MASK_USE_GPS)) { + stopGpsFusion(); + return; + } + + // Check for new GPS data that has fallen behind the fusion time horizon + if (_gps_data_ready) { + const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6); + const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6); + + controlGpsYawFusion(gps_checks_passing, gps_checks_failing); + + // Determine if we should use GPS aiding for velocity and horizontal position + // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently + const bool mandatory_conditions_passing = _control_status.flags.tilt_align + && _control_status.flags.yaw_align + && _NED_origin_initialised; + const bool continuing_conditions_passing = mandatory_conditions_passing + && !gps_checks_failing; + const bool starting_conditions_passing = continuing_conditions_passing + && gps_checks_passing; + + if (_control_status.flags.gps) { + if (mandatory_conditions_passing) { + if (continuing_conditions_passing + || !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { + + fuseGpsVelPos(); + + if (shouldResetGpsFusion()){ + const bool is_yaw_failure = !isVelStateAlignedWithObs(); + const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000); + + /* A reset is not performed when getting GPS back after a significant period of no data + * because the timeout could have been caused by bad GPS. + * The total number of resets allowed per boot cycle is limited. + */ + if (is_yaw_failure + && _control_status.flags.in_air + && !was_gps_signal_lost + && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit) { + + _do_ekfgsf_yaw_reset = true; + + } else { + // use GPS velocity data to check and correct yaw angle if a FW vehicle + if (_control_status.flags.fixed_wing && _control_status.flags.in_air) { + // if flying a fixed wing aircraft, do a complete reset that includes yaw + _control_status.flags.mag_aligned_in_flight = realignYawGPS(); + } + + _warning_events.flags.gps_fusion_timout = true; + ECL_WARN("GPS fusion timeout - resetting"); + _velpos_reset_request = true; + } + } + + } else { + stopGpsFusion(); + _warning_events.flags.gps_quality_poor = true; + ECL_WARN("GPS quality poor - stopping use"); + + // TODO: move this to EV control logic + // Reset position state to external vision if we are going to use absolute values + if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) { + resetHorizontalPosition(); + } + } + + } else { // mandatory conditions are not passing + stopGpsFusion(); + } + + } else { + if (starting_conditions_passing) { + // Do not use external vision for yaw if using GPS because yaw needs to be + // defined relative to an NED reference frame + if (_control_status.flags.ev_yaw + || _mag_inhibit_yaw_reset_req + || _mag_yaw_reset_req) { + + _mag_yaw_reset_req = true; + + // Stop the vision for yaw fusion and do not allow it to start again + stopEvYawFusion(); + _inhibit_ev_yaw_use = true; + + } else { + startGpsFusion(); + } + + } else if(!_control_status.flags.yaw_align + && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE)) { + // If no mag is used, align using the yaw estimator + _do_ekfgsf_yaw_reset = true; + } + } + + processYawEstimatorResetRequest(); + processVelPosResetRequest(); + + } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { + stopGpsFusion(); + _warning_events.flags.gps_data_stopped = true; + ECL_WARN("GPS data stopped"); + + } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) + && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { + // Handle the case where we are fusing another position source along GPS, + // stop waiting for GPS after 1 s of lost signal + stopGpsFusion(); + _warning_events.flags.gps_data_stopped_using_alternate = true; + ECL_WARN("GPS data stopped, using only EV, OF or air data"); + } +} + +bool Ekf::shouldResetGpsFusion() const +{ + /* We are relying on aiding to constrain drift so after a specified time + * with no aiding we need to do something + */ + const bool is_reset_required = hasHorizontalAidingTimedOut() + || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max); + + /* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of + * navigation casued by a bad yaw estimate. + + * A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity + * innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff, + * different test criteria are used that take longer to trigger and reduce false positives. A reset is + * not performed if the fault condition was present before flight to prevent triggering due to GPS glitches + * or other sensor errors. + */ + const bool is_recent_takeoff_nav_failure = _control_status.flags.in_air + && isRecent(_time_last_on_ground_us, 30000000) + && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) + && (_time_last_hor_vel_fuse > _time_last_on_ground_us); + + const bool is_inflight_nav_failure = _control_status.flags.in_air + && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) + && isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) + && (_time_last_hor_vel_fuse > _time_last_on_ground_us) + && (_time_last_hor_pos_fuse > _time_last_on_ground_us); + + return (is_reset_required || is_recent_takeoff_nav_failure || is_inflight_nav_failure); +} + +bool Ekf::hasHorizontalAidingTimedOut() const +{ + return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) + && isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max) + && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) + && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max); +} + +bool Ekf::isVelStateAlignedWithObs() const +{ + /* Do sanity check to see if the innovation failures is likely caused by a yaw angle error + * by measuring the angle between the velocity estimate and the last velocity observation + * Only use those vectors if their norm if they are larger than 4 times their noise standard deviation + */ + const float vel_obs_xy_norm_sq = _last_vel_obs.xy().norm_squared(); + const float vel_state_xy_norm_sq = _state.vel.xy().norm_squared(); + + const float vel_obs_threshold_sq = fmaxf(sq(4.f) * (_last_vel_obs_var(0) + _last_vel_obs_var(1)), sq(0.4f)); + const float vel_state_threshold_sq = fmaxf(sq(4.f) * (P(4, 4) + P(5, 5)), sq(0.4f)); + + if (vel_obs_xy_norm_sq > vel_obs_threshold_sq && vel_state_xy_norm_sq > vel_state_threshold_sq) { + const float obs_dot_vel = Vector2f(_last_vel_obs).dot(_state.vel.xy()); + const float cos_sq = sq(obs_dot_vel) / (vel_state_xy_norm_sq * vel_obs_xy_norm_sq); + + if (cos_sq < sq(cosf(math::radians(25.f))) || obs_dot_vel < 0.f) { + // The angle between the observation and the velocity estimate is greater than 25 degrees + return false; + } + } + + return true; +} + +void Ekf::processYawEstimatorResetRequest() +{ + /* The yaw reset to the EKF-GSF estimate can be requested externally at any time during flight. + * The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time + * to improve its estimate if the previous reset was not successful. + */ + if (_do_ekfgsf_yaw_reset + && isTimedOut(_ekfgsf_yaw_reset_time, 5000000)){ + if (resetYawToEKFGSF()) { + _ekfgsf_yaw_reset_time = _time_last_imu; + _time_last_hor_pos_fuse = _time_last_imu; + _time_last_hor_vel_fuse = _time_last_imu; + + _do_ekfgsf_yaw_reset = false; + _velpos_reset_request = false; // included in yaw reset + _ekfgsf_yaw_reset_count++; + } + } +} + +void Ekf::processVelPosResetRequest() +{ + if (_velpos_reset_request) { + resetVelocity(); + resetHorizontalPosition(); + _velpos_reset_request = false; + + // Reset the timeout counters + _time_last_hor_pos_fuse = _time_last_imu; + _time_last_hor_vel_fuse = _time_last_imu; + } +}