GCS_MAVLink: MissionItemProtocol_Rally: deal with alt frame

ArduPilot · Nov 1, 2023 · d25d1c2 · d25d1c2
1 parent 8f130f9
commit d25d1c2
Showing 1 changed file with 49 additions and 4 deletions.
diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp
@@ -26,6 +26,7 @@
 #include <AP_Logger/AP_Logger.h>
 #include <AP_Rally/AP_Rally.h>
 #include <GCS_MAVLink/GCS.h>
+#include <AP_Terrain/AP_Terrain.h>
 
 MAV_MISSION_RESULT MissionItemProtocol_Rally::append_item(const mavlink_mission_item_int_t &cmd)
 {
@@ -57,10 +58,6 @@ MAV_MISSION_RESULT MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyL
     if (cmd.command != MAV_CMD_NAV_RALLY_POINT) {
         return MAV_MISSION_UNSUPPORTED;
     }
-    if (cmd.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
-        cmd.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-        return MAV_MISSION_UNSUPPORTED_FRAME;
-    }
     if (!check_lat(cmd.x)) {
         return MAV_MISSION_INVALID_PARAM5_X;
     }
@@ -71,6 +68,32 @@ MAV_MISSION_RESULT MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyL
         return MAV_MISSION_INVALID_PARAM7;
     }
     ret = {};
+
+    switch (cmd.frame) {
+        case MAV_FRAME_GLOBAL:
+        case MAV_FRAME_GLOBAL_INT:
+            ret.alt_frame = uint8_t(Location::AltFrame::ABSOLUTE);
+            break;
+
+        case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+        case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
+            ret.alt_frame = uint8_t(Location::AltFrame::ABOVE_HOME);
+            break;
+
+#if AP_TERRAIN_AVAILABLE
+        case MAV_FRAME_GLOBAL_TERRAIN_ALT:
+        case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
+            ret.alt_frame = uint8_t(Location::AltFrame::ABOVE_TERRAIN);
+            break;
+#endif
+
+        default:
+            return MAV_MISSION_UNSUPPORTED_FRAME;
+    }
+
+    // Fresh points always use new alt frame format
+    ret.alt_frame_valid = true;
+
     ret.lat = cmd.x;
     ret.lng = cmd.y;
     ret.alt = cmd.z;
@@ -91,7 +114,29 @@ bool MissionItemProtocol_Rally::get_item_as_mission_item(uint16_t seq,
         return false;
     }
 
+    // Default to relative to home
     ret_packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+
+    if (rallypoint.alt_frame_valid == 1) {
+        switch (Location::AltFrame(rallypoint.alt_frame)) {
+            case Location::AltFrame::ABSOLUTE:
+                ret_packet.frame = MAV_FRAME_GLOBAL;
+                break;
+
+            case Location::AltFrame::ABOVE_HOME:
+                ret_packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+                break;
+
+            case Location::AltFrame::ABOVE_ORIGIN:
+                // Above origin alt frame is not supported
+                return false;
+
+            case Location::AltFrame::ABOVE_TERRAIN:
+                ret_packet.frame = MAV_FRAME_GLOBAL_TERRAIN_ALT;
+                break;
+        }
+    }
+
     ret_packet.command = MAV_CMD_NAV_RALLY_POINT;
     ret_packet.x = rallypoint.lat;
     ret_packet.y = rallypoint.lng;