From b21ecda76f2b604ee48350d00f9a4220405060d2 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 27 Feb 2020 20:23:45 +0900
Subject: [PATCH] Copter: fix guided mode handling of terrain altitudes

---
 ArduCopter/GCS_Mavlink.cpp | 28 ++++++++++++++--------------
 1 file changed, 14 insertions(+), 14 deletions(-)

diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
index 85cdc02099..e885816e70 100644
--- a/ArduCopter/GCS_Mavlink.cpp
+++ b/ArduCopter/GCS_Mavlink.cpp
@@ -1084,9 +1084,8 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
          * bool force           = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;
          */
 
-        Vector3f pos_neu_cm;  // position (North, East, Up coordinates) in centimeters
-        bool terrain_alt = false;
-
+        // extract location from message
+        Location loc;
         if (!pos_ignore) {
             // sanity check location
             if (!check_latlng(packet.lat_int, packet.lon_int)) {
@@ -1097,16 +1096,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
                 // unknown coordinate frame
                 break;
             }
-            const Location loc{
-                packet.lat_int,
-                packet.lon_int,
-                int32_t(packet.alt*100),
-                frame,
-            };
-            terrain_alt = (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN);
-            if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
-                break;
-            }
+            loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame};
         }
 
         // prepare yaw
@@ -1121,12 +1111,22 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
             yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
         }
 
+        // send targets to the appropriate guided mode controller
         if (!pos_ignore && !vel_ignore && acc_ignore) {
+            // convert Location to vector from ekf origin for posvel controller
+            if (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) {
+                // posvel controller does not support alt-above-terrain
+                break;
+            }
+            Vector3f pos_neu_cm;
+            if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
+                break;
+            }
             copter.mode_guided.set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
         } else if (pos_ignore && !vel_ignore && acc_ignore) {
             copter.mode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
         } else if (!pos_ignore && vel_ignore && acc_ignore) {
-            copter.mode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative, terrain_alt);
+            copter.mode_guided.set_destination(loc, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
         }
 
         break;