abort comm loss mode if counter above param and return home directly

4 files changed, 44 insertions, 23 deletions

diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 25effbd21..980a7a2bb 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -94,10 +94,9 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
  * @min 0
  * @max 1
  */
-PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
+PARAM_DEFINE_INT32(DL_LOSS_EN, 0);
 
-/**
- * Datalink timeout threshold
+ /** Datalink timeout threshold
  *
  * After this amount of seconds the data link lost mode triggers
  *
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index 2bd80165d..a98e21139 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -57,14 +57,16 @@
 
 DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
-	_dll_state(DLL_STATE_NONE),
+	_vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100),
 	_param_commsholdwaittime(this, "CH_T"),
 	_param_commsholdlat(this, "CH_LAT"),
 	_param_commsholdlon(this, "CH_LON"),
 	_param_commsholdalt(this, "CH_ALT"),
 	_param_airfieldhomelat(this, "AH_LAT"),
 	_param_airfieldhomelon(this, "AH_LON"),
-	_param_airfieldhomealt(this, "AH_ALT")
+	_param_airfieldhomealt(this, "AH_ALT"),
+	_param_numberdatalinklosses(this, "DLL_N"),
+	_dll_state(DLL_STATE_NONE)
 {
 	/* load initial params */
 	updateParams();
@@ -187,7 +189,13 @@ DataLinkLoss::advance_dll()
 {
 	switch (_dll_state) {
 	case DLL_STATE_NONE:
-		_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
+		/* Check the number of data link losses. If above home fly home directly */
+		updateSubscriptions();
+		if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) {
+			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+		} else {
+			_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
+		}
 		break;
 	case DLL_STATE_FLYTOCOMMSHOLDWP:
 		//XXX check here if time is over are over
diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h
index 101c88a25..650cc7bc5 100644
--- a/src/modules/navigator/datalinkloss.h
+++ b/src/modules/navigator/datalinkloss.h
@@ -43,10 +43,7 @@
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
-#include <uORB/topics/mission.h>
-#include <uORB/topics/mission.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/Subscription.hpp>
 
 #include "navigator_mode.h"
 #include "mission_block.h"
@@ -67,6 +64,25 @@ public:
 	virtual void on_active();
 
 private:
+	/* Subscriptions */
+	uORB::Subscription<vehicle_status_s> _vehicleStatus;
+
+	/* Params */
+	control::BlockParamFloat _param_commsholdwaittime;
+	control::BlockParamInt _param_commsholdlat; // * 1e7
+	control::BlockParamInt _param_commsholdlon; // * 1e7
+	control::BlockParamFloat _param_commsholdalt;
+	control::BlockParamInt _param_airfieldhomelat; // * 1e7
+	control::BlockParamInt _param_airfieldhomelon; // * 1e7
+	control::BlockParamFloat _param_airfieldhomealt;
+	control::BlockParamInt _param_numberdatalinklosses;
+
+	enum DLLState {
+		DLL_STATE_NONE = 0,
+		DLL_STATE_FLYTOCOMMSHOLDWP = 1,
+		DLL_STATE_FLYTOAIRFIELDHOMEWP = 2,
+	} _dll_state;
+
 	/**
 	 * Set the DLL item
 	 */
@@ -77,18 +93,5 @@ private:
 	 */
 	void		advance_dll();
 
-	enum DLLState {
-		DLL_STATE_NONE = 0,
-		DLL_STATE_FLYTOCOMMSHOLDWP = 1,
-		DLL_STATE_FLYTOAIRFIELDHOMEWP = 2,
-	} _dll_state;
-
-	control::BlockParamFloat _param_commsholdwaittime;
-	control::BlockParamInt _param_commsholdlat; // * 1e7
-	control::BlockParamInt _param_commsholdlon; // * 1e7
-	control::BlockParamFloat _param_commsholdalt;
-	control::BlockParamInt _param_airfieldhomelat; // * 1e7
-	control::BlockParamInt _param_airfieldhomelon; // * 1e7
-	control::BlockParamFloat _param_airfieldhomealt;
 };
 #endif
diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c
index a836fc8ca..038c80a1a 100644
--- a/src/modules/navigator/datalinkloss_params.c
+++ b/src/modules/navigator/datalinkloss_params.c
@@ -123,3 +123,14 @@ PARAM_DEFINE_INT32(NAV_DLL_AH_LON, 1518423250);
  * @group DLL
  */
 PARAM_DEFINE_FLOAT(NAV_DLL_AH_ALT, 600.0f);
+
+/**
+ * Number of allowed Datalink timeouts
+ *
+ * After more than this number of data link timeouts the aircraft returns home directly
+ *
+ * @group commander
+ * @min 0
+ * @max 1000
+ */
+PARAM_DEFINE_INT32(NAV_DLL_N, 2);