systemlib mixer: Add public method for multi_rotor saturation status

src/modules/systemlib/mixer/mixer.cpp

@@ -169,6 +169,12 @@ NullMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 	return 0;
 }
 
+uint16_t
+NullMixer::get_saturation_status()
+{
+	return 0;
+}
+
 void
 NullMixer::groups_required(uint32_t &groups)
 {

src/modules/systemlib/mixer/mixer.h

@@ -176,6 +176,13 @@ public:
 	 */
 	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg) = 0;
 
+	/**
+	 * Get the saturation status.
+	 *
+	 * @return			Integer bitmask containing saturation_status from multirotor_motor_limits.msg .
+	 */
+	virtual uint16_t		get_saturation_status(void) = 0;
+
 	/**
 	 * Analyses the mix configuration and updates a bitmask of groups
 	 * that are required.
@@ -261,6 +268,7 @@ public:
 	~MixerGroup();
 
 	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
+	virtual uint16_t		get_saturation_status(void);
 	virtual void			groups_required(uint32_t &groups);
 
 	/**
@@ -394,6 +402,7 @@ public:
 	static NullMixer		*from_text(const char *buf, unsigned &buflen);
 
 	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
+	virtual uint16_t		get_saturation_status(void);
 	virtual void			groups_required(uint32_t &groups);
 	virtual void 			set_offset(float trim) {};
 	unsigned set_trim(float trim)
@@ -465,6 +474,7 @@ public:
 			uint16_t max);
 
 	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
+	virtual uint16_t		get_saturation_status(void);
 	virtual void			groups_required(uint32_t &groups);
 
 	/**
@@ -571,6 +581,7 @@ public:
 			unsigned &buflen);
 
 	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
+	virtual uint16_t		get_saturation_status(void);
 	virtual void			groups_required(uint32_t &groups);
 
 	/**
@@ -602,19 +613,19 @@ private:
 	multirotor_motor_limits_s 	_limits;
 
 	union {
-	    struct {
+		struct {
-		uint16_t motor_pos	: 1; // 0 - true when any motor has saturated in the positive direction
+			uint16_t motor_pos	: 1; // 0 - true when any motor has saturated in the positive direction
-		uint16_t motor_neg	: 1; // 1 - true when any motor has saturated in the negative direction
+			uint16_t motor_neg	: 1; // 1 - true when any motor has saturated in the negative direction
-		uint16_t roll_pos	: 1; // 2 - true when a positive roll demand change will increase saturation
+			uint16_t roll_pos	: 1; // 2 - true when a positive roll demand change will increase saturation
-		uint16_t roll_neg	: 1; // 3 - true when a negative roll demand change will increase saturation
+			uint16_t roll_neg	: 1; // 3 - true when a negative roll demand change will increase saturation
-		uint16_t pitch_pos	: 1; // 4 - true when a positive pitch demand change will increase saturation
+			uint16_t pitch_pos	: 1; // 4 - true when a positive pitch demand change will increase saturation
-		uint16_t pitch_neg	: 1; // 5 - true when a negative pitch demand change will increase saturation
+			uint16_t pitch_neg	: 1; // 5 - true when a negative pitch demand change will increase saturation
-		uint16_t yaw_pos	: 1; // 6 - true when a positive yaw demand change will increase saturation
+			uint16_t yaw_pos	: 1; // 6 - true when a positive yaw demand change will increase saturation
-		uint16_t yaw_neg	: 1; // 7 - true when a negative yaw demand change will increase saturation
+			uint16_t yaw_neg	: 1; // 7 - true when a negative yaw demand change will increase saturation
-		uint16_t thrust_pos	: 1; // 8 - true when a positive thrust demand change will increase saturation
+			uint16_t thrust_pos	: 1; // 8 - true when a positive thrust demand change will increase saturation
-		uint16_t thrust_neg	: 1; // 9 - true when a negative thrust demand change will increase saturation
+			uint16_t thrust_neg	: 1; // 9 - true when a negative thrust demand change will increase saturation
-	    } flags;
+		} flags;
-	    uint16_t value;
+		uint16_t value;
 	} _saturation_status;
 
 	void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low);

src/modules/systemlib/mixer/mixer_group.cpp

@@ -141,6 +141,13 @@ MixerGroup::set_trims(int16_t *values, unsigned n)
 	return index;
 }
 
+uint16_t
+MixerGroup::get_saturation_status()
+{
+	Mixer	*mixer = _first;
+	return mixer->get_saturation_status();
+}
+
 unsigned
 MixerGroup::count()
 {

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -312,6 +312,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 	if (min_out < 0.0f) {
 		_saturation_status.flags.motor_neg = true;
 	}
+
 	if (max_out > 1.0f) {
 		_saturation_status.flags.motor_pos = true;
 	}
@@ -334,6 +335,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 				yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
 					roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale;
 			}
+
 		} else if (out > 1.0f) {
 			// allow to reduce thrust to get some yaw response
 			float thrust_reduction = fminf(0.15f, out - 1.0f);
@@ -506,3 +508,7 @@ MultirotorMixer::groups_required(uint32_t &groups)
 	groups |= (1 << 0);
 }
 
+uint16_t MultirotorMixer::get_saturation_status()
+{
+	return _saturation_status.value;
+}

src/modules/systemlib/mixer/mixer_simple.cpp

@@ -308,6 +308,12 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 	return 1;
 }
 
+uint16_t
+SimpleMixer::get_saturation_status()
+{
+	return 0;
+}
+
 void
 SimpleMixer::groups_required(uint32_t &groups)
 {