PositionControl: replace interfacemapping checks

Removing the skip_controller and interfaceMapping
concept and replacing it with a single method checking
if the position control update was successful and
return the result in the PositionControl.update().

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -159,7 +159,6 @@ Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
 		// notes:
 		// 	- s (=scaling factor) needs to be positive
 		// 	- (max - ||v||) always larger than zero, otherwise it never entered this if-statement
-
 		Vector2f u1 = v1.normalized();
 		float m = u1.dot(v0);
 		float c = v0.dot(v0) - max * max;

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -71,7 +71,7 @@ void PositionControl::setState(const PositionControlStates &states)
 	_vel_dot = states.acceleration;
 }
 
-bool PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint)
+void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint)
 {
 	_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
 	_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
@@ -79,14 +79,6 @@ bool PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &
 	_thr_sp = Vector3f(setpoint.thrust);
 	_yaw_sp = setpoint.yaw;
 	_yawspeed_sp = setpoint.yawspeed;
-	bool mapping_succeeded = _interfaceMapping();
-
-	// If full manual is required (thrust already generated), don't run position/velocity
-	// controller and just return thrust.
-	_skip_controller = PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))
-			   && PX4_ISFINITE(_thr_sp(2));
-
-	return mapping_succeeded;
 }
 
 void PositionControl::setConstraints(const vehicle_constraints_s &constraints)
@@ -110,132 +102,20 @@ void PositionControl::setConstraints(const vehicle_constraints_s &constraints)
 	// ignore _constraints.speed_xy TODO: remove it completely as soon as no task uses it anymore to avoid confusion
 }
 
-void PositionControl::update(const float dt)
+bool PositionControl::update(const float dt)
 {
-	if (_skip_controller) {
-		// Already received a valid thrust set-point.
-		// Limit the thrust vector.
-		float thr_mag = _thr_sp.length();
-
-		if (thr_mag > _lim_thr_max) {
-			_thr_sp = _thr_sp.normalized() * _lim_thr_max;
-
-		} else if (thr_mag < _lim_thr_min && thr_mag > FLT_EPSILON) {
-			_thr_sp = _thr_sp.normalized() * _lim_thr_min;
-		}
-
-		// Just set the set-points equal to the current vehicle state.
-		_pos_sp = _pos;
-		_vel_sp = _vel;
-		_acc_sp = _vel_dot;
-		return;
-	}
+	// x and y input setpoints always have to come in pairs
+	const bool valid = (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1)))
+			   && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1)))
+			   && (PX4_ISFINITE(_thr_sp(0)) == PX4_ISFINITE(_thr_sp(1)));
 
 	_positionControl();
 	_velocityControl(dt);
 
 	_yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f;
 	_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control
-}
-
-bool PositionControl::_interfaceMapping()
-{
-	// if nothing is valid, then apply failsafe landing
-	bool failsafe = false;
-
-	// Respects FlightTask interface, where NAN-set-points are of no interest
-	// and do not require control. A valid position and velocity setpoint will
-	// be mapped to a desired position setpoint with a feed-forward term.
-	// States and setpoints which are integrals of the reference setpoint are set to 0.
-	// For instance: reference is velocity-setpoint -> position and position-setpoint = 0
-	//               reference is thrust-setpoint -> position, velocity, position-/velocity-setpoint = 0
-	for (int i = 0; i <= 2; i++) {
-
-		if (PX4_ISFINITE(_pos_sp(i))) {
-			// Position control is required
-
-			if (!PX4_ISFINITE(_vel_sp(i))) {
-				// Velocity is not used as feedforward term.
-				_vel_sp(i) = 0.0f;
-			}
-
-			// thrust setpoint is not supported in position control
-			_thr_sp(i) = NAN;
-
-			// to run position control, we require valid position and velocity
-			if (!PX4_ISFINITE(_pos(i)) || !PX4_ISFINITE(_vel(i))) {
-				failsafe = true;
-			}
-
-		} else if (PX4_ISFINITE(_vel_sp(i))) {
-
-			// Velocity controller is active without position control.
-			// Set integral states and setpoints to 0
-			_pos_sp(i) = _pos(i) = 0.0f;
-
-			// thrust setpoint is not supported in velocity control
-			_thr_sp(i) = NAN;
-
-			// to run velocity control, we require valid velocity
-			if (!PX4_ISFINITE(_vel(i))) {
-				failsafe = true;
-			}
-
-		} else if (PX4_ISFINITE(_thr_sp(i))) {
-
-			// Thrust setpoint was generated from sticks directly.
-			// Set all integral states and setpoints to 0
-			_pos_sp(i) = _pos(i) = 0.0f;
-			_vel_sp(i) = _vel(i) = 0.0f;
-
-			// Reset the Integral term.
-			_vel_int(i) = 0.0f;
-			// Don't require velocity derivative.
-			_vel_dot(i) = 0.0f;
-
-		} else {
-			// nothing is valid. do failsafe
-			failsafe = true;
-		}
-	}
-
-	// ensure that vel_dot is finite, otherwise set to 0
-	if (!PX4_ISFINITE(_vel_dot(0)) || !PX4_ISFINITE(_vel_dot(1))) {
-		_vel_dot(0) = _vel_dot(1) = 0.0f;
-	}
-
-	if (!PX4_ISFINITE(_vel_dot(2))) {
-		_vel_dot(2) = 0.0f;
-	}
-
-	if (!PX4_ISFINITE(_yawspeed_sp)) {
-		// Set the yawspeed to 0 since not used.
-		_yawspeed_sp = 0.0f;
-	}
-
-	if (!PX4_ISFINITE(_yaw_sp)) {
-		// Set the yaw-sp equal the current yaw.
-		// That is the best we can do and it also
-		// agrees with FlightTask-interface definition.
-		if (PX4_ISFINITE(_yaw)) {
-			_yaw_sp = _yaw;
-
-		} else {
-			failsafe = true;
-		}
-	}
 
-	// check failsafe
-	if (failsafe) {
-		// point the thrust upwards
-		_thr_sp(0) = _thr_sp(1) = 0.0f;
-		// throttle down such that vehicle goes down with
-		// 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)
-	}
-
-	return !(failsafe);
+	return valid && _updateSuccessful();
 }
 
 void PositionControl::_positionControl()
