multirotor_motor_limits only publish for MC

msg/multirotor_motor_limits.msg

@@ -1,11 +1,13 @@
 uint16 saturation_status	# Integer bit mask indicating which axes in the control mixer are saturated
-# 0 - True if any motor is saturated at the upper limit
+
-# 1 - True if any motor is saturated at the lower limit
+# 0 - True if the saturation status is valid
-# 2 - True if a positive roll increment will increase motor saturation
+# 1 - True if any motor is saturated at the upper limit
-# 3 - True if negative roll increment will increase motor saturation
+# 2 - True if any motor is saturated at the lower limit
-# 4 - True if positive pitch increment will increase motor saturation
+# 3 - True if a positive roll increment will increase motor saturation
-# 5 - True if negative pitch increment will increase motor saturation
+# 4 - True if negative roll increment will increase motor saturation
-# 6 - True if positive yaw increment will increase motor saturation
+# 5 - True if positive pitch increment will increase motor saturation
-# 7 - True if negative yaw increment will increase motor saturation
+# 6 - True if negative pitch increment will increase motor saturation
-# 8 - True if positive thrust increment will increase motor saturation
+# 7 - True if positive yaw increment will increase motor saturation
-# 9 - True if negative thrust 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/linux_pwm_out/linux_pwm_out.cpp

