PositionControl: remove complicated internal control flags

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -71,18 +71,8 @@ void PositionControl::setState(const PositionControlStates &states)
 	_vel_dot = states.acceleration;
 }
 
-void PositionControl::_setCtrlFlag(bool value)
-{
-	for (int i = 0; i <= 2; i++) {
-		_ctrl_pos[i] = _ctrl_vel[i] = value;
-	}
-}
-
 bool PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint)
 {
-	// 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);
 	_acc_sp = Vector3f(setpoint.acceleration);
@@ -180,7 +170,6 @@ bool PositionControl::_interfaceMapping()
 			// Velocity controller is active without position control.
 			// Set integral states and setpoints to 0
 			_pos_sp(i) = _pos(i) = 0.0f;
-			_ctrl_pos[i] = false; // position control-loop is not used
 
 			// thrust setpoint is not supported in velocity control
 			_thr_sp(i) = NAN;
@@ -196,7 +185,6 @@ bool PositionControl::_interfaceMapping()
 			// Set all integral states and setpoints to 0
 			_pos_sp(i) = _pos(i) = 0.0f;
 			_vel_sp(i) = _vel(i) = 0.0f;
-			_ctrl_pos[i] = _ctrl_vel[i] = false; // position/velocity control loop is not used
 
 			// Reset the Integral term.
 			_vel_int(i) = 0.0f;
@@ -243,7 +231,6 @@ bool PositionControl::_interfaceMapping()
 		// 70% of throttle range between min and hover
 		_thr_sp(2) = -(_lim_thr_min + (_hover_thrust - _lim_thr_min) * 0.7f);
 		// position and velocity control-loop is currently unused (flag only for logging purpose)
-		_setCtrlFlag(false);
 	}
 
 	return !(failsafe);

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -158,27 +158,6 @@ public:
 	 */
 	void resetIntegral() { _vel_int.setZero(); }
 
-	/**
-	 * 	Get the
-	 * 	@see _vel_sp
-	 * 	@return The velocity set-point that was executed in the control-loop. Nan if velocity control-loop was skipped.
-	 */
-	const matrix::Vector3f getVelSp() const
-	{
-		matrix::Vector3f vel_sp{};
-
-		for (int i = 0; i <= 2; i++) {
-			if (_ctrl_vel[i]) {
-				vel_sp(i) = _vel_sp(i);
-
-			} else {
-				vel_sp(i) = NAN;
-			}
-		}
-
-		return vel_sp;
-	}
-
 	/**
 	 * Get the controllers output local position setpoint
 	 * These setpoints are the ones which were executed on including PID output and feed-forward.
@@ -204,7 +183,6 @@ private:
 
 	void _positionControl(); ///< Position proportional control
 	void _velocityControl(const float dt); ///< Velocity PID control
-	void _setCtrlFlag(bool value); /**< set control-loop flags (only required for logging) */
 
 	// Gains
 	matrix::Vector3f _gain_pos_p; ///< Position control proportional gain
@@ -240,6 +218,4 @@ private:
 	float _yawspeed_sp{}; /** desired yaw-speed */
 
 	bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */
-	bool _ctrl_pos[3] = {true, true, true}; /**< True if the control-loop for position was used */
-	bool _ctrl_vel[3] = {true, true, true}; /**< True if the control-loop for velocity was used */
 };

src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp

@@ -133,7 +133,7 @@ public:
 	}
 };
 
-TEST_F(PositionControlBasicDirectionTest, PositionControlDirectionP)
+TEST_F(PositionControlBasicDirectionTest, PositionDirection)
 {
 	_input_setpoint.x = .1f;
 	_input_setpoint.y = .1f;
@@ -142,7 +142,7 @@ TEST_F(PositionControlBasicDirectionTest, PositionControlDirectionP)
 	checkDirection();
 }
 
-TEST_F(PositionControlBasicDirectionTest, VelocityControlDirection)
+TEST_F(PositionControlBasicDirectionTest, VelocityDirection)
 {
 	_input_setpoint.vx = .1f;
 	_input_setpoint.vy = .1f;
@@ -151,7 +151,7 @@ TEST_F(PositionControlBasicDirectionTest, VelocityControlDirection)
 	checkDirection();
 }
 
-TEST_F(PositionControlBasicTest, PositionControlMaxTilt)
+TEST_F(PositionControlBasicTest, TiltLimit)
 {
 	_input_setpoint.x = 10.f;
 	_input_setpoint.y = 10.f;
@@ -171,7 +171,7 @@ TEST_F(PositionControlBasicTest, PositionControlMaxTilt)
 	EXPECT_LE(angle, .50001f);
 }
 
