px4io: add dynamic mixing support

Tools/module_config/generate_params.py

@@ -186,6 +186,8 @@ def get_actuator_output_params(yaml_config, output_functions,
                 standard_params = group.get('standard_params', [])
                 extra_function_groups = group.get('extra_function_groups', [])
                 pwm_timer_param = group.get('pwm_timer_param', None)
+                if 'timer_config_file' in group:
+                    timer_config_file = os.path.join(root_dir, group['timer_config_file'])
                 if timer_config_file is None:
                     raise Exception('trying to generate pwm outputs, but --timer-config not set')
                 timer_groups = get_timer_groups(timer_config_file, verbose)

src/drivers/px4io/CMakeLists.txt

@@ -39,6 +39,8 @@ px4_add_module(
 		px4io.cpp
 		px4io_serial.cpp
 		px4io_uploader.cpp
+	MODULE_CONFIG
+		module.yaml
 	DEPENDS
 		arch_px4io_serial
 		circuit_breaker

src/drivers/px4io/module.yaml

@@ -0,0 +1,29 @@
+module_name: IO PWM Output
+actuator_output:
+  output_groups:
+    - generator: pwm
+      param_prefix: PWM_MAIN
+      channel_labels: ['PWM Main', 'PWM Capture']
+      timer_config_file: "boards/px4/io-v2/src/timer_config.cpp"
+      standard_params:
+        disarmed: { min: 800, max: 2200, default: 900 }
+        min: { min: 800, max: 1400, default: 1000 }
+        max: { min: 1600, max: 2200, default: 2000 }
+        failsafe: { min: 800, max: 2200 }
+      pwm_timer_param:
+        description:
+            short: Output Protocol Configuration for ${label}
+            long: |
+                Select which Output Protocol to use for outputs ${label}.
+
+                Custom PWM rates can be used by directly setting any value >0.
+        type: enum
+        default: 400
+        values:
+            -1: OneShot
+            50: PWM50
+            100: PWM100
+            200: PWM200
+            400: PWM400
+        reboot_required: true
+

src/drivers/px4io/px4io.cpp

@@ -174,6 +174,7 @@ private:
 
 	void updateDisarmed();
 	void updateFailsafe();
+	void updateTimerRateGroups();
 
 	static int checkcrc(int argc, char *argv[]);
 	static int bind(int argc, char *argv[]);
@@ -218,6 +219,7 @@ private:
 	hrt_abstime             _last_status_publish{0};
 
 	bool			_param_update_force{true};	///< force a parameter update
+	bool			_timer_rates_configured{false};
 
 	/* advertised topics */
 	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
@@ -364,10 +366,14 @@ PX4IO::PX4IO(device::Device *interface) :
 	OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(PX4IO_SERIAL_DEVICE)),
 	_interface(interface)
 {
+	if (!_mixing_output.useDynamicMixing()) {
 		_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
 		_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
 	}
 
+	_mixing_output.setLowrateSchedulingInterval(20_ms);
+}
+
 PX4IO::~PX4IO()
 {
 	delete _interface;
@@ -555,7 +561,7 @@ void PX4IO::updateFailsafe()
 	pwm_output_values pwm{};
 
 	for (unsigned i = 0; i < _max_actuators; i++) {
-		pwm.values[i] = _mixing_output.failsafeValue(i);
+		pwm.values[i] = _mixing_output.actualFailsafeValue(i);
 	}
 
 	io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm.values, _max_actuators);
@@ -575,7 +581,9 @@ void PX4IO::Run()
 	perf_count(_interval_perf);
 
 	// schedule minimal update rate if there are no actuator controls
+	if (!_mixing_output.useDynamicMixing()) {
 		ScheduleDelayed(20_ms);
+	}
 
 	/* if we have new control data from the ORB, handle it */
 	if (_param_sys_hitl.get() <= 0) {
@@ -705,10 +713,60 @@ void PX4IO::Run()
 	perf_end(_cycle_perf);
 }
 
+void PX4IO::updateTimerRateGroups()
+{
+	if (_timer_rates_configured) { // avoid setting timer rates on each param update
+		return;
+	}
+
+	_timer_rates_configured = true;
+
+	int timer = 0;
+
+	uint16_t timer_rates[PX4IO_P_SETUP_PWM_RATE_GROUP3 - PX4IO_P_SETUP_PWM_RATE_GROUP0 + 1] {};
+
+	for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) {
+		char param_name[17];
+		snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
+
+		int32_t tim_config = 0;
+		param_t handle = param_find(param_name);
+
+		if (handle == PARAM_INVALID) {
+			break;
+		}
+
+		param_get(handle, &tim_config);
+
+		if (tim_config > 0) {
+			timer_rates[timer] = tim_config;
+
+		} else if (tim_config == -1) { // OneShot
+			timer_rates[timer] = 0;
+		}
+
+		++timer;
+	}
+
+	int ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP0, timer_rates, timer);
+
+	if (ret != 0) {
+		PX4_ERR("io_reg_set failed (%i)", ret);
+	}
+}
+
 void PX4IO::update_params()
 {
+	if (!_mixing_output.armed().armed && _mixing_output.useDynamicMixing()) {
+		// sync params to IO
+		updateTimerRateGroups();
+		updateFailsafe();
+		updateDisarmed();
+		return;
+	}
+
 	// skip update when armed or PWM disabled
-	if (_mixing_output.armed().armed || _class_instance == -1) {
+	if (_mixing_output.armed().armed || _class_instance == -1 || _mixing_output.useDynamicMixing()) {
 		return;
 	}
 
@@ -1140,6 +1198,16 @@ int PX4IO::io_get_status()
 			status.pwm_failsafe[i] = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i);
 		}
 
