Multicopter mixer saturation handling strategy -- continue

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/PWMSim.cpp

@@ -135,6 +135,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::run()
 {
@@ -146,6 +158,9 @@ PWMSim::run()
 	/* advertise the mixed control outputs, insist on the first group output */
 	_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_actuator_outputs);
 
+	update_params();
+	int params_sub = orb_subscribe(ORB_ID(parameter_update));
+
 	/* loop until killed */
 	while (!should_exit()) {
 
@@ -172,6 +187,10 @@ PWMSim::run()
 			_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);
@@ -283,6 +302,16 @@ PWMSim::run()
 				_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++) {
@@ -292,6 +321,7 @@ PWMSim::run()
 	}
 
 	orb_unsubscribe(_armed_sub);
+	orb_unsubscribe(params_sub);
 }
 
 int

src/drivers/pwm_out_sim/PWMSim.hpp

@@ -49,6 +49,7 @@
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/parameter_update.h>
 
 class PWMSim : public device::CDev, public ModuleBase<PWMSim>
 {
@@ -129,10 +130,13 @@ class PWMSim : public device::CDev, public ModuleBase<PWMSim>
 	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{false}; 	///< multicopter air-mode
+
 	static int	control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
 
 	void 	subscribe();
 
+	void 	update_params();
 };
 
 #endif /* DRIVERS_PWM_OUT_SIM_PWMSIM_HPP_ */

src/drivers/px4fmu/fmu.cpp

@@ -275,6 +275,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;
 
@@ -385,6 +386,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++) {
@@ -1360,6 +1362,8 @@ PX4FMU::cycle()
 							 ORB_PRIO_DEFAULT);
 				}
 
+				_mixers->set_airmode(_airmode);
+
 				perf_end(_ctl_latency);
 			}
 		}
@@ -1783,6 +1787,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));
+				}
 			}
 
 		}

src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp

@@ -44,6 +44,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_mixer.h>
@@ -117,23 +118,25 @@ MultirotorMixer *_mixer = nullptr;
  * forward declaration
  */
 
-void usage();
+static void usage();
 
-void start();
+static void start();
 
-void stop();
+static void stop();
 
-int pwm_initialize(const char *device);
+static int pwm_initialize(const char *device);
 
-void pwm_deinitialize();
+static void pwm_deinitialize();
 
-void send_outputs_pwm(const uint16_t *pwm);
+static void send_outputs_pwm(const uint16_t *pwm);
 
-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);
 
 int initialize_mixer(const char *mixer_filename);
 
@@ -157,6 +160,17 @@ int mixer_control_callback(uintptr_t handle,
 	return 0;
 }
 
+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)
 {
@@ -343,6 +357,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));
+
 	// Start disarmed
 	_armed.armed = false;
 	_armed.prearmed = false;
@@ -355,6 +373,10 @@ void task_main(int argc, char *argv[])
 	// Main loop
 	while (!_task_should_exit) {
 
+		if (_mixer) {
+			_mixer->set_airmode(airmode);
+		}
+
 		/* wait up to 10ms for data */
 		int pret = px4_poll(_poll_fds, _poll_fds_num, 10);
 
@@ -438,6 +460,15 @@ void task_main(int argc, char *argv[])
 			_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
 		}
 
+		/* 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);
+		}
 	}
 
 	pwm_deinitialize();
@@ -449,6 +480,7 @@ void task_main(int argc, char *argv[])
 	}
 
 	orb_unsubscribe(_armed_sub);
+	orb_unsubscribe(params_sub);
 	_is_running = false;
 
 }

src/lib/mixer/mixer.h

@@ -218,6 +218,13 @@ class __EXPORT Mixer
 	 */
 	virtual void 			set_thrust_factor(float val) {}
 
+	/**
+	 * @brief Set airmode. Airmode allows the mixer to increase the total thrust in order to unsaturate the motors.
+	 *
+	 * @param[in]  airmode   De/-activate airmode by setting it to false/true
+	 */
+	virtual void set_airmode(bool airmode) {};
+
 protected:
 	/** client-supplied callback used when fetching control values */
 	ControlCallback			_control_cb;
@@ -403,6 +410,8 @@ class __EXPORT MixerGroup : public Mixer
 	 */
 	virtual void	set_thrust_factor(float val);
 
+	virtual void 	set_airmode(bool airmode);
+
 private:
 	Mixer				*_first;	/**< linked list of mixers */
 
@@ -650,6 +659,8 @@ class __EXPORT MultirotorMixer : public Mixer
 	 */
 	virtual void			set_thrust_factor(float val) {_thrust_factor = val;}
 
+	virtual void 			set_airmode(bool airmode);
+
 	union saturation_status {
 		struct {
 			uint16_t valid		: 1; // 0 - true when the saturation status is used
@@ -675,6 +686,8 @@ class __EXPORT MultirotorMixer : public Mixer
 	float 				_delta_out_max;
 	float 				_thrust_factor;
 
+	bool                		_airmode;
+
 	void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low);
 	saturation_status _saturation_status;