dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MC pos control: use vz<->z_deriv blending in pure vel mode only 

This logic was introduced to avoid large altitude drifts in velocity
mode (altitude not controlled) due to potential velocity estimate bias
in case of IMU problems (see PR #8445). After some refactorings,
the logic started to be used in altitude-controlled modes as well, which is not needed.
This commit is contained in:
bresch 2021-06-07 16:49:37 +02:00 committed by Matthias Grob
parent 47b9016f5e
commit 841914462d
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@ -404,9 +404,11 @@ void MulticopterPositionControl::Run()
_control.setInputSetpoint(_setpoint);
// update states
if (PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON)
if (!PX4_ISFINITE(_setpoint.z)
&& PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON)
&& PX4_ISFINITE(local_pos.z_deriv) && local_pos.z_valid && local_pos.v_z_valid) {
// A change in velocity is demanded. Set velocity to the derivative of position
// A change in velocity is demanded and the altitude is not controlled.
// Set velocity to the derivative of position
// because it has less bias but blend it in across the landing speed range
// < MPC_LAND_SPEED: ramp up using altitude derivative without a step
// >= MPC_LAND_SPEED: use altitude derivative