From 0e3c94599ada4ada65e017dd2c2b31ce1c00e9d1 Mon Sep 17 00:00:00 2001
From: dvornikov-aa <dvornikov-aa@yandex.ru>
Date: Mon, 6 May 2019 18:19:09 +0300
Subject: [PATCH] Add precision land fallback action parameter

---
 src/modules/navigator/precland.cpp      | 39 ++++++++++++++++++-------
 src/modules/navigator/precland.h        |  9 +++++-
 src/modules/navigator/precland_params.c | 15 +++++++++-
 3 files changed, 51 insertions(+), 12 deletions(-)

diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp
index 89cd41825e0f..f0212ea992c8 100644
--- a/src/modules/navigator/precland.cpp
+++ b/src/modules/navigator/precland.cpp
@@ -460,7 +460,7 @@ PrecLand::run_state_search()
 void
 PrecLand::run_state_fallback()
 {
-	// nothing to do, will land
+	// nothing to do, we should be in the other module right now
 }
 
 bool
@@ -556,17 +556,36 @@ PrecLand::switch_to_state_search()
 bool
 PrecLand::switch_to_state_fallback()
 {
-	PX4_WARN("Falling back to normal land");
-	PLD_STATUS_MAVLINK(_MSG_PRIO_WARNING, "PLD: Falling back to normal land");
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-	pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-	pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
-	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
-	_navigator->set_position_setpoint_triplet_updated();
-
 	_state = PrecLandState::Fallback;
 	_state_start_time = hrt_absolute_time();
+
+	vehicle_command_s vcmd = {};
+
+	switch(_param_fallback_action.get())
+	{
+	case PrecLand::PLD_FOAC_LAND:
+		vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
+
+		PX4_WARN("Falling back to normal land");
+		PLD_STATUS_MAVLINK(_MSG_PRIO_WARNING, "PLD: Falling back to normal land");
+		break;
+	case PrecLand::PLD_FOAC_RTL:
+		vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH;
+
+		PX4_WARN("Falling back to RTL");
+		PLD_STATUS_MAVLINK(_MSG_PRIO_WARNING, "PLD: Falling back to RTL");
+		break;
+	default:
+		// Falling back to land if the action is unknown
+		vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
+
+		PX4_WARN("Unknown fallback action!");
+		PLD_STATUS_MAVLINK(_MSG_PRIO_WARNING, "PLD: Unknown fallback action!");
+		break;
+	}
+
+	_navigator->publish_vehicle_cmd(&vcmd);
+
 	return true;
 }
 
diff --git a/src/modules/navigator/precland.h b/src/modules/navigator/precland.h
index c7b5b29c7a13..7ebd44642d00 100644
--- a/src/modules/navigator/precland.h
+++ b/src/modules/navigator/precland.h
@@ -129,6 +129,12 @@ class PrecLand : public MissionBlock, public ModuleParams
 
 	bool _strict_stop; /** Strict precland stop flag */
 
+	/* Precland fallback actions */
+	enum {
+		PLD_FOAC_LAND = 0,
+		PLD_FOAC_RTL = 1
+	};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::PLD_BTOUT>) _param_timeout,
 		(ParamFloat<px4::params::PLD_HACC_RAD>) _param_hacc_rad,
@@ -142,6 +148,7 @@ class PrecLand : public MissionBlock, public ModuleParams
 		(ParamFloat<px4::params::PLD_FUNNEL_TR>) _param_funnel_top_rad,
 		(ParamFloat<px4::params::PLD_FUNNEL_LEA>) _param_funnel_le_alt,
 		(ParamFloat<px4::params::PLD_STATE_TIME>) _param_state_timeout,
-		(ParamBool<px4::params::PLD_INFO>) _param_info
+		(ParamBool<px4::params::PLD_INFO>) _param_info,
+		(ParamInt<px4::params::PLD_FOAC>) _param_fallback_action
 	)
 };
diff --git a/src/modules/navigator/precland_params.c b/src/modules/navigator/precland_params.c
index c7004d3750f2..4c4c9909dfd0 100644
--- a/src/modules/navigator/precland_params.c
+++ b/src/modules/navigator/precland_params.c
@@ -182,4 +182,17 @@ PARAM_DEFINE_FLOAT(PLD_STATE_TIME, 10);
  * @boolean
  * @group Precision Land
  */
-PARAM_DEFINE_INT32(PLD_INFO, 0);
\ No newline at end of file
+PARAM_DEFINE_INT32(PLD_INFO, 0);
+
+/**
+ * Precision land fallback action 
+ *
+ * An action to do if the precision land has failed.
+ *
+ * @min 0
+ * @max 1
+ * @value 0 Land
+ * @value 1 RTL
+ * @group Precision Land
+ */
+PARAM_DEFINE_INT32(PLD_FOAC, 0);