multirotor_motor_limits only publish for MC

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -64,6 +64,7 @@
 #include <px4_tasks.h>
 #include <systemlib/circuit_breaker.h>
 #include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
@@ -161,27 +162,12 @@ private:
 	struct actuator_controls_s			_actuators;			/**< actuator controls */
 	struct actuator_armed_s				_armed;				/**< actuator arming status */
 	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
-	struct multirotor_motor_limits_s	_motor_limits;		/**< motor limits */
 	struct mc_att_ctrl_status_s 		_controller_status; /**< controller status */
 	struct battery_status_s				_battery_status;	/**< battery status */
 	struct sensor_gyro_s			_sensor_gyro;		/**< gyro data before thermal correctons and ekf bias estimates are applied */
 	struct sensor_correction_s		_sensor_correction;		/**< sensor thermal corrections */
 
-	union {
-		struct {
-			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 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 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 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 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
-		} flags;
-		uint16_t value;
-	} _saturation_status;
+	MultirotorMixer::saturation_status _saturation_status{};
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 	perf_counter_t	_controller_latency_perf;
@@ -415,13 +401,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_actuators{},
 	_armed{},
 	_vehicle_status{},
-	_motor_limits{},
 	_controller_status{},
 	_battery_status{},
 	_sensor_gyro{},
 	_sensor_correction{},
-
-	_saturation_status{},
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
 	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
@@ -784,8 +767,10 @@ MulticopterAttitudeControl::vehicle_motor_limits_poll()
 	orb_check(_motor_limits_sub, &updated);
 
 	if (updated) {
-		orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &_motor_limits);
-		_saturation_status.value = _motor_limits.saturation_status;
+		multirotor_motor_limits_s motor_limits = {};
+		orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &motor_limits);
+
+		_saturation_status.value = motor_limits.saturation_status;
 	}
 }
 

src/modules/px4iofirmware/mixer.cpp

@@ -91,11 +91,7 @@ enum mixer_source {
 
 static volatile mixer_source source;
 
-static int	mixer_callback(uintptr_t handle,
-			       uint8_t control_group,
-			       uint8_t control_index,
-			       float &control);
-
+static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control);
 static int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits);
 
 static MixerGroup mixer_group(mixer_callback, 0);
@@ -107,10 +103,9 @@ int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits)
 		return 0;
 	}
 
-	uint16_t mixer_limits = 0;
 	in_mixer = true;
-	int mixcount = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &mixer_limits);
-	*limits = mixer_limits;
+	int mixcount = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
+	*limits = mixer_group.get_saturation_status();
 	in_mixer = false;
 
 	return mixcount;
@@ -600,7 +595,6 @@ mixer_set_failsafe()
 	}
 
 	/* mix */
-
 	mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
 
 	/* scale to PWM and update the servo outputs as required */

src/modules/systemlib/mixer/mixer.cpp

@@ -184,7 +184,7 @@ NullMixer::NullMixer() :
 }
 
 unsigned
-NullMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
+NullMixer::mix(float *outputs, unsigned space)
 {
 	if (space > 0) {
 		*outputs = 0.0f;

src/modules/systemlib/mixer/mixer.h

@@ -131,8 +131,6 @@
 #include <px4_config.h>
 #include "drivers/drv_mixer.h"
 
-#include <uORB/topics/multirotor_motor_limits.h>
-
 #include "mixer_load.h"
 
 /**
@@ -174,12 +172,12 @@ 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, uint16_t *status_reg) = 0;
+	virtual unsigned		mix(float *outputs, unsigned space) = 0;
 
 	/**
 	 * Get the saturation status.
 	 *
-	 * @return			Integer bitmask containing saturation_status from multirotor_motor_limits.msg .
+	 * @return			Integer bitmask containing saturation_status from multirotor_motor_limits.msg.
 	 */
 	virtual uint16_t		get_saturation_status(void) = 0;
 
@@ -284,7 +282,7 @@ public:
 	MixerGroup(ControlCallback control_cb, uintptr_t cb_handle);
 	~MixerGroup();
 
-	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
+	virtual unsigned		mix(float *outputs, unsigned space);
 	virtual uint16_t		get_saturation_status(void);
 	virtual void			groups_required(uint32_t &groups);
 
@@ -425,7 +423,7 @@ public:
 	 */
 	static NullMixer		*from_text(const char *buf, unsigned &buflen);
 
-	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
+	virtual unsigned		mix(float *outputs, unsigned space);
 	virtual uint16_t		get_saturation_status(void);
 	virtual void			groups_required(uint32_t &groups);
 	virtual void 			set_offset(float trim) {}
@@ -490,14 +488,10 @@ public:
 	 * @return			A new SimpleMixer instance, or nullptr if one could not be
 	 *				allocated.
 	 */
-	static SimpleMixer		*pwm_input(Mixer::ControlCallback control_cb,
-			uintptr_t cb_handle,
-			unsigned input,
-			uint16_t min,
-			uint16_t mid,
-			uint16_t max);
+	static SimpleMixer		*pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min,
+			uint16_t mid, uint16_t max);
 
-	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
+	virtual unsigned		mix(float *outputs, unsigned space);
 	virtual uint16_t		get_saturation_status(void);
 	virtual void			groups_required(uint32_t &groups);
 
@@ -599,12 +593,10 @@ public:
 	 * @return			A new MultirotorMixer instance, or nullptr
 	 *				if the text format is bad.
 	 */
-	static MultirotorMixer		*from_text(Mixer::ControlCallback control_cb,
-			uintptr_t cb_handle,
-			const char *buf,
+	static MultirotorMixer		*from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
 			unsigned &buflen);
 
-	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
+	virtual unsigned		mix(float *outputs, unsigned space);
 	virtual uint16_t		get_saturation_status(void);
 	virtual void			groups_required(uint32_t &groups);
 
@@ -618,7 +610,7 @@ public:
 	 * @param[in]  delta_out_max  Maximum delta output.
 	 *
 	 */
-	virtual void 			set_max_delta_out_once(float delta_out_max) {_delta_out_max = delta_out_max;}
+	virtual void 			set_max_delta_out_once(float delta_out_max) { _delta_out_max = delta_out_max; }
 
 	unsigned set_trim(float trim)
 	{
@@ -632,6 +624,23 @@ public:
 	 */
 	virtual void			set_thrust_factor(float val) {_thrust_factor = val;}
 
+	union saturation_status {
+		struct {
+			uint16_t valid		: 1; // 0 - true when the saturation status is used
+			uint16_t motor_pos	: 1; // 1 - true when any motor has saturated in the positive direction
+			uint16_t motor_neg	: 1; // 2 - true when any motor has saturated in the negative direction
+			uint16_t roll_pos	: 1; // 3 - true when a positive roll demand change will increase saturation
+			uint16_t roll_neg	: 1; // 4 - true when a negative roll demand change will increase saturation
+			uint16_t pitch_pos	: 1; // 5 - true when a positive pitch demand change will increase saturation
+			uint16_t pitch_neg	: 1; // 6 - true when a negative pitch demand change will increase saturation
+			uint16_t yaw_pos	: 1; // 7 - true when a positive yaw demand change will increase saturation
+			uint16_t yaw_neg	: 1; // 8 - true when a negative yaw demand change will increase saturation
+			uint16_t thrust_pos	: 1; // 9 - true when a positive thrust demand change will increase saturation
+			uint16_t thrust_neg	: 1; //10 - true when a negative thrust demand change will increase saturation
+		} flags;
+		uint16_t value;
+	};
+
 private:
 	float				_roll_scale;
 	float				_pitch_scale;
@@ -640,27 +649,8 @@ private:
 	float 				_delta_out_max;
 	float 				_thrust_factor;
 
-
-	orb_advert_t			_limits_pub;
-	multirotor_motor_limits_s 	_limits;
-
-	union {
-		struct {
-			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 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 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 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 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
-		} flags;
-		uint16_t value;
-	} _saturation_status;
-
 	void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low);
+	saturation_status _saturation_status;
 
 	unsigned			_rotor_count;
 	const Rotor			*_rotors;
@@ -729,15 +719,14 @@ public:
 	 * @return			A new HelicopterMixer instance, or nullptr
 	 *				if the text format is bad.
 	 */
-	static HelicopterMixer		*from_text(Mixer::ControlCallback control_cb,
-			uintptr_t cb_handle,
-			const char *buf,
+	static HelicopterMixer		*from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
 			unsigned &buflen);
 
-	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
+	virtual unsigned		mix(float *outputs, unsigned space);
 	virtual void			groups_required(uint32_t &groups);
 
 	virtual uint16_t		get_saturation_status(void) { return 0; }
+
 	unsigned set_trim(float trim)
 	{
 		return 4;

src/modules/systemlib/mixer/mixer_group.cpp

@@ -103,13 +103,13 @@ MixerGroup::reset()
 }
 
 unsigned
-MixerGroup::mix(float *outputs, unsigned space, uint16_t *status_reg)
+MixerGroup::mix(float *outputs, unsigned space)
 {
 	Mixer	*mixer = _first;
 	unsigned index = 0;
 
 	while ((mixer != nullptr) && (index < space)) {
-		index += mixer->mix(outputs + index, space - index, status_reg);
+		index += mixer->mix(outputs + index, space - index);
 		mixer = mixer->_next;
 	}
 

src/modules/systemlib/mixer/mixer_helicopter.cpp

@@ -234,7 +234,7 @@ HelicopterMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 }
 
 unsigned
-HelicopterMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
+HelicopterMixer::mix(float *outputs, unsigned space)
 {
 	/* Find index to use for curves */
 	float thrust_cmd = get_control(0, 3);

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -83,7 +83,6 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 	_idle_speed(-1.0f + idle_speed * 2.0f),	/* shift to output range here to avoid runtime calculation */
 	_delta_out_max(0.0f),
 	_thrust_factor(0.0f),
-	_limits_pub(),
 	_rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]),
 	_rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]),
 	_outputs_prev(new float[_rotor_count])
