Multicopter mixer saturation handling strategy

src/drivers/linux_pwm_out/linux_pwm_out.cpp

@@ -44,6 +44,7 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/rc_channels.h>
+#include <uORB/topics/parameter_update.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_mixer.h>
@@ -99,17 +100,19 @@ int32_t _pwm_max;
 
 MixerGroup *_mixer_group = nullptr;
 
-void usage();
+static void usage();
 
-void start();
+static void start();
 
-void stop();
+static void stop();
 
-void task_main_trampoline(int argc, char *argv[]);
+static void task_main_trampoline(int argc, char *argv[]);
 
-void subscribe();
+static void subscribe();
 
-void task_main(int argc, char *argv[]);
+static void task_main(int argc, char *argv[]);
+
+static void update_params(bool &airmode);
 
 /* mixer initialization */
 int initialize_mixer(const char *mixer_filename);
@@ -128,6 +131,19 @@ int mixer_control_callback(uintptr_t handle,
 }
 
 
+void update_params(bool &airmode)
+{
+	// multicopter air-mode
+	param_t param_handle = param_find("MC_AIRMODE");
+
+	if (param_handle != PARAM_INVALID) {
+		int32_t val;
+		param_get(param_handle, &val);
+		airmode = val > 0;
+	}
+}
+
+
 int initialize_mixer(const char *mixer_filename)
 {
 	char buf[4096];
@@ -227,6 +243,10 @@ void task_main(int argc, char *argv[])
 	// subscribe and set up polling
 	subscribe();
 
+	bool airmode = false;
+	update_params(airmode);
+	int params_sub = orb_subscribe(ORB_ID(parameter_update));
+
 	int rc_channels_sub = -1;
 
 	// Start disarmed
@@ -244,6 +264,10 @@ void task_main(int argc, char *argv[])
 			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 		}
 
+		if (_mixer_group) {
+			_mixer_group->set_airmode(airmode);
+		}
+
 		int pret = px4_poll(_poll_fds, _poll_fds_num, 10);
 
 		/* Timed out, do a periodic check for _task_should_exit. */
@@ -366,6 +390,17 @@ void task_main(int argc, char *argv[])
 			PX4_ERR("Could not mix output! Exiting...");
 			_task_should_exit = true;
 		}
+
+		/* check for parameter updates */
+		bool param_updated = false;
+		orb_check(params_sub, &param_updated);
+
+		if (param_updated) {
+			struct parameter_update_s update;
+			orb_copy(ORB_ID(parameter_update), params_sub, &update);
+			update_params(airmode);
+		}
+
 	}
 
 	delete pwm_out;
@@ -385,6 +420,10 @@ void task_main(int argc, char *argv[])
 		orb_unsubscribe(rc_channels_sub);
 	}
 
+	if (params_sub != -1) {
+		orb_unsubscribe(params_sub);
+	}
+
 	_is_running = false;
 
 }

src/drivers/pwm_out_sim/pwm_out_sim.cpp

@@ -69,7 +69,6 @@
 
 #include <drivers/device/device.h>
 #include <drivers/drv_pwm_output.h>
-#include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_mixer.h>
 
@@ -79,6 +78,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/parameter_update.h>
 
 #include <systemlib/err.h>
 
@@ -134,6 +134,8 @@ class PWMSim : public device::CDev
 	actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	orb_id_t	_control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 
+	bool _airmode; 		///< multicopter air-mode
+
 	static void	task_main_trampoline(int argc, char *argv[]);
 	void		task_main();
 
@@ -145,21 +147,7 @@ class PWMSim : public device::CDev
 	int		pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg);
 	void 	subscribe();
 
-	struct GPIOConfig {
-		uint32_t	input;
-		uint32_t	output;
-		uint32_t	alt;
-	};
-
-	static const GPIOConfig	_gpio_tab[];
-	static const unsigned	_ngpio;
-
-	void		gpio_reset();
-	void		gpio_set_function(uint32_t gpios, int function);
-	void		gpio_write(uint32_t gpios, int function);
-	uint32_t	gpio_read();
-	int		gpio_ioctl(device::file_t *filp, int cmd, unsigned long arg);
-
+	void	update_params();
 };
 
 namespace
@@ -188,7 +176,8 @@ PWMSim::PWMSim() :
 	_groups_required(0),
 	_groups_subscribed(0),
 	_task_should_exit(false),
-	_mixers(nullptr)
+	_mixers(nullptr),
+	_airmode(false)
 {
 	memset(_controls, 0, sizeof(_controls));
 
@@ -365,6 +354,18 @@ PWMSim::subscribe()
 	}
 }
 
