control_allocator: handle saturation flags for helicopters

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -44,6 +44,7 @@
 #include <cstdint>
 
 #include <matrix/matrix/math.hpp>
+#include <uORB/topics/control_allocator_status.h>
 
 enum class AllocationMethod {
 	NONE = -1,
@@ -189,13 +190,20 @@ public:
 	 * @param actuator_sp input & output setpoint
 	 */
 	virtual void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-				    int matrix_index, ActuatorVector &actuator_sp) {}
+				    int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+				    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) {}
 
 	/**
 	 * Get a bitmask of motors to be stopped
 	 */
 	virtual uint32_t getStoppedMotors() const { return 0; }
 
+	/**
+	 * Fill in the allocated and unallocated torque and thrust.
+	 * Should return false if not filled in and the effectivenes matrix should be used instead
+	 */
+	virtual bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const { return false; }
+
 protected:
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -62,7 +62,8 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 }
 
 void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp)
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	// apply flaps
 	actuator_controls_s actuator_controls_0;

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp

@@ -50,7 +50,8 @@ public:
 	const char *name() const override { return "Fixed Wing"; }
 
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp) override;
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 private:
 	ActuatorEffectivenessRotors _rotors;

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

@@ -128,8 +128,11 @@ ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configura
 }
 
 void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp)
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
+	_saturation_flags = {};
+
 	// throttle/collective pitch curve
 	const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
 			       _geometry.throttle_curve) * throttleSpoolupProgress();
@@ -142,13 +145,31 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 			 + fabsf(collective_pitch) * _geometry.yaw_collective_pitch_scale
 			 + throttle * _geometry.yaw_throttle_scale;
 
+	// Saturation check for yaw
+	if (actuator_sp(1) < actuator_min(1)) {
+		setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos);
+
+	} else if (actuator_sp(1) > actuator_max(1)) {
+		setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg);
+	}
+
 	for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
+		float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length;
+		float pitch_coeff = cosf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length;
 		actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch
-				+ control_sp(ControlAxis::PITCH) * cosf(_geometry.swash_plate_servos[i].angle) *
-				_geometry.swash_plate_servos[i].arm_length
-				- control_sp(ControlAxis::ROLL) * sinf(_geometry.swash_plate_servos[i].angle) *
-				_geometry.swash_plate_servos[i].arm_length
+				+ control_sp(ControlAxis::PITCH) * pitch_coeff
+				- control_sp(ControlAxis::ROLL) * roll_coeff
 				+ _geometry.swash_plate_servos[i].trim;
+
+		// Saturation check for roll & pitch
+		if (actuator_sp(_first_swash_plate_servo_index + i) < actuator_min(_first_swash_plate_servo_index + i)) {
+			setSaturationFlag(roll_coeff, _saturation_flags.roll_pos, _saturation_flags.roll_neg);
+			setSaturationFlag(pitch_coeff, _saturation_flags.pitch_neg, _saturation_flags.pitch_pos);
+
+		} else if (actuator_sp(_first_swash_plate_servo_index + i) > actuator_max(_first_swash_plate_servo_index + i)) {
+			setSaturationFlag(roll_coeff, _saturation_flags.roll_neg, _saturation_flags.roll_pos);
+			setSaturationFlag(pitch_coeff, _saturation_flags.pitch_pos, _saturation_flags.pitch_neg);
+		}
 	}
 }
 
@@ -182,3 +203,62 @@ float ActuatorEffectivenessHelicopter::throttleSpoolupProgress()
 
 	return 1.f;
 }
