From e2779523ea1732a0e5cdbbf26c3333d283b01e1a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 10 Sep 2012 09:40:29 +1000
Subject: [PATCH] APM: fixed fetch of relative altitude waypoints

when we fetch relative altitude waypoints from APM to a file, we need
to preserve the altitude
---
 ArduPlane/GCS_Mavlink.pde |  8 ++------
 ArduPlane/commands.pde    | 21 +++++++++++++++++----
 2 files changed, 19 insertions(+), 10 deletions(-)

diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index e920c2c28e..8e04284409 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -1191,7 +1191,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
 
         // send waypoint
-        tell_command = get_cmd_with_index(packet.seq);
+        tell_command = get_cmd_with_index_raw(packet.seq);
 
         // set frame of waypoint
         uint8_t frame;
@@ -1218,11 +1218,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             // command needs scaling
             x = tell_command.lat/1.0e7;     // local (x), global (latitude)
             y = tell_command.lng/1.0e7;     // local (y), global (longitude)
-            if ((tell_command.options & MASK_OPTIONS_RELATIVE_ALT) && tell_command.id != MAV_CMD_NAV_TAKEOFF) {
-                z = (tell_command.alt - home.alt) / 1.0e2;     // because tell_command.alt already includes a += home.alt
-            } else {
-                z = tell_command.alt/1.0e2;     // local (z), global/relative (altitude)
-            }
+            z = tell_command.alt/1.0e2;
         }
 
         switch (tell_command.id) {                                              // Switch to map APM command fields inot MAVLink command fields
diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde
index 55ba37e331..6b45a71824 100644
--- a/ArduPlane/commands.pde
+++ b/ArduPlane/commands.pde
@@ -38,9 +38,10 @@ static void reload_commands_airstart()
     decrement_cmd_index();
 }
 
-// Getters
-// -------
-static struct Location get_cmd_with_index(int16_t i)
+/*
+  fetch a mission item from EEPROM
+*/
+static struct Location get_cmd_with_index_raw(int16_t i)
 {
     struct Location temp;
     uint16_t mem;
@@ -71,6 +72,18 @@ static struct Location get_cmd_with_index(int16_t i)
         temp.lng = (long)eeprom_read_dword((uint32_t*)(uintptr_t)mem);
     }
 
+    return temp;
+}
+
+/*
+  fetch a mission item from EEPROM. Adjust altitude to be absolute
+*/
+static struct Location get_cmd_with_index(int16_t i)
+{
+    struct Location temp;
+
+    temp = get_cmd_with_index_raw(i);
+
     // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
     if ((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) &&
         (temp.options & MASK_OPTIONS_RELATIVE_ALT) &&
@@ -90,7 +103,7 @@ static void set_cmd_with_index(struct Location temp, int16_t i)
 
     // Set altitude options bitmask
     // XXX What is this trying to do?
-    if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0) {
+    if ((temp.options & MASK_OPTIONS_RELATIVE_ALT) && i != 0) {
         temp.options = MASK_OPTIONS_RELATIVE_ALT;
     } else {
         temp.options = 0;