From 573dd89cbfad6e389d7a10db20d4ac8a6ede682e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 8 Jan 2019 16:05:43 +0100 Subject: [PATCH] PositionControl: addess @bkueng 's comment in #11056 --- .../mc_pos_control/PositionControl.cpp | 19 ++++++------------- .../mc_pos_control/PositionControl.hpp | 3 +-- 2 files changed, 7 insertions(+), 15 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index 8e4c4f2f2d9c..c1c4bc62dc13 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -55,24 +55,17 @@ void PositionControl::updateState(const PositionControlStates &states) _vel_dot = states.acceleration; } -void PositionControl::_setCtrlFlagTrue() +void PositionControl::_setCtrlFlag(bool value) { for (int i = 0; i <= 2; i++) { - _ctrl_pos[i] = _ctrl_vel[i] = true; - } -} - -void PositionControl::_setCtrlFlagFalse() -{ - for (int i = 0; i <= 2; i++) { - _ctrl_pos[i] = _ctrl_vel[i] = false; + _ctrl_pos[i] = _ctrl_vel[i] = value; } } bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint) { - // Only for logging purpose: by default we use the entire position-velocity control-loop pipeline - _setCtrlFlagTrue(); + // by default we use the entire position-velocity control-loop pipeline (flag only for logging purpose) + _setCtrlFlag(true); _pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z); _vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz); @@ -212,8 +205,8 @@ bool PositionControl::_interfaceMapping() // throttle down such that vehicle goes down with // 70% of throttle range between min and hover _thr_sp(2) = -(MPC_THR_MIN.get() + (MPC_THR_HOVER.get() - MPC_THR_MIN.get()) * 0.7f); - // position and velocity control-loop is not used (note: only for logging purpose) - _setCtrlFlagFalse(); // position/velocity control-loop is not used + // position and velocity control-loop is currently unused (flag only for logging purpose) + _setCtrlFlag(false); } return !(failsafe); diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl.hpp index fd95b7b3a1af..8504ccb6dac3 100644 --- a/src/modules/mc_pos_control/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl.hpp @@ -204,8 +204,7 @@ class PositionControl : public ModuleParams void _positionController(); /** applies the P-position-controller */ void _velocityController(const float &dt); /** applies the PID-velocity-controller */ - void _setCtrlFlagTrue(); /**< set control-loop flags to true (only required for logging) */ - void _setCtrlFlagFalse(); /**< set control-loop flags to false (only required for logging) */ + void _setCtrlFlag(bool value); /**< set control-loop flags (only required for logging) */ matrix::Vector3f _pos{}; /**< MC position */ matrix::Vector3f _vel{}; /**< MC velocity */