From f034d2da197404a3f87fa5c6ba5f328e535fb0d4 Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Fri, 26 Apr 2019 21:33:36 +0200
Subject: [PATCH] ekf2: fixed bug in baro ground effect triggering logic
- when either terrain data was temporarily not valid (flying at high
distance to the ground) or the vehicle was not close to the ground
(_param_ekf2_gnd_max_hgt) the ekf switched to using the land detector
ground effect flag.
Signed-off-by: Roman <bapstroman@gmail.com>
---
src/modules/ekf2/ekf2_main.cpp | 25 +++++++++++++++++++------
1 file changed, 19 insertions(+), 6 deletions(-)
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 47ea7b44a2..8f33b24f38 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -247,6 +247,7 @@ private:
uint8_t _gps_slowest_index = 0; ///< index of the physical receiver with the slowest update rate
float _gps_dt[GPS_MAX_RECEIVERS] = {}; ///< average time step in seconds.
bool _gps_new_output_data = false; ///< true if there is new output data for the EKF
+ bool _had_valid_terrain = false; ///< true if at any time there was a valid terrain estimate
int32_t _gps_alttitude_ellipsoid[GPS_MAX_RECEIVERS] {}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid
uint64_t _gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS] {}; ///< storage for previous timestamp to compute dt
@@ -1414,14 +1415,26 @@ void Ekf2::run()
lpos.dist_bottom = _param_ekf2_min_rng.get();
}
- // update ground effect flag based on terrain estimation
- if (lpos.dist_bottom_valid && lpos.dist_bottom < _param_ekf2_gnd_max_hgt.get()) {
- _ekf.set_gnd_effect_flag(true);
+ if (!_had_valid_terrain) {
+ _had_valid_terrain = lpos.dist_bottom_valid;
}
- // update ground effect flag based on land detector state
- else if (vehicle_land_detected_updated && _param_ekf2_gnd_eff_dz.get() > 0.0f) {
- _ekf.set_gnd_effect_flag(vehicle_land_detected.in_ground_effect);
+ if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+ // set ground effect flag if vehicle is closer than a specified distance to the ground
+ if (lpos.dist_bottom_valid) {
+ _ekf.set_gnd_effect_flag(lpos.dist_bottom < _param_ekf2_gnd_max_hgt.get());
+
+ // if we have no valid terrain estimate and never had one then use ground effect flag from land detector
+ // _had_valid_terrain is used to make sure that we don't fall back to using this option
+ // if we temporarily lose terrain data due to the distance sensor getting out of range
+
+ } else if (vehicle_land_detected_updated && _param_ekf2_gnd_eff_dz.get() > 0.0f && !_had_valid_terrain) {
+ // update ground effect flag based on land detector state
+ _ekf.set_gnd_effect_flag(vehicle_land_detected.in_ground_effect);
+ }
+
+ } else {
+ _ekf.set_gnd_effect_flag(false);
}
lpos.dist_bottom_rate = -lpos.vz; // Distance to bottom surface (ground) change rate
--
GitLab