From 43105249cb59afbedaa13cb10292a84fbe752778 Mon Sep 17 00:00:00 2001
From: chobitsfan <mrchobits@gmail.com>
Date: Sat, 30 Jul 2022 17:54:25 +0800
Subject: [PATCH] PrecLand: support LANDING_TARGET ext field

---
 .../AC_PrecLand/AC_PrecLand_Companion.cpp     | 26 ++++++++++++++++---
 libraries/AC_PrecLand/AC_PrecLand_Companion.h |  1 +
 2 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp
index f8125dee15..e65ff1d686 100644
--- a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp
+++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp
@@ -1,4 +1,5 @@
 #include <AP_HAL/AP_HAL.h>
+#include <GCS_MAVLink/GCS.h>
 #include "AC_PrecLand_Companion.h"
 
 // perform any required initialisation of backend
@@ -46,9 +47,28 @@ void AC_PrecLand_Companion::handle_msg(const mavlink_landing_target_t &packet, u
 {
     _distance_to_target = packet.distance;
 
-    // compute unit vector towards target
-    _los_meas_body = Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f);
-    _los_meas_body /= _los_meas_body.length();
+    if (packet.position_valid == 1) {
+        if (packet.frame == MAV_FRAME_BODY_FRD) {
+            if (_distance_to_target > 0) {
+                _los_meas_body = Vector3f(packet.x, packet.y, packet.z);
+                _los_meas_body /= _distance_to_target;
+            } else {
+                // distance to target must be positive
+                return;
+            }
+        } else {
+            //we do not support this frame
+            if (!_wrong_frame_msg_sent) {
+                _wrong_frame_msg_sent = true;
+                gcs().send_text(MAV_SEVERITY_INFO,"Plnd: Frame not supported ");
+            }
+            return;
+        }
+    } else {
+        // compute unit vector towards target
+        _los_meas_body = Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f);
+        _los_meas_body /= _los_meas_body.length();
+    }
 
     _los_meas_time_ms = timestamp_ms;
     _have_los_meas = true;
diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.h b/libraries/AC_PrecLand/AC_PrecLand_Companion.h
index b773f4d2b8..67af707a9b 100644
--- a/libraries/AC_PrecLand/AC_PrecLand_Companion.h
+++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.h
@@ -44,4 +44,5 @@ private:
     Vector3f            _los_meas_body;         // unit vector in body frame pointing towards target
     bool                _have_los_meas;         // true if there is a valid measurement from the camera
     uint32_t            _los_meas_time_ms;      // system time in milliseconds when los was measured
+    bool                _wrong_frame_msg_sent;
 };