diff --git a/Tools/ecl_ekf/analyse_logdata_ekf.py b/Tools/ecl_ekf/analyse_logdata_ekf.py index 98e7c23b93..70b8da5582 100644 --- a/Tools/ecl_ekf/analyse_logdata_ekf.py +++ b/Tools/ecl_ekf/analyse_logdata_ekf.py @@ -111,13 +111,13 @@ def find_checks_that_apply( innov_fail_checks.append('yaw') # Velocity Sensor Checks - if (np.amax(estimator_status_flags['cs_gps']) > 0.5): + if (np.amax(estimator_status_flags['cs_gnss_vel']) > 0.5): sensor_checks.append('vel') innov_fail_checks.append('velh') innov_fail_checks.append('velv') # Position Sensor Checks - if (pos_checks_when_sensors_not_fused or (np.amax(estimator_status_flags['cs_gps']) > 0.5) + if (pos_checks_when_sensors_not_fused or (np.amax(estimator_status_flags['cs_gnss_pos']) > 0.5) or (np.amax(estimator_status_flags['cs_ev_pos']) > 0.5)): sensor_checks.append('pos') innov_fail_checks.append('posh') diff --git a/Tools/ecl_ekf/plotting/pdf_report.py b/Tools/ecl_ekf/plotting/pdf_report.py index 35821fbc46..3daf82efb0 100644 --- a/Tools/ecl_ekf/plotting/pdf_report.py +++ b/Tools/ecl_ekf/plotting/pdf_report.py @@ -185,7 +185,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str # plot control mode summary A data_plot = ControlModeSummaryPlot( status_flags_time, estimator_status_flags, [['cs_tilt_align', 'cs_yaw_align'], - ['cs_gps', 'cs_opt_flow', 'cs_ev_pos'], ['cs_baro_hgt', 'cs_gps_hgt', + ['cs_gnss_pos', 'cs_opt_flow', 'cs_ev_pos'], ['cs_baro_hgt', 'cs_gps_hgt', 'cs_rng_hgt', 'cs_ev_hgt'], ['cs_mag_hdg', 'cs_mag_3d', 'cs_mag_dec']], x_label='time (sec)', y_labels=['aligned', 'pos aiding', 'hgt aiding', 'mag aiding'], annotation_text=[['tilt alignment', 'yaw alignment'], ['GPS aiding', 'optical flow aiding', diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index 3c7985e630..3de2bfd722 100644 --- a/msg/EstimatorStatus.msg +++ b/msg/EstimatorStatus.msg @@ -20,7 +20,7 @@ uint8 GPS_CHECK_FAIL_SPOOFED = 10 # 10 : GPS signal is spoofed uint64 control_mode_flags # Bitmask to indicate EKF logic state uint8 CS_TILT_ALIGN = 0 # 0 - true if the filter tilt alignment is complete uint8 CS_YAW_ALIGN = 1 # 1 - true if the filter yaw alignment is complete -uint8 CS_GPS = 2 # 2 - true if GPS measurements are being fused +uint8 CS_GNSS_POS = 2 # 2 - true if GNSS position measurements are being fused uint8 CS_OPT_FLOW = 3 # 3 - true if optical flow measurements are being fused uint8 CS_MAG_HDG = 4 # 4 - true if a simple magnetic yaw heading is being fused uint8 CS_MAG_3D = 5 # 5 - true if 3-axis magnetometer measurement are being fused @@ -47,6 +47,7 @@ uint8 CS_SYNTHETIC_MAG_Z = 25 # 25 - true when we are using a synthesized measur uint8 CS_VEHICLE_AT_REST = 26 # 26 - true when the vehicle is at rest uint8 CS_GPS_YAW_FAULT = 27 # 27 - true when the GNSS heading has been declared faulty and is no longer being used uint8 CS_RNG_FAULT = 28 # 28 - true when the range finder has been declared faulty and is no longer being used +uint8 CS_GNSS_VEL = 44 # 44 - true if GNSS velocity measurements are being fused uint32 filter_fault_flags # Bitmask to indicate EKF internal faults # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 0e5525d545..8ab5b3d3e4 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -6,7 +6,7 @@ uint64 timestamp_sample # the timestamp of the raw data (micro uint32 control_status_changes # number of filter control status (cs) changes bool cs_tilt_align # 0 - true if the filter tilt alignment is complete bool cs_yaw_align # 1 - true if the filter yaw alignment is complete -bool cs_gps # 2 - true if GPS measurement fusion is intended +bool cs_gnss_pos # 2 - true if GNSS position measurement fusion is intended bool cs_opt_flow # 3 - true if optical flow measurements fusion is intended bool cs_mag_hdg # 4 - true if a simple magnetic yaw heading fusion is intended bool cs_mag_3d # 5 - true if 3-axis magnetometer measurement fusion is intended @@ -48,6 +48,7 @@ bool cs_opt_flow_terrain # 40 - true if we are fusing flow data for terra bool cs_valid_fake_pos # 41 - true if a valid constant position is being fused bool cs_constant_pos # 42 - true if the vehicle is at a constant position bool cs_baro_fault # 43 - true when the current baro has been declared faulty and is no longer being used +bool cs_gnss_vel # 44 - true if GNSS velocity measurement fusion is intended # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 03b9d48574..5d998a58f2 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -561,7 +561,8 @@ void AirspeedModule::poll_topics() _gnss_lpos_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s) && (_vehicle_local_position.timestamp > 0) && _vehicle_local_position.v_xy_valid - && _estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS); + && (_estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GNSS_POS) + || _estimator_status.control_mode_flags & (static_cast(1) << estimator_status_s::CS_GNSS_VEL)); } void AirspeedModule::update_wind_estimator_sideslip() diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 994bf9cbc2..8d1dbbd2df 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -224,7 +224,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing if (_param_sys_has_gps.get()) { - const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS); + const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GNSS_POS); const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0; if (ekf_gps_fusion) { diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp index 266b4cf548..e3fad93ad2 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp @@ -70,7 +70,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample pos = ev_sample.pos - pos_offset_earth; pos_cov = matrix::diag(ev_sample.position_var); - if (_control_status.flags.gps) { + if (_control_status.flags.gnss_pos) { _ev_pos_b_est.setFusionActive(); } else { @@ -109,7 +109,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max); } - if (_control_status.flags.gps) { + if (_control_status.flags.gnss_pos) { _ev_pos_b_est.setFusionActive(); } else { @@ -128,8 +128,8 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample #if defined(CONFIG_EKF2_GNSS) - // increase minimum variance if GPS active (position reference) - if (_control_status.flags.gps) { + // increase minimum variance if GNSS is active (position reference) + if (_control_status.flags.gnss_pos) { for (int i = 0; i < 2; i++) { pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise)); } @@ -148,7 +148,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite(); - // bias fusion activated (GPS activated) + // bias fusion activated (GNSS position activated) if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) { if (quality_sufficient) { // reset the bias estimator @@ -213,7 +213,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem { // activate fusion // TODO: (_params.position_sensor_ref == PositionSensor::EV) - if (_control_status.flags.gps) { + if (_control_status.flags.gnss_pos) { ECL_INFO("starting %s fusion", EV_AID_SRC_NAME); _ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement); _ev_pos_b_est.setFusionActive(); @@ -240,7 +240,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure if (quality_sufficient) { - if (!_control_status.flags.gps) { + if (!_control_status.flags.gnss_pos) { ECL_INFO("reset to %s", EV_AID_SRC_NAME); _information_events.flags.reset_pos_to_vision = true; resetHorizontalPositionTo(measurement, measurement_var); @@ -275,14 +275,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure // Data seems good, attempt a reset ECL_WARN("%s fusion failing, resetting", EV_AID_SRC_NAME); - if (_control_status.flags.gps && !pos_xy_fusion_failing) { + if (_control_status.flags.gnss_pos && !pos_xy_fusion_failing) { // reset EV position bias _ev_pos_b_est.setBias(-Vector2f(getLocalHorizontalPosition()) + measurement); } else { _information_events.flags.reset_pos_to_vision = true; - if (_control_status.flags.gps) { + if (_control_status.flags.gnss_pos) { resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar()); _ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp index ad15c4f6bb..98dd19f3a6 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp @@ -72,8 +72,8 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample && PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance); - // if GPS enabled don't allow EV yaw if EV isn't NED - if (_control_status.flags.gps && _control_status.flags.yaw_align + // if GNSS is enabled don't allow EV yaw if EV isn't NED + if ((_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) && _control_status.flags.yaw_align && (ev_sample.pos_frame != PositionFrame::LOCAL_FRAME_NED) ) { continuing_conditions_passing = false; 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 9998a52af1..bd37c0fb37 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 @@ -96,11 +96,10 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) } 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) { + || !isNorthEastAidingActive()) { // Reset before starting the fusion if (resetYawToGnss(gnss_sample.yaw, gnss_sample.yaw_offset)) { 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 6ce0cebc66..9eabfe156d 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -42,7 +42,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) { if (!_gps_buffer || (_params.gnss_ctrl == 0)) { - stopGpsFusion(); + stopGnssFusion(); return; } @@ -78,19 +78,20 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) // Skip this sample _gps_data_ready = false; - if (_control_status.flags.gps && isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) { - stopGpsFusion(); - ECL_WARN("GPS quality poor - stopping use"); + 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.gps) { + } else if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) { if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) { - stopGpsFusion(); - ECL_WARN("GPS data stopped"); + stopGnssFusion(); + ECL_WARN("GNSS data stopped"); } } @@ -102,81 +103,105 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) controlGnssYawEstimator(_aid_src_gnss_vel); - const bool gnss_vel_enabled = (_params.gnss_ctrl & static_cast(GnssCtrl::VEL)); - const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast(GnssCtrl::HPOS)); + bool do_vel_pos_reset = false; - const bool continuing_conditions_passing = (gnss_vel_enabled || 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_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.gps) { - if (continuing_conditions_passing) { - if (gnss_vel_enabled) { - fuseVelocity(_aid_src_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)) { + do_vel_pos_reset = tryYawEmergencyReset(); + } + } - if (gnss_pos_enabled) { - fuseHorizontalPosition(_aid_src_gnss_pos); - } + controlGnssVelFusion(_aid_src_gnss_vel, do_vel_pos_reset); + controlGnssPosFusion(_aid_src_gnss_pos, do_vel_pos_reset); + } +} - bool do_vel_pos_reset = shouldResetGpsFusion(); +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.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(); - } - - if (do_vel_pos_reset) { - ECL_WARN("GPS fusion timeout, resetting"); - } - - if (gnss_vel_enabled) { - if (do_vel_pos_reset) { - resetVelocityToGnss(_aid_src_gnss_vel); - - } else if (isHeightResetRequired()) { - // reset vertical velocity if height is failing - resetVerticalVelocityTo(_aid_src_gnss_vel.observation[2], _aid_src_gnss_vel.observation_variance[2]); - } - } - - if (gnss_pos_enabled && do_vel_pos_reset) { - resetHorizontalPositionToGnss(_aid_src_gnss_pos); - } + if (_control_status.flags.gnss_vel) { + if (continuing_conditions_passing) { + if (force_reset) { + ECL_WARN("GNSS fusion timeout, resetting"); + resetVelocityToGnss(aid_src); } else { - stopGpsFusion(); + fuseVelocity(aid_src); + + if (isHeightResetRequired()) { + // reset vertical velocity if height is failing + resetVerticalVelocityTo(aid_src.observation[2], aid_src.observation_variance[2]); + } } } else { - if (starting_conditions_passing) { - ECL_INFO("starting GPS fusion"); - _information_events.flags.starting_gps_fusion = true; + stopGnssVelFusion(); + } - // 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 - if (gnss_vel_enabled) { - resetVelocityToGnss(_aid_src_gnss_vel); - } - } + } else { + if (starting_conditions_passing) { + ECL_INFO("starting GNSS velocity fusion"); + _information_events.flags.starting_gps_fusion = true; - if (gnss_pos_enabled) { - resetHorizontalPositionToGnss(_aid_src_gnss_pos); - } - - _control_status.flags.gps = true; - - } else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) { - resetHorizontalPositionToGnss(_aid_src_gnss_pos); + // 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); } } } @@ -231,8 +256,8 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s // 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.gps)) { - // if we are not using another source of aiding, then we are reliant on the GPS + 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; @@ -366,17 +391,15 @@ bool Ekf::shouldResetGpsFusion() const return (is_reset_required || is_inflight_nav_failure); } -void Ekf::stopGpsFusion() +void Ekf::stopGnssFusion() { - if (_control_status.flags.gps) { - ECL_INFO("stopping GPS position and velocity fusion"); - + if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) { _last_gps_fail_us = 0; _last_gps_pass_us = 0; - - _control_status.flags.gps = false; } + stopGnssVelFusion(); + stopGnssPosFusion(); stopGpsHgtFusion(); #if defined(CONFIG_EKF2_GNSS_YAW) stopGnssYawFusion(); @@ -385,6 +408,34 @@ void Ekf::stopGpsFusion() _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 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 ae1142c86b..e3ac4a9d82 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -167,11 +167,8 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) checkMagHeadingConsistency(mag_sample); - const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos; - - { - const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding; + const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !isNorthEastAidingActive(); const bool common_conditions_passing = _control_status.flags.mag && ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding) || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) @@ -200,7 +197,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) // if we are using 3-axis magnetometer fusion, but without external NE aiding, // then the declination must be fused as an observation to prevent long term heading drift - const bool no_ne_aiding_or_not_moving = !using_ne_aiding || _control_status.flags.vehicle_at_rest; + const bool no_ne_aiding_or_not_moving = !isNorthEastAidingActive() || _control_status.flags.vehicle_at_rest; _control_status.flags.mag_dec = _control_status.flags.mag && no_ne_aiding_or_not_moving; if (_control_status.flags.mag) { @@ -321,10 +318,9 @@ void Ekf::stopMagFusion() resetMagBiasCov(); if (_control_status.flags.yaw_align && (_control_status.flags.mag_3D || _control_status.flags.mag_hdg)) { - // reset yaw alignment from mag unless using GNSS aiding - const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos; + // reset yaw alignment from mag unless yaw is observable through North-East aiding - if (!using_ne_aiding) { + if (!isNorthEastAidingActive()) { _control_status.flags.yaw_align = false; } } @@ -480,9 +476,8 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample) if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) { // Check if there has been enough change in horizontal velocity to make yaw observable - const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos; - if (using_ne_aiding && (_accel_horiz_lpf.getState().longerThan(_params.mag_acc_gate))) { + if (isNorthEastAidingActive() && (_accel_horiz_lpf.getState().longerThan(_params.mag_acc_gate))) { // yaw angle must be observable to consider consistency _control_status.flags.mag_heading_consistent = true; } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 95efc971bc..5ec5e42925 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -566,7 +566,7 @@ union filter_control_status_u { struct { uint64_t tilt_align : 1; ///< 0 - true if the filter tilt alignment is complete uint64_t yaw_align : 1; ///< 1 - true if the filter yaw alignment is complete - uint64_t gps : 1; ///< 2 - true if GPS measurement fusion is intended + uint64_t gnss_pos : 1; ///< 2 - true if GNSS position measurement fusion is intended uint64_t opt_flow : 1; ///< 3 - true if optical flow measurements fusion is intended uint64_t mag_hdg : 1; ///< 4 - true if a simple magnetic yaw heading fusion is intended uint64_t mag_3D : 1; ///< 5 - true if 3-axis magnetometer measurement fusion is intended @@ -620,8 +620,8 @@ uint64_t mag_heading_consistent : uint64_t opt_flow_terrain : 1; ///< 40 - true if we are fusing flow data for terrain uint64_t valid_fake_pos : 1; ///< 41 - true if a valid constant position is being fused uint64_t constant_pos : 1; ///< 42 - true if the vehicle is at a constant position - uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used - + uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used + uint64_t gnss_vel : 1; ///< 44 - true if GNSS velocity measurement fusion is intended } flags; uint64_t value; }; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 47320e8c12..981b31e094 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -862,7 +862,11 @@ private: #if defined(CONFIG_EKF2_GNSS) // control fusion of GPS observations void controlGpsFusion(const imuSample &imu_delayed); - void stopGpsFusion(); + void controlGnssVelFusion(estimator_aid_source3d_s &aid_src, bool force_reset); + void controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool force_reset); + 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); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 0d8fac2fb5..c68423f251 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -262,7 +262,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const if (_horizontal_deadreckon_time_exceeded) { #if defined(CONFIG_EKF2_GNSS) - if (_control_status.flags.gps) { + if (_control_status.flags.gnss_pos) { hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm()); } @@ -302,10 +302,14 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const #if defined(CONFIG_EKF2_GNSS) - if (_control_status.flags.gps) { + if (_control_status.flags.gnss_pos) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm()); } + if (_control_status.flags.gnss_vel) { + vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_vel.innovation).norm()); + } + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) @@ -446,7 +450,7 @@ float Ekf::getHorizontalVelocityInnovationTestRatio() const #if defined(CONFIG_EKF2_GNSS) - if (_control_status.flags.gps) { + if (_control_status.flags.gnss_vel) { for (int i = 0; i < 2; i++) { // only xy test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[i])); } @@ -488,7 +492,7 @@ float Ekf::getVerticalVelocityInnovationTestRatio() const #if defined(CONFIG_EKF2_GNSS) - if (_control_status.flags.gps) { + if (_control_status.flags.gnss_vel) { test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[2])); } @@ -516,7 +520,7 @@ float Ekf::getHorizontalPositionInnovationTestRatio() const #if defined(CONFIG_EKF2_GNSS) - if (_control_status.flags.gps) { + if (_control_status.flags.gnss_pos) { for (auto &test_ratio_filtered : _aid_src_gnss_pos.test_ratio_filtered) { test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); } @@ -706,7 +710,7 @@ uint16_t Ekf::get_ekf_soln_status() const soln_status.flags.pos_vert_agl = isTerrainEstimateValid(); #endif // CONFIG_EKF2_TERRAIN - // 128 ESTIMATOR_CONST_POS_MODE True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) + // 128 ESTIMATOR_CONST_POS_MODE True if the EKF is in a constant position mode and is not using external measurements (eg GNSS or optical flow) soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.valid_fake_pos || _control_status.flags.vehicle_at_rest; @@ -714,13 +718,13 @@ uint16_t Ekf::get_ekf_soln_status() const soln_status.flags.pred_pos_horiz_rel = isHorizontalAidingActive(); // 512 ESTIMATOR_PRED_POS_HORIZ_ABS True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate - soln_status.flags.pred_pos_horiz_abs = _control_status.flags.gps || _control_status.flags.aux_gpos; + soln_status.flags.pred_pos_horiz_abs = _control_status.flags.gnss_pos || _control_status.flags.aux_gpos; - // 1024 ESTIMATOR_GPS_GLITCH True if the EKF has detected a GPS glitch + // 1024 ESTIMATOR_GPS_GLITCH True if the EKF has detected a GNSS glitch #if defined(CONFIG_EKF2_GNSS) - const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f; - const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f; - soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad); + const bool gnss_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f; + const bool gnss_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f; + soln_status.flags.gps_glitch = (gnss_vel_innov_bad || gnss_pos_innov_bad); #endif // CONFIG_EKF2_GNSS // 2048 ESTIMATOR_ACCEL_ERROR True if the EKF has detected bad accelerometer data @@ -799,14 +803,14 @@ void Ekf::updateHorizontalDeadReckoningstatus() bool aiding_expected_in_air = false; // velocity aiding active - if ((_control_status.flags.gps || _control_status.flags.ev_vel) + if ((_control_status.flags.gnss_vel || _control_status.flags.ev_vel) && isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max) ) { inertial_dead_reckoning = false; } // position aiding active - if ((_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.aux_gpos) + if ((_control_status.flags.gnss_pos || _control_status.flags.ev_pos || _control_status.flags.aux_gpos) && isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) ) { inertial_dead_reckoning = false; diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 22ccf958f0..3fd119fb44 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -617,7 +617,7 @@ bool EstimatorInterface::isOtherSourceOfHorizontalPositionAidingThan(const bool int EstimatorInterface::getNumberOfActiveHorizontalPositionAidingSources() const { - return int(_control_status.flags.gps) + return int(_control_status.flags.gnss_pos) + int(_control_status.flags.ev_pos) + int(_control_status.flags.aux_gpos); } @@ -672,10 +672,17 @@ bool EstimatorInterface::isVerticalVelocityAidingActive() const int EstimatorInterface::getNumberOfActiveVerticalVelocityAidingSources() const { - return int(_control_status.flags.gps) + return int(_control_status.flags.gnss_vel) + int(_control_status.flags.ev_vel); } +bool EstimatorInterface::isNorthEastAidingActive() const +{ + return _control_status.flags.gnss_pos + || _control_status.flags.gnss_vel + || _control_status.flags.aux_gpos; +} + void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name) { if (buffer_name) { diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 93632bc109..5ff02ca7ab 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -227,6 +227,7 @@ public: // the flags considered are opt_flow, gps, ev_vel and ev_pos bool isHorizontalAidingActive() const; bool isVerticalAidingActive() const; + bool isNorthEastAidingActive() const; int getNumberOfActiveHorizontalAidingSources() const; int getNumberOfActiveHorizontalPositionAidingSources() const; diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 147e198921..44467fa2b8 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -205,7 +205,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const checks[1] = {ReferenceType::GNSS, _aid_src_gnss_hgt.innovation, _aid_src_gnss_hgt.innovation_variance}; } - if (_control_status.flags.gps) { + if (_control_status.flags.gnss_vel) { checks[2] = {ReferenceType::GNSS, _aid_src_gnss_vel.innovation[2], _aid_src_gnss_vel.innovation_variance[2]}; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 9e1f8f5378..099848802e 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -533,7 +533,8 @@ void EKF2::Run() } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) { if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning - || (!_ekf.control_status_flags().in_air && !_ekf.control_status_flags().gps)) && PX4_ISFINITE(vehicle_command.param2) + || (!_ekf.control_status_flags().in_air && !_ekf.control_status_flags().gnss_pos)) + && PX4_ISFINITE(vehicle_command.param2) && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6) ) { @@ -1892,7 +1893,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.control_status_changes = _filter_control_status_changes; status_flags.cs_tilt_align = _ekf.control_status_flags().tilt_align; status_flags.cs_yaw_align = _ekf.control_status_flags().yaw_align; - status_flags.cs_gps = _ekf.control_status_flags().gps; + status_flags.cs_gnss_pos = _ekf.control_status_flags().gnss_pos; status_flags.cs_opt_flow = _ekf.control_status_flags().opt_flow; status_flags.cs_mag_hdg = _ekf.control_status_flags().mag_hdg; status_flags.cs_mag_3d = _ekf.control_status_flags().mag_3D; @@ -1934,6 +1935,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_valid_fake_pos = _ekf.control_status_flags().valid_fake_pos; status_flags.cs_constant_pos = _ekf.control_status_flags().constant_pos; status_flags.cs_baro_fault = _ekf.control_status_flags().baro_fault; + status_flags.cs_gnss_vel = _ekf.control_status_flags().gnss_vel; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 9fd62885e3..e8a7982b87 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -117,7 +117,7 @@ void EkfWrapper::disableGpsFusion() bool EkfWrapper::isIntendingGpsFusion() const { - return _ekf->control_status_flags().gps; + return _ekf->control_status_flags().gnss_vel || _ekf->control_status_flags().gnss_pos; } void EkfWrapper::enableGpsHeadingFusion() diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index d67cac0e52..396d50ff76 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -103,7 +103,8 @@ TEST_F(EkfBasicsTest, initialControlMode) // THEN: EKF control status should be reasonable EXPECT_EQ(1, (int) _ekf->control_status_flags().tilt_align); EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align); - EXPECT_EQ(0, (int) _ekf->control_status_flags().gps); + EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_pos); + EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_vel); EXPECT_EQ(0, (int) _ekf->control_status_flags().opt_flow); EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D); @@ -158,7 +159,8 @@ TEST_F(EkfBasicsTest, gpsFusion) // THEN: EKF should fuse GPS, but no other position sensor EXPECT_EQ(1, (int) _ekf->control_status_flags().tilt_align); EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align); - EXPECT_EQ(1, (int) _ekf->control_status_flags().gps); + EXPECT_EQ(1, (int) _ekf->control_status_flags().gnss_pos); + EXPECT_EQ(1, (int) _ekf->control_status_flags().gnss_vel); EXPECT_EQ(0, (int) _ekf->control_status_flags().opt_flow); EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D);