diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index c355429d9f..6d46c5f435 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -102,41 +102,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) } } - if (_gps_buffer) { - _gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL); - - // check for arrival of new sensor data at the fusion time horizon - _time_prev_gps_us = _gps_sample_delayed.time_us; - _gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed); - - if (_gps_data_ready) { - // correct velocity for offset relative to IMU - const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; - const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; - const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; - _gps_sample_delayed.vel -= vel_offset_earth; - - // correct position and height for offset relative to IMU - const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - _gps_sample_delayed.pos -= pos_offset_earth.xy(); - _gps_sample_delayed.hgt += pos_offset_earth(2); - - // update GSF yaw estimator velocity (basic sanity check on GNSS velocity data) - if ((_gps_sample_delayed.sacc > 0.f) && (_gps_sample_delayed.sacc < _params.req_sacc) - && _gps_sample_delayed.vel.isAllFinite() - ) { - _yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), math::max(_gps_sample_delayed.sacc, _params.gps_vel_noise)); - } - } - } - - // run EKF-GSF yaw estimator once per imu_delayed update after all main EKF data samples available - runYawEKFGSF(imu_delayed); - // control use of observations for aiding controlMagFusion(); controlOpticalFlowFusion(imu_delayed); - controlGpsFusion(); + controlGpsFusion(imu_delayed); controlAirDataFusion(imu_delayed); controlBetaFusion(imu_delayed); controlDragFusion(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 3d90314af5..42cea5ac9c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -491,8 +491,6 @@ private: bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator - uint64_t _time_prev_gps_us{0}; ///< time stamp of previous GPS data retrieved from the buffer (uSec) - uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) uint64_t _time_last_v_pos_aiding{0}; uint64_t _time_last_v_vel_aiding{0}; @@ -856,7 +854,7 @@ private: void resetOnGroundMotionForOpticalFlowChecks(); // control fusion of GPS observations - void controlGpsFusion(); + void controlGpsFusion(const imuSample &imu_delayed); bool shouldResetGpsFusion() const; bool isYawFailure() const; @@ -1038,8 +1036,6 @@ private: HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref}; PositionBiasEstimator _ev_pos_b_est{static_cast(PositionSensor::EV), _position_sensor_ref}; - void runYawEKFGSF(const imuSample &imu_delayed); - // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator // Resets the horizontal velocity and position to the default navigation sensor // Returns true if the reset was successful diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 58b2121a95..964859381d 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1346,11 +1346,6 @@ bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_M return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight); } -void Ekf::runYawEKFGSF(const imuSample &imu_delayed) -{ - _yawEstimator.update(imu_delayed, _control_status.flags.in_air, getGyroBias()); -} - void Ekf::resetGpsDriftCheckFilters() { _gps_velNE_filt.setZero(); diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index cfbf82b571..8a2485ad12 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -39,13 +39,41 @@ #include "ekf.h" #include -void Ekf::controlGpsFusion() +void Ekf::controlGpsFusion(const imuSample &imu_delayed) { - if (!((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))) { + if (!_gps_buffer || !((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))) { stopGpsFusion(); return; } + _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) { + // correct velocity for offset relative to IMU + const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; + const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; + const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; + _gps_sample_delayed.vel -= vel_offset_earth; + + // correct position and height for offset relative to IMU + const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + _gps_sample_delayed.pos -= pos_offset_earth.xy(); + _gps_sample_delayed.hgt += pos_offset_earth(2); + + // update GSF yaw estimator velocity (basic sanity check on GNSS velocity data) + if ((_gps_sample_delayed.sacc > 0.f) && (_gps_sample_delayed.sacc < _params.req_sacc) + && _gps_sample_delayed.vel.isAllFinite() + ) { + _yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), math::max(_gps_sample_delayed.sacc, _params.gps_vel_noise)); + } + } + + // run EKF-GSF yaw estimator once per imu_delayed update after all main EKF data samples available + _yawEstimator.update(imu_delayed, _control_status.flags.in_air, getGyroBias()); + // Check for new GPS data that has fallen behind the fusion time horizon if (_gps_data_ready) { @@ -236,8 +264,8 @@ bool Ekf::shouldResetGpsFusion() const * with no aiding we need to do something */ const 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) - && isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max); + && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) + && isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max); const bool is_reset_required = has_horizontal_aiding_timed_out || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max); @@ -280,7 +308,8 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi const bool continuing_conditions_passing = !gps_checks_failing; - const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL); + const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, + 2 * GNSS_YAW_MAX_INTERVAL); const bool starting_conditions_passing = continuing_conditions_passing && _control_status.flags.tilt_align @@ -336,7 +365,9 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi } } - } else if (_control_status.flags.gps_yaw && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) { + } else if (_control_status.flags.gps_yaw + && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) { + // No yaw data in the message anymore. Stop until it comes back. stopGpsYawFusion(); }