diff options
author | Lorenz Meier <lorenz@px4.io> | 2015-05-13 13:58:15 +0200 |
---|---|---|
committer | Lorenz Meier <lorenz@px4.io> | 2015-05-13 13:58:15 +0200 |
commit | 164a254214ac37145e48df4b44606755e1b24ef7 (patch) | |
tree | b504240b43f7c80bbe3496353cd9a67495834cfd | |
parent | b3ed0cf36b985771c50108016277f2148c8b000f (diff) | |
parent | 1d544e028d6041130bd673b69d74e04ea9357547 (diff) | |
download | px4-firmware-164a254214ac37145e48df4b44606755e1b24ef7.tar.gz px4-firmware-164a254214ac37145e48df4b44606755e1b24ef7.tar.bz2 px4-firmware-164a254214ac37145e48df4b44606755e1b24ef7.zip |
Merge pull request #2143 from PX4/mc_pos_D_reset
mc_pos_control: always update previous velocity
-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 |