From 878219be3acb1518eebf1abec5f8e141e7a955ce Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 4 Jan 2022 15:52:47 +1100
Subject: [PATCH] AP_Follow: added APIs for plane ship landing

---
 libraries/AP_Follow/AP_Follow.cpp | 48 ++++++++++++++++++++++++++++++-
 libraries/AP_Follow/AP_Follow.h   | 16 +++++++++++
 2 files changed, 63 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp
index a71d8eefa3..ff5d52d65e 100644
--- a/libraries/AP_Follow/AP_Follow.cpp
+++ b/libraries/AP_Follow/AP_Follow.cpp
@@ -33,6 +33,14 @@ extern const AP_HAL::HAL& hal;
 
 #define AP_FOLLOW_POS_P_DEFAULT 0.1f    // position error gain default
 
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+#define AP_FOLLOW_ALT_TYPE_DEFAULT 0
+#else
+#define AP_FOLLOW_ALT_TYPE_DEFAULT AP_FOLLOW_ALTITUDE_TYPE_RELATIVE
+#endif
+
+AP_Follow *AP_Follow::_singleton;
+
 // table of user settable parameters
 const AP_Param::GroupInfo AP_Follow::var_info[] = {
 
@@ -117,7 +125,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     // @Description: Follow altitude type
     // @Values: 0:absolute, 1:relative
     // @User: Standard
-    AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALTITUDE_TYPE_RELATIVE),
+    AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
     AP_GROUPEND
@@ -131,6 +139,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
 AP_Follow::AP_Follow() :
         _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
+    _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
@@ -479,3 +488,40 @@ void AP_Follow::clear_dist_and_bearing_to_target()
     _dist_to_target = 0.0f;
     _bearing_to_target = 0.0f;
 }
+
+// get target's estimated location and velocity (in NED), with offsets added
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const
+{
+    Vector3f ofs;
+    if (!get_offsets_ned(ofs) ||
+        !get_target_location_and_velocity(loc, vel_ned)) {
+        return false;
+    }
+    // apply offsets
+    loc.offset(ofs.x, ofs.y);
+    loc.alt -= ofs.z*100;
+    return true;
+}
+
+// return true if we have a target
+bool AP_Follow::have_target(void) const
+{
+    if (!_enabled) {
+        return false;
+    }
+
+    // check for timeout
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        return false;
+    }
+    return true;
+}
+
+namespace AP {
+
+AP_Follow &follow()
+{
+    return *AP_Follow::get_singleton();
+}
+
+}
diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h
index fc5bc8eba6..860d9302ea 100644
--- a/libraries/AP_Follow/AP_Follow.h
+++ b/libraries/AP_Follow/AP_Follow.h
@@ -38,6 +38,11 @@ public:
     // constructor
     AP_Follow();
 
+    // enable as singleton
+    static AP_Follow *get_singleton(void) {
+        return _singleton;
+    }
+
     // returns true if library is enabled
     bool enabled() const { return _enabled; }
 
@@ -57,6 +62,9 @@ public:
     // get target's estimated location and velocity (in NED)
     bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const;
 
+    // get target's estimated location and velocity (in NED), with offsets added
+    bool get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const;
+    
     // get distance vector to target (in meters), target plus offsets, and target's velocity all in NED frame
     bool get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_ofs, Vector3f &vel_ned);
 
@@ -86,10 +94,14 @@ public:
     // get bearing to target (including offset) in degrees (for reporting purposes)
     float get_bearing_to_target() const { return _bearing_to_target; }
 
+    // get system time of last position update
+    uint32_t get_last_update_ms() const { return _last_location_update_ms; }
+
     // parameter list
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
+    static AP_Follow *_singleton;
 
     // get velocity estimate in m/s in NED frame using dt since last update
     bool get_velocity_ned(Vector3f &vel_ned, float dt) const;
@@ -132,3 +144,7 @@ private:
     // setup jitter correction with max transport lag of 3s
     JitterCorrection _jitter{3000};
 };
+
+namespace AP {
+    AP_Follow &follow();
+};