+void PWMSim::update_params()
+{
+	// multicopter air-mode
+	param_t param_handle = param_find("MC_AIRMODE");
+
+	if (param_handle != PARAM_INVALID) {
+		int32_t val;
+		param_get(param_handle, &val);
+		_airmode = val > 0;
+	}
+}
+
 void
 PWMSim::task_main()
 {
@@ -379,6 +380,8 @@ PWMSim::task_main()
 	/* advertise the mixed control outputs, insist on the first group output */
 	_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &outputs);
 
+	update_params();
+	int params_sub = orb_subscribe(ORB_ID(parameter_update));
 
 	/* loop until killed */
 	while (!_task_should_exit) {
@@ -406,6 +409,10 @@ PWMSim::task_main()
 			_current_update_rate = _update_rate;
 		}
 
+		if (_mixers) {
+			_mixers->set_airmode(_airmode);
+		}
+
 		/* this can happen during boot, but after the sleep its likely resolved */
 		if (_poll_fds_num == 0) {
 			usleep(1000 * 1000);
@@ -534,6 +541,16 @@ PWMSim::task_main()
 			_failsafe = aa.force_failsafe;
 			_lockdown = aa.manual_lockdown;
 		}
+
+		/* check for parameter updates */
+		bool param_updated = false;
+		orb_check(params_sub, &param_updated);
+
+		if (param_updated) {
+			struct parameter_update_s update;
+			orb_copy(ORB_ID(parameter_update), params_sub, &update);
+			update_params();
+		}
 	}
 
 	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
@@ -543,11 +560,7 @@ PWMSim::task_main()
 	}
 
 	orb_unsubscribe(_armed_sub);
-
-	/* make sure servos are off */
-	// up_pwm_servo_deinit();
-
-	/* note - someone else is responsible for restoring the GPIO config */
+	orb_unsubscribe(params_sub);
 
 	/* tell the dtor that we are exiting */
 	_task = -1;
@@ -862,13 +875,6 @@ PortMode g_port_mode = PORT_MODE_UNDEFINED;
 int
 hil_new_mode(PortMode new_mode)
 {
-	// uint32_t gpio_bits;
-
-
-//	/* reset to all-inputs */
-//	g_pwm_sim->ioctl(0, GPIO_RESET, 0);
-
-	// gpio_bits = 0;
 
 	PWMSim::Mode servo_mode = PWMSim::MODE_NONE;
 

src/drivers/px4fmu/fmu.cpp

@@ -276,6 +276,7 @@ class PX4FMU : public device::CDev, public ModuleBase<PX4FMU>
 
 	float _mot_t_max;	// maximum rise time for motor (slew rate limiting)
 	float _thr_mdl_fac;	// thrust to pwm modelling factor
+	bool _airmode; 		// multicopter air-mode
 
 	perf_counter_t	_ctl_latency;
 
@@ -386,6 +387,7 @@ PX4FMU::PX4FMU(bool run_as_task) :
 	_to_mixer_status(nullptr),
 	_mot_t_max(0.0f),
 	_thr_mdl_fac(0.0f),
+	_airmode(false),
 	_ctl_latency(perf_alloc(PC_ELAPSED, "ctl_lat"))
 {
 	for (unsigned i = 0; i < _max_actuators; i++) {
@@ -1362,6 +1364,8 @@ PX4FMU::cycle()
 							 ORB_PRIO_DEFAULT);
 				}
 
+				_mixers->set_airmode(_airmode);
+
 				perf_end(_ctl_latency);
 			}
 		}
@@ -1787,6 +1791,16 @@ void PX4FMU::update_params()
 	if (param_handle != PARAM_INVALID) {
 		param_get(param_handle, &_thr_mdl_fac);
 	}
+
+	// multicopter air-mode
+	param_handle = param_find("MC_AIRMODE");
+
+	if (param_handle != PARAM_INVALID) {
+		int32_t val;
+		param_get(param_handle, &val);
+		_airmode = val > 0;
+		PX4_DEBUG("%s: %d", "MC_AIRMODE", _airmode);
+	}
 }
 
 

src/drivers/px4io/px4io.cpp

@@ -1221,6 +1221,15 @@ PX4IO::task_main()
 					param_get(parm_handle, &param_val);
 					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_MOTOR_SLEW_MAX, FLOAT_TO_REG(param_val));
 				}
+
+				/* air-mode */
+				parm_handle = param_find("MC_AIRMODE");
+
+				if (parm_handle != PARAM_INVALID) {
+					int32_t param_val_int;
+					param_get(parm_handle, &param_val_int);
+					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_AIRMODE, SIGNED_TO_REG(param_val_int));
+				}
 			}
 
 		}