-TEST_F(PositionControlBasicTest, PositionControlMaxVelocity)
+TEST_F(PositionControlBasicTest, VelocityLimit)
 {
 	_input_setpoint.x = 10.f;
 	_input_setpoint.y = 10.f;
@@ -183,7 +183,7 @@ TEST_F(PositionControlBasicTest, PositionControlMaxVelocity)
 	EXPECT_LE(abs(_output_setpoint.vz), 1.f);
 }
 
-TEST_F(PositionControlBasicTest, PositionControlThrustLimit)
+TEST_F(PositionControlBasicTest, ThrustLimit)
 {
 	_input_setpoint.x = 10.f;
 	_input_setpoint.y = 10.f;
@@ -196,7 +196,7 @@ TEST_F(PositionControlBasicTest, PositionControlThrustLimit)
 	EXPECT_GE(_attitude.thrust_body[2], -0.9f);
 }
 
-TEST_F(PositionControlBasicTest, PositionControlFailsafeInput)
+TEST_F(PositionControlBasicTest, FailsafeInput)
 {
 	_input_setpoint.vz = .7f;
 	_input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f;
@@ -209,3 +209,39 @@ TEST_F(PositionControlBasicTest, PositionControlFailsafeInput)
 	EXPECT_GT(_attitude.thrust_body[2], -.5f);
 	EXPECT_LE(_attitude.thrust_body[2], -.1f);
 }
+
+TEST_F(PositionControlBasicTest, InputCombinationsPosition)
+{
+	_input_setpoint.x = .1f;
+	_input_setpoint.y = .2f;
+	_input_setpoint.z = .3f;
+
+	runController();
+	EXPECT_EQ(_output_setpoint.x, .1f);
+	EXPECT_EQ(_output_setpoint.y, .2f);
+	EXPECT_EQ(_output_setpoint.z, .3f);
+	EXPECT_FALSE(isnan(_output_setpoint.vx));
+	EXPECT_FALSE(isnan(_output_setpoint.vy));
+	EXPECT_FALSE(isnan(_output_setpoint.vz));
+	EXPECT_FALSE(isnan(_output_setpoint.thrust[0]));
+	EXPECT_FALSE(isnan(_output_setpoint.thrust[1]));
+	EXPECT_FALSE(isnan(_output_setpoint.thrust[2]));
+}
+
+TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
+{
+	_input_setpoint.vx = .1f;
+	_input_setpoint.vy = .2f;
+	_input_setpoint.z = .3f; // altitude
+
+	runController();
+	// EXPECT_TRUE(isnan(_output_setpoint.x));
+	// EXPECT_TRUE(isnan(_output_setpoint.y));
+	EXPECT_EQ(_output_setpoint.z, .3f);
+	EXPECT_EQ(_output_setpoint.vx, .1f);
+	EXPECT_EQ(_output_setpoint.vy, .2f);
+	EXPECT_FALSE(isnan(_output_setpoint.vz));
+	EXPECT_FALSE(isnan(_output_setpoint.thrust[0]));
+	EXPECT_FALSE(isnan(_output_setpoint.thrust[1]));
+	EXPECT_FALSE(isnan(_output_setpoint.thrust[2]));
+}

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -652,9 +652,6 @@ MulticopterPositionControl::Run()
 			local_pos_sp.x = setpoint.x;
 			local_pos_sp.y = setpoint.y;
 			local_pos_sp.z = setpoint.z;
-			local_pos_sp.vx = PX4_ISFINITE(_control.getVelSp()(0)) ? _control.getVelSp()(0) : setpoint.vx;
-			local_pos_sp.vy = PX4_ISFINITE(_control.getVelSp()(1)) ? _control.getVelSp()(1) : setpoint.vy;
-			local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz;
 
 			// Publish local position setpoint
 			// This message will be used by other modules (such as Landdetector) to determine vehicle intention.
@@ -662,7 +659,8 @@ MulticopterPositionControl::Run()
 
 			// Inform FlightTask about the input and output of the velocity controller
 			// This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock)
-			_flight_tasks.updateVelocityControllerIO(_control.getVelSp(), Vector3f(local_pos_sp.thrust));
+			_flight_tasks.updateVelocityControllerIO(Vector3f(local_pos_sp.vx, local_pos_sp.vy, local_pos_sp.vz),
+					Vector3f(local_pos_sp.thrust));
 
 			vehicle_attitude_setpoint_s attitude_setpoint{};
 			attitude_setpoint.timestamp = time_stamp_now;