datalink loss: add param to allow skipping of comms hold wp

3 files changed, 31 insertions, 8 deletions

diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index 3310984b0..4e3d25840 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -66,6 +66,7 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
 	_param_airfieldhomealt(this, "NAV_AH_ALT", false),
 	_param_airfieldhomewaittime(this, "AH_T"),
 	_param_numberdatalinklosses(this, "N"),
+	_param_skipcommshold(this, "CHSK"),
 	_dll_state(DLL_STATE_NONE)
 {
 	/* load initial params */
@@ -179,14 +180,23 @@ DataLinkLoss::advance_dll()
 	switch (_dll_state) {
 	case DLL_STATE_NONE:
 		/* Check the number of data link losses. If above home fly home directly */
-		if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
-			warnx("%d data link losses, limit is %d, fly to airfield home", _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home");
-			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+		if (!_param_skipcommshold.get()) {
+			/* if number of data link losses limit is not reached fly to comms hold wp */
+			if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
+				warnx("%d data link losses, limit is %d, fly to airfield home",
+						_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
+				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home");
+				_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+			} else {
+				warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
+				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold");
+				_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
+			}
 		} else {
-			warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold");
-			_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
+			/* comms hold wp not active, fly to airfield home directly */
+			warnx("Skipping comms hold wp. Flying directly to airfield home");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped");
+			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		}
 		break;
 	case DLL_STATE_FLYTOCOMMSHOLDWP:
diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h
index d0c9ad09a..31e0e3907 100644
--- a/src/modules/navigator/datalinkloss.h
+++ b/src/modules/navigator/datalinkloss.h
@@ -74,6 +74,7 @@ private:
 	control::BlockParamFloat _param_airfieldhomealt;
 	control::BlockParamFloat _param_airfieldhomewaittime;
 	control::BlockParamInt _param_numberdatalinklosses;
+	control::BlockParamInt _param_skipcommshold;
 
 	enum DLLState {
 		DLL_STATE_NONE = 0,
diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c
index db307c904..6780c0c88 100644
--- a/src/modules/navigator/datalinkloss_params.c
+++ b/src/modules/navigator/datalinkloss_params.c
@@ -107,8 +107,20 @@ PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f);
  *
  * After more than this number of data link timeouts the aircraft returns home directly
  *
- * @group commander
+ * @group DLL
  * @min 0
  * @max 1000
  */
 PARAM_DEFINE_INT32(NAV_DLL_N, 2);
+
+/**
+ * Skip comms hold wp
+ *
+ * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to
+ * airfield home
+ *
+ * @group DLL
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0);