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

msg/CMakeLists.txt

@@ -109,7 +109,6 @@ set(msg_files
 	mission.msg
 	mission_result.msg
 	mount_orientation.msg
-	multirotor_motor_limits.msg
 	navigator_mission_item.msg
 	obstacle_distance.msg
 	offboard_control_mode.msg

msg/multirotor_motor_limits.msg

@@ -1,14 +0,0 @@
-uint64 timestamp		# time since system start (microseconds)
-uint16 saturation_status	# Integer bit mask indicating which axes in the control mixer are saturated
-
-# 0 - True if the saturation status is valid
-# 1 - True if any motor is saturated at the upper limit
-# 2 - True if any motor is saturated at the lower limit
-# 3 - True if a positive roll increment will increase motor saturation
-# 4 - True if negative roll increment will increase motor saturation
-# 5 - True if positive pitch increment will increase motor saturation
-# 6 - True if negative pitch increment will increase motor saturation
-# 7 - True if positive yaw increment will increase motor saturation
-# 8 - True if negative yaw increment will increase motor saturation
-# 9 - True if positive thrust increment will increase motor saturation
-# 10 - True if negative thrust increment will increase motor saturation

src/drivers/pwm_out/PWMOut.hpp

@@ -59,7 +59,6 @@
 #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/parameter_update.h>
 
 using namespace time_literals;

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
+		// A positive change in thrust will increase saturation (Z neg is up)
-		_saturation_status.flags.thrust_pos = true;
+		_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
+		// A negative change in thrust will increase saturation (Z pos is down)
-		_saturation_status.flags.thrust_neg = true;
+		_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;
+		control_allocator_status_s sat{};
-		motor_limits.timestamp = actuator_outputs.timestamp;
+		sat.timestamp = hrt_absolute_time();
-		motor_limits.saturation_status = saturation_status.value;
+		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{};

src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp

@@ -68,9 +68,7 @@ ControlAllocation::setActuatorSetpoint(
 	// Clip
 	clipActuatorSetpoint(_actuator_sp);
 
-	// Compute achieved control
+	updateControlAllocated();
-	_control_allocated = _effectiveness * _actuator_sp;
-
 }
 
 void
@@ -89,6 +87,13 @@ ControlAllocation::clipActuatorSetpoint(matrix::Vector<float, ControlAllocation:
 	}
 }
 
