dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_pos_control: more accurate position setpoint reset, keep attitude setpoint continuous

This commit is contained in:
Anton Babushkin 2014-08-08 00:24:18 +02:00
parent faaeaeb113
commit 03f839a27a
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -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));
}
}