From aa716936bf6d479b62e329f02a5f3ec7ae2f480e Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 8 Sep 2022 14:41:50 +0200 Subject: [PATCH] ekf2: move synthetic_position flag to control_status.flags.fake_pos --- msg/estimator_status.msg | 2 +- msg/estimator_status_flags.msg | 1 + src/modules/ekf2/EKF/common.h | 67 ++++++++++++----------- src/modules/ekf2/EKF/control.cpp | 8 +-- src/modules/ekf2/EKF/ekf.h | 3 +- src/modules/ekf2/EKF/fake_pos_control.cpp | 10 ++-- src/modules/ekf2/EKF2.cpp | 1 + src/modules/ekf2/EKF2.hpp | 2 +- 8 files changed, 48 insertions(+), 46 deletions(-) diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index efbb3ed43c..55c996621e 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -16,7 +16,7 @@ uint8 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7 # 7 : maximum allowed vertical position uint8 GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR = 8 # 8 : maximum allowed horizontal speed fail - requires stationary vehicle uint8 GPS_CHECK_FAIL_MAX_VERT_SPD_ERR = 9 # 9 : maximum allowed vertical velocity discrepancy fail -uint32 control_mode_flags # Bitmask to indicate EKF logic state +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 diff --git a/msg/estimator_status_flags.msg b/msg/estimator_status_flags.msg index 0e59adaa75..d2b52cfcb2 100644 --- a/msg/estimator_status_flags.msg +++ b/msg/estimator_status_flags.msg @@ -36,6 +36,7 @@ bool cs_rng_fault # 28 - true when the range finder has been declare 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 bool cs_rng_kin_consistent # 31 - true when the range finder kinematic consistency check is passing +bool cs_fake_pos # 32 - true when fake position measurements are being fused # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index d3f5da12b0..478594e595 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -500,40 +500,41 @@ union gps_check_fail_status_u { // bitmask containing filter control status union filter_control_status_u { struct { - uint32_t tilt_align : 1; ///< 0 - true if the filter tilt alignment is complete - uint32_t yaw_align : 1; ///< 1 - true if the filter yaw alignment is complete - uint32_t gps : 1; ///< 2 - true if GPS measurement fusion is intended - uint32_t opt_flow : 1; ///< 3 - true if optical flow measurements fusion is intended - uint32_t mag_hdg : 1; ///< 4 - true if a simple magnetic yaw heading fusion is intended - uint32_t mag_3D : 1; ///< 5 - true if 3-axis magnetometer measurement fusion is intended - uint32_t mag_dec : 1; ///< 6 - true if synthetic magnetic declination measurements fusion is intended - uint32_t in_air : 1; ///< 7 - true when the vehicle is airborne - uint32_t wind : 1; ///< 8 - true when wind velocity is being estimated - uint32_t baro_hgt : 1; ///< 9 - true when baro height is being fused as a primary height reference - uint32_t rng_hgt : 1; ///< 10 - true when range finder height is being fused as a primary height reference - uint32_t gps_hgt : 1; ///< 11 - true when GPS height is being fused as a primary height reference - uint32_t ev_pos : 1; ///< 12 - true when local position data fusion from external vision is intended - uint32_t ev_yaw : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended - uint32_t ev_hgt : 1; ///< 14 - true when height data from external vision measurements is being fused - uint32_t fuse_beta : 1; ///< 15 - true when synthetic sideslip measurements are being fused - uint32_t mag_field_disturbed : 1; ///< 16 - true when the mag field does not match the expected strength - uint32_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle - uint32_t mag_fault : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used - uint32_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused - uint32_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active - uint32_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough - uint32_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended - uint32_t mag_aligned_in_flight : 1; ///< 23 - true when the in-flight mag field alignment has been completed - uint32_t ev_vel : 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended - uint32_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component - 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 - uint32_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing + 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 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 + uint64_t mag_dec : 1; ///< 6 - true if synthetic magnetic declination measurements fusion is intended + uint64_t in_air : 1; ///< 7 - true when the vehicle is airborne + uint64_t wind : 1; ///< 8 - true when wind velocity is being estimated + uint64_t baro_hgt : 1; ///< 9 - true when baro height is being fused as a primary height reference + uint64_t rng_hgt : 1; ///< 10 - true when range finder height is being fused as a primary height reference + uint64_t gps_hgt : 1; ///< 11 - true when GPS height is being fused as a primary height reference + uint64_t ev_pos : 1; ///< 12 - true when local position data fusion from external vision is intended + uint64_t ev_yaw : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended + uint64_t ev_hgt : 1; ///< 14 - true when height data from external vision measurements is being fused + uint64_t fuse_beta : 1; ///< 15 - true when synthetic sideslip measurements are being fused + uint64_t mag_field_disturbed : 1; ///< 16 - true when the mag field does not match the expected strength + uint64_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle + uint64_t mag_fault : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used + uint64_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused + 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 : 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 : 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended + 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 : 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 + uint64_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift + uint64_t wind_dead_reckoning : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements + uint64_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing + uint64_t fake_pos : 1; ///< 32 - true when fake position measurements are being fused } flags; - uint32_t value; + uint64_t value; }; // Mavlink bitmask containing state of estimator solution diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index a4a37d5fdc..d11f40a7d4 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -659,7 +659,7 @@ void Ekf::controlAirDataFusion() const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6); const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6); - if (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & SensorFusionMask::USE_DRAG))) { + if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & SensorFusionMask::USE_DRAG))) { _control_status.flags.wind = false; } @@ -673,7 +673,7 @@ void Ekf::controlAirDataFusion() _innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag - const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_using_synthetic_position; + const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_control_status.flags.fake_pos; const bool is_airspeed_significant = _airspeed_sample_delayed.true_airspeed > _params.arsp_thr; const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f); const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant @@ -707,7 +707,7 @@ void Ekf::controlAirDataFusion() void Ekf::controlBetaFusion() { - if (_using_synthetic_position) { + if (_control_status.flags.fake_pos) { return; } @@ -733,7 +733,7 @@ void Ekf::controlBetaFusion() void Ekf::controlDragFusion() { if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer && - !_using_synthetic_position && _control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) { + !_control_status.flags.fake_pos && _control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) { if (!_control_status.flags.wind) { // reset the wind states and covariances when starting drag accel fusion diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 84c335cc19..87e9065785 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -272,7 +272,7 @@ public: // return true if the local position estimate is valid bool local_position_is_valid() const { - return (!_deadreckon_time_exceeded && !_using_synthetic_position); + return (!_deadreckon_time_exceeded && !_control_status.flags.fake_pos); } bool isTerrainEstimateValid() const { return _hagl_valid; }; @@ -465,7 +465,6 @@ private: uint64_t _time_prev_gps_us{0}; ///< time stamp of previous GPS data retrieved from the buffer (uSec) uint64_t _time_last_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) - bool _using_synthetic_position{false}; ///< true if we are using a synthetic position to constrain drift uint64_t _time_last_hor_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec) uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of vertical position measurements was performed (uSec) diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/fake_pos_control.cpp index a59c006fdb..ca70c2fd99 100644 --- a/src/modules/ekf2/EKF/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/fake_pos_control.cpp @@ -53,7 +53,7 @@ void Ekf::controlFakePosFusion() const bool continuing_conditions_passing = !isHorizontalAidingActive(); const bool starting_conditions_passing = continuing_conditions_passing; - if (_using_synthetic_position) { + if (_control_status.flags.fake_pos) { if (continuing_conditions_passing) { fuseFakePosition(); @@ -83,9 +83,9 @@ void Ekf::controlFakePosFusion() void Ekf::startFakePosFusion() { - if (!_using_synthetic_position) { + if (!_control_status.flags.fake_pos) { ECL_INFO("start fake position fusion"); - _using_synthetic_position = true; + _control_status.flags.fake_pos = true; _fuse_hpos_as_odom = false; // TODO: needed? resetFakePosFusion(); } @@ -105,9 +105,9 @@ void Ekf::resetFakePosFusion() void Ekf::stopFakePosFusion() { - if (_using_synthetic_position) { + if (_control_status.flags.fake_pos) { ECL_INFO("stop fake position fusion"); - _using_synthetic_position = false; + _control_status.flags.fake_pos = false; resetEstimatorAidStatus(_aid_src_fake_pos); } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 60a45e9deb..08e67fa24c 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1428,6 +1428,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) 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.cs_rng_kin_consistent = _ekf.control_status_flags().rng_kin_consistent; + status_flags.cs_fake_pos = _ekf.control_status_flags().fake_pos; 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/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index c80831820c..239d033adb 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -314,7 +314,7 @@ private: hrt_abstime _last_event_flags_publish{0}; hrt_abstime _last_status_flags_publish{0}; - uint32_t _filter_control_status{0}; + uint64_t _filter_control_status{0}; uint32_t _filter_fault_status{0}; uint32_t _innov_check_fail_status{0};