@@ -299,9 +299,7 @@ void task_main(int argc, char *argv[])
 		if (_mixer_group != nullptr) {
 			_outputs.timestamp = hrt_absolute_time();
 			/* do mixing */
-			_outputs.noutputs = _mixer_group->mix(_outputs.output,
+			_outputs.noutputs = _mixer_group->mix(_outputs.output, actuator_outputs_s::NUM_ACTUATOR_OUTPUTS);
-							      actuator_outputs_s::NUM_ACTUATOR_OUTPUTS,
-							      NULL);
 
 			/* disable unused ports by setting their output to NaN */
 			for (size_t i = _outputs.noutputs; i < _outputs.NUM_ACTUATOR_OUTPUTS; i++) {

src/drivers/mkblctrl/mkblctrl.cpp

@@ -539,7 +539,7 @@ MK::task_main()
 				if (_mixers != nullptr) {
 
 					/* do mixing */
-					outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs, NULL);
+					outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs);
 					outputs.timestamp = hrt_absolute_time();
 
 					/* iterate actuators */

src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp

@@ -55,7 +55,6 @@
 #include <v1.0/mavlink_types.h>
 #include <v1.0/common/mavlink.h>
 
-
 /*
  * This driver is supposed to run on Snapdragon. It sends actuator_controls (PWM)
  * to a Pixhawk/Pixfalcon/Pixracer over UART (mavlink) and receives RC input.
@@ -415,7 +414,7 @@ void task_main(int argc, char *argv[])
 			_outputs.timestamp = _controls.timestamp;
 
 			/* do mixing */
-			_outputs.noutputs = _mixer->mix(_outputs.output, 0 /* not used */, NULL);
+			_outputs.noutputs = _mixer->mix(_outputs.output, 0);
 
 			/* disable unused ports by setting their output to NaN */
 			for (size_t i = _outputs.noutputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {

src/drivers/pwm_out_sim/pwm_out_sim.cpp

@@ -474,7 +474,7 @@ PWMSim::task_main()
 			}
 
 			/* do mixing */
-			num_outputs = _mixers->mix(&outputs.output[0], num_outputs, nullptr);
+			num_outputs = _mixers->mix(&outputs.output[0], num_outputs);
 			outputs.noutputs = num_outputs;
 			outputs.timestamp = hrt_absolute_time();
 

src/drivers/px4fmu/fmu.cpp

@@ -34,74 +34,52 @@
 /**
  * @file fmu.cpp
  *
- * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards.
+ * Driver/configurator for the PX4 FMU
  */
 
-#include <px4_config.h>
+#include <cfloat>
-#include <px4_log.h>
-#include <px4_module.h>
-#include <px4_getopt.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <float.h>
-#include <stdlib.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <nuttx/arch.h>
-
-#include <drivers/device/device.h>
-#include <drivers/device/i2c.h>
-#include <drivers/drv_pwm_output.h>
-#include <drivers/drv_input_capture.h>
-#include <drivers/drv_gpio.h>
-#include <drivers/drv_hrt.h>
 
 #include <board_config.h>
-
+#include <drivers/device/device.h>
-#include <systemlib/px4_macros.h>
+#include <drivers/device/i2c.h>
-#include <systemlib/systemlib.h>
+#include <drivers/drv_gpio.h>
-#include <systemlib/mixer/mixer.h>
+#include <drivers/drv_hrt.h>
-#include <systemlib/pwm_limit/pwm_limit.h>
-#include <systemlib/board_serial.h>
-#include <systemlib/param/param.h>
-#include <systemlib/perf_counter.h>
-#include <drivers/drv_mixer.h>
-#include <drivers/drv_rc_input.h>
 #include <drivers/drv_input_capture.h>
-
+#include <drivers/drv_mixer.h>
-#include <lib/rc/sbus.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_rc_input.h>
 #include <lib/rc/dsm.h>
+#include <lib/rc/sbus.h>
 #include <lib/rc/st24.h>
 #include <lib/rc/sumd.h>
-
+#include <px4_config.h>
+#include <px4_getopt.h>
+#include <px4_log.h>
+#include <px4_module.h>
+#include <systemlib/board_serial.h>
+#include <systemlib/circuit_breaker.h>
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/safety.h>
 #include <uORB/topics/adc_report.h>
 #include <uORB/topics/multirotor_motor_limits.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/safety.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_status.h>
 
 #ifdef HRT_PPM_CHANNEL
 # include <systemlib/ppm_decode.h>
 #endif
 
-#include <systemlib/circuit_breaker.h>
-
 #define SCHEDULE_INTERVAL	2000	/**< The schedule interval in usec (500 Hz) */
-#define NAN_VALUE	(0.0f/0.0f)		/**< NaN value for throttle lock mode */
+
-#define BUTTON_SAFETY	px4_arch_gpioread(GPIO_BTN_SAFETY)
+static constexpr uint8_t CYCLE_COUNT = 10; /* safety switch must be held for 1 second to activate */
-#define CYCLE_COUNT 10			/* safety switch must be held for 1 second to activate */
+static constexpr uint8_t MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS;
 
 /*
  * Define the various LED flash sequences for each system state.
@@ -127,10 +105,10 @@ enum PortMode {
 	PORT_CAPTURE,
 };
 
-
 #if !defined(BOARD_HAS_PWM)
 #  error "board_config.h needs to define BOARD_HAS_PWM"
 #endif
+
 class PX4FMU : public device::CDev, public ModuleBase<PX4FMU>
 {
 public:
@@ -305,9 +283,8 @@ private:
 	int			set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
 	int			pwm_ioctl(file *filp, int cmd, unsigned long arg);
 	void		update_pwm_rev_mask();
-	void		publish_pwm_outputs(uint16_t *values, size_t numvalues);
 	void		update_pwm_out_state(bool on);
-	void		pwm_output_set(unsigned i, unsigned value);
+
 	void		update_params();
 
 	struct GPIOConfig {
@@ -550,7 +527,7 @@ PX4FMU::safety_check_button(void)
 	 * Debounce the safety button, change state if it has been held for long enough.
 	 *
 	 */
-	bool safety_button_pressed = BUTTON_SAFETY;
+	bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY);
 
 	/*
 	 * Keep pressed for a while to arm.
@@ -951,26 +928,6 @@ PX4FMU::update_pwm_trims()
 	}
 }
 
-void
-PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues)
-{
-	actuator_outputs_s outputs = {};
-	outputs.noutputs = numvalues;
-	outputs.timestamp = hrt_absolute_time();
-
-	for (size_t i = 0; i < _max_actuators; ++i) {
-		outputs.output[i] = i < numvalues ? (float)values[i] : 0;
-	}
-
-	if (_outputs_pub == nullptr) {
-		int instance = _class_instance;
-		_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &instance, ORB_PRIO_DEFAULT);
-
-	} else {
-		orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
-	}
-}
-
 int
 PX4FMU::task_spawn(int argc, char *argv[])
 {
@@ -1006,7 +963,6 @@ PX4FMU::task_spawn(int argc, char *argv[])
 	if (!run_as_task) {
 
 		/* schedule a cycle to start things */
-
 		int ret = work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, nullptr, 0);
 
 		if (ret < 0) {
@@ -1161,14 +1117,6 @@ void PX4FMU::rc_io_invert(bool invert)
 }
 #endif
 
