diff options
| author | Roman Bapst <romanbapst@yahoo.de> | 2015-05-07 15:04:59 +0200 |
|---|---|---|
| committer | Roman Bapst <romanbapst@yahoo.de> | 2015-05-07 15:04:59 +0200 |
| commit | 1d544e028d6041130bd673b69d74e04ea9357547 (patch) | |
| tree | c5a80fb5ca12120406ea53590fdede1b964babbe | |
| parent | 5003875911b893884deeb97b97191016597c75b7 (diff) | |
| download | px4-firmware-1d544e028d6041130bd673b69d74e04ea9357547.tar.gz px4-firmware-1d544e028d6041130bd673b69d74e04ea9357547.tar.bz2 px4-firmware-1d544e028d6041130bd673b69d74e04ea9357547.zip | |
mc_pos_control: always update previous velocity to avoid spikes due to differentiation
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 |
1 files changed, 3 insertions, 1 deletions
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 |