From 99cf10ce1208fa12aa2528f788a4da66ff16eba8 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 26 Feb 2022 13:04:46 +1100
Subject: [PATCH] AP_Follow: support the FOLLOW_TARGET mavlink message

this is used by qgroundcontrol, allowing follow on a mobile device

note that you must set the qgc option to "always send follow" and also
must set FOLL_ALT_TYPE=2 in ArduPilot
---
 libraries/AP_Follow/AP_Follow.cpp | 79 ++++++++++++++++++++++++++-----
 1 file changed, 68 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp
index f1def162d2..4e996c49aa 100644
--- a/libraries/AP_Follow/AP_Follow.cpp
+++ b/libraries/AP_Follow/AP_Follow.cpp
@@ -275,13 +275,10 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
     }
 
     // decode global-position-int message
-    if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
-
-        // get estimated location and velocity (for logging)
-        Location loc_estimate{};
-        Vector3f vel_estimate;
-        UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
+    bool updated = false;
 
+    switch (msg.msgid) {
+    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
         // decode message
         mavlink_global_position_int_t packet;
         mavlink_msg_global_position_int_decode(&msg, &packet);
@@ -296,13 +293,11 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
 
         // select altitude source based on FOLL_ALT_TYPE param 
         if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            // relative altitude
-            _target_location.alt = packet.relative_alt / 10;        // convert millimeters to cm
-            _target_location.relative_alt = 1;                // set relative_alt flag
+            // above home alt
+            _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
         } else {
             // absolute altitude
-            _target_location.alt = packet.alt / 10;                 // convert millimeters to cm
-            _target_location.relative_alt = 0;                // reset relative_alt flag
+            _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
         }
 
         _target_velocity_ned.x = packet.vx * 0.01f; // velocity north
@@ -320,6 +315,68 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
             _sysid.set(msg.sysid);
             _automatic_sysid = true;
         }
+        updated = true;
+        break;
+    }
+    case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // decode message
+        mavlink_follow_target_t packet;
+        mavlink_msg_follow_target_decode(&msg, &packet);
+
+        // ignore message if lat and lon are (exactly) zero
+        if ((packet.lat == 0 && packet.lon == 0)) {
+            return;
+        }
+        // require at least position
+        if ((packet.est_capabilities & (1<<0)) == 0) {
+            return;
+        }
+
+        Location new_loc = _target_location;
+        new_loc.lat = packet.lat;
+        new_loc.lng = packet.lon;
+        new_loc.set_alt_cm(packet.alt*100, Location::AltFrame::ABSOLUTE);
+
+        // FOLLOW_TARGET is always AMSL, change the provided alt to
+        // above home if we are configured for relative alt
+        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE &&
+            !new_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) {
+            return;
+        }
+        _target_location = new_loc;
+
+        if (packet.est_capabilities & (1<<1)) {
+            _target_velocity_ned.x = packet.vel[0]; // velocity north
+            _target_velocity_ned.y = packet.vel[1]; // velocity east
+            _target_velocity_ned.z = packet.vel[2]; // velocity down
+        }
+
+        // get a local timestamp with correction for transport jitter
+        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
+
+        if (packet.est_capabilities & (1<<3)) {
+            Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
+            float r, p, y;
+            q.to_euler(r,p,y);
+            _target_heading = degrees(y);
+            _last_heading_update_ms = _last_location_update_ms;
+        }
+
+        // initialise _sysid if zero to sender's id
+        if (_sysid == 0) {
+            _sysid.set(msg.sysid);
+            _automatic_sysid = true;
+        }
+        updated = true;
+        break;
+    }
+    }
+    
+    if (updated) {
+        // get estimated location and velocity
+        Location loc_estimate{};
+        Vector3f vel_estimate;
+        UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
 
         // log lead's estimated vs reported position
 // @LoggerMessage: FOLL