Merge pull request #2008 from PX4/mixer_limit_publication

fixed publication of mixer limit flags

16 files changed, 69 insertions, 48 deletions

diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index 9d28b48d4..961ec4724 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -402,7 +402,7 @@ HIL::task_main()
 			if (_mixers != nullptr) {
 
 				/* do mixing */
-				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
 				outputs.timestamp = hrt_absolute_time();
 
 				/* iterate actuators */
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index cb463eb59..3b8c4ee77 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -536,7 +536,7 @@ MK::task_main()
 				if (_mixers != nullptr) {
 
 					/* do mixing */
-					outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs);
+					outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs, NULL);
 					outputs.timestamp = hrt_absolute_time();
 
 					/* iterate actuators */
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 7b09a4676..92afc7cd7 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -664,7 +664,7 @@ PX4FMU::task_main()
 				}
 
 				/* do mixing */
-				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
 				outputs.timestamp = hrt_absolute_time();
 
 				/* iterate actuators */
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 3bd2c06a4..33125699f 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -88,6 +88,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/servorail_status.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/multirotor_motor_limits.h>
 
 #include <debug.h>
 
@@ -288,6 +289,7 @@ private:
 	orb_advert_t		_to_battery;		///< battery status / voltage
 	orb_advert_t		_to_servorail;		///< servorail status
 	orb_advert_t		_to_safety;		///< status of safety
+	orb_advert_t 		_to_mixer_status; 	///< mixer status flags
 
 	actuator_outputs_s	_outputs;		///< mixed outputs
 	servorail_status_s	_servorail_status;	///< servorail status
@@ -513,6 +515,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_to_battery(0),
 	_to_servorail(0),
 	_to_safety(0),
+	_to_mixer_status(0),
 	_outputs{},
 	_servorail_status{},
 	_primary_pwm_device(false),
@@ -1687,6 +1690,8 @@ PX4IO::io_publish_pwm_outputs()
 {
 	/* data we are going to fetch */
 	actuator_outputs_s outputs;
+	multirotor_motor_limits_s motor_limits;
+
 	outputs.timestamp = hrt_absolute_time();
 
 	/* get servo values from IO */
@@ -1718,6 +1723,21 @@ PX4IO::io_publish_pwm_outputs()
 		orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs);
 	}
 
+	/* get mixer status flags from IO */
+	uint16_t mixer_status;
+	ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &mixer_status,sizeof(mixer_status)/sizeof(uint16_t));
+	memcpy(&motor_limits,&mixer_status,sizeof(motor_limits));
+
+	if (ret != OK)
+		return ret;
+
+	/* publish mixer status */
+	if(_to_mixer_status == 0) {
+		_to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &motor_limits);
+	} else {
+		orb_publish(ORB_ID(multirotor_motor_limits),_to_mixer_status, &motor_limits);
+	}
+
 	return OK;
 }
 
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 835dfc0f5..6fa26d4ff 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -230,7 +230,7 @@ mixer_tick(void)
 
 		/* poor mans mutex */
 		in_mixer = true;
-		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
+		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);
 		in_mixer = false;
 
 		/* the pwm limit call takes care of out of band errors */
@@ -453,7 +453,7 @@ mixer_set_failsafe()
 	unsigned mixed;
 
 	/* mix */
-	mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
+	mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);
 
 	/* scale to PWM and update the servo outputs as required */
 	for (unsigned i = 0; i < mixed; i++) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 874dc0c39..cbafa3641 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -133,6 +133,11 @@
 #define PX4IO_P_STATUS_VRSSI			7	/* [2] RSSI voltage */
 #define PX4IO_P_STATUS_PRSSI			8	/* [2] RSSI PWM value */
 
+#define PX4IO_P_STATUS_MIXER			9	 /* mixer actuator limit flags */
+#define PX4IO_P_STATUS_MIXER_LOWER_LIMIT 		(1 << 0) /**< at least one actuator output has reached lower limit */
+#define PX4IO_P_STATUS_MIXER_UPPER_LIMIT 		(1 << 1) /**< at least one actuator output has reached upper limit */
+#define PX4IO_P_STATUS_MIXER_YAW_LIMIT 			(1 << 2) /**< yaw control is limited because it causes output clipping */
+
 /* array of post-mix actuator outputs, -10000..10000 */
 #define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 17699b7c0..8ddf45a12 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -98,8 +98,9 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 #define r_raw_rc_count		r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
 #define r_raw_rc_values		(&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
 #define r_raw_rc_flags		r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
