fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 23:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: use current velocity to calculate position setpoint on reset to make transition to stabilized modes more smooth

Browse Source
This commit is contained in:
Anton Babushkin
2014-05-01 20:57:36 +02:00
parent d0a6fcf3d0
commit f4c36f8c5c
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+3 -3
View File
@@ -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));
}
}