@@ -178,13 +177,6 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	} else if (!strcmp(geomname, "6a")) {
 		geometry = MultirotorGeometry::DODECA_BOTTOM_COX;
 
-
-#if 0
-
-	} else if (!strcmp(geomname, "8cw")) {
-		geometry = MultirotorGeometry::OCTA_COX_WIDE;
-#endif
-
 	} else if (!strcmp(geomname, "2-")) {
 		geometry = MultirotorGeometry::TWIN_ENGINE;
 
@@ -209,7 +201,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 }
 
 unsigned
-MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
+MultirotorMixer::mix(float *outputs, unsigned space)
 {
 	/* Summary of mixing strategy:
 	1) mix roll, pitch and thrust without yaw.
@@ -404,22 +396,16 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 
 		// update the saturation status report
 		update_saturation_status(i, clipping_high, clipping_low);
-
 	}
 
 	// this will force the caller of the mixer to always supply new slew rate values, otherwise no slew rate limiting will happen
 	_delta_out_max = 0.0f;
 
-	// Notify saturation status
-	if (status_reg != nullptr) {
-		(*status_reg) = _saturation_status.value;
-	}
-
 	return _rotor_count;
 }
 
 /*
- * This function update the control saturation status report using hte following inputs:
+ * This function update the control saturation status report using the following inputs:
  *
  * index: 0 based index identifying the motor that is saturating
  * clipping_high: true if the motor demand is being limited in the positive direction
@@ -438,7 +424,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
 		} else if (_rotors[index].roll_scale < 0.0f) {
 			// A negative change in roll will increase saturation
 			_saturation_status.flags.roll_neg = true;
-
 		}
 
 		// check if the pitch input is saturating
@@ -449,7 +434,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
 		} else if (_rotors[index].pitch_scale < 0.0f) {
 			// A negative change in pitch will increase saturation
 			_saturation_status.flags.pitch_neg = true;
-
 		}
 
 		// check if the yaw input is saturating
@@ -460,7 +444,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
 		} else if (_rotors[index].yaw_scale < 0.0f) {
 			// A negative change in yaw will increase saturation
 			_saturation_status.flags.yaw_neg = true;
-
 		}
 
 		// A positive change in thrust will increase saturation
@@ -479,7 +462,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
 		} else if (_rotors[index].roll_scale < 0.0f) {
 			// A positive change in roll will increase saturation
 			_saturation_status.flags.roll_pos = true;
-
 		}
 
 		// check if the pitch input is saturating
@@ -490,7 +472,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
 		} else if (_rotors[index].pitch_scale < 0.0f) {
 			// A positive change in pitch will increase saturation
 			_saturation_status.flags.pitch_pos = true;
-
 		}
 
 		// check if the yaw input is saturating
@@ -501,13 +482,13 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
 		} else if (_rotors[index].yaw_scale < 0.0f) {
 			// A positive change in yaw will increase saturation
 			_saturation_status.flags.yaw_pos = true;
-
 		}
 
 		// A negative change in thrust will increase saturation
 		_saturation_status.flags.thrust_neg = true;
-
 	}
+
+	_saturation_status.flags.valid = true;
 }
 
 void

src/modules/systemlib/mixer/mixer_simple.cpp

@@ -286,7 +286,7 @@ out:
 }
 
 unsigned
-SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
+SimpleMixer::mix(float *outputs, unsigned space)
 {
 	float		sum = 0.0f;
 

src/modules/systemlib/system_params.c

@@ -77,19 +77,6 @@ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
  */
 PARAM_DEFINE_INT32(SYS_HITL, 0);
 
-/**
- * Set usage of IO board
- *
- * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up.
- *
- * @boolean
- * @min 0
- * @max 1
- * @reboot_required true
- * @group System
- */
-PARAM_DEFINE_INT32(SYS_USE_IO, 1);
-
 /**
  * Set restart type
  *

src/modules/uavcan/uavcan_main.cpp

@@ -909,7 +909,7 @@ int UavcanNode::run()
 				unsigned num_outputs_max = 8;
 
 				// Do mixing
-				_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max, NULL);
+				_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max);
 
 				new_output = true;
 			}