From 57e052d02d5ce598be8c56887488ca80382334cd Mon Sep 17 00:00:00 2001
From: Silvan Fuhrer <silvan@auterion.com>
Date: Fri, 12 Aug 2022 16:22:57 +0200
Subject: [PATCH] FW attitdue controller: use allocator status for anti-windup

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
---
 .../FixedwingAttitudeControl.cpp              | 44 +++++++++++++++++++
 .../FixedwingAttitudeControl.hpp              |  4 ++
 2 files changed, 48 insertions(+)

diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
index d0aed1c485..2a0525d3f2 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -415,6 +415,50 @@ void FixedwingAttitudeControl::Run()
 				_wheel_ctrl.reset_integrator();
 			}
 
+			// update saturation status from control allocation feedback
+			control_allocator_status_s control_allocator_status;
+
+			if (_vehicle_status.is_vtol) {
+				if (_control_allocator_status_subs[1].update(&control_allocator_status)) {
+					Vector<bool, 3> saturation_positive;
+					Vector<bool, 3> saturation_negative;
+
+					if (!control_allocator_status.torque_setpoint_achieved) {
+						for (size_t i = 0; i < 3; 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;
+							}
+						}
+					}
+
+					// TODO: send the unallocated value directly for better anti-windup
+					_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
+				}
+
+			} else {
+				if (_control_allocator_status_subs[0].update(&control_allocator_status)) {
+					Vector<bool, 3> saturation_positive;
+					Vector<bool, 3> saturation_negative;
+
+					if (!control_allocator_status.torque_setpoint_achieved) {
+						for (size_t i = 0; i < 3; 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;
+							}
+						}
+					}
+
+					// TODO: send the unallocated value directly for better anti-windup
+					_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
+				}
+			}
+
 			/* Prepare data for attitude controllers */
 			ECL_ControlData control_input{};
 			control_input.roll = euler_angles.phi();
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
index b82a6417f9..ae9d1fddb4 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
@@ -56,12 +56,14 @@
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_status.h>
 #include <uORB/topics/airspeed_validated.h>
 #include <uORB/topics/autotune_attitude_control_status.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/control_allocator_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/rate_ctrl_status.h>
@@ -127,6 +129,8 @@ private:
 	uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
 
+	uORB::SubscriptionMultiArray<control_allocator_status_s, 2> _control_allocator_status_subs{ORB_ID::control_allocator_status};
+
 	uORB::SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
 
 	uORB::Publication<actuator_controls_s>		_actuator_controls_0_pub;