VTOL: Pusher assist: add configuration for enabling it in LAND /disable below some altitude (#14706)

* VTOL type: add new parameter VT_FWD_TRHUST_EN for customizing pusher/tilt assist

Depending on the setting of this param, the pusher assist is:
-completely disabled (default)
-enabled in pos, alt and auto mode (except LAND)
-enabled in pos, alt and auto mode if above MPC_LAND_ALT1
-enabled in pos, alt and auto mode if above MPC_LAND_ALT2
-enabled in pos, alt and auto mode
(before it was always disabled in LAND mode)

-change default of VT_FWD_THRUST_SC from 0 to 0.7

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/vtol_att_control/standard_params.c

@@ -41,9 +41,27 @@
 
 
 /**
- * Maximum allowed down-pitch the controller is able to demand. This prevents large, negative
- * lift values being created when facing strong winds. The vehicle will use the pusher motor
- * to accelerate forward if necessary.
+ * Enable/disable usage of fixed-wing actuators in hover to generate forward force (instead of pitching down).
+ * This technique can be used to avoid the plane having to pitch down in order to move forward.
+ * This prevents large, negative lift values being created when facing strong winds.
+ * Fixed-wing forward actuators refers to puller/pusher (standard VTOL), or forward-tilt (tiltrotor VTOL).
+ * Only active if demaded down pitch is above VT_DWN_PITCH_MAX, and uses VT_FWD_THRUST_SC to get from
+ * demanded down pitch to fixed-wing actuation.
+ *
+ * @value 0 Disable FW forward actuation in hover.
+ * @value 1 Enable FW forward actuation in hover in altitude, position and auto modes (except LANDING).
+ * @value 2 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT1.
+ * @value 3 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT2.
+ * @value 4 Enable FW forward actuation in hover in altitude, position and auto modes.
+ *
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_FWD_THRUST_EN, 0);
+
+/**
+ * Maximum allowed angle the vehicle is allowed to pitch down to generate forward force
+ * when fixed-wing forward actuation is active (seeVT_FW_TRHUST_EN).
+ * If demanded down pitch exceeds this limmit, the fixed-wing forward actuators are used instead.
  *
  * @min 0.0
  * @max 45.0
@@ -52,16 +70,17 @@
 PARAM_DEFINE_FLOAT(VT_DWN_PITCH_MAX, 5.0f);
 
 /**
- * Fixed wing thrust scale for hover forward flight.
+ * Fixed-wing actuator thrust scale for hover forward flight.
+ *
+ * Scale applied to the demanded down-pitch to get the fixed-wing forward actuation in hover mode.
+ * Only active if demaded down pitch is above VT_DWN_PITCH_MAX.
+ * Enabled via VT_FWD_THRUST_EN.
  *
- * Scale applied to fixed wing thrust being used as source for forward acceleration in multirotor mode.
- * This technique can be used to avoid the plane having to pitch down a lot in order to move forward.
- * Setting this value to 0 (default) will disable this strategy.
  * @min 0.0
  * @max 2.0
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_FWD_THRUST_SC, 0.0f);
+PARAM_DEFINE_FLOAT(VT_FWD_THRUST_SC, 0.7f);
 
 /**
  * Back transition MC motor ramp up time

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -94,6 +94,12 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
 	_params_handles.vt_mc_on_fmu = param_find("VT_MC_ON_FMU");
 
+	_params_handles.vt_forward_thrust_enable_mode = param_find("VT_FWD_THRUST_EN");
+	_params_handles.mpc_land_alt1 = param_find("MPC_LAND_ALT1");
+	_params_handles.mpc_land_alt2 = param_find("MPC_LAND_ALT2");
+
+	_params_handles.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
+	_params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
 	/* fetch initial parameter values */
 	parameters_update();
 
@@ -301,6 +307,10 @@ VtolAttitudeControl::parameters_update()
 	param_get(_params_handles.vt_mc_on_fmu, &l);
 	_params.vt_mc_on_fmu = l;
 
+	param_get(_params_handles.vt_forward_thrust_enable_mode, &_params.vt_forward_thrust_enable_mode);
+	param_get(_params_handles.mpc_land_alt1, &_params.mpc_land_alt1);
+	param_get(_params_handles.mpc_land_alt2, &_params.mpc_land_alt2);
+
 	// update the parameters of the instances of base VtolType
 	if (_vtol_type != nullptr) {
 		_vtol_type->parameters_update();

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -207,6 +207,9 @@ private:
 		param_t dec_to_pitch_i;
 		param_t back_trans_dec_sp;
 		param_t vt_mc_on_fmu;
+		param_t vt_forward_thrust_enable_mode;
+		param_t mpc_land_alt1;
+		param_t mpc_land_alt2;
 	} _params_handles{};
 
 	/* for multicopters it is usual to have a non-zero idle speed of the engines

src/modules/vtol_att_control/vtol_type.cpp

@@ -422,6 +422,43 @@ bool VtolType::is_channel_set(const int channel, const int target)
 
 float VtolType::pusher_assist()
 {
+	// Altitude above ground is distance sensor altitude if available, otherwise local z-position
+	float dist_to_ground = -_local_pos->z;
+
+	if (_local_pos->dist_bottom_valid) {
+		dist_to_ground = _local_pos->dist_bottom;
+	}
+
+	// disable pusher assist depending on setting of forward_thrust_enable_mode:
+	switch (_params->vt_forward_thrust_enable_mode) {
+	case DISABLE: // disable in all modes
+		return 0.0f;
+		break;
+
+	case ENABLE_WITHOUT_LAND: // disable in land mode
+		if (_attc->get_pos_sp_triplet()->current.valid
+		    && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
+		    && _v_control_mode->flag_control_auto_enabled) {
+			return 0.0f;
+		}
+
+		break;
+
+	case ENABLE_ABOVE_MPC_LAND_ALT1: // disable if below MPC_LAND_ALT1
+		if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1)) {
+			return 0.0f;
+		}
+
+		break;
+
+	case ENABLE_ABOVE_MPC_LAND_ALT2: // disable if below MPC_LAND_ALT2
+		if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2)) {
+			return 0.0f;
+		}
+
+		break;
+	}
+
 	// if the thrust scale param is zero or the drone is not in some position or altitude control mode,
 	// then the pusher-for-pitch strategy is disabled and we can return
 	if (_params->forward_thrust_scale < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled
@@ -434,12 +471,6 @@ float VtolType::pusher_assist()
 		return 0.0f;
 	}
 
-	// disable pusher assist during landing
-	if (_attc->get_pos_sp_triplet()->current.valid
-	    && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
-		return 0.0f;
-	}
-
 	const Dcmf R(Quatf(_v_att->q));
 	const Dcmf R_sp(Quatf(_v_att_sp->q_d));
 	const Eulerf euler(R);

src/modules/vtol_att_control/vtol_type.h

@@ -76,6 +76,9 @@ struct Params {
 	float dec_to_pitch_i;
 	float back_trans_dec_sp;
 	bool vt_mc_on_fmu;
+	int vt_forward_thrust_enable_mode;
+	float mpc_land_alt1;
+	float mpc_land_alt2;
 };
 
 // Has to match 1:1 msg/vtol_vehicle_status.msg
@@ -92,6 +95,14 @@ enum class vtol_type {
 	STANDARD
 };
 
+enum VtolForwardActuationMode {
+	DISABLE = 0,
+	ENABLE_WITHOUT_LAND,
+	ENABLE_ABOVE_MPC_LAND_ALT1,
+	ENABLE_ABOVE_MPC_LAND_ALT2,
+	ENABLE_ALL_MODES
+};
+
 // these are states that can be applied to a selection of multirotor motors.
 // e.g. if we need to shut off some motors after transitioning to fixed wing mode
 // we can individually disable them while others might still need to be enabled to produce thrust.