diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-05-01 20:57:36 +0200 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-05-01 20:57:36 +0200 |
| commit | f4c36f8c5cf69bd3af55a1065ab7a18c999c054c (patch) | |
| tree | 22351d29950f75096a5e7e73be47b509e98ad386 /src/modules/mc_pos_control | |
| parent | d0a6fcf3d007fc8b92160cb341a566224cad5276 (diff) | |
| download | px4-firmware-f4c36f8c5cf69bd3af55a1065ab7a18c999c054c.tar.gz px4-firmware-f4c36f8c5cf69bd3af55a1065ab7a18c999c054c.tar.bz2 px4-firmware-f4c36f8c5cf69bd3af55a1065ab7a18c999c054c.zip | |
mc_pos_control: use current velocity to calculate position setpoint on reset to make transition to stabilized modes more smooth
Diffstat (limited to 'src/modules/mc_pos_control')
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 |
1 files changed, 3 insertions, 3 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 7c625a0c5..fe0e8fe1a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -493,8 +493,8 @@ MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); + _pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0); + _pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -504,7 +504,7 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2); + _pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } |