datalink loss (obc): add termination after loitering at airfield home

4 files changed, 39 insertions, 1 deletions

diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index 893d6d93a..3310984b0 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -64,6 +64,7 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
 	_param_airfieldhomelat(this, "NAV_AH_LAT", false),
 	_param_airfieldhomelon(this, "NAV_AH_LON", false),
 	_param_airfieldhomealt(this, "NAV_AH_ALT", false),
+	_param_airfieldhomewaittime(this, "AH_T"),
 	_param_numberdatalinklosses(this, "N"),
 	_dll_state(DLL_STATE_NONE)
 {
@@ -140,7 +141,8 @@ DataLinkLoss::set_dll_item()
 		_mission_item.yaw = NAN;
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
-		_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+		_mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get();
 		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
@@ -149,6 +151,15 @@ DataLinkLoss::set_dll_item()
 		_navigator->set_can_loiter_at_sp(true);
 		break;
 	}
+	case DLL_STATE_TERMINATE: {
+		/* Request flight termination from the commander */
+		pos_sp_triplet->flight_termination = true;
+		warnx("not switched to manual: request flight termination");
+		pos_sp_triplet->previous.valid = false;
+		pos_sp_triplet->current.valid = false;
+		pos_sp_triplet->next.valid = false;
+		break;
+	}
 	default:
 		break;
 	}
@@ -185,6 +196,18 @@ DataLinkLoss::advance_dll()
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->publish_mission_result();
 		break;
+	case DLL_STATE_FLYTOAIRFIELDHOMEWP:
+		_dll_state = DLL_STATE_TERMINATE;
+		warnx("time is up, state should have been changed manually by now");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
+		_navigator->get_mission_result()->stay_in_failsafe = true;
+		_navigator->publish_mission_result();
+		break;
+	case DLL_STATE_TERMINATE:
+		warnx("dll end");
+		_dll_state = DLL_STATE_END;
+		break;
+
 	default:
 		break;
 	}
diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h
index 96b4ce010..d0c9ad09a 100644
--- a/src/modules/navigator/datalinkloss.h
+++ b/src/modules/navigator/datalinkloss.h
@@ -72,12 +72,15 @@ private:
 	control::BlockParamInt _param_airfieldhomelat; // * 1e7
 	control::BlockParamInt _param_airfieldhomelon; // * 1e7
 	control::BlockParamFloat _param_airfieldhomealt;
+	control::BlockParamFloat _param_airfieldhomewaittime;
 	control::BlockParamInt _param_numberdatalinklosses;
 
 	enum DLLState {
 		DLL_STATE_NONE = 0,
 		DLL_STATE_FLYTOCOMMSHOLDWP = 1,
 		DLL_STATE_FLYTOAIRFIELDHOMEWP = 2,
+		DLL_STATE_TERMINATE = 3,
+		DLL_STATE_END = 4
 	} _dll_state;
 
 	/**
diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c
index 02f7ca4c3..db307c904 100644
--- a/src/modules/navigator/datalinkloss_params.c
+++ b/src/modules/navigator/datalinkloss_params.c
@@ -92,6 +92,17 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890);
 PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
 
 /**
+ * Aifield hole wait time
+ *
+ * The amount of time in seconds the system should wait at the airfield home waypoint
+ *
+ * @unit seconds
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f);
+
+/**
  * Number of allowed Datalink timeouts
  *
  * After more than this number of data link timeouts the aircraft returns home directly
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
index 427001a01..54f1813f5 100644
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@@ -132,6 +132,7 @@ RCLoss::set_rcl_item()
 		pos_sp_triplet->previous.valid = false;
 		pos_sp_triplet->current.valid = false;
 		pos_sp_triplet->next.valid = false;
+		break;
 	}
 	default:
 		break;