diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 2d6a7c39a171..23e6e15fd48f 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -26,12 +26,12 @@ bool cs_mag_fault # 18 - true when the magnetometer has been declare bool cs_fuse_aspd # 19 - true when airspeed measurements are being fused bool cs_gnd_effect # 20 - true when protection from ground effect induced static pressure rise is active bool cs_rng_stuck # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough -bool cs_gps_yaw # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended +bool cs_gnss_yaw # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment has been completed bool cs_ev_vel # 24 - true when local frame velocity data fusion from external vision measurements is intended bool cs_synthetic_mag_z # 25 - true when we are using a synthesized measurement for the magnetometer Z component bool cs_vehicle_at_rest # 26 - true when the vehicle is at rest -bool cs_gps_yaw_fault # 27 - true when the GNSS heading has been declared faulty and is no longer being used +bool cs_gnss_yaw_fault # 27 - true when the GNSS heading has been declared faulty and is no longer being used bool cs_rng_fault # 28 - true when the range finder has been declared faulty and is no longer being used bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift bool cs_wind_dead_reckoning # 30 - true if we are navigationg reliant on wind relative measurements diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 7441e76f08c2..d1bc06a74218 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -623,7 +623,7 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report & } } - if (estimator_status_flags.cs_gps_yaw_fault) { + if (estimator_status_flags.cs_gnss_yaw_fault) { /* EVENT * @description * Land now diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 3f5d9d142f36..c4d0d1162584 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -175,7 +175,7 @@ if(CONFIG_EKF2_GNSS) ) if(CONFIG_EKF2_GNSS_YAW) - list(APPEND EKF_SRCS EKF/aid_sources/gnss/gps_yaw_fusion.cpp) + list(APPEND EKF_SRCS EKF/aid_sources/gnss/gnss_yaw_control.cpp) endif() list(APPEND EKF_LIBS yaw_estimator) diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 1134d1942b12..f404817ef4e1 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -96,7 +96,7 @@ if(CONFIG_EKF2_GNSS) ) if(CONFIG_EKF2_GNSS_YAW) - list(APPEND EKF_SRCS aid_sources/gnss/gps_yaw_fusion.cpp) + list(APPEND EKF_SRCS aid_sources/gnss/gnss_yaw_control.cpp) endif() add_subdirectory(yaw_estimator) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp similarity index 57% rename from src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp rename to src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp index d66fbac7c6f3..456691635a00 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp @@ -32,37 +32,122 @@ ****************************************************************************/ /** - * @file gps_yaw_fusion.cpp - * Definition of functions required to use yaw obtained from GPS dual antenna measurements. - * Equations generated using EKF/python/ekf_derivation/main.py - * - * @author Paul Riseborough + * @file gnss_yaw_control.cpp + * Definition of functions required to use yaw obtained from GNSS dual antenna measurements. + * Equations generated using src/modules/ekf2/EKF/python/ekf_derivation/derivation.py * */ #include "ekf.h" -#include #include #include -void Ekf::updateGpsYaw(const gnssSample &gps_sample) +#include + +void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) +{ + if (!(_params.gnss_ctrl & static_cast(GnssCtrl::YAW)) + || _control_status.flags.gnss_yaw_fault) { + + stopGnssYawFusion(); + return; + } + + const bool is_new_data_available = PX4_ISFINITE(gnss_sample.yaw); + + if (is_new_data_available) { + + updateGnssYaw(gnss_sample); + + const bool continuing_conditions_passing = _control_status.flags.tilt_align; + + const bool is_gnss_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, + 2 * GNSS_YAW_MAX_INTERVAL); + + const bool starting_conditions_passing = continuing_conditions_passing + && _gps_checks_passed + && !is_gnss_yaw_data_intermittent + && !_gps_intermittent; + + if (_control_status.flags.gnss_yaw) { + if (continuing_conditions_passing) { + + fuseGnssYaw(gnss_sample.yaw_offset); + + const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max); + + if (is_fusion_failing) { + stopGnssYawFusion(); + + // Before takeoff, we do not want to continue to rely on the current heading + // if we had to stop the fusion + if (!_control_status.flags.in_air) { + ECL_INFO("clearing yaw alignment"); + _control_status.flags.yaw_align = false; + } + } + + } else { + // Stop GNSS yaw fusion but do not declare it faulty + stopGnssYawFusion(); + } + + } else { + if (starting_conditions_passing) { + // Try to activate GNSS yaw fusion + const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos; + + if (!_control_status.flags.in_air + || !_control_status.flags.yaw_align + || not_using_ne_aiding) { + + // Reset before starting the fusion + if (resetYawToGnss(gnss_sample.yaw, gnss_sample.yaw_offset)) { + + resetAidSourceStatusZeroInnovation(_aid_src_gnss_yaw); + + _control_status.flags.gnss_yaw = true; + _control_status.flags.yaw_align = true; + } + + } else if (!_aid_src_gnss_yaw.innovation_rejected) { + // Do not force a reset but wait for the consistency check to pass + _control_status.flags.gnss_yaw = true; + fuseGnssYaw(gnss_sample.yaw_offset); + } + + if (_control_status.flags.gnss_yaw) { + ECL_INFO("starting GNSS yaw fusion"); + } + } + } + + } else if (_control_status.flags.gnss_yaw + && !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, _params.reset_timeout_max)) { + + // No yaw data in the message anymore. Stop until it comes back. + stopGnssYawFusion(); + } +} + +void Ekf::updateGnssYaw(const gnssSample &gnss_sample) { // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement - const float measured_hdg = wrap_pi(gps_sample.yaw + gps_sample.yaw_offset); + const float measured_hdg = wrap_pi(gnss_sample.yaw + gnss_sample.yaw_offset); - const float yaw_acc = PX4_ISFINITE(gps_sample.yaw_acc) ? gps_sample.yaw_acc : 0.f; - const float R_YAW = sq(fmaxf(yaw_acc, _params.gps_heading_noise)); + const float yaw_acc = PX4_ISFINITE(gnss_sample.yaw_acc) ? gnss_sample.yaw_acc : 0.f; + const float R_YAW = sq(fmaxf(yaw_acc, _params.gnss_heading_noise)); float heading_pred; float heading_innov_var; VectorState H; - sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gps_sample.yaw_offset, R_YAW, FLT_EPSILON, + sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gnss_sample.yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); updateAidSourceStatus(_aid_src_gnss_yaw, - gps_sample.time_us, // sample timestamp + gnss_sample.time_us, // sample timestamp measured_hdg, // observation R_YAW, // observation variance wrap_pi(heading_pred - measured_hdg), // innovation @@ -70,7 +155,7 @@ void Ekf::updateGpsYaw(const gnssSample &gps_sample) math::max(_params.heading_innov_gate, 1.f)); // innovation gate } -void Ekf::fuseGpsYaw(float antenna_yaw_offset) +void Ekf::fuseGnssYaw(float antenna_yaw_offset) { auto &aid_src = _aid_src_gnss_yaw; @@ -99,7 +184,8 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset) // we reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); - ECL_ERR("GPS yaw numerical error - covariance reset"); + ECL_ERR("GNSS yaw numerical error - covariance reset"); + stopGnssYawFusion(); return; } @@ -129,7 +215,7 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset) } } -bool Ekf::resetYawToGps(const float gnss_yaw, const float gnss_yaw_offset) +bool Ekf::resetYawToGnss(const float gnss_yaw, const float gnss_yaw_offset) { // define the predicted antenna array vector and rotate into earth frame const Vector3f ant_vec_bf = {cosf(gnss_yaw_offset), sinf(gnss_yaw_offset), 0.0f}; @@ -140,11 +226,21 @@ bool Ekf::resetYawToGps(const float gnss_yaw, const float gnss_yaw_offset) return false; } - // GPS yaw measurement is alreday compensated for antenna offset in the driver + // GNSS yaw measurement is already compensated for antenna offset in the driver const float measured_yaw = gnss_yaw; - const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f)); + const float yaw_variance = sq(fmaxf(_params.gnss_heading_noise, 1.e-2f)); resetQuatStateYaw(measured_yaw, yaw_variance); return true; } + +void Ekf::stopGnssYawFusion() +{ + if (_control_status.flags.gnss_yaw) { + + _control_status.flags.gnss_yaw = false; + + ECL_INFO("stopping GNSS yaw fusion"); + } +} diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index b8440595aecd..7bfb2e283107 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -98,7 +98,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) if (_gps_data_ready) { #if defined(CONFIG_EKF2_GNSS_YAW) const gnssSample &gnss_sample = _gps_sample_delayed; - controlGpsYawFusion(gnss_sample); + controlGnssYawFusion(gnss_sample); #endif // CONFIG_EKF2_GNSS_YAW controlGnssYawEstimator(_aid_src_gnss_vel); @@ -285,8 +285,8 @@ bool Ekf::tryYawEmergencyReset() #if defined(CONFIG_EKF2_GNSS_YAW) - if (_control_status.flags.gps_yaw) { - _control_status.flags.gps_yaw_fault = true; + if (_control_status.flags.gnss_yaw) { + _control_status.flags.gnss_yaw_fault = true; } #endif // CONFIG_EKF2_GNSS_YAW @@ -354,104 +354,6 @@ bool Ekf::shouldResetGpsFusion() const return (is_reset_required || is_inflight_nav_failure); } -#if defined(CONFIG_EKF2_GNSS_YAW) -void Ekf::controlGpsYawFusion(const gnssSample &gps_sample) -{ - if (!(_params.gnss_ctrl & static_cast(GnssCtrl::YAW)) - || _control_status.flags.gps_yaw_fault) { - - stopGpsYawFusion(); - return; - } - - const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw); - - if (is_new_data_available) { - - updateGpsYaw(gps_sample); - - const bool continuing_conditions_passing = _control_status.flags.tilt_align; - - 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 - && _gps_checks_passed - && !is_gps_yaw_data_intermittent - && !_gps_intermittent; - - if (_control_status.flags.gps_yaw) { - if (continuing_conditions_passing) { - - fuseGpsYaw(gps_sample.yaw_offset); - - const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max); - - if (is_fusion_failing) { - stopGpsYawFusion(); - - // Before takeoff, we do not want to continue to rely on the current heading - // if we had to stop the fusion - if (!_control_status.flags.in_air) { - ECL_INFO("clearing yaw alignment"); - _control_status.flags.yaw_align = false; - } - } - - } else { - // Stop GPS yaw fusion but do not declare it faulty - stopGpsYawFusion(); - } - - } else { - if (starting_conditions_passing) { - // Try to activate GPS yaw fusion - const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos; - - if (!_control_status.flags.in_air - || !_control_status.flags.yaw_align - || not_using_ne_aiding) { - - // Reset before starting the fusion - if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) { - - resetAidSourceStatusZeroInnovation(_aid_src_gnss_yaw); - - _control_status.flags.gps_yaw = true; - _control_status.flags.yaw_align = true; - } - - } else if (!_aid_src_gnss_yaw.innovation_rejected) { - // Do not force a reset but wait for the consistency check to pass - _control_status.flags.gps_yaw = true; - fuseGpsYaw(gps_sample.yaw_offset); - } - - if (_control_status.flags.gps_yaw) { - ECL_INFO("starting GPS yaw fusion"); - } - } - } - - } 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(); - } -} - -void Ekf::stopGpsYawFusion() -{ - if (_control_status.flags.gps_yaw) { - - _control_status.flags.gps_yaw = false; - - ECL_INFO("stopping GPS yaw fusion"); - } -} -#endif // CONFIG_EKF2_GNSS_YAW - void Ekf::stopGpsFusion() { if (_control_status.flags.gps) { @@ -465,7 +367,7 @@ void Ekf::stopGpsFusion() stopGpsHgtFusion(); #if defined(CONFIG_EKF2_GNSS_YAW) - stopGpsYawFusion(); + stopGnssYawFusion(); #endif // CONFIG_EKF2_GNSS_YAW _yawEstimator.reset(); diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 2dab0da57eaf..35dd869e1fb7 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -153,7 +153,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) && !_control_status.flags.mag_fault && !_control_status.flags.mag_field_disturbed && !_control_status.flags.ev_yaw - && !_control_status.flags.gps_yaw; + && !_control_status.flags.gnss_yaw; _control_status.flags.mag_3D = common_conditions_passing && (_params.mag_fusion_type == MagFuseType::AUTO) diff --git a/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp index 3c0a2c1d6cf7..fded4cfbcc13 100644 --- a/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp +++ b/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp @@ -41,7 +41,7 @@ void Ekf::controlZeroInnovationHeadingUpdate() { const bool yaw_aiding = _control_status.flags.mag_hdg || _control_status.flags.mag_3D - || _control_status.flags.ev_yaw || _control_status.flags.gps_yaw; + || _control_status.flags.ev_yaw || _control_status.flags.gnss_yaw; // fuse zero innovation at a limited rate if the yaw variance is too large if (!yaw_aiding diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 05610bca5858..da07028edce8 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -338,7 +338,7 @@ struct parameters { # if defined(CONFIG_EKF2_GNSS_YAW) // GNSS heading fusion - float gps_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad) + float gnss_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad) # endif // CONFIG_EKF2_GNSS_YAW // Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value @@ -580,7 +580,7 @@ uint64_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active uint64_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough -uint64_t gps_yaw : +uint64_t gnss_yaw : 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended uint64_t mag_aligned_in_flight : 1; ///< 23 - true when the in-flight mag field alignment has been completed uint64_t ev_vel : @@ -588,7 +588,7 @@ uint64_t ev_vel : uint64_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component uint64_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest -uint64_t gps_yaw_fault : +uint64_t gnss_yaw_fault : 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used uint64_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f23f514fd6f8..7070c71bf363 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -144,7 +144,7 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_GNSS_YAW) - if (_control_status.flags.gps_yaw) { + if (_control_status.flags.gnss_yaw) { return _aid_src_gnss_yaw.innovation; } @@ -173,7 +173,7 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_GNSS_YAW) - if (_control_status.flags.gps_yaw) { + if (_control_status.flags.gnss_yaw) { return _aid_src_gnss_yaw.innovation_variance; } @@ -202,7 +202,7 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_GNSS_YAW) - if (_control_status.flags.gps_yaw) { + if (_control_status.flags.gnss_yaw) { return _aid_src_gnss_yaw.test_ratio; } @@ -995,17 +995,17 @@ class Ekf final : public EstimatorInterface void resetGpsDriftCheckFilters(); # if defined(CONFIG_EKF2_GNSS_YAW) - void controlGpsYawFusion(const gnssSample &gps_sample); - void stopGpsYawFusion(); + void controlGnssYawFusion(const gnssSample &gps_sample); + void stopGnssYawFusion(); // fuse the yaw angle obtained from a dual antenna GPS unit - void fuseGpsYaw(float antenna_yaw_offset); + void fuseGnssYaw(float antenna_yaw_offset); // reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit // return true if the reset was successful - bool resetYawToGps(float gnss_yaw, float gnss_yaw_offset); + bool resetYawToGnss(float gnss_yaw, float gnss_yaw_offset); - void updateGpsYaw(const gnssSample &gps_sample); + void updateGnssYaw(const gnssSample &gps_sample); # endif // CONFIG_EKF2_GNSS_YAW diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index e44d6c0bf61b..02675d1f746f 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -333,7 +333,7 @@ float Ekf::getHeadingInnovationTestRatio() const #if defined(CONFIG_EKF2_GNSS_YAW) - if (_control_status.flags.gps_yaw) { + if (_control_status.flags.gnss_yaw) { test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_yaw.test_ratio_filtered)); } diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index d4a579f5b8cf..f7a6c70dec39 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -193,7 +193,7 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample) #if defined(CONFIG_EKF2_GNSS_YAW) if (PX4_ISFINITE(gnss_sample.yaw)) { - _time_last_gps_yaw_buffer_push = _time_latest_us; + _time_last_gnss_yaw_buffer_push = _time_latest_us; } #endif // CONFIG_EKF2_GNSS_YAW diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 4477f17b5164..75e65268304d 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -397,7 +397,7 @@ class EstimatorInterface # if defined(CONFIG_EKF2_GNSS_YAW) // innovation consistency check monitoring ratios - uint64_t _time_last_gps_yaw_buffer_push{0}; + uint64_t _time_last_gnss_yaw_buffer_push{0}; # endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 44656a206737..7f4c8d6516c3 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1912,12 +1912,12 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_fuse_aspd = _ekf.control_status_flags().fuse_aspd; status_flags.cs_gnd_effect = _ekf.control_status_flags().gnd_effect; status_flags.cs_rng_stuck = _ekf.control_status_flags().rng_stuck; - status_flags.cs_gps_yaw = _ekf.control_status_flags().gps_yaw; + status_flags.cs_gnss_yaw = _ekf.control_status_flags().gnss_yaw; status_flags.cs_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight; status_flags.cs_ev_vel = _ekf.control_status_flags().ev_vel; status_flags.cs_synthetic_mag_z = _ekf.control_status_flags().synthetic_mag_z; status_flags.cs_vehicle_at_rest = _ekf.control_status_flags().vehicle_at_rest; - status_flags.cs_gps_yaw_fault = _ekf.control_status_flags().gps_yaw_fault; + status_flags.cs_gnss_yaw_fault = _ekf.control_status_flags().gnss_yaw_fault; status_flags.cs_rng_fault = _ekf.control_status_flags().rng_fault; status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning; status_flags.cs_wind_dead_reckoning = _ekf.control_status_flags().wind_dead_reckoning; diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index 424a2a208a6e..800b568d96d7 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -45,7 +45,7 @@ px4_add_unit_gtest(SRC test_EKF_flow_generated.cpp LINKLIBS ecl_EKF ecl_sensor_s px4_add_unit_gtest(SRC test_EKF_gyroscope.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_fusionLogic.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_gps.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) -px4_add_unit_gtest(SRC test_EKF_gps_yaw.cpp LINKLIBS ecl_EKF ecl_sensor_sim) +px4_add_unit_gtest(SRC test_EKF_gnss_yaw.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_gnss_yaw_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_height_fusion.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_imuSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim) diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 26a1dd73c605..9fd62885e36b 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -132,7 +132,7 @@ void EkfWrapper::disableGpsHeadingFusion() bool EkfWrapper::isIntendingGpsHeadingFusion() const { - return _ekf->control_status_flags().gps_yaw; + return _ekf->control_status_flags().gnss_yaw; } void EkfWrapper::enableFlowFusion() diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index c7a8ffa83643..e95443344235 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -123,7 +123,7 @@ TEST_F(EkfBasicsTest, initialControlMode) EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault); EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect); EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck); - EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_yaw); + EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight); EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel); EXPECT_EQ(0, (int) _ekf->control_status_flags().synthetic_mag_z); @@ -178,7 +178,7 @@ TEST_F(EkfBasicsTest, gpsFusion) EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault); EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect); EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck); - EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_yaw); + EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight); EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel); EXPECT_EQ(0, (int) _ekf->control_status_flags().synthetic_mag_z); diff --git a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp b/src/modules/ekf2/test/test_EKF_gnss_yaw.cpp similarity index 100% rename from src/modules/ekf2/test/test_EKF_gps_yaw.cpp rename to src/modules/ekf2/test/test_EKF_gnss_yaw.cpp