+void
+ControlAllocation::updateControlAllocated()
+{
+	// Compute achieved control
+	_control_allocated = (_effectiveness * _actuator_sp).emult(_control_allocation_scale);
+}
+
 matrix::Vector<float, ControlAllocation::NUM_ACTUATORS>
 ControlAllocation::normalizeActuatorSetpoint(const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator)
 const

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -75,7 +75,7 @@
 class ControlAllocation
 {
 public:
-	ControlAllocation() = default;
+	ControlAllocation() { _control_allocation_scale.setAll(1.f); }
 	virtual ~ControlAllocation() = default;
 
 	static constexpr uint8_t NUM_ACTUATORS = 16;
@@ -190,6 +190,11 @@ public:
 	 */
 	void clipActuatorSetpoint(matrix::Vector<float, NUM_ACTUATORS> &actuator) const;
 
+	/**
+	 * Compute the amount of allocated control thrust and torque
+	 */
+	void updateControlAllocated();
+
 	/**
 	 * Normalize the actuator setpoint between minimum and maximum values.
 	 *
@@ -208,6 +213,7 @@ public:
 
 protected:
 	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness;  //< Effectiveness matrix
+	matrix::Vector<float, NUM_AXES> _control_allocation_scale;  	//< Scaling applied during allocation
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_trim; 	//< Neutral actuator values
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_min; 	//< Minimum actuator values
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_max; 	//< Maximum actuator values

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp

@@ -66,26 +66,28 @@ ControlAllocationPseudoInverse::normalizeControlAllocationMatrix()
 	// Same scale on roll and pitch
 	const float roll_norm_sq = _mix.col(0).norm_squared();
 	const float pitch_norm_sq = _mix.col(1).norm_squared();
-	const float roll_pitch_scale = sqrtf(fmaxf(roll_norm_sq, pitch_norm_sq) / (_num_actuators / 2.f));
+	_control_allocation_scale(0) = sqrtf(fmaxf(roll_norm_sq, pitch_norm_sq) / (_num_actuators / 2.f));
+	_control_allocation_scale(1) = _control_allocation_scale(0);
 
-	if (roll_pitch_scale > FLT_EPSILON) {
+	if (_control_allocation_scale(0) > FLT_EPSILON) {
-		_mix.col(0) /= roll_pitch_scale;
+		_mix.col(0) /= _control_allocation_scale(0);
-		_mix.col(1) /= roll_pitch_scale;
+		_mix.col(1) /= _control_allocation_scale(1);
 	}
 
 	// Scale yaw separately
-	const float yaw_scale = _mix.col(2).max();
+	_control_allocation_scale(2) = _mix.col(2).max();
 
-	if (yaw_scale > FLT_EPSILON) {
+	if (_control_allocation_scale(2) > FLT_EPSILON) {
-		_mix.col(2) /= yaw_scale;
+		_mix.col(2) /= _control_allocation_scale(2);
 	}
 
 	// Same scale on X and Y
-	const float xy_scale = fmaxf(_mix.col(3).max(), _mix.col(4).max());
+	_control_allocation_scale(3) = fmaxf(_mix.col(3).max(), _mix.col(4).max());
+	_control_allocation_scale(4) = _control_allocation_scale(3);
 
-	if (xy_scale > FLT_EPSILON) {
+	if (_control_allocation_scale(3) > FLT_EPSILON) {
-		_mix.col(3) /= xy_scale;
+		_mix.col(3) /= _control_allocation_scale(3);
-		_mix.col(4) /= xy_scale;
+		_mix.col(4) /= _control_allocation_scale(4);
 	}
 
 	// Scale Z thrust separately
@@ -97,7 +99,8 @@ ControlAllocationPseudoInverse::normalizeControlAllocationMatrix()
 	}
 
 	if ((-z_sum > FLT_EPSILON) && (_num_actuators > 0)) {
-		_mix.col(5) /= -z_sum / _num_actuators;
+		_control_allocation_scale(5) = -z_sum / _num_actuators;
+		_mix.col(5) /= _control_allocation_scale(5);
 	}
 
 	// Set all the small elements to 0 to avoid issues
@@ -123,6 +126,5 @@ ControlAllocationPseudoInverse::allocate()
 	// Clip
 	clipActuatorSetpoint(_actuator_sp);
 
-	// Compute achieved control
+	ControlAllocation::updateControlAllocated();
-	_control_allocated = _effectiveness * _actuator_sp;
 }

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp

@@ -66,8 +66,7 @@ ControlAllocationSequentialDesaturation::allocate()
 	// Clip
 	clipActuatorSetpoint(_actuator_sp);
 
-	// Compute achieved control
+	ControlAllocation::updateControlAllocated();
-	_control_allocated = _effectiveness * _actuator_sp;
 }
 
 void ControlAllocationSequentialDesaturation::desaturateActuators(

src/modules/control_allocator/ControlAllocator.hpp

@@ -112,7 +112,6 @@ private:
 	void publish_control_allocator_status();
 
 	void publish_legacy_actuator_controls();
-	void publish_legacy_multirotor_motor_limits();
 
 	enum class AllocationMethod {
 		NONE = -1,

src/modules/logger/logged_topics.cpp

@@ -118,7 +118,7 @@ void LoggedTopics::add_default_topics()
 	// multi topics
 	add_topic_multi("actuator_outputs", 100, 3);
 	add_topic_multi("airspeed_wind", 1000);
-	add_topic_multi("multirotor_motor_limits", 1000, 2);
+	add_topic_multi("control_allocator_status", 200, 2);
 	add_topic_multi("rate_ctrl_status", 200, 2);
 	add_topic_multi("telemetry_status", 1000, 4);
 

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -215,17 +215,27 @@ MulticopterRateControl::Run()
 				_rate_control.resetIntegral();
 			}
 
-			// update saturation status from mixer feedback
+			// update saturation status from control allocation feedback
-			if (_motor_limits_sub.updated()) {
+			control_allocator_status_s control_allocator_status;
-				multirotor_motor_limits_s motor_limits;
 
-				if (_motor_limits_sub.copy(&motor_limits)) {
+			if (_control_allocator_status_sub.update(&control_allocator_status)) {
-					MultirotorMixer::saturation_status saturation_status;
+				Vector<bool, 3> saturation_positive;
-					saturation_status.value = motor_limits.saturation_status;
+				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
 			const Vector3f att_control = _rate_control.update(rates, _rates_sp, angular_accel, dt, _maybe_landed || _landed);

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;
+	_control_allocator_saturation_positive = saturation_positive;
-	_mixer_saturation_positive[1] = status.flags.pitch_pos;
+	_control_allocator_saturation_negative = saturation_negative;
-	_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;
 }
 
 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] {};
+	// Feedback from control allocation
-	bool _mixer_saturation_negative[3] {};
+	matrix::Vector<bool, 3> _control_allocator_saturation_negative;
+	matrix::Vector<bool, 3> _control_allocator_saturation_positive;
 };