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

mc_pos_control: fix derivative spike when regaining velocity estimate 

When having no velocity estimate the derivative was updated with zero.
When losing the velocity estimate this is fine since the resulting
derivative spike doesn't get used and acceleration is set to NAN.
But when regaining the velocity estimate the spike from zero to
the first estimated velocity gets used as acceleration in the position
controller and results in a twitch.

To solve this I use the derivative reset I introduced in pr #13522
commit b64abf48b2
This commit is contained in:
Matthias Grob 2020-01-11 22:02:34 +01:00 committed by Lorenz Meier
parent a6cc972b5c
commit 1e06f6bbd2
1 changed files with 5 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -456,10 +456,9 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
} else {
_states.velocity(0) = _states.velocity(1) = NAN;
_states.acceleration(0) = _states.acceleration(1) = NAN;
// since no valid velocity, update derivate with 0
_vel_x_deriv.update(0.0f);
_vel_y_deriv.update(0.0f);
// reset derivatives to prevent acceleration spikes when regaining velocity
_vel_x_deriv.reset();
_vel_y_deriv.reset();
}
if (PX4_ISFINITE(_local_pos.vz) && _local_pos.v_z_valid) {
@ -476,9 +475,8 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
} else {
_states.velocity(2) = _states.acceleration(2) = NAN;
// since no valid velocity, update derivate with 0
_vel_z_deriv.update(0.0f);
// reset derivative to prevent acceleration spikes when regaining velocity
_vel_z_deriv.reset();
}
}