Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 18 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+1 -7
View File
@@ -462,13 +462,7 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
_vel_y_deriv.update(0.0f); _vel_y_deriv.update(0.0f);
} }
if (_param_mpc_alt_mode.get() && _local_pos.dist_bottom_valid && PX4_ISFINITE(_local_pos.dist_bottom_rate)) { if (PX4_ISFINITE(_local_pos.vz)) {
// 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)) {
_states.velocity(2) = _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)) { if (PX4_ISFINITE(vel_sp_z) && fabsf(vel_sp_z) > FLT_EPSILON && PX4_ISFINITE(_local_pos.z_deriv)) {