Navigator: failsafe: Add descend mode as a failsafe.

The land mode supposes we have a GPS causing issues when flying with a local position (such as mocap or vision) This adds the alternative to use the descend mode.
PX4 · Oct 24, 2024 · 2fad6ba · 2fad6ba
1 parent 4d4c5ef
commit 2fad6ba
Show file tree

Hide file tree

Showing 4 changed files with 67 additions and 4 deletions.
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
@@ -271,6 +271,9 @@ PARAM_DEFINE_INT32(COM_DISARM_MAN, 1);
  * @value 0 Warning
  * @value 2 Land mode
  * @value 3 Return at critical level, land at emergency level
+ * @value 4 Descend mode
+ * @min 0
+ * @max 4
  */
 PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
 
@@ -304,6 +307,7 @@ PARAM_DEFINE_FLOAT(COM_FAIL_ACT_T, 5.f);
  * @value 0 Warning
  * @value 1 Return
  * @value 2 Land
+ * @value 3 Descend
  * @increment 1
  */
 PARAM_DEFINE_INT32(COM_IMB_PROP_ACT, 0);
@@ -328,6 +332,7 @@ PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 1.0f);
  * @value  0 Return mode
  * @value  1 Land mode
  * @value  2 Hold mode
+ * @value  3 Descend mode
  * @group Commander
  */
 PARAM_DEFINE_INT32(COM_QC_ACT, 0);
@@ -346,6 +351,9 @@ PARAM_DEFINE_INT32(COM_QC_ACT, 0);
  * @value 5 Hold mode
  * @value 6 Terminate
  * @value 7 Disarm
+ * @value 8 Descend
+ * @min 0
+ * @max 8
  * @group Commander
  */
 PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0);
@@ -596,8 +604,9 @@ PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0);
  * @value 3 Land mode
  * @value 5 Terminate
  * @value 6 Disarm
+ * @value 7 Descend
  * @min 0
- * @max 6
+ * @max 7
  *
  * @group Commander
  */
@@ -615,8 +624,9 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
  * @value 3 Land mode
  * @value 5 Terminate
  * @value 6 Disarm
+ * @value 7 Descend
  * @min 1
- * @max 6
+ * @max 7
  *
  * @group Commander
  */
@@ -649,6 +659,7 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
  * @value 2 Land mode
  * @value 3 Return mode
  * @value 4 Terminate
+ * @value 5 Descend
  * @group Commander
  */
 PARAM_DEFINE_INT32(COM_ACT_FAIL_ACT, 0);
@@ -923,6 +934,7 @@ PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f);
  * @value 3 Return
  * @value 4 Terminate
  * @value 5 Land
+ * @value 6 Descend
  * @increment 1
  */
 PARAM_DEFINE_INT32(COM_WIND_MAX_ACT, 0);

diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp
@@ -63,6 +63,11 @@ FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value)
 		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
 		break;
 
+	case gcs_connection_loss_failsafe_mode::Descend:
+		options.action = Action::Descend;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
 	case gcs_connection_loss_failsafe_mode::Terminate:
 		options.allow_user_takeover = UserTakeoverAllowed::Never;
 		options.action = Action::Terminate;
@@ -117,6 +122,11 @@ FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value)
 		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
 		break;
 
+	case geofence_violation_action::Descend_mode:
+		options.action = Action::Descend;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
 	default:
 		options.action = Action::Warn;
 		break;
@@ -148,6 +158,11 @@ FailsafeBase::ActionOptions Failsafe::fromImbalancedPropActParam(int param_value
 		options.action = Action::Land;
 		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
 		break;
+
+	case imbalanced_propeller_failsafe_mode::Descend:
+		options.action = Action::Descend;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
 	}
 
 	return options;
