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

mc_pos_control: do not ignore EKF vz with terrain following

Browse Source 
This caused bad altitude control performance when enabling
terrain following. It even leads to complete vertical control
instability in case dist_bottom is inaccurate.

Relying on the estimator states is the way to go instead of
silently using one altitude source as state.
This commit is contained in:
Matthias Grob 2020-01-11 21:40:23 +01:00 committed by Lorenz Meier
parent bb465ca5b7
commit 4d83170746
1 changed files with 1 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -462,13 +462,7 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
_vel_y_deriv.update(0.0f);
}
if (_param_mpc_alt_mode.get() && _local_pos.dist_bottom_valid && PX4_ISFINITE(_local_pos.dist_bottom_rate)) {
// terrain following
_states.velocity(2) = -_local_pos.dist_bottom_rate;
_states.acceleration(2) = _vel_z_deriv.update(-_states.velocity(2));
} else if (PX4_ISFINITE(_local_pos.vz)) {
if (PX4_ISFINITE(_local_pos.vz)) {
_states.velocity(2) = _local_pos.vz;
if (PX4_ISFINITE(vel_sp_z) && fabsf(vel_sp_z) > FLT_EPSILON && PX4_ISFINITE(_local_pos.z_deriv)) {