From 6e077247c5648990ff3f83d32593ac2eca86f11a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 5 Mar 2024 09:32:24 +1100
Subject: [PATCH] Plane: support precland in QLAND for pos, velocity and
 descent rate

allow full override in QLAND
---
 ArduPlane/ArduPlane.cpp   | 20 ++++++++++----------
 ArduPlane/GCS_Mavlink.cpp | 10 ++++++++++
 ArduPlane/GCS_Mavlink.h   |  1 +
 ArduPlane/mode.h          |  2 ++
 ArduPlane/mode_qland.cpp  | 31 +++++++++++++++++++++++++++++++
 5 files changed, 54 insertions(+), 10 deletions(-)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index cbc2d0215a..c23c57aba6 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -863,21 +863,20 @@ bool Plane::get_target_location(Location& target_loc)
  */
 bool Plane::update_target_location(const Location &old_loc, const Location &new_loc)
 {
-    if (!old_loc.same_loc_as(next_WP_loc)) {
+    /*
+      by checking the caller has provided the correct old target
+      location we prevent a race condition where the user changes mode
+      or commands a different target in the controlling lua script
+     */
+    if (!old_loc.same_loc_as(next_WP_loc) ||
+        old_loc.get_alt_frame() != new_loc.get_alt_frame()) {
         return false;
     }
     next_WP_loc = new_loc;
-    next_WP_loc.change_alt_frame(old_loc.get_alt_frame());
 
 #if HAL_QUADPLANE_ENABLED
     if (control_mode == &mode_qland) {
-        /*
-          support precision landing controlled by lua in QLAND mode
-         */
-        Vector2f rel_origin;
-        if (new_loc.get_vector_xy_from_origin_NE(rel_origin)) {
-            quadplane.pos_control->set_pos_target_xy_cm(rel_origin.x, rel_origin.y);
-        }
+        mode_qland.last_target_loc_set_ms = AP_HAL::millis();
     }
 #endif
 
@@ -901,7 +900,8 @@ bool Plane::set_velocity_match(const Vector2f &velocity)
 bool Plane::set_land_descent_rate(float descent_rate)
 {
 #if HAL_QUADPLANE_ENABLED
-    if (quadplane.in_vtol_land_descent()) {
+    if (quadplane.in_vtol_land_descent() ||
+        control_mode == &mode_qland) {
         quadplane.poscontrol.override_descent_rate = descent_rate;
         quadplane.poscontrol.last_override_descent_ms = AP_HAL::millis();
         return true;
diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index 9a4cf9fbae..efc6968030 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -748,6 +748,16 @@ void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &c
 }
 
 
+/*
+  handle a LANDING_TARGET command. The timestamp has been jitter corrected
+*/
+void GCS_MAVLINK_Plane::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
+{
+#if AC_PRECLAND_ENABLED
+    plane.g2.precland.handle_msg(packet, timestamp_ms);
+#endif
+}
+
 MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
 {
     plane.in_calibration = true;
diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h
index d3c476318b..992cee53e9 100644
--- a/ArduPlane/GCS_Mavlink.h
+++ b/ArduPlane/GCS_Mavlink.h
@@ -45,6 +45,7 @@ protected:
     void send_pid_tuning() override;
 
     void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) override;
+    void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override;
 
 private:
 
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
index d2a4d3d11e..97fd4d4911 100644
--- a/ArduPlane/mode.h
+++ b/ArduPlane/mode.h
@@ -654,6 +654,7 @@ protected:
 class ModeQLand : public Mode
 {
 public:
+    friend class Plane;
 
     Number mode_number() const override { return Number::QLAND; }
     const char *name() const override { return "QLAND"; }
@@ -671,6 +672,7 @@ protected:
     bool _enter() override;
     bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
 
+    uint32_t last_target_loc_set_ms;
 };
 
 class ModeQRTL : public Mode
diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp
index e646c80746..681f7cb26c 100644
--- a/ArduPlane/mode_qland.cpp
+++ b/ArduPlane/mode_qland.cpp
@@ -19,6 +19,10 @@ bool ModeQLand::_enter()
 #if AP_FENCE_ENABLED
     plane.fence.auto_disable_fence_for_landing();
 #endif
+
+    // clear precland timestamp
+    last_target_loc_set_ms = 0;
+
     return true;
 }
 
@@ -29,6 +33,33 @@ void ModeQLand::update()
 
 void ModeQLand::run()
 {
+    /*
+      see if precision landing is active with an override of the
+      target location
+     */
+    const uint32_t last_pos_set_ms = last_target_loc_set_ms;
+    const uint32_t last_vel_set_ms = quadplane.poscontrol.last_velocity_match_ms;
+    const uint32_t now_ms = AP_HAL::millis();
+
+    if (last_pos_set_ms != 0 && now_ms - last_pos_set_ms < 500) {
+        // we have an active landing target override
+        Vector2f rel_origin;
+        if (plane.next_WP_loc.get_vector_xy_from_origin_NE(rel_origin)) {
+            quadplane.pos_control->set_pos_target_xy_cm(rel_origin.x, rel_origin.y);
+        }
+    }
+
+    // allow for velocity override as well
+    if (last_vel_set_ms != 0 && now_ms - last_vel_set_ms < 500) {
+        // we have an active landing velocity override
+        Vector2f target_accel;
+        Vector2f target_speed_xy_cms{quadplane.poscontrol.velocity_match.x*100, quadplane.poscontrol.velocity_match.y*100};
+        quadplane.pos_control->input_vel_accel_xy(target_speed_xy_cms, target_accel);
+    }
+
+    /*
+      use QLOITER to do the main control
+     */
     plane.mode_qloiter.run();
 }