fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: fixed bug in baro ground effect triggering logic

Browse Source 
- 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>
This commit is contained in:
Roman 2019-04-26 21:33:36 +02:00 committed by Roman Bapst
parent 9bad61b86b
commit f034d2da19
1 changed files with 19 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/modules/ekf2/ekf2_main.cpp
View File

@ -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