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/lib/mixer/MixerBase/Mixer.hpp

@@ -179,7 +179,7 @@ public:
 	/**
 	 * Get the saturation status.
 	 *
-	 * @return			Integer bitmask containing saturation_status from multirotor_motor_limits.msg.
+	 * @return			Integer bitmask containing saturation_status from control_allocator_status.msg.
 	 */
 	virtual uint16_t		get_saturation_status() { return 0; }
 

src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp

@@ -451,8 +451,8 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
 			_saturation_status.flags.yaw_neg = true;
 		}
 
-		// A positive change in thrust will increase saturation
-		_saturation_status.flags.thrust_pos = true;
+		// A positive change in thrust will increase saturation (Z neg is up)
+		_saturation_status.flags.thrust_neg = true;
 	}
 
 	// The motor is saturated at the lower limit
@@ -478,8 +478,8 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
 			_saturation_status.flags.pitch_pos = true;
 		}
 
-		// A negative change in thrust will increase saturation
-		_saturation_status.flags.thrust_neg = true;
+		// A negative change in thrust will increase saturation (Z pos is down)
+		_saturation_status.flags.thrust_pos = true;
 	}
 
 	if (clipping_low_yaw) {

src/lib/mixer_module/mixer_module.cpp

@@ -861,11 +861,51 @@ MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs)
 	saturation_status.value = _mixers->get_saturation_status();
 
 	if (saturation_status.flags.valid) {
-		multirotor_motor_limits_s motor_limits;
-		motor_limits.timestamp = actuator_outputs.timestamp;
-		motor_limits.saturation_status = saturation_status.value;
+		control_allocator_status_s sat{};
+		sat.timestamp = hrt_absolute_time();
+		sat.torque_setpoint_achieved = true;
+		sat.thrust_setpoint_achieved = true;
 
-		_to_mixer_status.publish(motor_limits);
+		// 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_status.flags.roll_pos) {
+			sat.unallocated_torque[0] = 1.f;
+			sat.torque_setpoint_achieved = false;
+
+		} else if (saturation_status.flags.roll_neg) {
+			sat.unallocated_torque[0] = -1.f;
+			sat.torque_setpoint_achieved = false;
+		}
+
+		if (saturation_status.flags.pitch_pos) {
+			sat.unallocated_torque[1] = 1.f;
+			sat.torque_setpoint_achieved = false;
+
+		} else if (saturation_status.flags.pitch_neg) {
+			sat.unallocated_torque[1] = -1.f;
+			sat.torque_setpoint_achieved = false;
+		}
+
+		if (saturation_status.flags.yaw_pos) {
+			sat.unallocated_torque[2] = 1.f;
+			sat.torque_setpoint_achieved = false;
+
+		} else if (saturation_status.flags.yaw_neg) {
+			sat.unallocated_torque[2] = -1.f;
+			sat.torque_setpoint_achieved = false;
+		}
+
+		if (saturation_status.flags.thrust_pos) {
+			sat.unallocated_thrust[2] = 1.f;
+			sat.thrust_setpoint_achieved = false;
+
+		} else if (saturation_status.flags.thrust_neg) {
+			sat.unallocated_thrust[2] = -1.f;
+			sat.thrust_setpoint_achieved = false;
+		}
+
+		_control_allocator_status_pub.publish(sat);
 	}
 }
 

src/lib/mixer_module/mixer_module.hpp

@@ -50,7 +50,7 @@
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/multirotor_motor_limits.h>
+#include <uORB/topics/control_allocator_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/test_motor.h>
 
@@ -294,7 +294,7 @@ private:
 	uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 
 	uORB::PublicationMulti<actuator_outputs_s> _outputs_pub{ORB_ID(actuator_outputs)};
-	uORB::PublicationMulti<multirotor_motor_limits_s> _to_mixer_status{ORB_ID(multirotor_motor_limits)}; 	///< mixer status flags
+	uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub{ORB_ID(control_allocator_status)};
 
 	actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {};
 	actuator_armed_s _armed{};