From 1f0d559d65b8516f8c20b47916aafe9096df274f Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 26 Sep 2018 11:47:45 +0200 Subject: [PATCH] PositionControl: check and set _skip_controller flag at the end of the setpoint update --- src/modules/mc_pos_control/PositionControl.cpp | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index d46d94b5aa..17577421a6 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -56,10 +56,6 @@ void PositionControl::updateState(const PositionControlStates &states) bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint) { - // If full manual is required (thrust already generated), don't run position/velocity - // controller and just return thrust. - _skip_controller = false; - _pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z); _vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz); _acc_sp = Vector3f(setpoint.acc_x, setpoint.acc_y, setpoint.acc_z); @@ -68,9 +64,10 @@ bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se _yawspeed_sp = setpoint.yawspeed; bool mapping_succeeded = _interfaceMapping(); - if (PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]) && PX4_ISFINITE(setpoint.thrust[2])) { - _skip_controller = true; - } + // If full manual is required (thrust already generated), don't run position/velocity + // controller and just return thrust. + _skip_controller = PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]) + && PX4_ISFINITE(setpoint.thrust[2]); return mapping_succeeded; } @@ -203,8 +200,6 @@ bool PositionControl::_interfaceMapping() // check failsafe if (failsafe) { - _skip_controller = true; - // point the thrust upwards _thr_sp(0) = _thr_sp(1) = 0.0f; // throttle down such that vehicle goes down with