-void
-PX4FMU::pwm_output_set(unsigned i, unsigned value)
-{
-	if (_pwm_initialized) {
-		up_pwm_servo_set(i, value);
-	}
-}
-
 void
 PX4FMU::update_pwm_out_state(bool on)
 {
@@ -1248,7 +1196,7 @@ PX4FMU::cycle()
 
 		/* wait for an update */
 		unsigned n_updates = 0;
-		int ret = ::poll(_poll_fds, _poll_fds_num, poll_timeout);
+		int ret = px4_poll(_poll_fds, _poll_fds_num, poll_timeout);
 
 		/* this would be bad... */
 		if (ret < 0) {
@@ -1261,41 +1209,19 @@ PX4FMU::cycle()
 		} else {
 			perf_begin(_ctl_latency);
 
+			if (_mixers != nullptr) {
 				/* get controls for required topics */
 				unsigned poll_id = 0;
 
 				for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 					if (_control_subs[i] > 0) {
+
 						if (_poll_fds[poll_id].revents & POLLIN) {
 							if (i == 0) {
 								n_updates++;
 							}
 
 							orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
-
-#if defined(DEBUG_BUILD)
-
-						static int main_out_latency = 0;
-						static int sum_latency = 0;
-						static uint64_t last_cycle_time = 0;
-
-						if (i == 0) {
-							uint64_t now = hrt_absolute_time();
-							uint64_t latency = now - _controls[i].timestamp;
-
-							if (latency > main_out_latency) { main_out_latency = latency; }
-
-							sum_latency += latency;
-
-							if ((now - last_cycle_time) >= 1000000) {
-								last_cycle_time = now;
-								PX4_DEBUG("pwm max latency: %d, avg: %5.3f", main_out_latency, (double)(sum_latency / 100.0));
-								main_out_latency = latency;
-								sum_latency = 0;
-							}
-						}
-
-#endif
 						}
 
 						poll_id++;
@@ -1308,19 +1234,20 @@ PX4FMU::cycle()
 						memset(&_controls[i], 0, sizeof(_controls[i]));
 
 						/* except thrust to maximum. */
-					_controls[i].control[3] = 1.0f;
+						_controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f;
 
 						/* Switch off the PWM limit ramp for the calibration. */
 						_pwm_limit.state = PWM_LIMIT_STATE_ON;
 					}
 				}
+			}
 		} // poll_fds
 
 		/* run the mixers on every cycle */
 		{
-			/* can we mix? */
 			if (_mixers != nullptr) {
 
+				if (_mot_t_max > FLT_EPSILON) {
 					hrt_abstime now = hrt_absolute_time();
 					float dt = (now - _time_last_mix) / 1e6f;
 					_time_last_mix = now;
@@ -1332,10 +1259,9 @@ PX4FMU::cycle()
 						dt = 0.02f;
 					}
 
-				if (_mot_t_max > FLT_EPSILON) {
+					// maximum value the outputs of the multirotor mixer are allowed to change in this cycle
-					// maximum value the ouputs of the multirotor mixer are allowed to change in this cycle
+					// factor 2 is needed because actuator outputs are in the range [-1,1]
-					// factor 2 is needed because actuator ouputs are in the range [-1,1]
+					const float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max;
-					float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max;
 					_mixers->set_max_delta_out_once(delta_out_max);
 				}
 
@@ -1345,37 +1271,14 @@ PX4FMU::cycle()
 
 				/* do mixing */
 				float outputs[_max_actuators];
-				size_t mixed_num_outputs = _mixers->mix(outputs, _num_outputs, NULL);
+				const unsigned mixed_num_outputs = _mixers->mix(outputs, _num_outputs);
-
-				/* publish mixer status */
-				multirotor_motor_limits_s multirotor_motor_limits = {};
-				multirotor_motor_limits.saturation_status = _mixers->get_saturation_status();
-
-				if (_to_mixer_status == nullptr) {
-					/* publish limits */
-					int instance = _class_instance;
-					_to_mixer_status = orb_advertise_multi(ORB_ID(multirotor_motor_limits), &multirotor_motor_limits, &instance,
-									       ORB_PRIO_DEFAULT);
-
-				} else {
-					orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &multirotor_motor_limits);
-
-				}
-
-				/* disable unused ports by setting their output to NaN */
-				for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) {
-					if (i >= mixed_num_outputs) {
-						outputs[i] = NAN_VALUE;
-					}
-				}
-
-				uint16_t pwm_limited[_max_actuators];
 
 				/* the PWM limit call takes care of out of band errors, NaN and constrains */
