MC mixer: replace multirotor_motor_limits by control_allocator_status

CA: fix saturation computation
Since the CA matrix is normalized, the same scale applied to be used when using the effectiveness matrix

MCRateControl: use control_allocator_status to get saturation info

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -215,16 +215,26 @@ MulticopterRateControl::Run()
 				_rate_control.resetIntegral();
 			}
 
-			// update saturation status from mixer feedback
-			if (_motor_limits_sub.updated()) {
-				multirotor_motor_limits_s motor_limits;
+			// update saturation status from control allocation feedback
+			control_allocator_status_s control_allocator_status;
 
-				if (_motor_limits_sub.copy(&motor_limits)) {
-					MultirotorMixer::saturation_status saturation_status;
-					saturation_status.value = motor_limits.saturation_status;
+			if (_control_allocator_status_sub.update(&control_allocator_status)) {
+				Vector<bool, 3> saturation_positive;
+				Vector<bool, 3> saturation_negative;
 
-					_rate_control.setSaturationStatus(saturation_status);
+				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);
 			}
 
 			// run rate controller

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -50,9 +50,9 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_status.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/control_allocator_status.h>
 #include <uORB/topics/landing_gear.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/multirotor_motor_limits.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/rate_ctrl_status.h>
 #include <uORB/topics/vehicle_angular_acceleration.h>
@@ -101,7 +101,7 @@ private:
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
 	uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)};
+	uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
 	uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};
 	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};

src/modules/mc_rate_control/RateControl/RateControl.cpp

@@ -47,14 +47,11 @@ void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f
 	_gain_d = D;
 }
 
-void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status &status)
+void RateControl::setSaturationStatus(const Vector<bool, 3> &saturation_positive,
+				      const Vector<bool, 3> &saturation_negative)
 {
-	_mixer_saturation_positive[0] = status.flags.roll_pos;
-	_mixer_saturation_positive[1] = status.flags.pitch_pos;
-	_mixer_saturation_positive[2] = status.flags.yaw_pos;
-	_mixer_saturation_negative[0] = status.flags.roll_neg;
-	_mixer_saturation_negative[1] = status.flags.pitch_neg;
-	_mixer_saturation_negative[2] = status.flags.yaw_neg;
+	_control_allocator_saturation_positive = saturation_positive;
+	_control_allocator_saturation_negative = saturation_negative;
 }
 
 Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel,
@@ -78,12 +75,12 @@ void RateControl::updateIntegral(Vector3f &rate_error, const float dt)
 {
 	for (int i = 0; i < 3; i++) {
 		// prevent further positive control saturation
-		if (_mixer_saturation_positive[i]) {
+		if (_control_allocator_saturation_positive(i)) {
 			rate_error(i) = math::min(rate_error(i), 0.f);
 		}
 
 		// prevent further negative control saturation
-		if (_mixer_saturation_negative[i]) {
+		if (_control_allocator_saturation_negative(i)) {
 			rate_error(i) = math::max(rate_error(i), 0.f);
 		}
 

src/modules/mc_rate_control/RateControl/RateControl.hpp

@@ -73,9 +73,10 @@ public:
 
 	/**
 	 * Set saturation status
-	 * @param status message from mixer reporting about saturation
+	 * @param control saturation vector from control allocator
 	 */
-	void setSaturationStatus(const MultirotorMixer::saturation_status &status);
+	void setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
+				 const matrix::Vector<bool, 3> &saturation_negative);
 
 	/**
 	 * Run one control loop cycle calculation
@@ -112,6 +113,7 @@ private:
 	// States
 	matrix::Vector3f _rate_int; ///< integral term of the rate controller
 
-	bool _mixer_saturation_positive[3] {};
-	bool _mixer_saturation_negative[3] {};
+	// Feedback from control allocation
+	matrix::Vector<bool, 3> _control_allocator_saturation_negative;
+	matrix::Vector<bool, 3> _control_allocator_saturation_positive;
 };