Control Allocation: fixes in yaw saturation detection for vehicles wi…

…th tilt-for-yaw (PX4#21994) * ActuatorEffectiveness: base yaw saturation on tilt actuator limits (VTOL Tiltrotor) * ActuatorEffectiveness: add custom yaw saturation for MC Tilt --------- Signed-off-by: Silvan Fuhrer <[email protected]>
ryrobotics · Jan 4, 2024 · c0f560b · c0f560b
1 parent ae69ee0
commit c0f560b
Show file tree

Hide file tree

Showing 4 changed files with 88 additions and 19 deletions.
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -55,7 +55,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
 	const bool rotors_added_successfully = _mc_rotors.addActuators(configuration);
 
 	// Tilts
-	int first_tilt_idx = configuration.num_actuators_matrix[0];
+	_first_tilt_idx = configuration.num_actuators_matrix[0];
 	_tilts.updateTorqueSign(_mc_rotors.geometry());
 	const bool tilts_added_successfully = _tilts.addActuators(configuration);
 
@@ -69,7 +69,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
 
 		if (delta_angle > FLT_EPSILON) {
 			float trim = -1.f - 2.f * _tilts.config(i).min_angle / delta_angle;
-			_tilt_offsets(first_tilt_idx + i) = trim;
+			_tilt_offsets(_first_tilt_idx + i) = trim;
 		}
 	}
 
@@ -82,4 +82,49 @@ void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector<float, NUM
 {
 	actuator_sp += _tilt_offsets;
 	// TODO: dynamic matrix update
+
+	bool yaw_saturated_positive = true;
+	bool yaw_saturated_negative = true;
+
+	for (int i = 0; i < _tilts.count(); ++i) {
+
+		// custom yaw saturation logic: only declare yaw saturated if all tilts are at the negative or positive yawing limit
+		if (_tilts.getYawTorqueOfTilt(i) > FLT_EPSILON) {
+
+			if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
+				yaw_saturated_positive = false;
+			}
+
+			if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
+				yaw_saturated_negative = false;
+			}
+
+		} else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) {
+			if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
+				yaw_saturated_negative = false;
+			}
+
+			if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
+				yaw_saturated_positive = false;
+			}
+		}
+	}
+
+	_yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative;
+	_yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive;
+}
+
+void ActuatorEffectivenessMCTilt::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
+{
+	// 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 (_yaw_tilt_saturation_flags.tilt_yaw_pos) {
+		status.unallocated_torque[2] = 1.f;
+
+	} else if (_yaw_tilt_saturation_flags.tilt_yaw_neg) {
+		status.unallocated_torque[2] = -1.f;
+
+	} else {
+		status.unallocated_torque[2] = 0.f;
+	}
 }
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -61,8 +61,18 @@ class ActuatorEffectivenessMCTilt : public ModuleParams, public ActuatorEffectiv
 
 	const char *name() const override { return "MC Tilt"; }
 
+	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
+
 protected:
 	ActuatorVector _tilt_offsets;
 	ActuatorEffectivenessRotors _mc_rotors;
 	ActuatorEffectivenessTilts _tilts;
+	int _first_tilt_idx{0};
+
+	struct YawTiltSaturationFlags {
+		bool tilt_yaw_pos;
+		bool tilt_yaw_neg;
+	};
+
+	YawTiltSaturationFlags _yaw_tilt_saturation_flags{};
 };
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
@@ -148,12 +148,39 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 				_last_collective_tilt_control = -1.f;
 			}
 
+			bool yaw_saturated_positive = true;
+			bool yaw_saturated_negative = true;
+
 			for (int i = 0; i < _tilts.count(); ++i) {
 				if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
 					actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
 				}
+
+				// custom yaw saturation logic: only declare yaw saturated if all tilts are at the negative or positive yawing limit
+				if (_tilts.getYawTorqueOfTilt(i) > FLT_EPSILON) {
+
+					if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
+						yaw_saturated_positive = false;
+					}
+
+					if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
+						yaw_saturated_negative = false;
+					}
+
+				} else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) {
+					if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
+						yaw_saturated_negative = false;
+					}
+
+					if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
+						yaw_saturated_positive = false;
+					}
+				}
 			}
 
+			_yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative;
+			_yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive;
+
 			// in FW directly use throttle sp
 			if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
 				for (int i = 0; i < _first_tilt_idx; ++i) {
@@ -166,21 +193,6 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 			stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp);
 		}
 	}
-
-	// Set yaw saturation flag in case of yaw through tilt. As in this case the yaw actuation is decoupled from
-	// the other axes (for now neglecting the case of 0 collective thrust), we set the saturation flags
-	// directly if the (normalized) yaw torque setpoint is outside of range (-1, 1).
-	if (matrix_index == 0 && _tilts.hasYawControl()) {
-		_yaw_tilt_saturation_flags.tilt_yaw_neg = false;
-		_yaw_tilt_saturation_flags.tilt_yaw_pos = false;
-
-		if (control_sp(2) < -1.f) {
-			_yaw_tilt_saturation_flags.tilt_yaw_neg = true;
-
-		} else if (control_sp(2) > 1.f) {
-			_yaw_tilt_saturation_flags.tilt_yaw_pos = true;
-		}
-	}
 }
 
 void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)

diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp
@@ -79,6 +79,8 @@ class ActuatorEffectivenessTilts : public ModuleParams, public ActuatorEffective
 
 	bool hasYawControl() const;
 
+	float getYawTorqueOfTilt(int tilt_index) const { return _torque[tilt_index](2); }
+
 private:
 	void updateParams() override;