@@ -336,6 +216,26 @@ void PositionControl::_velocityControl(const float dt)
 	_vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * math::sign(_vel_int(2));
 }
 
+bool PositionControl::_updateSuccessful()
+{
+	bool valid = true;
+
+	// For each controlled state the estimate has to be valid
+	for (int i = 0; i <= 2; i++) {
+		if (PX4_ISFINITE(_pos_sp(i))) {
+			valid = valid && PX4_ISFINITE(_pos(i));
+		}
+
+		if (PX4_ISFINITE(_vel_sp(i))) {
+			valid = valid && PX4_ISFINITE(_vel(i)) && PX4_ISFINITE(_vel_dot(i));
+		}
+	}
+
+	// There has to be a valid output thrust setpoint otherwise there was no
+	// setpoint-state pair for each axis that can get controlled
+	valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));
+	return valid;
+}
 
 void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const
 {

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -129,9 +129,8 @@ public:
 	 * Pass the desired setpoints
 	 * Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint.
 	 * @param setpoint a vehicle_local_position_setpoint_s structure
-	 * @return true if a valid setpoint was set
 	 */
-	bool setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint);
+	void setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint);
 
 	/**
 	 * Pass constraints that are stricter than the global limits
@@ -147,9 +146,9 @@ public:
 	 * @see _yaw_sp
 	 * @see _yawspeed_sp
 	 * @param dt time in seconds since last iteration
-	 * @return true if output setpoint is executable, false if not
+	 * @return true if update succeeded and output setpoint is executable, false if not
 	 */
-	void update(const float dt);
+	bool update(const float dt);
 
 	/**
 	 * Set the integral term in xy to 0.
@@ -174,11 +173,7 @@ public:
 	void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;
 
 private:
-	/**
-	 * Maps setpoints to internal-setpoints.
-	 * @return true if mapping succeeded.
-	 */
-	bool _interfaceMapping();
+	bool _updateSuccessful();
 
 	void _positionControl(); ///< Position proportional control
 	void _velocityControl(const float dt); ///< Velocity PID control
@@ -215,6 +210,4 @@ private:
 	matrix::Vector3f _thr_sp; /**< desired thrust */
 	float _yaw_sp{}; /**< desired heading */
 	float _yawspeed_sp{}; /** desired yaw-speed */
-
-	bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */
 };

src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp

@@ -88,6 +88,11 @@ public:
 		_contraints.speed_up = NAN;
 		_contraints.speed_down = NAN;
 
+		resetInputSetpoint();
+	}
+
+	void resetInputSetpoint()
+	{
 		_input_setpoint.x = NAN;
 		_input_setpoint.y = NAN;
 		_input_setpoint.z = NAN;
@@ -100,13 +105,14 @@ public:
 		Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.thrust);
 	}
 
-	void runController()
+	bool runController()
 	{
 		_position_control.setConstraints(_contraints);
 		_position_control.setInputSetpoint(_input_setpoint);
-		_position_control.update(.1f);
+		const bool ret = _position_control.update(.1f);
 		_position_control.getLocalPositionSetpoint(_output_setpoint);
 		_position_control.getAttitudeSetpoint(_attitude);
+		return ret;
 	}
 
 	PositionControl _position_control;
