From d996af4647ce93d9015df7e62fd95512ed0f8421 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 29 Jul 2022 20:12:53 -0400 Subject: [PATCH] ekf2: pass gpsSample around where required - this minimizes potential misuse accessing _gps_sample_delayed and makes the dependency clear --- src/modules/ekf2/EKF/control.cpp | 10 ++--- src/modules/ekf2/EKF/covariance.cpp | 2 +- src/modules/ekf2/EKF/ekf.h | 36 +++++++++--------- src/modules/ekf2/EKF/ekf_helper.cpp | 40 ++++++++------------ src/modules/ekf2/EKF/gnss_height_control.cpp | 21 +++++----- src/modules/ekf2/EKF/gps_control.cpp | 23 +++++++---- src/modules/ekf2/EKF/gps_fusion.cpp | 4 +- src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 10 ++--- src/modules/ekf2/EKF/height_control.cpp | 2 +- 9 files changed, 75 insertions(+), 73 deletions(-) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 72ee7bb568..a4a37d5fdc 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -566,7 +566,7 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks() _time_good_motion_us = _imu_sample_delayed.time_us; } -void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) +void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing) { if (!(_params.gnss_ctrl & GnssCtrl::YAW) || _control_status.flags.gps_yaw_fault) { @@ -575,7 +575,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) return; } - const bool is_new_data_available = PX4_ISFINITE(_gps_sample_delayed.yaw); + const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw); if (is_new_data_available) { @@ -593,14 +593,14 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) if (continuing_conditions_passing) { - fuseGpsYaw(); + fuseGpsYaw(gps_sample); const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max); if (is_fusion_failing) { if (_nb_gps_yaw_reset_available > 0) { // Data seems good, attempt a reset - resetYawToGps(); + resetYawToGps(gps_sample.yaw); if (_control_status.flags.in_air) { _nb_gps_yaw_reset_available--; @@ -629,7 +629,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) } else { if (starting_conditions_passing) { // Try to activate GPS yaw fusion - startGpsYawFusion(); + startGpsYawFusion(gps_sample); if (_control_status.flags.gps_yaw) { _nb_gps_yaw_reset_available = 1; diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 21a7fc2953..af9a058315 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -73,7 +73,7 @@ void Ekf::initialiseCovariance() P(9,9) = sq(fmaxf(_params.range_noise, 0.01f)); } else if (_control_status.flags.gps_hgt) { - P(9,9) = getGpsHeightVariance(); + P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f)); } else { P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f)); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 2b613233fa..84c335cc19 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -653,14 +653,14 @@ private: // update quaternion states and covariances using an innovation, observation variance and Jacobian vector // innovation : prediction - measurement // variance : observaton variance - bool fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s& aid_src_status); + bool fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s &aid_src_status); // fuse the yaw angle obtained from a dual antenna GPS unit - void fuseGpsYaw(); + void fuseGpsYaw(const gpsSample &gps_sample); // reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit // return true if the reset was successful - bool resetYawToGps(); + bool resetYawToGps(const float gnss_yaw); // fuse magnetometer declination measurement // argument passed in is the declination uncertainty in radians @@ -692,12 +692,12 @@ private: void resetHorizontalVelocityTo(const Vector2f &new_horz_vel); void resetVerticalVelocityTo(float new_vert_vel); - void resetVelocityToGps(const gpsSample &gps_sample_delayed); + void resetVelocityToGps(const gpsSample &gps_sample); void resetHorizontalVelocityToOpticalFlow(); void resetVelocityToVision(); void resetHorizontalVelocityToZero(); - void resetHorizontalPositionToGps(const gpsSample &gps_sample_delayed); + void resetHorizontalPositionToGps(const gpsSample &gps_sample); void resetHorizontalPositionToVision(); void resetHorizontalPositionToOpticalFlow(); void resetHorizontalPositionToLastKnown(); @@ -708,21 +708,23 @@ private: void resetVerticalPositionTo(float new_vert_pos); void resetHeightToBaro(); - void resetHeightToGps(); + void resetHeightToGps(const gpsSample &gps_sample); void resetHeightToRng(); void resetHeightToEv(); - void resetVerticalVelocityToGps(const gpsSample &gps_sample_delayed); + void resetVerticalVelocityToGps(const gpsSample &gps_sample); void resetVerticalVelocityToZero(); // fuse optical flow line of sight rate measurements void fuseOptFlow(); - void updateVelocityAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& velocity, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& vel_aid_src) const; - void updatePositionAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& position, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& pos_aid_src) const; + void updateVelocityAidSrcStatus(const uint64_t &sample_time_us, const Vector3f &velocity, const Vector3f &obs_var, + const float innov_gate, estimator_aid_source_3d_s &vel_aid_src) const; + void updatePositionAidSrcStatus(const uint64_t &sample_time_us, const Vector3f &position, const Vector3f &obs_var, + const float innov_gate, estimator_aid_source_3d_s &pos_aid_src) const; - void fuseVelocity(estimator_aid_source_3d_s& vel_aid_src); - void fusePosition(estimator_aid_source_3d_s& pos_aid_src); + void fuseVelocity(estimator_aid_source_3d_s &vel_aid_src); + void fusePosition(estimator_aid_source_3d_s &pos_aid_src); bool fuseHorizontalVelocity(const Vector3f &innov, float innov_gate, const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio); @@ -893,7 +895,7 @@ private: bool hasHorizontalAidingTimedOut() const; bool isYawFailure() const; - void controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing); + void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing); // control fusion of magnetometer observations void controlMagFusion(); @@ -945,7 +947,7 @@ private: void controlHeightFusion(); void checkHeightSensorRefFallback(); void controlBaroHeightFusion(); - void controlGnssHeightFusion(); + void controlGnssHeightFusion(const gpsSample &gps_sample); void controlRangeHeightFusion(); void controlEvHeightFusion(); @@ -960,7 +962,7 @@ private: void startBaroHgtFusion(); void stopBaroHgtFusion(); - void startGpsHgtFusion(); + void startGpsHgtFusion(const gpsSample &gps_sample); void stopGpsHgtFusion(); void startRngHgtFusion(); @@ -972,7 +974,7 @@ private: void updateGroundEffect(); // return an estimation of the sensor altitude variance - float getGpsHeightVariance(); + float getGpsHeightVariance(const gpsSample &gps_sample); float getRngHeightVariance() const; // calculate the measurement variance for the optical flow sensor @@ -1040,12 +1042,12 @@ private: void startAirspeedFusion(); void stopAirspeedFusion(); - void startGpsFusion(); + void startGpsFusion(const gpsSample &gps_sample); void stopGpsFusion(); void stopGpsPosFusion(); void stopGpsVelFusion(); - void startGpsYawFusion(); + void startGpsYawFusion(const gpsSample &gps_sample); void stopGpsYawFusion(); void startEvPosFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 986fb23436..5a8ab7a7ac 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -44,12 +44,12 @@ #include #include -void Ekf::resetVelocityToGps(const gpsSample &gps_sample_delayed) +void Ekf::resetVelocityToGps(const gpsSample &gps_sample) { _information_events.flags.reset_vel_to_gps = true; ECL_INFO("reset velocity to GPS"); - resetVelocityTo(gps_sample_delayed.vel); - P.uncorrelateCovarianceSetVariance<3>(4, sq(gps_sample_delayed.sacc)); + resetVelocityTo(gps_sample.vel); + P.uncorrelateCovarianceSetVariance<3>(4, sq(gps_sample.sacc)); } void Ekf::resetHorizontalVelocityToOpticalFlow() @@ -144,12 +144,12 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel) _time_last_ver_vel_fuse = _imu_sample_delayed.time_us; } -void Ekf::resetHorizontalPositionToGps(const gpsSample &gps_sample_delayed) +void Ekf::resetHorizontalPositionToGps(const gpsSample &gps_sample) { _information_events.flags.reset_pos_to_gps = true; ECL_INFO("reset position to GPS"); - resetHorizontalPositionTo(gps_sample_delayed.pos); - P.uncorrelateCovarianceSetVariance<2>(7, sq(gps_sample_delayed.hacc)); + resetHorizontalPositionTo(gps_sample.pos); + P.uncorrelateCovarianceSetVariance<2>(7, sq(gps_sample.hacc)); } void Ekf::resetHorizontalPositionToVision() @@ -251,12 +251,12 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos) _time_last_hgt_fuse = _imu_sample_delayed.time_us; } -void Ekf::resetVerticalVelocityToGps(const gpsSample &gps_sample_delayed) +void Ekf::resetVerticalVelocityToGps(const gpsSample &gps_sample) { - resetVerticalVelocityTo(gps_sample_delayed.vel(2)); + resetVerticalVelocityTo(gps_sample.vel(2)); // the state variance is the same as the observation - P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_sample_delayed.sacc)); + P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_sample.sacc)); } void Ekf::resetVerticalVelocityToZero() @@ -1230,13 +1230,13 @@ void Ekf::startMag3DFusion() } } -float Ekf::getGpsHeightVariance() +float Ekf::getGpsHeightVariance(const gpsSample &gps_sample) { // observation variance - receiver defined and parameter limited // use 1.5 as a typical ratio of vacc/hacc const float lower_limit = fmaxf(1.5f * _params.gps_pos_noise, 0.01f); const float upper_limit = fmaxf(1.5f * _params.pos_noaid_noise, lower_limit); - const float gps_alt_var = sq(math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit)); + const float gps_alt_var = sq(math::constrain(gps_sample.vacc, lower_limit, upper_limit)); return gps_alt_var; } @@ -1415,14 +1415,14 @@ void Ekf::stopAirspeedFusion() _control_status.flags.fuse_aspd = false; } -void Ekf::startGpsFusion() +void Ekf::startGpsFusion(const gpsSample &gps_sample) { if (!_control_status.flags.gps) { - resetHorizontalPositionToGps(_gps_sample_delayed); + resetHorizontalPositionToGps(gps_sample); // when already using another velocity source velocity reset is not necessary if (!_control_status.flags.opt_flow && !_control_status.flags.ev_vel) { - resetVelocityToGps(_gps_sample_delayed); + resetVelocityToGps(gps_sample); } _information_events.flags.starting_gps_fusion = true; @@ -1467,9 +1467,9 @@ void Ekf::stopGpsVelFusion() resetEstimatorAidStatus(_aid_src_gnss_vel); } -void Ekf::startGpsYawFusion() +void Ekf::startGpsYawFusion(const gpsSample &gps_sample) { - if (!_control_status.flags.gps_yaw && resetYawToGps()) { + if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) { ECL_INFO("starting GPS yaw fusion"); _control_status.flags.yaw_align = true; _control_status.flags.mag_dec = false; @@ -1674,14 +1674,6 @@ void Ekf::runYawEKFGSF() const Vector3f imu_gyro_bias = getGyroBias(); _yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias); - - // basic sanity check on GPS velocity data - if (_gps_data_ready - && (_gps_sample_delayed.sacc > FLT_EPSILON) && (_gps_sample_delayed.sacc < _params.req_sacc) - && PX4_ISFINITE(_gps_sample_delayed.vel(0)) && PX4_ISFINITE(_gps_sample_delayed.vel(1)) - ) { - _yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.sacc); - } } void Ekf::resetGpsDriftCheckFilters() diff --git a/src/modules/ekf2/EKF/gnss_height_control.cpp b/src/modules/ekf2/EKF/gnss_height_control.cpp index 7b24e0bf27..231c492a8a 100644 --- a/src/modules/ekf2/EKF/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/gnss_height_control.cpp @@ -38,7 +38,7 @@ #include "ekf.h" -void Ekf::controlGnssHeightFusion() +void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) { if (!(_params.gnss_ctrl & GnssCtrl::VPOS)) { stopGpsHgtFusion(); @@ -59,8 +59,8 @@ void Ekf::controlGnssHeightFusion() if (isHeightResetRequired()) { // All height sources are failing - resetHeightToGps(); - resetVerticalVelocityToGps(_gps_sample_delayed); + resetHeightToGps(gps_sample); + resetVerticalVelocityToGps(gps_sample); } else if (is_fusion_failing) { // Some other height source is still working @@ -70,9 +70,10 @@ void Ekf::controlGnssHeightFusion() } else { stopGpsHgtFusion(); } + } else { if (starting_conditions_passing) { - startGpsHgtFusion(); + startGpsHgtFusion(gps_sample); } } @@ -81,17 +82,17 @@ void Ekf::controlGnssHeightFusion() } } -void Ekf::startGpsHgtFusion() +void Ekf::startGpsHgtFusion(const gpsSample &gps_sample) { if (!_control_status.flags.gps_hgt) { if (_params.height_sensor_ref == HeightSensor::GNSS) { _gps_hgt_b_est.reset(); _height_sensor_ref = HeightSensor::GNSS; - resetHeightToGps(); + resetHeightToGps(gps_sample); } else { - _gps_hgt_b_est.setBias(_state.pos(2) + (_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude())); + _gps_hgt_b_est.setBias(_state.pos(2) + (gps_sample.hgt - getEkfGlobalOriginAltitude())); // Reset the timeout value here because the fusion isn't done at the same place and would immediately trigger a timeout _aid_src_gnss_pos.time_last_fuse[2] = _imu_sample_delayed.time_us; @@ -103,15 +104,15 @@ void Ekf::startGpsHgtFusion() } } -void Ekf::resetHeightToGps() +void Ekf::resetHeightToGps(const gpsSample &gps_sample) { ECL_INFO("reset height to GPS"); _information_events.flags.reset_hgt_to_gps = true; - resetVerticalPositionTo(-(_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude() - _gps_hgt_b_est.getBias())); + resetVerticalPositionTo(-(gps_sample.hgt - getEkfGlobalOriginAltitude() - _gps_hgt_b_est.getBias())); // the state variance is the same as the observation - P.uncorrelateCovarianceSetVariance<1>(9, getGpsHeightVariance()); + P.uncorrelateCovarianceSetVariance<1>(9, getGpsHeightVariance(gps_sample)); _baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change); _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change); diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index d5e2ac1567..fd1798ec6d 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * 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 @@ -49,14 +49,21 @@ void Ekf::controlGpsFusion() // Check for new GPS data that has fallen behind the fusion time horizon if (_gps_data_ready) { - updateGpsYaw(_gps_sample_delayed); - updateGpsVel(_gps_sample_delayed); - updateGpsPos(_gps_sample_delayed); + const gpsSample &gps_sample{_gps_sample_delayed}; + + updateGpsYaw(gps_sample); + updateGpsVel(gps_sample); + updateGpsPos(gps_sample); 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); + controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing); + + // update GSF yaw estimator velocity (basic sanity check on GNSS velocity data) + if (gps_checks_passing && !gps_checks_failing) { + _yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.sacc); + } // 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 @@ -144,7 +151,7 @@ void Ekf::controlGpsFusion() _inhibit_ev_yaw_use = true; } else { - startGpsFusion(); + startGpsFusion(gps_sample); } } else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) { @@ -152,8 +159,8 @@ void Ekf::controlGpsFusion() if (resetYawToEKFGSF()) { _information_events.flags.yaw_aligned_to_imu_gps = true; ECL_INFO("Yaw aligned using IMU and GPS"); - resetVelocityToGps(_gps_sample_delayed); - resetHorizontalPositionToGps(_gps_sample_delayed); + resetVelocityToGps(gps_sample); + resetHorizontalPositionToGps(gps_sample); } } } diff --git a/src/modules/ekf2/EKF/gps_fusion.cpp b/src/modules/ekf2/EKF/gps_fusion.cpp index 82ba9e1e65..13c95d951f 100644 --- a/src/modules/ekf2/EKF/gps_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_fusion.cpp @@ -48,7 +48,7 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample) // initially populate for estimator_aid_src_gnss_yaw logging // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement - const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset); + const float measured_hdg = wrap_pi(gps_sample.yaw + _gps_yaw_offset); gps_yaw.observation = measured_hdg; gps_yaw.observation_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f)); @@ -103,7 +103,7 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample) resetEstimatorAidStatus(gps_pos); const float height_measurement = gps_sample.hgt - getEkfGlobalOriginAltitude(); - const float height_measurement_var = getGpsHeightVariance(); + const float height_measurement_var = getGpsHeightVariance(gps_sample); Vector3f position; position(0) = gps_sample.pos(0); diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 1ae5608b55..c5017d5fed 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -45,7 +45,7 @@ #include #include -void Ekf::fuseGpsYaw() +void Ekf::fuseGpsYaw(const gpsSample& gps_sample) { // assign intermediate state variables const float q0 = _state.quat_nominal(0); @@ -54,7 +54,7 @@ void Ekf::fuseGpsYaw() const float q3 = _state.quat_nominal(3); // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement - const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset); + const float measured_hdg = wrap_pi(gps_sample.yaw + _gps_yaw_offset); // define the predicted antenna array vector and rotate into earth frame const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; @@ -70,7 +70,7 @@ void Ekf::fuseGpsYaw() // TODO extend interface to use yaw uncertainty provided by GPS if available const float R_YAW = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f)); - _aid_src_gnss_yaw.timestamp_sample = _gps_sample_delayed.time_us; + _aid_src_gnss_yaw.timestamp_sample = gps_sample.time_us; _aid_src_gnss_yaw.observation = measured_hdg; _aid_src_gnss_yaw.observation_variance = R_YAW; _aid_src_gnss_yaw.innovation = heading_innov; @@ -201,7 +201,7 @@ void Ekf::fuseGpsYaw() } } -bool Ekf::resetYawToGps() +bool Ekf::resetYawToGps(const float gnss_yaw) { // define the predicted antenna array vector and rotate into earth frame const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; @@ -213,7 +213,7 @@ bool Ekf::resetYawToGps() } // GPS yaw measurement is alreday compensated for antenna offset in the driver - const float measured_yaw = _gps_sample_delayed.yaw; + const float measured_yaw = gnss_yaw; const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f)); resetQuatStateYaw(measured_yaw, yaw_variance); diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 5dc2919b7f..ab07ea9785 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -44,7 +44,7 @@ void Ekf::controlHeightFusion() updateGroundEffect(); controlBaroHeightFusion(); - controlGnssHeightFusion(); + controlGnssHeightFusion(_gps_sample_delayed); controlRangeHeightFusion(); controlEvHeightFusion();