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

EKF: Protect against ground effect induced static pressure rise during landing and takeoff.

Browse Source 
Apply a dead-zone to the vertical position innovation if using baro for height and if in the ground effect region during and just after takeoff.
Method needs to be activated externally.
Turns off automatically after 10 seconds or if specified height gained.
This commit is contained in:
Paul Riseborough
2017-11-17 21:41:31 +11:00
parent c8d459eecd
commit a2b24fa960
4 changed files with 44 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+14
View File
@@ -102,6 +102,20 @@ void Ekf::fuseVelPosHeight()
// innovation gate size
gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
// Compensate for positive static pressure transients (negative vertical position innovations)
// casued by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
float deadzone_start = 0.25f * _params.baro_noise;
float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
if (_control_status.flags.gnd_effect) {
if (_vel_pos_innov[5] < -deadzone_start) {
if (_vel_pos_innov[5] <= -deadzone_end) {
_vel_pos_innov[5] += deadzone_end;
} else {
_vel_pos_innov[5] = -deadzone_start;
}
}
}
} else if (_control_status.flags.gps_hgt) {
fuse_map[5] = true;
// vertical position innovation - gps measurement has opposite sign to earth z axis