@@ -138,7 +144,7 @@ TEST_F(PositionControlBasicDirectionTest, PositionDirection)
 	_input_setpoint.x = .1f;
 	_input_setpoint.y = .1f;
 	_input_setpoint.z = -.1f;
-	runController();
+	EXPECT_TRUE(runController());
 	checkDirection();
 }
 
@@ -147,7 +153,7 @@ TEST_F(PositionControlBasicDirectionTest, VelocityDirection)
 	_input_setpoint.vx = .1f;
 	_input_setpoint.vy = .1f;
 	_input_setpoint.vz = -.1f;
-	runController();
+	EXPECT_TRUE(runController());
 	checkDirection();
 }
 
@@ -157,14 +163,14 @@ TEST_F(PositionControlBasicTest, TiltLimit)
 	_input_setpoint.y = 10.f;
 	_input_setpoint.z = -0.f;
 
-	runController();
+	EXPECT_TRUE(runController());
 	Vector3f body_z = Quatf(_attitude.q_d).dcm_z();
 	float angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f)));
 	EXPECT_GT(angle, 0.f);
 	EXPECT_LE(angle, 1.f);
 
 	_contraints.tilt = .5f;
-	runController();
+	EXPECT_TRUE(runController());
 	body_z = Quatf(_attitude.q_d).dcm_z();
 	angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f)));
 	EXPECT_GT(angle, 0.f);
@@ -177,7 +183,7 @@ TEST_F(PositionControlBasicTest, VelocityLimit)
 	_input_setpoint.y = 10.f;
 	_input_setpoint.z = -10.f;
 
-	runController();
+	EXPECT_TRUE(runController());
 	Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy);
 	EXPECT_LE(velocity_xy.norm(), 1.f);
 	EXPECT_LE(abs(_output_setpoint.vz), 1.f);
@@ -189,7 +195,7 @@ TEST_F(PositionControlBasicTest, ThrustLimit)
 	_input_setpoint.y = 10.f;
 	_input_setpoint.z = -10.f;
 
-	runController();
+	EXPECT_TRUE(runController());
 	EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f);
 	EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f);
 	EXPECT_LT(_attitude.thrust_body[2], -.1f);
@@ -202,7 +208,7 @@ TEST_F(PositionControlBasicTest, FailsafeInput)
 	_input_setpoint.thrust[0] = _input_setpoint.thrust[1] = 0.f;
 	_input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f;
 
-	runController();
+	EXPECT_TRUE(runController());
 	EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f);
 	EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f);
 	EXPECT_LT(_output_setpoint.thrust[2], -.1f);
@@ -217,7 +223,7 @@ TEST_F(PositionControlBasicTest, InputCombinationsPosition)
 	_input_setpoint.y = .2f;
 	_input_setpoint.z = .3f;
 
-	runController();
+	EXPECT_TRUE(runController());
 	EXPECT_FLOAT_EQ(_output_setpoint.x, .1f);
 	EXPECT_FLOAT_EQ(_output_setpoint.y, .2f);
 	EXPECT_FLOAT_EQ(_output_setpoint.z, .3f);
@@ -235,7 +241,7 @@ TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
 	_input_setpoint.vy = .2f;
 	_input_setpoint.z = .3f; // altitude
 
-	runController();
+	EXPECT_TRUE(runController());
 	// EXPECT_TRUE(isnan(_output_setpoint.x));
 	// EXPECT_TRUE(isnan(_output_setpoint.y));
 	EXPECT_FLOAT_EQ(_output_setpoint.z, .3f);
@@ -246,3 +252,78 @@ TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
 	EXPECT_FALSE(isnan(_output_setpoint.thrust[1]));
 	EXPECT_FALSE(isnan(_output_setpoint.thrust[2]));
 }