@@ -172,6 +187,11 @@ FailsafeBase::ActionOptions Failsafe::fromActuatorFailureActParam(int param_valu
 		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
 		break;
 
+	case actuator_failure_failsafe_mode::Descend_mode:
+		options.action = Action::Descend;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
 	case actuator_failure_failsafe_mode::Return_mode:
 		options.action = Action::RTL;
 		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
@@ -211,6 +231,10 @@ FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value
 			options.action = Action::RTL;
 			break;
 
+		case LowBatteryAction::Descend:
+			options.action = Action::Descend;
+			break;
+
 		case LowBatteryAction::Land:
 			options.action = Action::Land;
 			break;
@@ -231,6 +255,10 @@ FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value
 			options.action = Action::RTL;
 			break;
 
+		case LowBatteryAction::Descend:
+			options.action = Action::Descend;
+			break;
+
 		case LowBatteryAction::ReturnOrLand:
 		case LowBatteryAction::Land:
 			options.action = Action::Land;
@@ -267,6 +295,11 @@ FailsafeBase::ActionOptions Failsafe::fromQuadchuteActParam(int param_value)
 		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
 		break;
 
+	case command_after_quadchute::Descend_mode:
+		options.action = Action::Descend;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
 	case command_after_quadchute::Hold_mode:
 		options.action = Action::Hold;
 		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
@@ -307,6 +340,10 @@ FailsafeBase::Action Failsafe::fromOffboardLossActParam(int param_value, uint8_t
 		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
 		break;
 
+	case offboard_loss_failsafe_mode::Descend:
+		action = Action::Descend;
+		break;
+
 	case offboard_loss_failsafe_mode::Hold_mode:
 		action = Action::Hold;
 		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
@@ -360,6 +397,11 @@ FailsafeBase::ActionOptions Failsafe::fromHighWindLimitActParam(int param_value)
 		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
 		break;
 
+	case command_after_high_wind_failsafe::Descend:
+		options.action = Action::Descend;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
 	default:
 		options.action = Action::Warn;
 		break;

diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h
@@ -64,7 +64,8 @@ class Failsafe : public FailsafeBase
 		Warning = 0,        // Warning
 		Return = 1,         // Return mode (deprecated)
 		Land = 2,           // Land mode
-		ReturnOrLand = 3    // Return mode at critically low level, Land mode at current position if reaching dangerously low levels
+		ReturnOrLand = 3,   // Return mode at critically low level, Land mode at current position if reaching dangerously low levels
+		Descend = 4,        // Land without using position
 	};
 
 	enum class offboard_loss_failsafe_mode : int32_t {
@@ -76,6 +77,7 @@ class Failsafe : public FailsafeBase
 		Hold_mode = 5,
 		Terminate = 6,
 		Disarm = 7,
+		Descend = 8,
 	};
 
 	enum class position_control_navigation_loss_response : int32_t {
@@ -89,13 +91,15 @@ class Failsafe : public FailsafeBase
 		Land_mode = 2,
 		Return_mode = 3,
 		Terminate = 4,
+		Descend_mode = 5,
 	};
 
 	enum class imbalanced_propeller_failsafe_mode : int32_t {
 		Disabled = -1,
 		Warning = 0,
 		Return = 1,
 		Land = 2,
+		Descend = 3,
 	};
 
 	enum class geofence_violation_action : int32_t {
@@ -105,6 +109,7 @@ class Failsafe : public FailsafeBase
 		Return_mode = 3,
 		Terminate = 4,
 		Land_mode = 5,
+		Descend_mode = 6,
 	};
 
 	enum class gcs_connection_loss_failsafe_mode : int32_t {
@@ -114,13 +119,15 @@ class Failsafe : public FailsafeBase
 		Land_mode = 3,
 		Terminate = 5,
 		Disarm = 6,
+		Descend = 7,
 	};
 
 	enum class command_after_quadchute : int32_t {
 		Warning_only = -1,
 		Return_mode = 0,
 		Land_mode = 1,
 		Hold_mode = 2,
+		Descend_mode = 3,
 	};
 
 	// COM_RC_IN_MODE parameter values
@@ -138,7 +145,8 @@ class Failsafe : public FailsafeBase
 		Hold_mode = 2,
 		Return_mode = 3,
 		Terminate = 4,
-		Land_mode = 5
+		Land_mode = 5,
+		Descend = 6,
 	};
 
 	enum class command_after_remaining_flight_time_low : int32_t {

diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c
@@ -57,6 +57,7 @@
  * @value 3 Return mode
  * @value 4 Terminate
  * @value 5 Land mode
+ * @value 6 Descend mode
  * @group Geofence
  */
 PARAM_DEFINE_INT32(GF_ACTION, 2);