FixedwingRateControl: rework VTOL differential thrust saturation

PX4 · Jun 14, 2023 · 687c049 · 687c049
1 parent 4df1b22
commit 687c049
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 47 deletions.
diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp
@@ -278,64 +278,41 @@ void FixedwingRateControl::Run()
 				_rate_control.resetIntegral();
 			}
 
-			// update saturation status from control allocation feedback
-			control_allocator_status_s control_allocator_status;
-			control_allocator_status_s control_allocator_status_diff_thr;
-
-			// VTOL differential thrust feature: the motors are in matrix 0, and if differential thrust is enabled
-			// assume that it's the only means of control around this axis and thus only check the matrix 0 saturation
-			// around the corresponding axis.
-			Vector<bool, 3> diff_thr_en;
-			diff_thr_en(0) = (_vehicle_status.is_vtol_tailsitter
-					  && _param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT))
-					 || (!_vehicle_status.is_vtol_tailsitter
-					     && _param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::ROLL_BIT));
-			diff_thr_en(1) = _param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::PITCH_BIT);
-			diff_thr_en(2) = (_vehicle_status.is_vtol_tailsitter
-					  && _param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::ROLL_BIT))
-					 || (!_vehicle_status.is_vtol_tailsitter
-					     && _param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT));
+			// Update saturation status from control allocation feedback
+			// TODO: send the unallocated value directly for better anti-windup
+			Vector3<bool> diffthr_enabled(
+				_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::ROLL_BIT),
+				_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::PITCH_BIT),
+				_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::YAW_BIT)
+			);
+
+			if (_vehicle_status.is_vtol_tailsitter) {
+				// Swap roll and yaw
+				diffthr_enabled.swapRows(0, 2);
+			}
 
 			// saturation handling for axis controlled by differential thrust (VTOL only)
-			if (_param_vt_fw_difthr_en.get() > 0
-			    && _control_allocator_status_subs[0].update(&control_allocator_status_diff_thr)) {
-				Vector<bool, 3> saturation_positive;
-				Vector<bool, 3> saturation_negative;
+			control_allocator_status_s control_allocator_status;
 
+			// Set saturation flags for VTOL differential thrust feature
+			// If differential thrust is enabled in an axis, assume it's the only torque authority and only update saturation using matrix 0 allocating the motors.
+			if (_control_allocator_status_subs[0].update(&control_allocator_status)) {
 				for (size_t i = 0; i < 3; i++) {
-					if (diff_thr_en(i)) {
-						if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
-							saturation_positive(i) = true;
-
-						} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
-							saturation_negative(i) = true;
-						}
+					if (diffthr_enabled(i)) {
+						_rate_control.setPositiveSaturationFlag(i, control_allocator_status.unallocated_torque[i] > FLT_EPSILON);
+						_rate_control.setNegativeSaturationFlag(i, control_allocator_status.unallocated_torque[i] < -FLT_EPSILON);
 					}
 				}
-
-				// TODO: send the unallocated value directly for better anti-windup
-				_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
 			}
 
-			// saturation handling for axis NOT controlled by differential thrust
+			// Set saturation flags for control surface controlled axes
 			if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) {
-				Vector<bool, 3> saturation_positive;
-				Vector<bool, 3> saturation_negative;
-
 				for (size_t i = 0; i < 3; i++) {
-					// only consider the axes that are not handled by the differential thrust
-					if (!diff_thr_en(i)) {
-						if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
-							saturation_positive(i) = true;
-
-						} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
-							saturation_negative(i) = true;
-						}
+					if (!diffthr_enabled(i)) {
+						_rate_control.setPositiveSaturationFlag(i, control_allocator_status.unallocated_torque[i] > FLT_EPSILON);
+						_rate_control.setNegativeSaturationFlag(i, control_allocator_status.unallocated_torque[i] < -FLT_EPSILON);
 					}
 				}
-
-				// TODO: send the unallocated value directly for better anti-windup
-				_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
 			}
 
 			/* bi-linear interpolation over airspeed for actuator trim scheduling */

diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp
@@ -145,7 +145,7 @@ class FixedwingRateControl final : public ModuleBase<FixedwingRateControl>, publ
 	bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states
 
 	// enum for bitmask of VT_FW_DIFTHR_EN parameter options
-	enum class VtFwDifthrEnBits : int32_t {
+	enum class VTOLFixedWingDifferentialThrustEnabledBit : int32_t {
 		YAW_BIT = (1 << 0),
 		ROLL_BIT = (1 << 1),
 		PITCH_BIT = (1 << 2),