diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-08-08 00:24:18 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-08-08 00:24:18 +0200 |
commit | 03f839a27ac61178be8ef77526faa13a46c4cd6b (patch) | |
tree | 7b904fb3f34817fd389ac62dc0a402241314ea65 /src | |
parent | faaeaeb11333598abd17db2189a2bc47216d0a98 (diff) | |
download | px4-firmware-03f839a27ac61178be8ef77526faa13a46c4cd6b.tar.gz px4-firmware-03f839a27ac61178be8ef77526faa13a46c4cd6b.tar.bz2 px4-firmware-03f839a27ac61178be8ef77526faa13a46c4cd6b.zip |
mc_pos_control: more accurate position setpoint reset, keep attitude setpoint continuous
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 |
1 files changed, 6 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 a0837a2dd..0106af80a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -514,8 +514,11 @@ MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; - _pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0) * (1.0f - _params.vel_ff(0)); - _pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1) * (1.0f - _params.vel_ff(1)); + /* shift position setpoint to make attitude setpoint continuous */ + _pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0) + - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) + - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -525,7 +528,7 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2) * (1.0f - _params.vel_ff(2)); + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } |