From 573dd89cbfad6e389d7a10db20d4ac8a6ede682e Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
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 8e4c4f2f2d..c1c4bc62dc 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 fd95b7b3a1..8504ccb6da 100644
--- a/src/modules/mc_pos_control/PositionControl.hpp
+++ b/src/modules/mc_pos_control/PositionControl.hpp
@@ -204,8 +204,7 @@ private:
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 */
--
GitLab