vtol_type: treat Descend as Land for pusher assist

Signed-off-by: Silvan Fuhrer <[email protected]>
PX4 · Mar 6, 2024 · 5818474 · 5818474
1 parent 471c3e0
commit 5818474
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 11 deletions.
diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c
@@ -48,6 +48,7 @@
  * Fixed-wing forward actuators refers to puller/pusher (standard VTOL), or forward-tilt (tiltrotor VTOL).
  * Only active if demanded down pitch is below VT_PITCH_MIN.
  * Use VT_FWD_THRUST_SC to tune it.
+ * Descend mode is treated as Landing too.
  *
  * Only active (if enabled) in Altitude, Position and Auto modes, not in Stabilized.
  *

diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp
@@ -444,16 +444,21 @@ float VtolType::pusher_assist()
 
 	}
 
+	// the vehicle is "landing" if it is in auto mode and the type is set to LAND, and
+	// "descending" if it is in auto and climb rate controlled but not altitude controlled
+	const bool vehicle_is_landing_or_descending = _v_control_mode->flag_control_auto_enabled
+			&& ((_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_climb_rate_enabled && !_v_control_mode->flag_control_altitude_enabled));
+
 	// disable pusher assist depending on setting of forward_thrust_enable_mode:
 	switch (_param_vt_fwd_thrust_en.get()) {
 	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) {
+	case ENABLE_WITHOUT_LAND: // disable in land/descend mode
+		if (vehicle_is_landing_or_descending) {
 			return 0.0f;
 		}
 
@@ -473,20 +478,16 @@ float VtolType::pusher_assist()
 
 		break;
 
-	case ENABLE_ABOVE_MPC_LAND_ALT1_WITHOUT_LAND: // disable if below MPC_LAND_ALT1 or 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) ||
+	case ENABLE_ABOVE_MPC_LAND_ALT1_WITHOUT_LAND: // disable if below MPC_LAND_ALT1 or in land/descend mode
+		if (vehicle_is_landing_or_descending ||
 		    (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt1.get()))) {
 			return 0.0f;
 		}
 
 		break;
 
 	case ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND: // disable if below MPC_LAND_ALT2 or 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) ||
+		if (vehicle_is_landing_or_descending ||
 		    (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt2.get()))) {
 			return 0.0f;
 		}