+				uint16_t pwm_limited[MAX_ACTUATORS];
+
 				pwm_limit_calc(_throttle_armed, arm_nothrottle(), mixed_num_outputs, _reverse_pwm_mask,
 					       _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit);
 
-
 				/* overwrite outputs in case of force_failsafe with _failsafe_pwm PWM values */
 				if (_armed.force_failsafe) {
 					for (size_t i = 0; i < mixed_num_outputs; i++) {
@@ -1391,19 +1294,43 @@ PX4FMU::cycle()
 				}
 
 				/* output to the servos */
+				if (_pwm_initialized) {
 					for (size_t i = 0; i < mixed_num_outputs; i++) {
-					pwm_output_set(i, pwm_limited[i]);
+						up_pwm_servo_set(i, pwm_limited[i]);
+					}
 				}
 
 				/* Trigger all timer's channels in Oneshot mode to fire
 				 * the oneshots with updated values.
 				 */
-
 				if (n_updates > 0) {
 					up_pwm_update();
 				}
 
-				publish_pwm_outputs(pwm_limited, mixed_num_outputs);
+				actuator_outputs_s actuator_outputs = {};
+				actuator_outputs.timestamp = hrt_absolute_time();
+				actuator_outputs.noutputs = mixed_num_outputs;
+
+				// zero unused outputs
+				for (size_t i = 0; i < mixed_num_outputs; ++i) {
+					actuator_outputs.output[i] = pwm_limited[i];
+				}
+
+				orb_publish_auto(ORB_ID(actuator_outputs), &_outputs_pub, &actuator_outputs, &_class_instance, ORB_PRIO_DEFAULT);
+
+				/* publish mixer status */
+				MultirotorMixer::saturation_status saturation_status;
+				saturation_status.value = _mixers->get_saturation_status();
+
+				if (saturation_status.flags.valid) {
+					multirotor_motor_limits_s motor_limits;
+					motor_limits.timestamp = hrt_absolute_time();
+					motor_limits.saturation_status = saturation_status.value;
+
+					orb_publish_auto(ORB_ID(multirotor_motor_limits), &_to_mixer_status, &motor_limits, &_class_instance,
+							 ORB_PRIO_DEFAULT);
+				}
+
 				perf_end(_ctl_latency);
 			}
 		}
@@ -1729,8 +1656,7 @@ PX4FMU::cycle()
 					// we have a new PPM frame. Publish it.
 					rc_updated = true;
 					_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
-					fill_rc_in(ppm_decoded_channels, ppm_buffer, _cycle_timestamp,
+					fill_rc_in(ppm_decoded_channels, ppm_buffer, _cycle_timestamp, false, false, 0);
-						   false, false, 0);
 					_rc_scan_locked = true;
 					_rc_in.rc_ppm_frame_length = ppm_frame_length;
 					_rc_in.timestamp_last_signal = ppm_last_valid_decode;
