fixed publication of mixer limit flags

5 files changed, 26 insertions, 31 deletions

diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp
index 20b1f18ed..3ab41c5c5 100644
--- a/src/modules/systemlib/mixer/mixer.cpp
+++ b/src/modules/systemlib/mixer/mixer.cpp
@@ -151,7 +151,7 @@ NullMixer::NullMixer() :
 }
 
 unsigned
-NullMixer::mix(float *outputs, unsigned space)
+NullMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 {
 	if (space > 0) {
 		*outputs = 0.0f;
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index 67ef521b4..119068301 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -174,7 +174,7 @@ public:
 	 * @param space			The number of available entries in the output array;
 	 * @return			The number of entries in the output array that were populated.
 	 */
-	virtual unsigned		mix(float *outputs, unsigned space) = 0;
+	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg) = 0;
 
 	/**
 	 * Analyses the mix configuration and updates a bitmask of groups
@@ -250,7 +250,7 @@ public:
 	MixerGroup(ControlCallback control_cb, uintptr_t cb_handle);
 	~MixerGroup();
 
-	virtual unsigned		mix(float *outputs, unsigned space);
+	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
 	virtual void			groups_required(uint32_t &groups);
 
 	/**
@@ -346,7 +346,7 @@ public:
 	 */
 	static NullMixer		*from_text(const char *buf, unsigned &buflen);
 
-	virtual unsigned		mix(float *outputs, unsigned space);
+	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
 	virtual void			groups_required(uint32_t &groups);
 };
 
@@ -411,7 +411,7 @@ public:
 			uint16_t mid,
 			uint16_t max);
 
-	virtual unsigned		mix(float *outputs, unsigned space);
+	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
 	virtual void			groups_required(uint32_t &groups);
 
 	/**
@@ -515,7 +515,7 @@ public:
 			const char *buf,
 			unsigned &buflen);
 
-	virtual unsigned		mix(float *outputs, unsigned space);
+	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
 	virtual void			groups_required(uint32_t &groups);
 
 private:
diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp
index 4220b168d..ca5101e65 100644
--- a/src/modules/systemlib/mixer/mixer_group.cpp
+++ b/src/modules/systemlib/mixer/mixer_group.cpp
@@ -99,13 +99,13 @@ MixerGroup::reset()
 }
 
 unsigned
-MixerGroup::mix(float *outputs, unsigned space)
+MixerGroup::mix(float *outputs, unsigned space, uint16_t *status_reg)
 {
 	Mixer	*mixer = _first;
 	unsigned index = 0;
 
 	while ((mixer != nullptr) && (index < space)) {
-		index += mixer->mix(outputs + index, space - index);
+		index += mixer->mix(outputs + index, space - index, status_reg);
 		mixer = mixer->_next;
 	}
 
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index b753da5ce..e9a8a87ca 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -36,10 +36,7 @@
  *
  * Multi-rotor mixers.
  */
-#include <uORB/uORB.h>
-#include <uORB/topics/multirotor_motor_limits.h>
 #include <nuttx/config.h>
-
 #include <sys/types.h>
 #include <stdint.h>
 #include <stdbool.h>
@@ -53,6 +50,8 @@
 #include <unistd.h>
 #include <math.h>
 
+#include <px4iofirmware/protocol.h>
+
 #include "mixer.h"
 
 // This file is generated by the multi_tables script which is invoked during the build process
@@ -199,7 +198,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 }
 
 unsigned
-MultirotorMixer::mix(float *outputs, unsigned space)
+MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 {
 	float		roll    = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
 	//lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale));
@@ -210,10 +209,9 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	float		min_out = 0.0f;
 	float		max_out = 0.0f;
 
-	_limits.roll_pitch = false;
-	_limits.yaw = false;
-	_limits.throttle_upper = false;
-	_limits.throttle_lower = false;
+	if (status_reg != NULL) {
+		(*status_reg) = 0;
+	}
 
 	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
 	for (unsigned i = 0; i < _rotor_count; i++) {
@@ -226,7 +224,9 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 		/* limit yaw if it causes outputs clipping */
 		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
 			yaw = -out / _rotors[i].yaw_scale;
-			_limits.yaw = true;
+			if(status_reg != NULL) {
+				(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
+			}
 		}
 
 		/* calculate min and max output values */
@@ -257,7 +257,10 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 
 			outputs[i] = out;
 		}
-		_limits.roll_pitch = true;
+
+		if(status_reg != NULL) {
+			(*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT;
+		}
 
 	} else {
 		/* roll/pitch mixed without lower side limiting, add yaw control */
@@ -270,7 +273,10 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	float scale_out;
 	if (max_out > 1.0f) {
 		scale_out = 1.0f / max_out;
-		_limits.throttle_upper = true;
+
+		if(status_reg != NULL) {
+			(*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT;
+		}
 
 	} else {
 		scale_out = 1.0f;
@@ -278,20 +284,9 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 
 	/* scale outputs to range _idle_speed..1, and do final limiting */
 	for (unsigned i = 0; i < _rotor_count; i++) {
-		if (outputs[i] < _idle_speed) {
-			_limits.throttle_lower = true;
-		}
 		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
 	}
 
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
-        /* publish/advertise motor limits if running on FMU */
-        if (_limits_pub > 0) {
-            orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits);
-        } else {
-            _limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits);
-        }
-#endif
 	return _rotor_count;
 }
 
diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp
index c3985b5de..e48bda691 100644
--- a/src/modules/systemlib/mixer/mixer_simple.cpp
+++ b/src/modules/systemlib/mixer/mixer_simple.cpp
@@ -265,7 +265,7 @@ out:
 }
 
 unsigned
-SimpleMixer::mix(float *outputs, unsigned space)
+SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 {
 	float		sum = 0.0f;