From 1b236558b0a980174f6cfde0bb1c8c7819ddbd52 Mon Sep 17 00:00:00 2001
From: Claudio Chies <chiesc@ethz.ch>
Date: Fri, 27 Sep 2024 17:19:20 +0200
Subject: [PATCH] integrated PrecisionLanding

---
 .../flight_mode_manager/FlightModeManager.cpp |  3 ++-
 .../tasks/Land/FlightTaskLand.cpp             | 25 +++++++++++++++++++
 .../tasks/Land/FlightTaskLand.hpp             |  4 +++
 src/modules/navigator/land.cpp                | 11 ++++++++
 4 files changed, 42 insertions(+), 1 deletion(-)

diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp
index 328cf4c9b4a9..324002983b98 100644
--- a/src/modules/flight_mode_manager/FlightModeManager.cpp
+++ b/src/modules/flight_mode_manager/FlightModeManager.cpp
@@ -153,7 +153,8 @@ void FlightModeManager::start_flight_task()
 	bool matching_task_running = true;
 	bool task_failure = false;
 	const bool nav_state_descend = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);
-	const bool nav_state_land = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_LAND);
+	const bool nav_state_land = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_LAND
+				     || _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND);
 
 	// Follow me
 	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
diff --git a/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp
index 7249538e1b4a..c783129aaf92 100644
--- a/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp
+++ b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp
@@ -84,6 +84,25 @@ FlightTaskLand::reActivate()
 	_position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position);
 }
 
+bool
+FlightTaskLand::updateInitialize()
+{
+	bool ret = FlightTask::updateInitialize();
+	_sub_triplet_setpoint.update();
+
+	double ref_lat = _sub_vehicle_local_position.get().ref_lat;
+	double ref_lon = _sub_vehicle_local_position.get().ref_lon;
+
+	_reference_position.initReference(ref_lat, ref_lon);
+
+	// if we have valid current position setpoint, then we are in the precision landing mode
+	if (_sub_triplet_setpoint.get().current.valid) {
+		_precision_landing = true;
+	}
+
+	return ret;
+}
+
 bool
 FlightTaskLand::update()
 {
@@ -99,6 +118,12 @@ FlightTaskLand::update()
 	}
 
 	if (_landing) {
+		if (_precision_landing) {
+			// set the landing position based on the current position setpoint which gets updated by precision landing
+			_reference_position.project(_sub_triplet_setpoint.get().current.lat, _sub_triplet_setpoint.get().current.lon,
+						    _land_position(0), _land_position(1));
+		}
+
 		_PerformLanding();
 
 	} else {
diff --git a/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp
index e4a0f9826758..55846d61aa9e 100644
--- a/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp
+++ b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp
@@ -62,6 +62,7 @@ class FlightTaskLand : public FlightTask
 	bool update() override;
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	void reActivate() override;
+	bool updateInitialize() override;
 
 protected:
 	void updateParams() override;
@@ -95,12 +96,15 @@ class FlightTaskLand : public FlightTask
 					(ParamFloat<px4::params::NAV_MC_ALT_RAD>) 	_param_nav_mc_alt_rad
 				       );
 private:
+	MapProjection _reference_position{};
+
 	void _CalculateBrakingLocation();
 	void _HandleHighVelocities();
 	void _PerformLanding();
 	void _SmoothBrakingPath();
 	void _UpdateTrajConstraints();
 
+	bool _precision_landing{false};
 	bool _landing{false};
 	bool _is_initialized{false};
 	bool _exceeded_max_vel{false};
diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp
index 33fdab641805..6f3b06693f5b 100644
--- a/src/modules/navigator/land.cpp
+++ b/src/modules/navigator/land.cpp
@@ -61,6 +61,17 @@ Land::on_activation()
 	pos_sp_triplet->previous.valid = false;
 	pos_sp_triplet->next.valid = false;
 
+	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		// set the lat and lon to NAN, as the target setpoint for landing is handeled in FlightTaskLand, not in the navigator.
+		pos_sp_triplet->current.valid = false;
+		pos_sp_triplet->current.lat = (double) NAN;
+		pos_sp_triplet->current.lon = (double) NAN;
+		pos_sp_triplet->previous.lat = (double) NAN;
+		pos_sp_triplet->previous.lon = (double) NAN;
+		pos_sp_triplet->next.lat = (double) NAN;
+		pos_sp_triplet->next.lon = (double) NAN;
+	}
+
 	_navigator->set_position_setpoint_triplet_updated();
 
 	// reset cruising speed to default