Quadchute param update (#19351)

New parameter for actions after a quadchute (COM_QC_ACT)

Co-authored-by: Jonas <jonas.perolini@rigi.tech>
Co-authored-by: Matthias Grob <maetugr@gmail.com>

src/modules/commander/Commander.cpp

@@ -2881,6 +2881,7 @@ Commander::run()
 						       _vehicle_land_detected.landed,
 						       static_cast<link_loss_actions_t>(_param_nav_rcl_act.get()),
 						       static_cast<offboard_loss_actions_t>(_param_com_obl_act.get()),
+						       static_cast<quadchute_actions_t>(_param_com_qc_act.get()),
 						       static_cast<offboard_loss_rc_actions_t>(_param_com_obl_rc_act.get()),
 						       static_cast<position_nav_loss_actions_t>(_param_com_posctl_navl.get()),
 						       _param_com_rcl_act_t.get(),

src/modules/commander/Commander.hpp

@@ -223,6 +223,9 @@ private:
 		(ParamInt<px4::params::RC_MAP_FLTMODE>) _param_rc_map_fltmode,
 		(ParamInt<px4::params::RC_MAP_MODE_SW>) _param_rc_map_mode_sw,
 
+		// Quadchute
+		(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
+
 		// Offboard
 		(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
 		(ParamInt<px4::params::COM_OBL_ACT>) _param_com_obl_act,

src/modules/commander/commander_params.c

@@ -416,6 +416,17 @@ PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 1.0f);
  */
 PARAM_DEFINE_INT32(COM_OBL_ACT, 0);
 
+/**
+ * Set command after a quadchute
+ *
+ * @value -1 No action: stay in current flight mode
+ * @value  0 Return mode
+ * @value  1 Land mode
+ * @value  2 Hold mode
+ * @group Commander
+ */
+PARAM_DEFINE_INT32(COM_QC_ACT, 0);
+
 /**
  * Set offboard loss failsafe mode when RC is available
  *

src/modules/commander/state_machine_helper.cpp

@@ -147,6 +147,10 @@ void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &arm
 				 const vehicle_status_flags_s &status_flags,
 				 const offboard_loss_actions_t offboard_loss_act);
 
+void set_quadchute_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
+			     const vehicle_status_flags_s &status_flags,
+			     const quadchute_actions_t quadchute_act);
+
 void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
 				    const vehicle_status_flags_s &status_flags,
 				    const offboard_loss_rc_actions_t offboard_loss_rc_act);
@@ -497,6 +501,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 		   orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished,
 		   const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed,
 		   const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act,
+		   const quadchute_actions_t quadchute_act,
 		   const offboard_loss_rc_actions_t offb_loss_rc_act,
 		   const position_nav_loss_actions_t posctl_nav_loss_act,
 		   const float param_com_rcl_act_t, const int param_com_rcl_except)
@@ -588,7 +593,8 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 
 		} else if (status_flags.vtol_transition_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+
+			set_quadchute_nav_state(status, armed, status_flags, quadchute_act);
 
 		} else if (status.mission_failure) {
 			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
@@ -641,7 +647,8 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 			// nothing to do - everything done in check_invalid_pos_nav_state
 
 		} else if (status_flags.vtol_transition_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+
+			set_quadchute_nav_state(status, armed, status_flags, quadchute_act);
 
 		} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
 			// Data link lost, data link loss reaction configured -> do configured reaction
@@ -756,7 +763,8 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 			// nothing to do - everything done in check_invalid_pos_nav_state
 
 		} else if (status_flags.vtol_transition_failure) {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+
+			set_quadchute_nav_state(status, armed, status_flags, quadchute_act);
 
 		} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
 			// Data link lost, data link loss reaction configured -> do configured reaction
@@ -985,6 +993,41 @@ void reset_link_loss_globals(actuator_armed_s &armed, const bool old_failsafe, c
 	}
 }
 
+void set_quadchute_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
+			     const vehicle_status_flags_s &status_flags,
+			     const quadchute_actions_t quadchute_act)
+{
+	switch (quadchute_act) {
+	case quadchute_actions_t::NO_ACTION:
+		// If quadchute action is disabled then no action must be taken.
+		break;
+
+	default:
+	case quadchute_actions_t::AUTO_RTL:
+		if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
+			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+		}
+
+		break;
+
+	// FALLTHROUGH
+	case quadchute_actions_t::AUTO_LAND:
+		if (status_flags.condition_global_position_valid) {
+			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+		}
+
+		break;
+
+	// FALLTHROUGH
+	case quadchute_actions_t::AUTO_LOITER:
+		if (status_flags.condition_global_position_valid) {
+			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+		}
+
+		break;
+	}
+}
+
 void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
 				 const vehicle_status_flags_s &status_flags,
 				 const offboard_loss_actions_t offboard_loss_act)

src/modules/commander/state_machine_helper.h

@@ -70,6 +70,13 @@ enum class link_loss_actions_t {
 	LOCKDOWN = 6,		// Lock actuators (set actuator outputs to disarmed values)
 };
 
+enum class quadchute_actions_t {
+	NO_ACTION = -1,
+	AUTO_RTL = 0,		// Return mode
+	AUTO_LAND = 1,		// Land mode
+	AUTO_LOITER = 2,	// Hold mode
+};
+
 enum class offboard_loss_actions_t {
 	DISABLED = -1,
 	AUTO_LAND = 0,		// Land mode
@@ -122,6 +129,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 		   orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished,
 		   const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed,
 		   const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act,
+		   const quadchute_actions_t quadchute_act,
 		   const offboard_loss_rc_actions_t offb_loss_rc_act,
 		   const position_nav_loss_actions_t posctl_nav_loss_act,
 		   const float param_com_rcl_act_t, const int param_com_rcl_except);