-#define r_rc_valid		r_page_rc_input[PX4IO_P_RC_VALID]
-#define r_rc_values		(&r_page_rc_input[PX4IO_P_RC_BASE])
+#define r_rc_valid			r_page_rc_input[PX4IO_P_RC_VALID]
+#define r_rc_values			(&r_page_rc_input[PX4IO_P_RC_BASE])
+#define r_mixer_limits 		r_page_status[PX4IO_P_STATUS_MIXER]
 
 #define r_setup_features	r_page_setup[PX4IO_P_SETUP_FEATURES]
 #define r_setup_arming		r_page_setup[PX4IO_P_SETUP_ARMING]
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index ae7aec34e..90e0fb10e 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -90,6 +90,7 @@ uint16_t		r_page_status[] = {
 	[PX4IO_P_STATUS_VSERVO]			= 0,
 	[PX4IO_P_STATUS_VRSSI]			= 0,
 	[PX4IO_P_STATUS_PRSSI]			= 0,
+	[PX4IO_P_STATUS_MIXER]			= 0,
 };
 
 /**
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;
 
diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h
index 44c51e4d9..589f8a650 100644
--- a/src/modules/uORB/topics/multirotor_motor_limits.h
+++ b/src/modules/uORB/topics/multirotor_motor_limits.h
@@ -52,11 +52,10 @@
  * Motor limits
  */
 struct multirotor_motor_limits_s {
-        uint8_t roll_pitch	: 1; // roll/pitch limit reached
-        uint8_t yaw		: 1; // yaw limit reached
-        uint8_t throttle_lower	: 1; // lower throttle limit reached
-        uint8_t throttle_upper	: 1; // upper throttle limit reached
-        uint8_t reserved	: 4;
+        uint8_t lower_limit	: 1; // at least one actuator command has saturated on the lower limit
+        uint8_t upper_limit	: 1; // at least one actuator command has saturated on the upper limit
+        uint8_t yaw			: 1; // yaw limit reached
+        uint8_t reserved	: 5; // reserved
 };
 
 /**
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index b93a95f96..02cde9656 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -421,7 +421,7 @@ int UavcanNode::run()
 				unsigned num_outputs_max = 8;
 
 				// Do mixing
-				_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max);
+				_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max, NULL);
 
 				new_output = true;
 			}
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index 64e734b00..19aa05970 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -202,7 +202,7 @@ int test_mixer(int argc, char *argv[])
 	while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) {
 
 		/* mix */
-		mixed = mixer_group.mix(&outputs[0], output_max);
+		mixed = mixer_group.mix(&outputs[0], output_max, NULL);
 
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
 			       r_page_servos, &pwm_limit);
@@ -248,7 +248,7 @@ int test_mixer(int argc, char *argv[])
 		}
 
 		/* mix */
-		mixed = mixer_group.mix(&outputs[0], output_max);
+		mixed = mixer_group.mix(&outputs[0], output_max, NULL);
 
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
 			       r_page_servos, &pwm_limit);
@@ -275,7 +275,7 @@ int test_mixer(int argc, char *argv[])
 	while (hrt_elapsed_time(&starttime) < 600000) {
 
 		/* mix */
-		mixed = mixer_group.mix(&outputs[0], output_max);
+		mixed = mixer_group.mix(&outputs[0], output_max, NULL);
 
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
 			       r_page_servos, &pwm_limit);
@@ -311,7 +311,7 @@ int test_mixer(int argc, char *argv[])
 	while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
 
 		/* mix */
-		mixed = mixer_group.mix(&outputs[0], output_max);
+		mixed = mixer_group.mix(&outputs[0], output_max, NULL);
 
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
 			       r_page_servos, &pwm_limit);