+		if (_mixing_output.useDynamicMixing()) {
+
+			int i = 0;
+
+			for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) {
+				// This is a bit different than below, setting the groups, not the channels
+				status.pwm_rate_hz[i++] = io_reg_get(PX4IO_PAGE_SETUP, offset);
+			}
+
+		} else {
 			// PWM rates, 0 = low rate, 1 = high rate
 			const uint16_t pwm_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
 
@@ -1154,6 +1222,7 @@ int PX4IO::io_get_status()
 					status.pwm_rate_hz[i] = pwm_low_rate;
 				}
 			}
+		}
 
 		uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
 
@@ -1532,6 +1601,12 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 
 	case PWM_SERVO_SET_UPDATE_RATE:
 		PX4_DEBUG("PWM_SERVO_SET_UPDATE_RATE");
+
+		if (_mixing_output.useDynamicMixing()) {
+			ret = -EINVAL;
+			break;
+		}
+
 		/* set the requested alternate rate */
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
 		break;
@@ -1545,6 +1620,11 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 	case PWM_SERVO_SET_SELECT_UPDATE_RATE: {
 			PX4_DEBUG("PWM_SERVO_SET_SELECT_UPDATE_RATE");
 
+			if (_mixing_output.useDynamicMixing()) {
+				ret = -EINVAL;
+				break;
+			}
+
 			/* blindly clear the PWM update alarm - might be set for some other reason */
 			io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
 
@@ -1579,7 +1659,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			}
 
 			for (unsigned i = 0; i < pwm->channel_count; i++) {
-				if (pwm->values[i] != 0) {
+				if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) {
 					_mixing_output.failsafeValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MAX);
 				}
 			}
@@ -1613,7 +1693,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			}
 
 			for (unsigned i = 0; i < pwm->channel_count; i++) {
-				if (pwm->values[i] != 0) {
+				if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) {
 					_mixing_output.disarmedValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MAX);
 				}
 			}
@@ -1645,7 +1725,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			}
 
 			for (unsigned i = 0; i < pwm->channel_count; i++) {
-				if (pwm->values[i] != 0) {
+				if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) {
 					_mixing_output.minValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MIN);
 				}
 			}
@@ -1675,7 +1755,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			}
 
 			for (unsigned i = 0; i < pwm->channel_count; i++) {
-				if (pwm->values[i] != 0) {
+				if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) {
 					_mixing_output.maxValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MAX, (uint16_t)PWM_HIGHEST_MAX);
 				}
 			}

src/lib/mixer_module/mixer_module.cpp

@@ -461,7 +461,7 @@ bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool li
 			}
 
 		} else if (all_disabled) {
-			_interface.ScheduleOnInterval(300_ms);
+			_interface.ScheduleOnInterval(_lowrate_schedule_interval);
 			PX4_DEBUG("Scheduling at low rate");
 
 		} else {
@@ -817,19 +817,7 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat
 	/* overwrite outputs in case of force_failsafe with _failsafe_value values */
 	if (_armed.force_failsafe) {
 		for (size_t i = 0; i < _max_num_outputs; i++) {
-			if (_failsafe_value[i] == UINT16_MAX) { // if set to default, use the one provided by the function
-				float default_failsafe = NAN;
-
-				if (_functions[i]) {
-					default_failsafe = _functions[i]->defaultFailsafeValue(_function_assignment[i]);
-				}
-
-				_current_output_value[i] = output_limit_calc_single(_reverse_output_mask & (1 << i),
-							   _disarmed_value[i], _min_value[i], _max_value[i], default_failsafe);
-
-			} else {
-				_current_output_value[i] = _failsafe_value[i];
-			}
+			_current_output_value[i] = actualFailsafeValue(i);
 		}
 	}
 
@@ -908,6 +896,32 @@ MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_output
 	}
 }
 
