diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 181978792c..6f29d89f7a 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -174,8 +174,10 @@ endif() if(CONFIG_EKF2_GNSS) list(APPEND EKF_SRCS EKF/aid_sources/gnss/gnss_height_control.cpp - EKF/aid_sources/gnss/gps_checks.cpp - EKF/aid_sources/gnss/gps_control.cpp + EKF/aid_sources/gnss/gnss_checks.cpp + EKF/aid_sources/gnss/gnss_control.cpp + EKF/aid_sources/gnss/gnss_position.cpp + EKF/aid_sources/gnss/gnss_velocity.cpp ) if(CONFIG_EKF2_GNSS_YAW) diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 2c4c898d41..b626e11210 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -91,8 +91,10 @@ endif() if(CONFIG_EKF2_GNSS) list(APPEND EKF_SRCS aid_sources/gnss/gnss_height_control.cpp - aid_sources/gnss/gps_checks.cpp - aid_sources/gnss/gps_control.cpp + aid_sources/gnss/gnss_checks.cpp + aid_sources/gnss/gnss_control.cpp + aid_sources/gnss/gnss_position.cpp + aid_sources/gnss/gnss_velocity.cpp ) if(CONFIG_EKF2_GNSS_YAW) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp similarity index 95% rename from src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp rename to src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp index 81b3fcc76b..4df8a0bc9d 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp @@ -167,6 +167,18 @@ bool Ekf::runGnssChecks(const gnssSample &gps) _last_gps_fail_us = _time_delayed_us; } + if ( + _gps_check_fail_status.flags.fix || + (_gps_check_fail_status.flags.nsats && (_params.gps_check_mask & MASK_GPS_NSATS)) || + (_gps_check_fail_status.flags.pdop && (_params.gps_check_mask & MASK_GPS_PDOP)) || + (_gps_check_fail_status.flags.spoofed && (_params.gps_check_mask & MASK_GPS_SPOOFED)) + ) { + _gnss_common_checks_passed = false; + + } else { + _gnss_common_checks_passed = true; + } + // if any user selected checks have failed, record the fail time if ( _gps_check_fail_status.flags.fix || diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_control.cpp new file mode 100644 index 0000000000..a2002bf241 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_control.cpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2021-2025 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. + * + ****************************************************************************/ + +#include "ekf.h" +#include + +void Ekf::controlGnssFusion(const imuSample &imu_sample) +{ + if (!_gps_buffer || (_params.gnss_ctrl == 0)) { + stopGnssFusion(); + return; + } + + if (!gyro_bias_inhibited()) { + _yawEstimator.setGyroBias(getGyroBias(), _control_status.flags.vehicle_at_rest); + } + + // run EKF-GSF yaw estimator once per IMU update + _yawEstimator.predict(imu_sample.delta_ang, imu_sample.delta_ang_dt, + imu_sample.delta_vel, imu_sample.delta_vel_dt, + (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest)); + + + // check for arrival of new sensor data at the fusion time horizon + gnssSample gnss_sample; + + if (_gps_buffer->pop_first_older_than(imu_sample.time_us, &gnss_sample)) { + + if (runGnssChecks(gnss_sample) + && isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) { + if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) { + // First time checks are passing, latching. + if (!_gps_checks_passed) { + _information_events.flags.gps_checks_passed = true; + } + + _gps_checks_passed = true; + } + + } else { + // Skip this sample + _gps_data_ready = false; + + if ((_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) + && isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) { + stopGnssFusion(); + ECL_WARN("GNSS quality poor - stopping use"); + } + } + + controlGnssHeightFusion(gnss_sample); + +#if defined(CONFIG_EKF2_GNSS_YAW) + controlGnssYawFusion(gnss_sample); +#endif // CONFIG_EKF2_GNSS_YAW + + controlGnssYawEstimator(_aid_src_gnss_vel); + + bool do_vel_pos_reset = false; + + if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) { + // Reset checks need to run together, before each control function runs because the result would change + // after the first one runs + do_vel_pos_reset = shouldResetGpsFusion(); + + if (_control_status.flags.in_air + && isYawFailure() + && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) + && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) { + do_vel_pos_reset = tryYawEmergencyReset(); + } + } + + controlGnssVelocityFusion(imu_sample, gnss_sample, do_vel_pos_reset); + controlGnssPositionFusion(imu_sample, gnss_sample, do_vel_pos_reset); + + + } else if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) { + if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) { + stopGnssFusion(); + ECL_WARN("GNSS data stopped"); + } + } +} + +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 + */ + bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) + && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max); + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + + if (has_horizontal_aiding_timed_out) { + // horizontal aiding hasn't timed out if optical flow still active + if (_control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)) { + has_horizontal_aiding_timed_out = false; + } + } + +#endif // CONFIG_EKF2_OPTICAL_FLOW + + const bool is_reset_required = has_horizontal_aiding_timed_out + || (isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max) + && (_params.gnss_ctrl & static_cast(GnssCtrl::HPOS))); + + 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_inflight_nav_failure); +} + +void Ekf::stopGnssFusion() +{ + if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) { + _last_gps_fail_us = 0; + _last_gps_pass_us = 0; + } + + stopGnssVelFusion(); + stopGnssPosFusion(); + stopGpsHgtFusion(); +#if defined(CONFIG_EKF2_GNSS_YAW) + stopGnssYawFusion(); +#endif // CONFIG_EKF2_GNSS_YAW + + _yawEstimator.reset(); +} diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_position.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_position.cpp new file mode 100644 index 0000000000..283a95d61a --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_position.cpp @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * Copyright (c) 2021-2025 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. + * + ****************************************************************************/ + +#include "ekf.h" +#include + +void Ekf::controlGnssPositionFusion(const imuSample &imu_sample, const gnssSample &gnss_sample, bool reset) +{ + static constexpr const char *AID_SRC_NAME = "GNSS position"; + + auto &aid_src = _aid_src_gnss_pos; + + // correct position and height for offset relative to IMU + const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; + const Vector3f pos_offset_earth = Vector3f(_R_to_earth * pos_offset_body); + const LatLonAlt measurement(gnss_sample.lat, gnss_sample.lon, gnss_sample.alt); + const LatLonAlt measurement_corrected = measurement + (-pos_offset_earth); + const Vector2f innovation = (_gpos - measurement_corrected).xy(); + + // relax the upper observation noise limit which prevents bad GPS perturbing the position estimate + float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise); + + if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gnss_pos)) { + // if we are not using another source of aiding, then we are reliant on the GNSS + // observations to constrain attitude errors and must limit the observation noise value. + if (pos_noise > _params.pos_noaid_noise) { + pos_noise = _params.pos_noaid_noise; + } + } + + const float pos_var = math::max(sq(pos_noise), sq(0.01f)); + const Vector2f pos_obs_var(pos_var, pos_var); + const matrix::Vector2d observation(measurement_corrected.latitude_deg(), measurement_corrected.longitude_deg()); + + updateAidSourceStatus(aid_src, + gnss_sample.time_us, // sample timestamp + observation, // observation + pos_obs_var, // observation variance + innovation, // innovation + Vector2f(getStateVariance()) + pos_obs_var, // innovation variance + math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate + + const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast(GnssCtrl::HPOS)); + + const bool continuing_conditions_passing = gnss_pos_enabled + && _control_status.flags.tilt_align + && _control_status.flags.yaw_align + && (gnss_sample.hacc < _params.req_hacc); + + const bool starting_conditions_passing = continuing_conditions_passing + && _gnss_common_checks_passed; + + const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed; + + if (_control_status.flags.gnss_pos) { + if (continuing_conditions_passing) { + if (reset) { + ECL_WARN("%s fusion timeout, resetting", AID_SRC_NAME); + + _information_events.flags.reset_pos_to_gps = true; + resetLatLonTo(aid_src.observation[0], aid_src.observation[1], + aid_src.observation_variance[0] + aid_src.observation_variance[1]); + + resetAidSourceStatusZeroInnovation(aid_src); + + } else { + fuseHorizontalPosition(aid_src); + } + + } else { + stopGnssPosFusion(); + } + + } else { + if (starting_conditions_passing) { + ECL_INFO("%s starting fusion", AID_SRC_NAME); + _information_events.flags.starting_gps_fusion = true; + + _information_events.flags.reset_pos_to_gps = true; + resetLatLonTo(aid_src.observation[0], aid_src.observation[1], + aid_src.observation_variance[0] + + aid_src.observation_variance[1]); + + resetAidSourceStatusZeroInnovation(aid_src); + + _control_status.flags.gnss_pos = true; + + } else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) { + + _information_events.flags.reset_pos_to_gps = true; + resetLatLonTo(aid_src.observation[0], aid_src.observation[1], + aid_src.observation_variance[0] + + aid_src.observation_variance[1]); + + resetAidSourceStatusZeroInnovation(aid_src); + } + } +} + +void Ekf::stopGnssPosFusion() +{ + if (_control_status.flags.gnss_pos) { + ECL_INFO("stopping GNSS position fusion"); + _control_status.flags.gnss_pos = false; + + //TODO: what if gnss yaw or height is used? + if (!_control_status.flags.gnss_vel) { + _last_gps_fail_us = 0; + _last_gps_pass_us = 0; + } + } +} diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_velocity.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_velocity.cpp new file mode 100644 index 0000000000..1134f3b0b9 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_velocity.cpp @@ -0,0 +1,283 @@ +/**************************************************************************** + * + * Copyright (c) 2021-2025 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. + * + ****************************************************************************/ + +#include "ekf.h" +#include + +void Ekf::controlGnssVelocityFusion(const imuSample &imu_sample, const gnssSample &gnss_sample, bool reset) +{ + static constexpr const char *AID_SRC_NAME = "GNSS velocity"; + + auto &aid_src = _aid_src_gnss_vel; + + // correct velocity for offset relative to IMU + const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; + + const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias; + const Vector3f vel_offset_body = angular_velocity % pos_offset_body; + const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; + const Vector3f velocity = gnss_sample.vel - vel_offset_earth; + + const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise, 0.01f)); + const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f)); + + const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f); + + updateAidSourceStatus(aid_src, + gnss_sample.time_us, // sample timestamp + velocity, // observation + vel_obs_var, // observation variance + _state.vel - velocity, // innovation + getVelocityVariance() + vel_obs_var, // innovation variance + innovation_gate); // innovation gate + + // vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable, + // but limit innovation to prevent spikes that could destabilise the filter + bool bad_acc_vz_rejected = _fault_status.flags.bad_acc_vertical + && (aid_src.test_ratio[2] > 1.f) // vz rejected + && (aid_src.test_ratio[0] < 1.f) && (aid_src.test_ratio[1] < 1.f); // vx & vy accepted + + if (bad_acc_vz_rejected + && (gnss_sample.sacc < _params.req_sacc) + ) { + const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]); + aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit); + aid_src.innovation_rejected = false; + } + + if (_control_status.flags.gnss_vel) { + + if (_control_status.flags.in_air + && isYawFailure() + && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) + && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) { + + if (tryYawEmergencyReset()) { + reset = true; + } + } + } + + const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast(GnssCtrl::VEL)) + && _control_status.flags.tilt_align + && _control_status.flags.yaw_align + && velocity.isAllFinite() + && (gnss_sample.sacc < _params.req_sacc) + && !_gps_check_fail_status.flags.hspeed; + + const bool starting_conditions_passing = continuing_conditions_passing + && _gnss_common_checks_passed + && velocity.longerThan(0.f); + + if (_control_status.flags.gnss_vel) { + if (continuing_conditions_passing) { + if (reset) { + ECL_WARN("%s fusion timeout, resetting", AID_SRC_NAME); + + _information_events.flags.reset_vel_to_gps = true; + resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance)); + + resetAidSourceStatusZeroInnovation(aid_src); + + } else { + fuseVelocity(aid_src); + + if (isHeightResetRequired()) { + // reset vertical velocity if height is failing + resetVerticalVelocityTo(aid_src.observation[2], aid_src.observation_variance[2]); + } + } + + } else { + stopGnssVelFusion(); + } + + } else { + if (starting_conditions_passing) { + ECL_INFO("%s starting fusion", AID_SRC_NAME); + _information_events.flags.starting_gps_fusion = true; + + // when already using another velocity source velocity reset is not necessary + if (!isHorizontalAidingActive() + || isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) + || !_control_status_prev.flags.yaw_align + ) { + // reset velocity + _information_events.flags.reset_vel_to_gps = true; + resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance)); + + resetAidSourceStatusZeroInnovation(aid_src); + + } + + _control_status.flags.gnss_vel = true; + } + } +} + +void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel) +{ + // update yaw estimator velocity (basic sanity check on GNSS velocity data) + const float vel_var = aid_src_vel.observation_variance[0]; + const Vector2f vel_xy(aid_src_vel.observation); + + if ((vel_var > 0.f) + && (vel_var < _params.req_sacc) + && vel_xy.isAllFinite() + ) { + _yawEstimator.fuseVelocity(vel_xy, vel_var, _control_status.flags.in_air); + + // Try to align yaw using estimate if available + if ((_params.gnss_ctrl & static_cast(GnssCtrl::VEL)) + && !_control_status.flags.yaw_align + && _control_status.flags.tilt_align + ) { + + if (resetYawToEKFGSF()) { + ECL_INFO("GNSS yaw aligned using IMU"); + } + } + } +} + +bool Ekf::tryYawEmergencyReset() +{ + bool success = false; + + /* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously + * fails while the difference between the yaw emergency estimator and the yaw estimate is large. + * This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was + * present before flight to prevent triggering due to GPS glitches or other sensor errors. + */ + if (resetYawToEKFGSF()) { + ECL_WARN("GNSS emergency yaw reset"); + + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { + // stop using the magnetometer in the main EKF otherwise its fusion could drag the yaw around + // and cause another navigation failure + _control_status.flags.mag_fault = true; + } + +#if defined(CONFIG_EKF2_GNSS_YAW) + + if (_control_status.flags.gnss_yaw) { + _control_status.flags.gnss_yaw_fault = true; + } + +#endif // CONFIG_EKF2_GNSS_YAW + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + + if (_control_status.flags.ev_yaw) { + _control_status.flags.ev_yaw_fault = true; + } + +#endif // CONFIG_EKF2_EXTERNAL_VISION + + success = true; + } + + return success; +} + +void Ekf::stopGnssVelFusion() +{ + if (_control_status.flags.gnss_vel) { + ECL_INFO("stopping GNSS velocity fusion"); + _control_status.flags.gnss_vel = false; + + //TODO: what if gnss yaw or height is used? + if (!_control_status.flags.gnss_pos) { + _last_gps_fail_us = 0; + _last_gps_pass_us = 0; + } + } +} + +bool Ekf::isYawEmergencyEstimateAvailable() const +{ + // don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity + // data and the yaw estimate has converged + if (!_yawEstimator.isActive()) { + return false; + } + + const float yaw_var = _yawEstimator.getYawVar(); + + return (yaw_var > 0.f) + && (yaw_var < sq(_params.EKFGSF_yaw_err_max)) + && PX4_ISFINITE(yaw_var); +} + +bool Ekf::isYawFailure() const +{ + if (!isYawEmergencyEstimateAvailable()) { + return false; + } + + const float euler_yaw = getEulerYaw(_R_to_earth); + const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw()); + + return fabsf(yaw_error) > math::radians(25.f); +} + +bool Ekf::resetYawToEKFGSF() +{ + if (!isYawEmergencyEstimateAvailable()) { + return false; + } + + // don't allow reset if there's just been a yaw reset + const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); + const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat); + + if (yaw_alignment_changed || quat_reset) { + return false; + } + + ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad", + (double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw()); + + resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar()); + + _control_status.flags.yaw_align = true; + _information_events.flags.yaw_aligned_to_imu_gps = true; + + return true; +} + +bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], + float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) +{ + return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight); +} diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp index bd37c0fb37..e2a0ffa233 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp @@ -67,8 +67,7 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed - && !is_gnss_yaw_data_intermittent - && !_gps_intermittent; + && !is_gnss_yaw_data_intermittent; if (_control_status.flags.gnss_yaw) { if (continuing_conditions_passing) { diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp deleted file mode 100644 index 9eabfe156d..0000000000 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ /dev/null @@ -1,495 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021-2022 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(const imuSample &imu_delayed) -{ - if (!_gps_buffer || (_params.gnss_ctrl == 0)) { - stopGnssFusion(); - return; - } - - if (!gyro_bias_inhibited()) { - _yawEstimator.setGyroBias(getGyroBias(), _control_status.flags.vehicle_at_rest); - } - - // run EKF-GSF yaw estimator once per imu_delayed update - _yawEstimator.predict(imu_delayed.delta_ang, imu_delayed.delta_ang_dt, - imu_delayed.delta_vel, imu_delayed.delta_vel_dt, - (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest)); - - _gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL); - - // check for arrival of new sensor data at the fusion time horizon - _gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed); - - if (_gps_data_ready) { - const gnssSample &gnss_sample = _gps_sample_delayed; - - if (runGnssChecks(gnss_sample) - && isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) { - if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) { - // First time checks are passing, latching. - if (!_gps_checks_passed) { - _information_events.flags.gps_checks_passed = true; - } - - _gps_checks_passed = true; - } - - } else { - // Skip this sample - _gps_data_ready = false; - - if ((_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) - && isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) { - stopGnssFusion(); - ECL_WARN("GNSS quality poor - stopping use"); - } - } - - updateGnssPos(gnss_sample, _aid_src_gnss_pos); - updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel); - - } else if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) { - if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) { - stopGnssFusion(); - ECL_WARN("GNSS data stopped"); - } - } - - if (_gps_data_ready) { -#if defined(CONFIG_EKF2_GNSS_YAW) - const gnssSample &gnss_sample = _gps_sample_delayed; - controlGnssYawFusion(gnss_sample); -#endif // CONFIG_EKF2_GNSS_YAW - - controlGnssYawEstimator(_aid_src_gnss_vel); - - bool do_vel_pos_reset = false; - - if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) { - // Reset checks need to run together, before each control function runs because the result would change - // after the first one runs - do_vel_pos_reset = shouldResetGpsFusion(); - - if (_control_status.flags.in_air - && isYawFailure() - && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) - && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) { - do_vel_pos_reset = tryYawEmergencyReset(); - } - } - - controlGnssVelFusion(_aid_src_gnss_vel, do_vel_pos_reset); - controlGnssPosFusion(_aid_src_gnss_pos, do_vel_pos_reset); - } -} - -void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool force_reset) -{ - const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast(GnssCtrl::VEL)) - && _control_status.flags.tilt_align - && _control_status.flags.yaw_align; - const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed; - - if (_control_status.flags.gnss_vel) { - if (continuing_conditions_passing) { - if (force_reset) { - ECL_WARN("GNSS fusion timeout, resetting"); - resetVelocityToGnss(aid_src); - - } else { - fuseVelocity(aid_src); - - if (isHeightResetRequired()) { - // reset vertical velocity if height is failing - resetVerticalVelocityTo(aid_src.observation[2], aid_src.observation_variance[2]); - } - } - - } else { - stopGnssVelFusion(); - } - - } else { - if (starting_conditions_passing) { - ECL_INFO("starting GNSS velocity fusion"); - _information_events.flags.starting_gps_fusion = true; - - // when already using another velocity source velocity reset is not necessary - if (!isHorizontalAidingActive() - || isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) - || !_control_status_prev.flags.yaw_align - ) { - // reset velocity - resetVelocityToGnss(aid_src); - } - - _control_status.flags.gnss_vel = true; - } - } -} - -void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool force_reset) -{ - const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast(GnssCtrl::HPOS)); - - const bool continuing_conditions_passing = gnss_pos_enabled - && _control_status.flags.tilt_align - && _control_status.flags.yaw_align; - const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed; - const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed; - - if (_control_status.flags.gnss_pos) { - if (continuing_conditions_passing) { - if (force_reset) { - ECL_WARN("GNSS fusion timeout, resetting"); - resetHorizontalPositionToGnss(aid_src); - - } else { - fuseHorizontalPosition(aid_src); - } - - } else { - stopGnssPosFusion(); - } - - } else { - if (starting_conditions_passing) { - ECL_INFO("starting GNSS position fusion"); - _information_events.flags.starting_gps_fusion = true; - - resetHorizontalPositionToGnss(aid_src); - _control_status.flags.gnss_pos = true; - - } else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) { - resetHorizontalPositionToGnss(aid_src); - } - } -} - -void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src) -{ - // correct velocity for offset relative to IMU - const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; - - const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias; - const Vector3f vel_offset_body = angular_velocity % pos_offset_body; - const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; - const Vector3f velocity = gnss_sample.vel - vel_offset_earth; - - const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise, 0.01f)); - const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f)); - - const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f); - - updateAidSourceStatus(aid_src, - gnss_sample.time_us, // sample timestamp - velocity, // observation - vel_obs_var, // observation variance - _state.vel - velocity, // innovation - getVelocityVariance() + vel_obs_var, // innovation variance - innovation_gate); // innovation gate - - // vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable, - // but limit innovation to prevent spikes that could destabilise the filter - bool bad_acc_vz_rejected = _fault_status.flags.bad_acc_vertical - && (aid_src.test_ratio[2] > 1.f) // vz rejected - && (aid_src.test_ratio[0] < 1.f) && (aid_src.test_ratio[1] < 1.f); // vx & vy accepted - - if (bad_acc_vz_rejected - && (gnss_sample.sacc < _params.req_sacc) - ) { - const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]); - aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit); - aid_src.innovation_rejected = false; - } -} - -void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src) -{ - // correct position and height for offset relative to IMU - const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; - const Vector3f pos_offset_earth = Vector3f(_R_to_earth * pos_offset_body); - const LatLonAlt measurement(gnss_sample.lat, gnss_sample.lon, gnss_sample.alt); - const LatLonAlt measurement_corrected = measurement + (-pos_offset_earth); - const Vector2f innovation = (_gpos - measurement_corrected).xy(); - - // relax the upper observation noise limit which prevents bad GPS perturbing the position estimate - float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise); - - if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gnss_pos)) { - // if we are not using another source of aiding, then we are reliant on the GNSS - // observations to constrain attitude errors and must limit the observation noise value. - if (pos_noise > _params.pos_noaid_noise) { - pos_noise = _params.pos_noaid_noise; - } - } - - const float pos_var = math::max(sq(pos_noise), sq(0.01f)); - const Vector2f pos_obs_var(pos_var, pos_var); - const matrix::Vector2d observation(measurement_corrected.latitude_deg(), measurement_corrected.longitude_deg()); - - updateAidSourceStatus(aid_src, - gnss_sample.time_us, // sample timestamp - observation, // observation - pos_obs_var, // observation variance - innovation, // innovation - Vector2f(getStateVariance()) + pos_obs_var, // innovation variance - math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate -} - -void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel) -{ - // update yaw estimator velocity (basic sanity check on GNSS velocity data) - const float vel_var = aid_src_vel.observation_variance[0]; - const Vector2f vel_xy(aid_src_vel.observation); - - if ((vel_var > 0.f) - && (vel_var < _params.req_sacc) - && vel_xy.isAllFinite()) { - - _yawEstimator.fuseVelocity(vel_xy, vel_var, _control_status.flags.in_air); - - // Try to align yaw using estimate if available - if (((_params.gnss_ctrl & static_cast(GnssCtrl::VEL)) - || (_params.gnss_ctrl & static_cast(GnssCtrl::HPOS))) - && !_control_status.flags.yaw_align - && _control_status.flags.tilt_align) { - if (resetYawToEKFGSF()) { - ECL_INFO("GPS yaw aligned using IMU"); - } - } - } -} - -bool Ekf::tryYawEmergencyReset() -{ - bool success = false; - - /* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously - * fails while the difference between the yaw emergency estimator and the yaw estimate is large. - * This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was - * present before flight to prevent triggering due to GPS glitches or other sensor errors. - */ - if (resetYawToEKFGSF()) { - ECL_WARN("GPS emergency yaw reset"); - - if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { - // stop using the magnetometer in the main EKF otherwise its fusion could drag the yaw around - // and cause another navigation failure - _control_status.flags.mag_fault = true; - } - -#if defined(CONFIG_EKF2_GNSS_YAW) - - if (_control_status.flags.gnss_yaw) { - _control_status.flags.gnss_yaw_fault = true; - } - -#endif // CONFIG_EKF2_GNSS_YAW - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - - if (_control_status.flags.ev_yaw) { - _control_status.flags.ev_yaw_fault = true; - } - -#endif // CONFIG_EKF2_EXTERNAL_VISION - - success = true; - } - - return success; -} - -void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src) -{ - _information_events.flags.reset_vel_to_gps = true; - resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance)); - - resetAidSourceStatusZeroInnovation(aid_src); -} - -void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src) -{ - _information_events.flags.reset_pos_to_gps = true; - resetLatLonTo(aid_src.observation[0], aid_src.observation[1], - aid_src.observation_variance[0] + - aid_src.observation_variance[1]); - - resetAidSourceStatusZeroInnovation(aid_src); -} - -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 - */ - bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max); - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - - if (has_horizontal_aiding_timed_out) { - // horizontal aiding hasn't timed out if optical flow still active - if (_control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)) { - has_horizontal_aiding_timed_out = false; - } - } - -#endif // CONFIG_EKF2_OPTICAL_FLOW - - const bool is_reset_required = has_horizontal_aiding_timed_out - || (isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max) - && (_params.gnss_ctrl & static_cast(GnssCtrl::HPOS))); - - 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_inflight_nav_failure); -} - -void Ekf::stopGnssFusion() -{ - if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) { - _last_gps_fail_us = 0; - _last_gps_pass_us = 0; - } - - stopGnssVelFusion(); - stopGnssPosFusion(); - stopGpsHgtFusion(); -#if defined(CONFIG_EKF2_GNSS_YAW) - stopGnssYawFusion(); -#endif // CONFIG_EKF2_GNSS_YAW - - _yawEstimator.reset(); -} - -void Ekf::stopGnssVelFusion() -{ - if (_control_status.flags.gnss_vel) { - ECL_INFO("stopping GNSS velocity fusion"); - _control_status.flags.gnss_vel = false; - - //TODO: what if gnss yaw or height is used? - if (!_control_status.flags.gnss_pos) { - _last_gps_fail_us = 0; - _last_gps_pass_us = 0; - } - } -} - -void Ekf::stopGnssPosFusion() -{ - if (_control_status.flags.gnss_pos) { - ECL_INFO("stopping GNSS position fusion"); - _control_status.flags.gnss_pos = false; - - //TODO: what if gnss yaw or height is used? - if (!_control_status.flags.gnss_vel) { - _last_gps_fail_us = 0; - _last_gps_pass_us = 0; - } - } -} - -bool Ekf::isYawEmergencyEstimateAvailable() const -{ - // don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity - // data and the yaw estimate has converged - if (!_yawEstimator.isActive()) { - return false; - } - - const float yaw_var = _yawEstimator.getYawVar(); - - return (yaw_var > 0.f) - && (yaw_var < sq(_params.EKFGSF_yaw_err_max)) - && PX4_ISFINITE(yaw_var); -} - -bool Ekf::isYawFailure() const -{ - if (!isYawEmergencyEstimateAvailable()) { - return false; - } - - const float euler_yaw = getEulerYaw(_R_to_earth); - const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw()); - - return fabsf(yaw_error) > math::radians(25.f); -} - -bool Ekf::resetYawToEKFGSF() -{ - if (!isYawEmergencyEstimateAvailable()) { - return false; - } - - // don't allow reset if there's just been a yaw reset - const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); - const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat); - - if (yaw_alignment_changed || quat_reset) { - return false; - } - - ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad", - (double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw()); - - resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar()); - - _control_status.flags.yaw_align = true; - _information_events.flags.yaw_aligned_to_imu_gps = true; - - return true; -} - -bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], - float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) -{ - return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight); -} diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 8cb9e4be67..13417e098e 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -112,7 +112,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_GNSS) - controlGpsFusion(imu_delayed); + controlGnssFusion(imu_delayed); #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 981b31e094..46be23fe8a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -562,11 +562,11 @@ private: uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time + bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed + bool _gnss_common_checks_passed{false}; gps_check_fail_status_u _gps_check_fail_status{}; - // height sensor status - bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref}; @@ -861,19 +861,8 @@ private: #if defined(CONFIG_EKF2_GNSS) // control fusion of GPS observations - void controlGpsFusion(const imuSample &imu_delayed); - void controlGnssVelFusion(estimator_aid_source3d_s &aid_src, bool force_reset); - void controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool force_reset); + void controlGnssFusion(const imuSample &imu_sample); void stopGnssFusion(); - void stopGnssVelFusion(); - void stopGnssPosFusion(); - void updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src); - void updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src); - void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel); - bool tryYawEmergencyReset(); - void resetVelocityToGnss(estimator_aid_source3d_s &aid_src); - void resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src); - bool shouldResetGpsFusion() const; /* * Return true if the GPS solution quality is adequate. @@ -882,6 +871,18 @@ private: */ bool runGnssChecks(const gnssSample &gps); + + void controlGnssVelocityFusion(const imuSample &imu_sample, const gnssSample &gnss_sample, const bool force_reset); + void stopGnssVelFusion(); + void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel); + bool tryYawEmergencyReset(); + + void controlGnssPositionFusion(const imuSample &imu_sample, const gnssSample &gnss_sample, const bool force_reset); + void stopGnssPosFusion(); + + bool shouldResetGpsFusion() const; + + void controlGnssHeightFusion(const gnssSample &gps_sample); void stopGpsHgtFusion(); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 5ff02ca7ab..a8e85985f9 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -87,8 +87,6 @@ public: #if defined(CONFIG_EKF2_GNSS) void setGpsData(const gnssSample &gnss_sample); - const gnssSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } - float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; } float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; } float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; } @@ -395,8 +393,6 @@ protected: RingBuffer *_gps_buffer {nullptr}; uint64_t _time_last_gps_buffer_push{0}; - gnssSample _gps_sample_delayed{}; - float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s) float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s) float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s) diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 44467fa2b8..ad996286bb 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -47,10 +47,6 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed) controlBaroHeightFusion(imu_delayed); #endif // CONFIG_EKF2_BAROMETER -#if defined(CONFIG_EKF2_GNSS) - controlGnssHeightFusion(_gps_sample_delayed); -#endif // CONFIG_EKF2_GNSS - #if defined(CONFIG_EKF2_RANGE_FINDER) controlRangeHaglFusion(imu_delayed); #endif // CONFIG_EKF2_RANGE_FINDER diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 099848802e..728590db5e 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1053,11 +1053,11 @@ void EKF2::PublishBaroBias(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_GNSS) void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp) { - if (_ekf.get_gps_sample_delayed().time_us != 0) { + if ( _ekf.aid_src_gnss_hgt().timestamp_sample != 0) { const BiasEstimator::status &status = _ekf.getGpsHgtBiasEstimatorStatus(); if (fabsf(status.bias - _last_gnss_hgt_bias_published) > 0.001f) { - _estimator_gnss_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_gps_sample_delayed().time_us, timestamp)); + _estimator_gnss_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.aid_src_gnss_hgt().timestamp_sample, timestamp)); _last_gnss_hgt_bias_published = status.bias; } @@ -1221,7 +1221,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_GNSS) void EKF2::PublishGpsStatus(const hrt_abstime ×tamp) { - const hrt_abstime timestamp_sample = _ekf.get_gps_sample_delayed().time_us; + const hrt_abstime timestamp_sample = _ekf.aid_src_gnss_pos().timestamp_sample; if (timestamp_sample == _last_gps_status_published) { return;