From 2d2b0a2d43d336b7f7067d282f0dd2b54d8e91ce Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 2 Mar 2016 15:41:00 +0100 Subject: [PATCH] mc pos: in auto mode reset the position to current position. Velocity controller ensures continuous attitude setpoint --- .../mc_pos_control/mc_pos_control_main.cpp | 25 ++++++++----------- 1 file changed, 11 insertions(+), 14 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 262f3fa566..00b2d9a25f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -706,14 +706,12 @@ MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; - if (_mode_auto) { - // in auto mode we are controlling position directly and we need to reset it such that the attitude setpoint is continuous - _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0)) / _params.pos_p(0); - _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1)) / _params.pos_p(1); - } else { - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - } + + // we have logic in the main function which chooses the velocity setpoint such that the attitude setpoint is + // continuous when switching into velocity controlled mode, therefore, we don't need to bother about resetting + // position in a special way. In position control mode the position will be reset anyway until the vehicle has reduced speed. + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); } } @@ -721,13 +719,12 @@ void MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { - if (_mode_auto) { - // in auto mode we are controlling altitude directly and we need to reset it such that the attitude setpoint is continuous - _pos_sp(2) = _pos(2) + (_vel(2)) / _params.pos_p(2); - } else { - _pos_sp(2) = _pos(2); - } _reset_alt_sp = false; + + // we have logic in the main function which choosed the velocity setpoint such that the attitude setpoint is + // continuous when switching into velocity controlled mode, therefore, we don't need to bother about resetting + // altitude in a special way + _pos_sp(2) = _pos(2); } }