From 1d544e028d6041130bd673b69d74e04ea9357547 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 7 May 2015 15:04:59 +0200 Subject: mc_pos_control: always update previous velocity to avoid spikes due to differentiation --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 0c8b040c1..e5ea4d104 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1102,7 +1102,6 @@ MulticopterPositionControl::task_main() /* derivative of velocity error, not includes setpoint acceleration */ math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; - _vel_prev = _vel; /* thrust vector in NED frame */ math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; @@ -1406,6 +1405,9 @@ MulticopterPositionControl::task_main() reset_yaw_sp = true; } + /* update previous velocity for velocity controller D part */ + _vel_prev = _vel; + /* publish attitude setpoint * Do not publish if offboard is enabled but position/velocity control is disabled, * in this case the attitude setpoint is published by the mavlink app -- cgit v1.2.3