Airmode - Add airmode parameter for multicopter mixer

src/drivers/px4fmu/fmu.cpp

@@ -275,6 +275,7 @@ private:
 
 	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++) {
@@ -1292,6 +1294,7 @@ PX4FMU::cycle()
 					// factor 2 is needed because actuator outputs 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;
 					_mixers->set_max_delta_out_once(delta_out_max);
+					_mixers->set_airmode(_airmode);
 				}
 
 				if (_thr_mdl_fac > FLT_EPSILON) {
@@ -1783,6 +1786,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) {
+		int val;
+		param_get(param_handle, &val);
+		_airmode = val > 0;
+		PX4_DEBUG("%s: %d", "MC_AIRMODE", _airmode);
+	}
 }
 
 

src/drivers/px4io/px4io.cpp

@@ -1221,6 +1221,14 @@ 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) {
+					param_get(parm_handle, &param_val);
+					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_AIRMODE, SIGNED_TO_REG(param_val));
+				}
 			}
 
 		}

src/lib/mixer/mixer.h

@@ -218,6 +218,13 @@ public:
 	 */
 	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   Dis/-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 @@ public:
 	 */
 	virtual void	set_thrust_factor(float val);
 
+	virtual void 	set_airmode(bool airmode);
+
 private:
 	Mixer				*_first;	/**< linked list of mixers */
 
@@ -650,6 +659,8 @@ public:
 	 */
 	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 @@ private:
 	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;
 

src/lib/mixer/mixer_group.cpp

@@ -185,7 +185,17 @@ MixerGroup::set_thrust_factor(float val)
 		mixer->set_thrust_factor(val);
 		mixer = mixer->_next;
 	}
+}
 
+void
+MixerGroup::set_airmode(bool airmode)
+{
+	Mixer	*mixer = _first;
+
+	while (mixer != nullptr) {
+		mixer->set_airmode(airmode);
+		mixer = mixer->_next;
+	}
 }
 
 uint16_t

src/lib/mixer/mixer_multirotor.cpp

@@ -84,6 +84,7 @@ 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),
+	_airmode(false),
 	_rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]),
 	_rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]),
 	_outputs_prev(new float[_rotor_count])
@@ -224,6 +225,9 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 		boost = 1.0f - ((max_out - thrust) * roll_pitch_scale + thrust);
 	}
 
+	if (!_airmode) {
+		boost = math::min(boost, 0.0f); // Disable positive boosting if not in air-mode
+	}
 
 	// capture saturation
 	if (min_out < 0.0f) {
@@ -425,6 +429,12 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
 	_saturation_status.flags.valid = true;
 }
 
+void
+MultirotorMixer::set_airmode(bool airmode)
+{
+	_airmode = airmode;
+}
+
 void
 MultirotorMixer::groups_required(uint32_t &groups)
 {

src/modules/mc_att_control/mc_att_control_params.c

@@ -542,4 +542,18 @@ PARAM_DEFINE_FLOAT(MC_TPA_RATE_D, 0.0f);
  * @increment 10
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 30.f);
+PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f);
+
+/**
+ * Multicopter air-mode
+ *
+ * The air-mode enables the mixer to increase or decrease the total thrust of the multirotor
+ * in order to keep attitude and rate control even at low and high throttle.
+ * This function should be disabled during tuning as it will help the controller
+ * to diverge if the closed-loop is unstable.
+ *
+ * @boolean
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_INT32(MC_AIRMODE, 0);
+

src/modules/px4iofirmware/mixer.cpp

@@ -254,6 +254,11 @@ mixer_tick(void)
 	 */
 	mixer_group.set_trims(r_page_servo_control_trim, PX4IO_SERVO_COUNT);
 
+	/*
+	 * Update air-mode parameter
+	 */
+	mixer_group.set_airmode(REG_TO_BOOL(r_setup_airmode));
+
 
 	/*
 	 * Run the mixers.

src/modules/px4iofirmware/protocol.h

@@ -78,6 +78,8 @@
 #define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)floorf((_float + 0.00005f) * 10000.0f))
 
+#define REG_TO_BOOL(_reg) 	((bool)(_reg))
+
 #define PX4IO_PROTOCOL_VERSION		4
 
 /* maximum allowable sizes on this protocol version */
@@ -232,6 +234,8 @@ enum {							/* DSM bind states */
 
 #define PX4IO_P_SETUP_MOTOR_SLEW_MAX		24 	/* max motor slew rate */
 
+#define PX4IO_P_SETUP_AIRMODE 			27 	/* air-mode */
+
 #define PX4IO_P_SETUP_THR_MDL_FAC 		25	/* factor for modelling static pwm output to thrust relationship */
 
 #define PX4IO_P_SETUP_THERMAL			26	/* thermal management */

src/modules/px4iofirmware/px4io.h

@@ -132,6 +132,7 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 #define r_setup_sbus_rate	r_page_setup[PX4IO_P_SETUP_SBUS_RATE]
 #define r_setup_thr_fac		r_page_setup[PX4IO_P_SETUP_THR_MDL_FAC]
 #define r_setup_slew_max	r_page_setup[PX4IO_P_SETUP_MOTOR_SLEW_MAX]
+#define r_setup_airmode		r_page_setup[PX4IO_P_SETUP_AIRMODE]
 
 #define r_control_values	(&r_page_controls[0])
 

src/modules/px4iofirmware/registers.c

@@ -178,6 +178,7 @@ volatile uint16_t	r_page_setup[] = {
 	[PX4IO_P_SETUP_SCALE_PITCH] = 10000,
 	[PX4IO_P_SETUP_SCALE_YAW] = 10000,
 	[PX4IO_P_SETUP_MOTOR_SLEW_MAX] = 0,
+	[PX4IO_P_SETUP_AIRMODE] = 0,
 	[PX4IO_P_SETUP_THR_MDL_FAC] = 0,
 	[PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE
 };
@@ -704,6 +705,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			sbus1_set_output_rate_hz(value);
 			break;
 
+		case PX4IO_P_SETUP_AIRMODE:
+			r_page_setup[PX4IO_P_SETUP_AIRMODE] = value;
+			break;
+
 		case PX4IO_P_SETUP_THR_MDL_FAC:
 			update_mc_thrust_param = true;
 			r_page_setup[offset] = value;