VTOL: rework forward actuation assist

-allow positive pitch offsets in hover
-add param for min pitch during LAND

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

src/lib/parameters/param_translation.cpp

@@ -186,6 +186,15 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	// 2021-07-12: translate VT_DWN_PITCH_MAX to VT_PITCH_MIN
+	{
+		if (strcmp("VT_DWN_PITCH_MAX", node->name) == 0) {
+			strcpy(node->name, "VT_PITCH_MIN");
+			node->d *= -1;
+			PX4_INFO("copying and inverting sign %s -> %s", "VT_DWN_PITCH_MAX", "VT_PITCH_MIN");
+		}
+	}
+
 	// translate (SPI) calibration ID parameters. This can be removed after the next release (current release=1.10)
 
 	if (node->type != BSON_INT32) {
@@ -193,7 +202,6 @@ bool param_modify_on_import(bson_node_t node)
 	}
 
 	int64_t *ivalue = &node->i;
-
 	const char *cal_id_params[] = {
 		"CAL_ACC0_ID",
 		"CAL_GYRO0_ID",

src/modules/vtol_att_control/standard_params.c

@@ -61,18 +61,6 @@
  */
 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 (see VT_FW_TRHUST_EN).
- * If demanded down pitch exceeds this limmit, the fixed-wing forward actuators are used instead.
- *
- * @min 0.0
- * @max 45.0
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_DWN_PITCH_MAX, 5.0f);
-
 /**
  * Fixed-wing actuator thrust scale for hover forward flight.
  *

src/modules/vtol_att_control/tiltrotor.cpp

@@ -483,6 +483,6 @@ float Tiltrotor::thrust_compensation_for_tilt()
 	// only compensate for tilt angle up to 0.5 * max tilt
 	float compensated_tilt = math::constrain(_tilt_control, 0.0f, 0.5f);
 
-	// increase vertical thrust by 1/cos(tilt), limmit to [-1,0]
+	// increase vertical thrust by 1/cos(tilt), limit to [-1,0]
 	return math::constrain(_v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F), -1.0f, 0.0f);
 }

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -91,8 +91,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_params_handles.dec_to_pitch_i = param_find("VT_B_DEC_I");
 	_params_handles.back_trans_dec_sp = param_find("VT_B_DEC_MSS");
 
-
-	_params_handles.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
+	_params_handles.pitch_min_rad = param_find("VT_PTCH_MIN");
 	_params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
 	_params_handles.vt_mc_on_fmu = param_find("VT_MC_ON_FMU");
 
@@ -100,8 +99,8 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_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");
+	_params_handles.land_pitch_min_rad = param_find("VT_LND_PTCH_MIN");
+
 	/* fetch initial parameter values */
 	parameters_update();
 
@@ -326,8 +325,12 @@ VtolAttitudeControl::parameters_update()
 	_params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
 
 	/* maximum down pitch allowed */
-	param_get(_params_handles.down_pitch_max, &v);
-	_params.down_pitch_max = math::radians(v);
+	param_get(_params_handles.pitch_min_rad, &v);
+	_params.pitch_min_rad = math::radians(v);
+
+	/* maximum down pitch allowed during landing*/
+	param_get(_params_handles.land_pitch_min_rad, &v);
+	_params.land_pitch_min_rad = math::radians(v);
 
 	/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
 	param_get(_params_handles.forward_thrust_scale, &_params.forward_thrust_scale);

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -217,7 +217,8 @@ private:
 		param_t fw_motors_off;
 		param_t diff_thrust;
 		param_t diff_thrust_scale;
-		param_t down_pitch_max;
+		param_t pitch_min_rad;
+		param_t land_pitch_min_rad;
 		param_t forward_thrust_scale;
 		param_t dec_to_pitch_ff;
 		param_t dec_to_pitch_i;

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -358,3 +358,29 @@ PARAM_DEFINE_FLOAT(VT_B_DEC_I, 0.1f);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_INT32(VT_MC_ON_FMU, 0);
+
+/**
+ * Minimum pitch angle during hover.
+ *
+ * Minimum pitch angle during hover flight. If the desired pitch angle is is lower than this value
+ * then the fixed-wing forward actuation can be used to compensate for the missing thrust in forward direction
+ * (see VT_FW_TRHUST_EN)
+ *
+ * @min -10.0
+ * @max 45.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_PTCH_MIN, -5.0f);
+
+/**
+ * Minimum pitch angle during hover landing.
+ *
+ * Overrides  VT_PTCH_MIN when the vehicle is in LAND mode (hovering).
+ * During landing it can be beneficial to allow lower minimum pitch angles as it can avoid the wings
+ * generating too much lift and preventing the vehicle from sinking at the desired rate.
+ *
+ * @min -10.0
+ * @max 45.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_LND_PTCH_MIN, -5.0f);

src/modules/vtol_att_control/vtol_type.cpp

@@ -559,23 +559,32 @@ float VtolType::pusher_assist()
 
 	// calculate the desired pitch seen in the heading frame
 	// this value corresponds to the amount the vehicle would try to pitch down
-	float pitch_down = atan2f(body_z_sp(0), body_z_sp(2));
+	const float pitch_setpoint = atan2f(body_z_sp(0), body_z_sp(2));
 
 	// normalized pusher support throttle (standard VTOL) or tilt (tiltrotor), initialize to 0
 	float forward_thrust = 0.0f;
 
+	float pitch_setpoint_min = _params->pitch_min_rad;
+
+	if (_attc->get_pos_sp_triplet()->current.valid
+	    && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
+		pitch_setpoint_min = _params->land_pitch_min_rad; // set min pitch during LAND (usually lower to generate less lift)
+	}
+
 	// only allow pitching down up to threshold, the rest of the desired
 	// forward acceleration will be compensated by the pusher/tilt
-	if (pitch_down < -_params->down_pitch_max) {
-		// desired roll angle in heading frame stays the same
-		float roll_new = -asinf(body_z_sp(1));
 
-		forward_thrust = (sinf(-pitch_down) - sinf(_params->down_pitch_max)) * _params->forward_thrust_scale;
+	if (pitch_setpoint < pitch_setpoint_min) {
+		// desired roll angle in heading frame stays the same
+		const float roll_new = -asinf(body_z_sp(1));
+
+		forward_thrust = (sinf(pitch_setpoint_min) - sinf(pitch_setpoint)) * _params->forward_thrust_scale;
 		// limit forward actuation to [0, 0.9]
 		forward_thrust = math::constrain(forward_thrust, 0.0f, 0.9f);
 
-		// return the vehicle to level position
-		float pitch_new = 0.0f;
+		// Set the pitch to 0 if the pitch limit is negative (pitch down), but allow a positive (pitch up) pitch.
+		// This can be used for tiltrotor to make them hover with a positive angle of attack
+		const float pitch_new = pitch_setpoint_min > 0.f ? pitch_setpoint_min : 0.f;
 
 		// create corrected desired body z axis in heading frame
 		const Dcmf R_tmp = Eulerf(roll_new, pitch_new, 0.0f);

src/modules/vtol_att_control/vtol_type.h

@@ -70,7 +70,8 @@ struct Params {
 	int32_t fw_motors_off;			/**< bitmask of all motors that should be off in fixed wing mode */
 	int32_t diff_thrust;
 	float diff_thrust_scale;
-	float down_pitch_max;
+	float pitch_min_rad;
+	float land_pitch_min_rad;
 	float forward_thrust_scale;
 	float dec_to_pitch_ff;
 	float dec_to_pitch_i;