@@ -1790,10 +1716,8 @@ PX4FMU::cycle()
 				exit_and_cleanup();
 
 			} else {
-
 				/* schedule next cycle */
 				work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL));
-				//																			  USEC2TICK(SCHEDULE_INTERVAL - main_out_latency));
 			}
 
 			break;
@@ -1863,7 +1787,7 @@ PX4FMU::control_callback(uintptr_t handle,
 		     control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
 		    control_index == actuator_controls_s::INDEX_THROTTLE) {
 			/* set the throttle to an invalid value */
-			input = NAN_VALUE;
+			input = NAN;
 		}
 	}
 
@@ -2508,8 +2432,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case MIXERIOCADDSIMPLE: {
 			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
 
-			SimpleMixer *mixer = new SimpleMixer(control_callback,
+			SimpleMixer *mixer = new SimpleMixer(control_callback, (uintptr_t)_controls, mixinfo);
-							     (uintptr_t)_controls, mixinfo);
 
 			if (mixer->check()) {
 				delete mixer;
@@ -2517,9 +2440,9 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 				ret = -EINVAL;
 
 			} else {
-				if (_mixers == nullptr)
+				if (_mixers == nullptr) {
-					_mixers = new MixerGroup(control_callback,
+					_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
-								 (uintptr_t)_controls);
+				}
 
 				_mixers->add_mixer(mixer);
 				_mixers->groups_required(_groups_required);

src/drivers/px4io/CMakeLists.txt

@@ -40,7 +40,6 @@ px4_add_module(
 		px4io_uploader.cpp
 		px4io_serial.cpp
 		px4io_i2c.cpp
-		px4io_params.c
 	DEPENDS
 		platforms__common
 	)

src/drivers/px4io/px4io.cpp

@@ -1928,12 +1928,6 @@ PX4IO::io_publish_raw_rc()
 int
 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 */
 	uint16_t ctl[_max_actuators];
 	int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators);
@@ -1942,38 +1936,33 @@ PX4IO::io_publish_pwm_outputs()
 		return ret;
 	}
 
+	actuator_outputs_s outputs = {};
+	outputs.timestamp = hrt_absolute_time();
+	outputs.noutputs = _max_actuators;
+
 	/* convert from register format to float */
 	for (unsigned i = 0; i < _max_actuators; i++) {
 		outputs.output[i] = ctl[i];
 	}
 
-	outputs.noutputs = _max_actuators;
-
-	/* lazily advertise on first publication */
-	if (_to_outputs == nullptr) {
 	int instance;
-		_to_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
+	orb_publish_auto(ORB_ID(actuator_outputs), &_to_outputs, &outputs, &instance, ORB_PRIO_DEFAULT);
-						  &outputs, &instance, ORB_PRIO_MAX);
-
-	} else {
-		orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs);
-	}
 
 	/* get mixer status flags from IO */
-	uint16_t mixer_status;
+	MultirotorMixer::saturation_status saturation_status;
-	ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &mixer_status, sizeof(mixer_status) / sizeof(uint16_t));
+	ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &saturation_status.value, 1);
-	motor_limits.saturation_status = mixer_status;
 
 	if (ret != OK) {
 		return ret;
 	}
 
 	/* publish mixer status */
-	if (_to_mixer_status == nullptr) {
+	if (saturation_status.flags.valid) {
-		_to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &motor_limits);
+		multirotor_motor_limits_s motor_limits;
+		motor_limits.timestamp = hrt_absolute_time();
+		motor_limits.saturation_status = saturation_status.value;
 
-	} else {
+		orb_publish_auto(ORB_ID(multirotor_motor_limits), &_to_mixer_status, &motor_limits, &instance, ORB_PRIO_DEFAULT);
-		orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &motor_limits);
 	}
 
 	return OK;

src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp

@@ -402,9 +402,7 @@ void task_main(int argc, char *argv[])
 		_outputs.timestamp = hrt_absolute_time();
 
 		/* do  mixing for virtual control group */
-		_outputs.noutputs = _mixer->mix(_outputs.output,
+		_outputs.noutputs = _mixer->mix(_outputs.output, _outputs.NUM_ACTUATOR_OUTPUTS);
-						_outputs.NUM_ACTUATOR_OUTPUTS,
-						NULL);
 
 		//set max, min and disarmed pwm
 		const uint16_t reverse_mask = 0;

src/drivers/tap_esc/tap_esc.cpp

@@ -655,7 +655,7 @@ TAP_ESC::cycle()
 		if (_is_armed && _mixers != nullptr) {
 
 			/* do mixing */
-			num_outputs = _mixers->mix(&_outputs.output[0], num_outputs, NULL);
+			num_outputs = _mixers->mix(&_outputs.output[0], num_outputs);
 			_outputs.noutputs = num_outputs;
 			_outputs.timestamp = hrt_absolute_time();
 

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 {
+	MultirotorMixer::saturation_status _saturation_status{};
-		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;
 
 	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);
+		multirotor_motor_limits_s motor_limits = {};
-		_saturation_status.value = _motor_limits.saturation_status;
+		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,
+static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control);
-			       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);
+	int mixcount = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
-	*limits = mixer_limits;
+	*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,
+	static SimpleMixer		*pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min,
-			uintptr_t cb_handle,
+			uint16_t mid, uint16_t max);
-			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,
+	static MultirotorMixer		*from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
-			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,
+	static HelicopterMixer		*from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
-			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;
 			}

src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp

@@ -377,9 +377,7 @@ void task_main(int argc, char *argv[])
 
 			if (_mixers != nullptr) {
 				/* do mixing */
-				_outputs.noutputs = _mixers->mix(_outputs.output,
+				_outputs.noutputs = _mixers->mix(_outputs.output, 4);
-								 4,
-								 NULL);
 			}
 
 			// Set last throttle for battery calculations

src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp

@@ -337,9 +337,7 @@ void task_main(int argc, char *argv[])
 				int16_t motor_rpms[UART_ESC_MAX_MOTORS];
 
 				if (_armed.armed) {
-					_outputs.noutputs = mixer->mix(&_outputs.output[0],
+					_outputs.noutputs = mixer->mix(&_outputs.output[0], actuator_controls_s::NUM_ACTUATOR_CONTROLS);
-								       actuator_controls_s::NUM_ACTUATOR_CONTROLS,
-								       NULL);
 
 					// Make sure we support only up to UART_ESC_MAX_MOTORS motors
 					if (_outputs.noutputs > UART_ESC_MAX_MOTORS) {

src/systemcmds/tests/test_mixer.cpp

@@ -395,7 +395,7 @@ bool MixerTest::mixerTest()
 
 	/* mix */
 	should_prearm = true;
-	mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
+	mixed = mixer_group.mix(&outputs[0], output_max);
 
 	pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
 		       r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
@@ -436,7 +436,7 @@ bool MixerTest::mixerTest()
 	while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US + 2 * sleep_quantum_us) {
 
 		/* mix */
-		mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
+		mixed = mixer_group.mix(&outputs[0], output_max);
 
 		pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
 			       r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
@@ -480,7 +480,7 @@ bool MixerTest::mixerTest()
 		}
 
 		/* mix */
-		mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
+		mixed = mixer_group.mix(&outputs[0], output_max);
 
 		pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
 			       r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
@@ -508,7 +508,7 @@ bool MixerTest::mixerTest()
 	while (hrt_elapsed_time(&starttime) < 600000) {
 
 		/* mix */
-		mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
+		mixed = mixer_group.mix(&outputs[0], output_max);
 
 		pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
 			       r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
@@ -545,7 +545,7 @@ bool MixerTest::mixerTest()
 	while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
 
 		/* mix */
-		mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
+		mixed = mixer_group.mix(&outputs[0], output_max);
 
 		pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
 			       r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);