+uint16_t
+MixingOutput::actualFailsafeValue(int index)
+{
+	if (!_use_dynamic_mixing) {
+		return failsafeValue(index);
+	}
+
+	uint16_t value = 0;
+
+	if (_failsafe_value[index] == UINT16_MAX) { // if set to default, use the one provided by the function
+		float default_failsafe = NAN;
+
+		if (_functions[index]) {
+			default_failsafe = _functions[index]->defaultFailsafeValue(_function_assignment[index]);
+		}
+
+		value = output_limit_calc_single(_reverse_output_mask & (1 << index),
+						 _disarmed_value[index], _min_value[index], _max_value[index], default_failsafe);
+
+	} else {
+		value = _failsafe_value[index];
+	}
+
+	return value;
+}
+
 void
 MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS])
 {

src/lib/mixer_module/mixer_module.hpp

@@ -54,6 +54,8 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/test_motor.h>
 
+using namespace time_literals;
+
 /**
  * @class OutputModuleInterface
  * Base class for an output module.
@@ -187,6 +189,11 @@ public:
 	uint16_t &minValue(int index) { return _min_value[index]; }
 	uint16_t &maxValue(int index) { return _max_value[index]; }
 
+	/**
+	 * Returns the actual failsafe value taking into account the assigned function
+	 */
+	uint16_t actualFailsafeValue(int index);
+
 	/**
 	 * Get the motor index that maps from PX4 convention to the configured one
 	 * @param index motor index in [0, num_motors-1]
@@ -203,6 +210,8 @@ public:
 
 	const char *paramPrefix() const { return _param_prefix; }
 
+	void setLowrateSchedulingInterval(hrt_abstime interval) { _lowrate_schedule_interval = interval; }
+
 protected:
 	void updateParams() override;
 
@@ -330,6 +339,7 @@ private:
 		false; ///< whether or not the output module supports reversible motors (range [-1, 0] for motors)
 	const char *const _param_prefix;
 	ParamHandles _param_handles[MAX_ACTUATORS];
+	hrt_abstime _lowrate_schedule_interval{300_ms};
 
 	uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback
 

src/modules/px4iofirmware/protocol.h

@@ -193,6 +193,10 @@ enum {							/* DSM bind states */
 #define PX4IO_P_SETUP_SBUS_RATE			16	/**< frame rate of SBUS1 output in Hz */
 #define PX4IO_P_SETUP_THERMAL			17	/**< thermal management */
 #define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION	18	/**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */
+#define PX4IO_P_SETUP_PWM_RATE_GROUP0            19	/* Configure timer group 0 update rate in Hz */
+#define PX4IO_P_SETUP_PWM_RATE_GROUP1            20	/* Configure timer group 1 update rate in Hz */
+#define PX4IO_P_SETUP_PWM_RATE_GROUP2            21	/* Configure timer group 2 update rate in Hz */
+#define PX4IO_P_SETUP_PWM_RATE_GROUP3            22	/* Configure timer group 3 update rate in Hz */
 
 #define PX4IO_THERMAL_IGNORE			UINT16_MAX
 #define PX4IO_THERMAL_OFF			0

src/modules/px4iofirmware/registers.c

@@ -145,7 +145,8 @@ volatile uint16_t	r_page_setup[] = {
 	[PX4IO_P_SETUP_REBOOT_BL]		= 0,
 	[PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0,
 	[PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE,
-	[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0
+	[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0,
+	[PX4IO_P_SETUP_PWM_RATE_GROUP0 ... PX4IO_P_SETUP_PWM_RATE_GROUP3] = 0
 };
 
 #define PX4IO_P_SETUP_FEATURES_VALID	(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT | PX4IO_P_SETUP_FEATURES_ADC_RSSI)
@@ -465,6 +466,33 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			r_page_setup[offset] = value;
 			break;
 
+		case PX4IO_P_SETUP_PWM_RATE_GROUP0:
+		case PX4IO_P_SETUP_PWM_RATE_GROUP1:
+		case PX4IO_P_SETUP_PWM_RATE_GROUP2:
+		case PX4IO_P_SETUP_PWM_RATE_GROUP3:
+
+			/* For PWM constrain to [25,400]Hz
+			 * For Oneshot there is no rate, 0 is therefore used to select Oneshot mode
+			 */
+			if (value != 0) {
+				if (value < 25) {
+					value = 25;
+				}
+
+				if (value > 400) {
+					value = 400;
+				}
+			}
+
+			if (up_pwm_servo_set_rate_group_update(offset - PX4IO_P_SETUP_PWM_RATE_GROUP0, value) == OK) {
+				r_page_setup[offset] = value;
+
+			} else {
+				r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
+			}
+
+			break;
+
 		default:
 			return -1;
 		}

validation/module_schema.yaml

@@ -333,6 +333,10 @@ actuator_output:
                         maxlength: 2
                         schema:
                             type: string
+                    timer_config_file:
+                        # Only used for 'pwm' generator, specifies
+                        # board-specific timer_config.cpp file
+                        type: string
                     pwm_timer_param:
                         # Only used for 'pwm' generator, per-timer config param
                         type: dict