+
+TEST_F(PositionControlBasicTest, SetpointValiditySimple)
+{
+	EXPECT_FALSE(runController());
+	_input_setpoint.x = .1f;
+	EXPECT_FALSE(runController());
+	_input_setpoint.y = .2f;
+	EXPECT_FALSE(runController());
+	_input_setpoint.thrust[2] = .3f;
+	EXPECT_TRUE(runController());
+}
+
+TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
+{
+	// This test runs any position, velocity, thrust setpoint combination and checks if it gets accepted or rejected correctly
+	float *const setpoint_loop_access_map[] = {&_input_setpoint.x, &_input_setpoint.vx, &_input_setpoint.thrust[0],
+						   &_input_setpoint.y, &_input_setpoint.vy, &_input_setpoint.thrust[1],
+						   &_input_setpoint.z, &_input_setpoint.vz, &_input_setpoint.thrust[2]
+						  };
+
+	for (int combination = 0; combination < 512; combination++) {
+		resetInputSetpoint();
+
+		for (int j = 0; j < 9; j++) {
+			if (combination & (1 << j)) {
+				// Set arbitrary finite value, some values clearly hit the limits to check these corner case combinations
+				*(setpoint_loop_access_map[j]) = static_cast<float>(combination) / static_cast<float>(j + 1);
+			}
+		}
+
+		// Expect at least one setpoint per axis
+		const bool has_x_setpoint = ((combination & 7) != 0);
+		const bool has_y_setpoint = (((combination >> 3) & 7) != 0);
+		const bool has_z_setpoint = (((combination >> 6) & 7) != 0);
+		// Expect xy setpoints to come in pairs
+		const bool has_xy_pairs = (combination & 7) == ((combination >> 3) & 7);
+		const bool expected_result = has_x_setpoint && has_y_setpoint && has_z_setpoint && has_xy_pairs;
+
+		EXPECT_EQ(runController(), expected_result) << "combination " << combination
+				<< std::endl << "input" << std::endl
+				<< "position " << _input_setpoint.x << ", " << _input_setpoint.y << ", " << _input_setpoint.z << std::endl
+				<< "velocity " << _input_setpoint.vx << ", " << _input_setpoint.vy << ", " << _input_setpoint.vz << std::endl
+				<< "thrust   " << _input_setpoint.thrust[0] << ", " << _input_setpoint.thrust[1] << ", " << _input_setpoint.thrust[2]
+				<< std::endl << "output" << std::endl
+				<< "position " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl
+				<< "velocity " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl
+				<< "thrust   " << _output_setpoint.thrust[0] << ", " << _output_setpoint.thrust[1] << ", " << _output_setpoint.thrust[2]
+				<< std::endl;
+	}
+}
+
+TEST_F(PositionControlBasicTest, InvalidState)
+{
+	_input_setpoint.x = .1f;
+	_input_setpoint.y = .2f;
+	_input_setpoint.z = .3f;
+
+	PositionControlStates states{};
+	states.position(0) = NAN;
+	_position_control.setState(states);
+	EXPECT_FALSE(runController());
+
+	states.velocity(0) = NAN;
+	_position_control.setState(states);
+	EXPECT_FALSE(runController());
+
+	states.position(0) = 0.f;
+	_position_control.setState(states);
+	EXPECT_FALSE(runController());
+
+	states.velocity(0) = 0.f;
+	states.acceleration(1) = NAN;
+	_position_control.setState(states);
+	EXPECT_FALSE(runController());
+}

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -563,19 +563,6 @@ MulticopterPositionControl::Run()
 				constraints = _flight_tasks.getConstraints();
 
 				_failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now);
-
-				// Check if position, velocity or thrust pairs are valid -> trigger failsaife if no pair is valid
-				if (!(PX4_ISFINITE(setpoint.x) && PX4_ISFINITE(setpoint.y)) &&
-				    !(PX4_ISFINITE(setpoint.vx) && PX4_ISFINITE(setpoint.vy)) &&
-				    !(PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]))) {
-					failsafe(setpoint, _states, true, !was_in_failsafe);
-				}
-
-				// Check if altitude, climbrate or thrust in D-direction are valid -> trigger failsafe if none
-				// of these setpoints are valid
-				if (!PX4_ISFINITE(setpoint.z) && !PX4_ISFINITE(setpoint.vz) && !PX4_ISFINITE(setpoint.thrust[2])) {
-					failsafe(setpoint, _states, true, !was_in_failsafe);
-				}
 			}
 
 			// publish trajectory setpoint
@@ -617,16 +604,16 @@ MulticopterPositionControl::Run()
 			// Run position control
 			_control.setState(_states);
 			_control.setConstraints(constraints);
+			_control.setInputSetpoint(setpoint);
 
-			if (!_control.setInputSetpoint(setpoint)) {
+			if (!_control.update(_dt)) {
 				warn_rate_limited("PositionControl: invalid setpoints");
 				failsafe(setpoint, _states, true, !was_in_failsafe);
 				_control.setInputSetpoint(setpoint);
 				constraints = FlightTask::empty_constraints;
+				_control.update(_dt);
 			}
 
-			_control.update(_dt);
-
 			// Fill local position, velocity and thrust setpoint.
 			// This message contains setpoints where each type of setpoint is either the input to the PositionController
 			// or was generated by the PositionController and therefore corresponds to the PositioControl internal states (states that were generated by P-PID).