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

ekf2: consolidate and simplify gnd effect logic

Browse Source
This commit is contained in:
Daniel Agar
2021-12-16 14:58:46 -05:00
parent 1d7791dad6
commit d6d529539d
9 changed files with 609 additions and 612 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+15 -26
View File
@@ -515,25 +515,28 @@ void EKF2::Run()
vehicle_land_detected_s vehicle_land_detected;
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
const bool was_in_air = _ekf.control_status_flags().in_air;
_ekf.set_in_air_status(!vehicle_land_detected.landed);
_ekf.set_vehicle_at_rest(vehicle_land_detected.at_rest);
if (_armed && (_param_ekf2_gnd_eff_dz.get() > 0.f)) {
if (!_had_valid_terrain) {
// update ground effect flag based on land detector state if we've never had valid terrain data
_ekf.set_gnd_effect_flag(vehicle_land_detected.in_ground_effect);
}
} else {
_ekf.set_gnd_effect_flag(false);
if (vehicle_land_detected.in_ground_effect) {
_ekf.set_gnd_effect();
}
// reset learned sensor calibrations on takeoff
if (_ekf.control_status_flags().in_air && !was_in_air) {
const bool was_in_air = _ekf.control_status_flags().in_air;
const bool in_air = !vehicle_land_detected.landed;
if (!was_in_air && in_air) {
// takeoff
_ekf.set_in_air_status(true);
// reset learned sensor calibrations on takeoff
_accel_cal = {};
_gyro_cal = {};
_mag_cal = {};
} else if (was_in_air && !in_air) {
// landed
_ekf.set_in_air_status(false);
}
}
}
@@ -934,20 +937,6 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield();
if (!_had_valid_terrain) {
_had_valid_terrain = lpos.dist_bottom_valid;
}
// only consider ground effect if compensation is configured and the vehicle is armed (props spinning)
if ((_param_ekf2_gnd_eff_dz.get() > 0.0f) && _armed && lpos.dist_bottom_valid) {
// set ground effect flag if vehicle is closer than a specified distance to the ground
_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
}
_ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);
_ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);