added offboard lost actions with additional timeout

posix-configs/SITL/init/rcS_gazebo_iris

@@ -23,9 +23,11 @@ param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
 param set COM_DISARM_LAND 3
 param set NAV_ACC_RAD 2.0
+param set COM_OF_LOSS_T 5
 param set RTL_RETURN_ALT 30.0
-param set RTL_DESCEND_ALT 10.0
+param set RTL_DESCEND_ALT 5.0
-param set RTL_LAND_DELAY 0
+param set RTL_LAND_DELAY 5
+param set COM_DISARM_LAND 5
 param set MIS_TAKEOFF_ALT 2.5
 param set MC_ROLLRATE_P 0.3
 param set MC_PITCHRATE_P 0.3

src/modules/commander/commander.cpp

@@ -1161,6 +1161,8 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_component_id = param_find("MAV_COMP_ID");
 	param_t _param_enable_datalink_loss = param_find("NAV_DLL_ACT");
 	param_t _param_enable_rc_loss = param_find("NAV_RCL_ACT");
+	param_t _param_offboard_loss_act = param_find("NAV_OBL_ACT");
+	param_t _param_offboard_loss_rc_act = param_find("NAV_OBL_RC_ACT");
 	param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
 	param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T");
 	param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T");
@@ -1176,6 +1178,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_geofence_action = param_find("GF_ACTION");
 	param_t _param_disarm_land = param_find("COM_DISARM_LAND");
 	param_t _param_low_bat_act = param_find("COM_LOW_BAT_ACT");
+	param_t _param_offboard_loss_timeout = param_find("COM_OF_LOSS_T");
 
 	param_t _param_fmode_1 = param_find("COM_FLTMODE1");
 	param_t _param_fmode_2 = param_find("COM_FLTMODE2");
@@ -1256,6 +1259,7 @@ int commander_thread_main(int argc, char *argv[])
 	status.rc_signal_lost = true;
 	status_flags.offboard_control_signal_lost = true;
 	status.data_link_lost = true;
+	status_flags.offboard_control_loss_timeout = false;
 
 	status_flags.condition_system_prearm_error_reported = false;
 	status_flags.condition_system_hotplug_timeout = false;
@@ -1515,6 +1519,9 @@ int commander_thread_main(int argc, char *argv[])
 	int32_t datalink_loss_timeout = 10;
 	float rc_loss_timeout = 0.5;
 	int32_t datalink_regain_timeout = 0;
+	float offboard_loss_timeout = 0.0f;
+	int32_t offboard_loss_act = 0;
+	int32_t offboard_loss_rc_act = 0;
 
 	int32_t geofence_action = 0;
 
@@ -1609,6 +1616,9 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_geofence_action, &geofence_action);
 			param_get(_param_disarm_land, &disarm_when_landed);
 			param_get(_param_low_bat_act, &low_bat_action);
+			param_get(_param_offboard_loss_timeout, &offboard_loss_timeout);
+			param_set(_param_offboard_loss_act, &offboard_loss_act);
+			param_set(_param_offboard_loss_rc_act, &offboard_loss_rc_act);
 
 			/* Autostart id */
 			param_get(_param_autostart_id, &autostart_id);
@@ -1651,12 +1661,23 @@ int commander_thread_main(int argc, char *argv[])
 		    offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
 			if (status_flags.offboard_control_signal_lost) {
 				status_flags.offboard_control_signal_lost = false;
+				status_flags.offboard_control_loss_timeout = false;
 				status_changed = true;
 			}
 
 		} else {
 			if (!status_flags.offboard_control_signal_lost) {
 				status_flags.offboard_control_signal_lost = true;
+
+				if (offboard_loss_timeout < FLT_EPSILON) {
+					/* execute loss action immediately */
+					status_flags.offboard_control_loss_timeout = true;
+
+				} else {
+					/* wait for timeout if set */
+					status_flags.offboard_control_loss_timeout = offboard_control_mode.timestamp +
+						OFFBOARD_TIMEOUT + offboard_loss_timeout * 1e6f > hrt_absolute_time();
+				}
 				status_changed = true;
 			}
 		}
@@ -2669,7 +2690,9 @@ int commander_thread_main(int argc, char *argv[])
 						       mission_result.stay_in_failsafe,
 						       &status_flags,
 						       land_detector.landed,
-						       (rc_loss_enabled > 0));
+						       (rc_loss_enabled > 0),
+						       offboard_loss_act,
+						       offboard_loss_rc_act);
 
 		if (status.failsafe != failsafe_old) {
 			status_changed = true;

src/modules/commander/commander_params.c

@@ -413,3 +413,15 @@ PARAM_DEFINE_INT32(COM_FLTMODE5, -1);
  * @value 12 Follow Me
  */
 PARAM_DEFINE_INT32(COM_FLTMODE6, -1);
+
+/**
+ * Time-out to wait when offboard connection is lost before triggering offboard lost action.
+ * See NAV_OBL_ACT and NAV_OBL_RC_ACT to configure action.
+ *
+ * @group Commander
+ * @unit second
+ * @min 0
+ * @max 60
+ * @increment 1
+ */
+PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 0.0f);

src/modules/commander/state_machine_helper.cpp

@@ -588,7 +588,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
  */
 bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
 		   const bool data_link_loss_enabled, const bool mission_finished,
-		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, const bool rc_loss_enabled)
+		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, const bool rc_loss_enabled,
+		   const int offb_loss_act, const int offb_loss_rc_act)
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
@@ -847,17 +848,75 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 		if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) {
 			status->failsafe = true;
 
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+			if (status_flags->offboard_control_loss_timeout && offb_loss_rc_act < 5 && offb_loss_rc_act >= 0) {
-		} else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) {
+				if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid
-			status->failsafe = true;
+					&& offb_loss_rc_act == 3) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
-			if (status_flags->condition_local_position_valid) {
+				} else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 0) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+
+				} else if (status_flags->condition_local_altitude_valid && offb_loss_rc_act == 1) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+
+				} else if (offb_loss_rc_act == 2) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
+
+				} else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 4) {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
 				} else if (status_flags->condition_local_altitude_valid) {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
 				} else {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 				}
+
+			} else {
+				if (status_flags->condition_global_position_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+
+				} else if (status_flags->condition_local_altitude_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+
+				} else {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+				}
+			}
+
+		} else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) {
+			status->failsafe = true;
+
+			if (status_flags->offboard_control_loss_timeout && offb_loss_act < 3 && offb_loss_act >= 0) {
+				if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid
+					&& offb_loss_rc_act == 2) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+
+				} else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 1) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+
+				} else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 0) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
+				} else if (status_flags->condition_local_altitude_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+				} else {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+				}
+
+			} else {
+				if (status_flags->condition_global_position_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+
+				} else if (status_flags->condition_local_altitude_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+				} else {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+				}
+			}
+
 		} else {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
 		}

src/modules/commander/state_machine_helper.h

@@ -81,6 +81,7 @@ struct status_flags_s {
 	bool offboard_control_signal_lost;
 	bool offboard_control_signal_weak;
 	bool offboard_control_set_by_command;		// true if the offboard mode was set by a mavlink command and should not be overridden by RC
+	bool offboard_control_loss_timeout;		// true if offboard is lost for a certain amount of time
 	bool rc_signal_found_once;
 	bool rc_signal_lost_cmd;			// true if RC lost mode is commanded
 	bool rc_input_blocked;				// set if RC input should be ignored temporarily
@@ -113,7 +114,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
 		   const bool data_link_loss_enabled, const bool mission_finished,
 		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed,
-		   const bool rc_loss_enabled);
+		   const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act);
 
 int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery);
 

src/modules/navigator/navigator_params.c

@@ -104,6 +104,36 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
  */
 PARAM_DEFINE_INT32(NAV_RCL_ACT, 0);
 
+/**
+ * Set offboard loss failsafe mode
+ *
+ * The offboard loss failsafe will only be entered after a timeout,
+ * set by COM_OF_LOSS_T in seconds.
+ *
+ * @value 0 Land at current position
+ * @value 1 Loiter
+ * @value 2 Return to Land
+ *
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(NAV_OBL_ACT, 0);
+
+/**
+ * Set offboard loss failsafe mode when RC is available
+ *
+ * The offboard loss failsafe will only be entered after a timeout,
+ * set by COM_OF_LOSS_T in seconds.
+ *
+ * @value 0 Position control
+ * @value 1 Altitude control
+ * @value 2 Manual
+ * @value 3 Return to Land
+ * @value 4 Land at current position
+ *
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(NAV_OBL_RC_ACT, 0);
+
 /**
  * Airfield home Lat
  *