From 1d7791dad6f28340d95390e4bdbd8089a0a82531 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 3 Dec 2021 11:42:05 -0500 Subject: [PATCH] commander: monitor GPS validity and EKF2 dead reckoning - ekf2: expose dead reckoning as control status flags - commander: - add GPS validity check - in AUTO MISSION if dependent on GPS then a loss of GPS will --- msg/estimator_status_flags.msg | 2 + msg/vehicle_status_flags.msg | 7 ++ src/lib/events/enums.json | 4 + src/modules/commander/Commander.cpp | 102 +++++++++++++----- src/modules/commander/Commander.hpp | 11 +- .../commander/state_machine_helper.cpp | 3 + src/modules/ekf2/EKF/airspeed_fusion.cpp | 2 +- src/modules/ekf2/EKF/common.h | 2 + src/modules/ekf2/EKF/control.cpp | 2 +- src/modules/ekf2/EKF/ekf_helper.cpp | 17 +-- src/modules/ekf2/EKF/estimator_interface.h | 5 - .../beta_fusion_generated_compare.cpp | 2 +- src/modules/ekf2/EKF/sideslip_fusion.cpp | 2 +- src/modules/ekf2/EKF2.cpp | 5 +- 14 files changed, 121 insertions(+), 45 deletions(-) diff --git a/msg/estimator_status_flags.msg b/msg/estimator_status_flags.msg index 3aedeb1002..232f22af1d 100644 --- a/msg/estimator_status_flags.msg +++ b/msg/estimator_status_flags.msg @@ -33,6 +33,8 @@ bool cs_synthetic_mag_z # 25 - true when we are using a synthesized measur 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_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 # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index de0f22b148..3255af1ce0 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -13,12 +13,19 @@ bool condition_local_altitude_valid bool condition_local_position_valid # set to true by the commander app if the quality of the local position estimate is good enough to use for navigation bool condition_local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation bool condition_global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation +bool condition_gps_position_valid bool condition_home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) bool condition_power_input_valid # set if input power is valid bool condition_battery_healthy # set if battery is available and not low bool condition_escs_error # set to true if one or more ESCs reporting esc_status are offline bool condition_escs_failure # set to true if one or more ESCs reporting esc_status has a failure +bool position_reliant_on_gps +bool position_reliant_on_optical_flow +bool position_reliant_on_vision_position + +bool dead_reckoning + bool circuit_breaker_engaged_power_check bool circuit_breaker_engaged_airspd_check bool circuit_breaker_engaged_enginefailure_check diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 8d96384909..1dd476076d 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -110,6 +110,10 @@ "6": { "name": "no_rc_and_no_datalink", "description": "No RC and no datalink" + }, + "7": { + "name": "no_gps", + "description": "No valid GPS" } } }, diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 206477b3fc..9c25b298bd 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -811,6 +811,9 @@ Commander::Commander() : // default for vtol is rotary wing _vtol_status.vtol_in_rw_mode = true; + + _vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME); + _vehicle_gps_position_valid.set_hysteresis_time_from(true, GPS_VALID_TIME); } bool @@ -3983,9 +3986,8 @@ void Commander::estimator_check() _heading_reset_counter = lpos.heading_reset_counter; } - const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)); - const bool gnss_heading_fault_prev = (_estimator_status_sub.get().control_mode_flags & - (1 << estimator_status_s::CS_GPS_YAW_FAULT)); + const bool mag_fault_prev = _estimator_status_flags_sub.get().cs_mag_fault; + const bool gnss_heading_fault_prev = _estimator_status_flags_sub.get().cs_gps_yaw_fault; // use primary estimator_status if (_estimator_selector_status_sub.updated()) { @@ -3994,41 +3996,58 @@ void Commander::estimator_check() if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) { _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance); + _estimator_status_flags_sub.ChangeInstance(estimator_selector_status.primary_instance); } } } - if (_estimator_status_sub.update()) { - const estimator_status_s &estimator_status = _estimator_status_sub.get(); + if (_estimator_status_flags_sub.update()) { + const estimator_status_flags_s &estimator_status_flags = _estimator_status_flags_sub.get(); - // Check for a magnetomer fault and notify the user - const bool mag_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)); - const bool gnss_heading_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS_YAW_FAULT)); + _status_flags.dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning + || estimator_status_flags.cs_inertial_dead_reckoning; - if (!mag_fault_prev && mag_fault) { + if (!(estimator_status_flags.cs_inertial_dead_reckoning || estimator_status_flags.cs_wind_dead_reckoning)) { + // position requirements (update if not dead reckoning) + bool gps = estimator_status_flags.cs_gps; + bool optical_flow = estimator_status_flags.cs_opt_flow; + bool vision_position = estimator_status_flags.cs_ev_pos; + + _status_flags.position_reliant_on_gps = gps && !optical_flow && !vision_position; + _status_flags.position_reliant_on_optical_flow = !gps && optical_flow && !vision_position; + _status_flags.position_reliant_on_vision_position = !gps && !optical_flow && vision_position; + } + + // Check for a magnetomer fault and notify the user + if (!mag_fault_prev && estimator_status_flags.cs_mag_fault) { mavlink_log_critical(&_mavlink_log_pub, "Compass needs calibration - Land now!\t"); events::send(events::ID("commander_stopping_mag_use"), events::Log::Critical, "Stopping compass use! Land now and calibrate the compass"); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, true, true, false, _status); } - if (!gnss_heading_fault_prev && gnss_heading_fault) { + if (!gnss_heading_fault_prev && estimator_status_flags.cs_gps_yaw_fault) { mavlink_log_critical(&_mavlink_log_pub, "GNSS heading not reliable - Land now!\t"); events::send(events::ID("commander_stopping_gnss_heading_use"), events::Log::Critical, "GNSS heading not reliable. Land now!"); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, false, _status); } + } - /* Check estimator status for signs of bad yaw induced post takeoff navigation failure - * for a short time interval after takeoff. - * Most of the time, the drone can recover from a bad initial yaw using GPS-inertial - * heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but - * if this does not fix the issue we need to stop using a position controlled - * mode to prevent flyaway crashes. - */ - if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + /* Check estimator status for signs of bad yaw induced post takeoff navigation failure + * for a short time interval after takeoff. + * Most of the time, the drone can recover from a bad initial yaw using GPS-inertial + * heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but + * if this does not fix the issue we need to stop using a position controlled + * mode to prevent flyaway crashes. + */ + if (_estimator_status_sub.updated() && run_quality_checks + && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + + estimator_status_s estimator_status; + if (_estimator_status_sub.copy(&estimator_status)) { if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { _nav_test_failed = false; _nav_test_passed = false; @@ -4036,9 +4055,10 @@ void Commander::estimator_check() } else { if (!_nav_test_passed) { // Both test ratios need to pass/fail together to change the nav test status - const bool innovation_pass = (estimator_status.vel_test_ratio < 1.0f) && (estimator_status.pos_test_ratio < 1.0f) + const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f) && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON); - const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.0f) && (estimator_status.pos_test_ratio >= 1.0f); + + const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f); if (innovation_pass) { _time_last_innov_pass = hrt_absolute_time(); @@ -4122,13 +4142,47 @@ void Commander::estimator_check() } _status_flags.condition_angular_velocity_valid = condition_angular_velocity_valid; + + + // gps + const bool condition_gps_position_was_valid = _status_flags.condition_gps_position_valid; + + if (_vehicle_gps_position_sub.updated()) { + vehicle_gps_position_s vehicle_gps_position; + + if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) { + + bool time = (vehicle_gps_position.timestamp != 0) && (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 1_s); + + bool fix = vehicle_gps_position.fix_type >= 2; + bool eph = vehicle_gps_position.eph < _param_com_pos_fs_eph.get(); + bool epv = vehicle_gps_position.epv < _param_com_pos_fs_epv.get(); + bool evh = vehicle_gps_position.s_variance_m_s < _param_com_vel_fs_evh.get(); + + _vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh, hrt_absolute_time()); + _status_flags.condition_gps_position_valid = _vehicle_gps_position_valid.get_state(); + + _vehicle_gps_position_timestamp_last = vehicle_gps_position.timestamp; + } + + } else { + const hrt_abstime now_us = hrt_absolute_time(); + + if (now_us > _vehicle_gps_position_timestamp_last + GPS_VALID_TIME) { + _vehicle_gps_position_valid.set_state_and_update(false, now_us); + _status_flags.condition_gps_position_valid = false; + } + } + + if (condition_gps_position_was_valid && !_status_flags.condition_gps_position_valid) { + PX4_WARN("GPS no longer valid"); + } } void Commander::UpdateEstimateValidity() { const vehicle_local_position_s &lpos = _local_position_sub.get(); const vehicle_global_position_s &gpos = _global_position_sub.get(); - const estimator_status_s &status = _estimator_status_sub.get(); float lpos_eph_threshold_adj = _param_com_pos_fs_eph.get(); @@ -4139,11 +4193,7 @@ void Commander::UpdateEstimateValidity() // Set the allowable position uncertainty based on combination of flight and estimator state // When we are in a operator demanded position control mode and are solely reliant on optical flow, do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift - const bool reliant_on_opt_flow = ((status.control_mode_flags & (1 << estimator_status_s::CS_OPT_FLOW)) - && !(status.control_mode_flags & (1 << estimator_status_s::CS_GPS)) - && !(status.control_mode_flags & (1 << estimator_status_s::CS_EV_POS))); - - if (reliant_on_opt_flow) { + if (_status_flags.position_reliant_on_optical_flow) { lpos_eph_threshold_adj = INFINITY; } } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 625826513a..a1385045ba 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -84,6 +85,7 @@ #include #include #include +#include #include #include #include @@ -314,6 +316,10 @@ private: bool _nav_test_passed{false}; /**< true if the post takeoff navigation test has passed */ bool _nav_test_failed{false}; /**< true if the post takeoff navigation test has failed */ + static constexpr hrt_abstime GPS_VALID_TIME{3_s}; + Hysteresis _vehicle_gps_position_valid{false}; + hrt_abstime _vehicle_gps_position_timestamp_last{0}; + bool _geofence_loiter_on{false}; bool _geofence_rtl_on{false}; bool _geofence_land_on{false}; @@ -411,6 +417,7 @@ private: uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; + uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)}; uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; @@ -420,6 +427,7 @@ private: uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; uORB::Subscription _wind_sub{ORB_ID(wind)}; @@ -433,8 +441,7 @@ private: uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)}; #endif // BOARD_HAS_POWER_CONTROL - uORB::SubscriptionData _airspeed_sub{ORB_ID(airspeed)}; - uORB::SubscriptionData _estimator_status_sub{ORB_ID(estimator_status)}; + uORB::SubscriptionData _estimator_status_flags_sub{ORB_ID(estimator_status_flags)}; uORB::SubscriptionData _mission_result_sub{ORB_ID(mission_result)}; uORB::SubscriptionData _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; uORB::SubscriptionData _global_position_sub{ORB_ID(vehicle_global_position)}; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d9f56b2813..78faad6f1c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -59,6 +59,7 @@ static constexpr const char reason_no_local_position[] = "no local position"; static constexpr const char reason_no_global_position[] = "no global position"; static constexpr const char reason_no_datalink[] = "no datalink"; static constexpr const char reason_no_rc_and_no_datalink[] = "no RC and no datalink"; +static constexpr const char reason_no_gps[] = "no GPS"; // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which @@ -476,6 +477,8 @@ static void enable_failsafe(vehicle_status_s &status, bool old_failsafe, orb_adv case event_failsafe_reason_t::no_datalink: reason = reason_no_datalink; break; case event_failsafe_reason_t::no_rc_and_no_datalink: reason = reason_no_rc_and_no_datalink; break; + + case event_failsafe_reason_t::no_gps: reason = reason_no_gps; break; } mavlink_log_critical(mavlink_log_pub, "Failsafe enabled: %s\t", reason); diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 5d0db4d245..d93033693c 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -58,7 +58,7 @@ void Ekf::fuseAirspeed() math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); // determine if we need the airspeed fusion to correct states other than wind - const bool update_wind_only = !_is_wind_dead_reckoning; + const bool update_wind_only = !_control_status.flags.wind_dead_reckoning; // Intermediate variables const float HK0 = vn - vwn; diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index a045e8180e..dc67b7c527 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -491,6 +491,8 @@ union filter_control_status_u { uint32_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest uint32_t gps_yaw_fault : 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used uint32_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used + uint32_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift + uint32_t wind_dead_reckoning : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements } flags; uint32_t value; }; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 962155a02c..db24ce30ff 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -422,7 +422,7 @@ void Ekf::controlOpticalFlowFusion() // Check if we are in-air and require optical flow to control position drift const bool is_flow_required = _control_status.flags.in_air - && (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently + && (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow) || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index b59c47a06c..9023d94076 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -739,11 +739,14 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors // and using state variances for accuracy reporting is overly optimistic in these situations - if (_is_dead_reckoning && (_control_status.flags.gps)) { - hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1)))); + if (_control_status.flags.inertial_dead_reckoning) { + if (_control_status.flags.gps) { + hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1)))); + } - } else if (_is_dead_reckoning && (_control_status.flags.ev_pos)) { - hpos_err = math::max(hpos_err, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1)))); + if (_control_status.flags.ev_pos) { + hpos_err = math::max(hpos_err, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1)))); + } } *ekf_eph = hpos_err; @@ -1017,10 +1020,10 @@ void Ekf::update_deadreckoning_status() isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) && isRecent(_time_last_beta_fuse, _params.no_aid_timeout_max); - _is_wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding; - _is_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding; + _control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding; + _control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding; - if (!_is_dead_reckoning) { + if (!_control_status.flags.inertial_dead_reckoning) { _time_last_aiding = _time_last_imu - _params.no_aid_timeout_max; } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 33f6b711e3..d893d1dd8b 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -180,9 +180,6 @@ public: int getNumberOfActiveHorizontalAidingSources() const; - // return true if the EKF is dead reckoning the position using inertial data only - bool inertial_dead_reckoning() const { return _is_dead_reckoning; } - const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; } // get the velocity of the body frame origin in local NED earth frame @@ -345,9 +342,7 @@ protected: Vector2f _drag_test_ratio{}; // drag innovation consistency check ratio innovation_fault_status_u _innov_check_fail_status{}; - bool _is_dead_reckoning{false}; // true if we are no longer fusing measurements that constrain horizontal velocity drift bool _deadreckon_time_exceeded{true}; // true if the horizontal nav solution has been deadreckoning for too long and is invalid - bool _is_wind_dead_reckoning{false}; // true if we are navigationg reliant on wind relative measurements float _gps_horizontal_position_drift_rate_m_s{0}; // Horizontal position drift rate (m/s) float _gps_vertical_position_drift_rate_m_s{0}; // Vertical position drift rate (m/s) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp index 50940ba03b..9f4cc2f22c 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp @@ -138,7 +138,7 @@ int main() const float HK52 = HK16/_beta_innov_var; // determine if we need the sideslip fusion to correct states other than wind - // bool update_wind_only = !_is_wind_dead_reckoning; + // bool update_wind_only = !_control_status.flags.wind_dead_reckoning; bool update_wind_only = false; // // Calculate predicted sideslip angle and innovation using small angle approximation diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 8179413de0..830efa3872 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -72,7 +72,7 @@ void Ekf::fuseSideslip() const float R_BETA = sq(_params.beta_noise); // observation noise variance // determine if we need the sideslip fusion to correct states other than wind - bool update_wind_only = !_is_wind_dead_reckoning; + bool update_wind_only = !_control_status.flags.wind_dead_reckoning; // Intermediate Values const float HK0 = vn - vwn; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a32e7b0c5e..e0b9093b5b 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -744,7 +744,8 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) global_pos.terrain_alt_valid = false; } - global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning + global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning + || _ekf.control_status_flags().wind_dead_reckoning; global_pos.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _global_position_pub.publish(global_pos); } @@ -1273,6 +1274,8 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) 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_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; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;