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: fix derivative spike when regaining velocity estimate

Browse Source 
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 b64abf48b211b66fbfc7a3b900adbbbdf0e2e14c
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();
}
}