From b1773df441f2de7a3ca68dc4492638046e41ba2d Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 7 Jan 2025 09:53:24 +0100 Subject: [PATCH] ekf2: fix comments about primary height sources Having the xxx_hgt flag set only means that the sensor is fused for altitude/height aiding. It's not necessarily the height reference. --- msg/EstimatorStatus.msg | 6 +++--- msg/EstimatorStatusFlags.msg | 6 +++--- .../EKF/aid_sources/range_finder/range_height_control.cpp | 2 +- src/modules/ekf2/EKF/common.h | 6 +++--- src/modules/ekf2/EKF/ekf_helper.cpp | 1 - 5 files changed, 10 insertions(+), 11 deletions(-) diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index dd62bc4aca3f..3c7985e630bb 100644 --- a/msg/EstimatorStatus.msg +++ b/msg/EstimatorStatus.msg @@ -27,9 +27,9 @@ uint8 CS_MAG_3D = 5 # 5 - true if 3-axis magnetometer measurement are being fus uint8 CS_MAG_DEC = 6 # 6 - true if synthetic magnetic declination measurements are being fused uint8 CS_IN_AIR = 7 # 7 - true when thought to be airborne uint8 CS_WIND = 8 # 8 - true when wind velocity is being estimated -uint8 CS_BARO_HGT = 9 # 9 - true when baro height is being fused as a primary height reference -uint8 CS_RNG_HGT = 10 # 10 - true when range finder height is being fused as a primary height reference -uint8 CS_GPS_HGT = 11 # 11 - true when GPS height is being fused as a primary height reference +uint8 CS_BARO_HGT = 9 # 9 - true when baro data is being fused +uint8 CS_RNG_HGT = 10 # 10 - true when range finder data is being fused for height aiding +uint8 CS_GPS_HGT = 11 # 11 - true when GPS altitude is being fused uint8 CS_EV_POS = 12 # 12 - true when local position data from external vision is being fused uint8 CS_EV_YAW = 13 # 13 - true when yaw data from external vision measurements is being fused uint8 CS_EV_HGT = 14 # 14 - true when height data from external vision measurements is being fused diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 6b17842c2650..705a5f9a71fd 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -13,9 +13,9 @@ bool cs_mag_3d # 5 - true if 3-axis magnetometer measurement fus bool cs_mag_dec # 6 - true if synthetic magnetic declination measurements fusion is intended bool cs_in_air # 7 - true when the vehicle is airborne bool cs_wind # 8 - true when wind velocity is being estimated -bool cs_baro_hgt # 9 - true when baro height is being fused as a primary height reference -bool cs_rng_hgt # 10 - true when range finder height is being fused as a primary height reference -bool cs_gps_hgt # 11 - true when GPS height is being fused as a primary height reference +bool cs_baro_hgt # 9 - true when baro data is being fused +bool cs_rng_hgt # 10 - true when range finder data is being fused for height aiding +bool cs_gps_hgt # 11 - true when GPS altitude is being fused bool cs_ev_pos # 12 - true when local position data fusion from external vision is intended bool cs_ev_yaw # 13 - true when yaw data from external vision measurements fusion is intended bool cs_ev_hgt # 14 - true when height data from external vision measurements is being fused diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 6ab803656076..e6cd566a3fef 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -78,7 +78,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } } else { - // If we are supposed to be using range finder data as the primary height sensor, have bad range measurements + // If we are supposed to be using range finder data but have bad range measurements // and are on the ground, then synthesise a measurement at the expected on ground value if (!_control_status.flags.in_air && _range_sensor.isRegularlySendingData() diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 1589047e818d..85d075e0f5cf 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -573,10 +573,10 @@ union filter_control_status_u { 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 baro_hgt : 1; ///< 9 - true when baro data is being fused 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 + 1; ///< 10 - true when range finder data is being fused for height aiding + uint64_t gps_hgt : 1; ///< 11 - true when GPS altitude is being fused 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 diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index a6872b40d257..664613a2b659 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -345,7 +345,6 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl // TODO : calculate visual odometry limits const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt); - // Keep within range sensor limit when using rangefinder as primary height source if (relying_on_rangefinder) { *hagl_min = rangefinder_hagl_min; *hagl_max = rangefinder_hagl_max;