+
+
+void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag)
+{
+	if (coeff > 0.f) {
+		// A positive change in given axis will increase saturation
+		positive_flag = true;
+
+	} else if (coeff < 0.f) {
+		// A negative change in given axis will increase saturation
+		negative_flag = true;
+	}
+}
+
+bool ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const
+{
+	status.torque_setpoint_achieved = true;
+	status.thrust_setpoint_achieved = true;
+
+	// Note: the values '-1', '1' and '0' are just to indicate a negative,
+	// positive or no saturation to the rate controller. The actual magnitude is not used.
+	if (_saturation_flags.roll_pos) {
+		status.unallocated_torque[0] = 1.f;
+		status.torque_setpoint_achieved = false;
+
+	} else if (_saturation_flags.roll_neg) {
+		status.unallocated_torque[0] = -1.f;
+		status.torque_setpoint_achieved = false;
+	}
+
+	if (_saturation_flags.pitch_pos) {
+		status.unallocated_torque[1] = 1.f;
+		status.torque_setpoint_achieved = false;
+
+	} else if (_saturation_flags.pitch_neg) {
+		status.unallocated_torque[1] = -1.f;
+		status.torque_setpoint_achieved = false;
+	}
+
+	if (_saturation_flags.yaw_pos) {
+		status.unallocated_torque[2] = 1.f;
+		status.torque_setpoint_achieved = false;
+
+	} else if (_saturation_flags.yaw_neg) {
+		status.unallocated_torque[2] = -1.f;
+		status.torque_setpoint_achieved = false;
+	}
+
+	if (_saturation_flags.thrust_pos) {
+		status.unallocated_thrust[2] = 1.f;
+		status.thrust_setpoint_achieved = false;
+
+	} else if (_saturation_flags.thrust_neg) {
+		status.unallocated_thrust[2] = -1.f;
+		status.thrust_setpoint_achieved = false;
+	}
+
+	return true;
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp

@@ -76,13 +76,28 @@ public:
 	const Geometry &geometry() const { return _geometry; }
 
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp) override;
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
+	bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const override;
 private:
 	float throttleSpoolupProgress();
 	bool mainMotorEnaged();
 
 	void updateParams() override;
 
+	struct SaturationFlags {
+		bool roll_pos;
+		bool roll_neg;
+		bool pitch_pos;
+		bool pitch_neg;
+		bool yaw_pos;
+		bool yaw_neg;
+		bool thrust_pos;
+		bool thrust_neg;
+	};
+	static void setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag);
+
 	struct ParamHandlesSwashPlate {
 		param_t angle;
 		param_t arm_length;
@@ -103,6 +118,7 @@ private:
 	Geometry _geometry{};
 
 	int _first_swash_plate_servo_index{};
+	SaturationFlags _saturation_flags;
 
 	// Throttle spoolup state
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp

@@ -60,48 +60,52 @@ TEST(ActuatorEffectivenessHelicopterTest, ThrottleCurve)
 
 	Vector<float, 6> control_sp{};
 	ActuatorEffectiveness::ActuatorVector actuator_sp{};
+	ActuatorEffectiveness::ActuatorVector actuator_min{};
+	actuator_min.setAll(0.f);
+	ActuatorEffectiveness::ActuatorVector actuator_max{};
+	actuator_max.setAll(1.f);
 
 	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = 0.1f;
-	helicopter.updateSetpoint(control_sp, 0, actuator_sp);
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
 	EXPECT_FLOAT_EQ(actuator_sp(0), 0.f);
 
 	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = 0.f;
-	helicopter.updateSetpoint(control_sp, 0, actuator_sp);
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
 	EXPECT_FLOAT_EQ(actuator_sp(0), 0.f);
 
 	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.125f;
-	helicopter.updateSetpoint(control_sp, 0, actuator_sp);
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
 	EXPECT_FLOAT_EQ(actuator_sp(0), .15f);
 
 	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.25f;
-	helicopter.updateSetpoint(control_sp, 0, actuator_sp);
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
 	EXPECT_FLOAT_EQ(actuator_sp(0), .3f);
 
 	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.375f;
-	helicopter.updateSetpoint(control_sp, 0, actuator_sp);
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
 	EXPECT_FLOAT_EQ(actuator_sp(0), .45f);
 
 	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.5f;
-	helicopter.updateSetpoint(control_sp, 0, actuator_sp);
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
 	EXPECT_FLOAT_EQ(actuator_sp(0), .6f);
 
 	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.625f;
-	helicopter.updateSetpoint(control_sp, 0, actuator_sp);
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
 	EXPECT_FLOAT_EQ(actuator_sp(0), .7f);
 
 	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.75f;
-	helicopter.updateSetpoint(control_sp, 0, actuator_sp);
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
 	EXPECT_FLOAT_EQ(actuator_sp(0), .8f);
 
 	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.875f;
-	helicopter.updateSetpoint(control_sp, 0, actuator_sp);
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
 	EXPECT_FLOAT_EQ(actuator_sp(0), .9f);
 
 	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -1.f;
-	helicopter.updateSetpoint(control_sp, 0, actuator_sp);
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
 	EXPECT_FLOAT_EQ(actuator_sp(0), 1.f);
 
 	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -1.1f;
-	helicopter.updateSetpoint(control_sp, 0, actuator_sp);
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
 	EXPECT_FLOAT_EQ(actuator_sp(0), 1.f);
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp

@@ -77,7 +77,8 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
 }
 
 void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp)
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	actuator_sp += _tilt_offsets;
 	// TODO: dynamic matrix update

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp

@@ -56,7 +56,8 @@ public:
 	}
 
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp) override;
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	const char *name() const override { return "MC Tilt"; }
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -64,7 +64,8 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu
 }
 
 void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp)
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	// apply flaps
 	if (matrix_index == 1) {

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -73,7 +73,8 @@ public:
 	}
 
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp) override;
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -92,7 +92,8 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 }
 
 void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp)
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	// apply flaps
 	if (matrix_index == 1) {

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -75,7 +75,8 @@ public:
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp) override;
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	const char *name() const override { return "VTOL Tiltrotor"; }
 

src/modules/control_allocator/ControlAllocator.cpp

@@ -416,7 +416,8 @@ ControlAllocator::Run()
 
 			// Do allocation
 			_control_allocation[i]->allocate();
-			_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp);
+			_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp,
+								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());
 
 			if (_has_slew_rate) {
 				_control_allocation[i]->applySlewRateLimit(dt);
@@ -585,30 +586,33 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)
 
 	// TODO: disabled motors (?)
 
-	// Allocated control
-	const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->getAllocatedControl();
-	control_allocator_status.allocated_torque[0] = allocated_control(0);
-	control_allocator_status.allocated_torque[1] = allocated_control(1);
-	control_allocator_status.allocated_torque[2] = allocated_control(2);
-	control_allocator_status.allocated_thrust[0] = allocated_control(3);
-	control_allocator_status.allocated_thrust[1] = allocated_control(4);
-	control_allocator_status.allocated_thrust[2] = allocated_control(5);
+	if (!_actuator_effectiveness->getAllocatedAndUnallocatedControl(control_allocator_status)) {
+		// Allocated control
+		const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->getAllocatedControl();
+		control_allocator_status.allocated_torque[0] = allocated_control(0);
+		control_allocator_status.allocated_torque[1] = allocated_control(1);
+		control_allocator_status.allocated_torque[2] = allocated_control(2);
+		control_allocator_status.allocated_thrust[0] = allocated_control(3);
+		control_allocator_status.allocated_thrust[1] = allocated_control(4);
+		control_allocator_status.allocated_thrust[2] = allocated_control(5);
 
-	// Unallocated control
-	const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() -
-			allocated_control;
-	control_allocator_status.unallocated_torque[0] = unallocated_control(0);
-	control_allocator_status.unallocated_torque[1] = unallocated_control(1);
-	control_allocator_status.unallocated_torque[2] = unallocated_control(2);
-	control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
-	control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
-	control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
+		// Unallocated control
+		const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() -
+				allocated_control;
+		control_allocator_status.unallocated_torque[0] = unallocated_control(0);
+		control_allocator_status.unallocated_torque[1] = unallocated_control(1);
+		control_allocator_status.unallocated_torque[2] = unallocated_control(2);
+		control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
+		control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
+		control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
+
+		// Allocation success flags
+		control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1),
+				unallocated_control(2)).norm_squared() < 1e-6f);
+		control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4),
+				unallocated_control(5)).norm_squared() < 1e-6f);
+	}
 
-	// Allocation success flags
-	control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1),
-			unallocated_control(2)).norm_squared() < 1e-6f);
-	control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4),
-			unallocated_control(5)).norm_squared() < 1e-6f);
 
 	// Actuator saturation
 	const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint();