AP_Rally: support altitude frames

ArduCopter/mode_rtl.cpp

@@ -408,9 +408,10 @@ void ModeRTL::build_path()
 //   return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set)
 void ModeRTL::compute_return_target()
 {
-    // set return target to nearest rally point or home position (Note: alt is absolute)
+    // set return target to nearest rally point or home position
 #if HAL_RALLY_ENABLED
     rtl_path.return_target = copter.rally.calc_best_rally_or_home_location(copter.current_loc, ahrs.get_home().alt);
+    rtl_path.return_target.change_alt_frame(Location::AltFrame::ABSOLUTE);
 #else
     rtl_path.return_target = ahrs.get_home();
 #endif

Tools/autotest/arduplane.py

@@ -1908,7 +1908,40 @@ def terrain_wait_path(loc1, loc2, steps):
                            accuracy=200,
                            target_altitude=None,
                            timeout=600)
-        self.progress("Reached rally point")
+        self.progress("Reached rally point with TERRAIN_FOLLOW")
+
+        # Fly back to guided location
+        self.change_mode("GUIDED")
+        self.do_reposition(guided_loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT)
+        self.progress("Flying to back to guided location")
+
+        # Disable terrain following and re-load rally point with relative to terrain altitude
+        self.set_parameter("TERRAIN_FOLLOW", 0)
+
+        rally_item = [self.create_MISSION_ITEM_INT(
+            mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT,
+            x=int(rally_loc.lat*1e7),
+            y=int(rally_loc.lng*1e7),
+            z=rally_loc.alt,
+            frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT,
+            mission_type=mavutil.mavlink.MAV_MISSION_TYPE_RALLY
+        )]
+        self.correct_wp_seq_numbers(rally_item)
+        self.check_rally_upload_download(rally_item)
+
+        # Once back at guided location re-trigger RTL
+        self.wait_location(guided_loc,
+                           accuracy=200,
+                           target_altitude=None,
+                           timeout=600)
+
+        self.change_mode("RTL")
+        self.progress("Flying to rally point")
+        self.wait_location(rally_loc,
+                           accuracy=200,
+                           target_altitude=None,
+                           timeout=600)
+        self.progress("Reached rally point with terrain alt frame")
 
         self.context_pop()
         self.disarm_vehicle(force=True)

libraries/AP_Logger/AP_Logger_Backend.cpp

@@ -519,7 +519,8 @@ bool AP_Logger_Backend::Write_RallyPoint(uint8_t total,
         sequence        : sequence,
         latitude        : rally_point.lat,
         longitude       : rally_point.lng,
-        altitude        : rally_point.alt
+        altitude        : rally_point.alt,
+        flags           : rally_point.flags
     };
     return WriteBlock(&pkt_rally, sizeof(pkt_rally));
 }

libraries/AP_Logger/LogStructure.h

@@ -548,6 +548,7 @@ struct PACKED log_Rally {
     int32_t latitude;
     int32_t longitude;
     int16_t altitude;
+    uint8_t flags;
 };
 
 struct PACKED log_Performance {
@@ -968,6 +969,7 @@ struct PACKED log_VER {
 // @Field: Lat: latitude of rally point
 // @Field: Lng: longitude of rally point
 // @Field: Alt: altitude of rally point
+// @Field: Flags: altitude frame flags
 
 // @LoggerMessage: RCI2
 // @Description: (More) RC input channels to vehicle
@@ -1286,7 +1288,7 @@ LOG_STRUCTURE_FROM_FENCE \
     { LOG_DF_FILE_STATS, sizeof(log_DSF), \
       "DSF", "QIHIIII", "TimeUS,Dp,Blk,Bytes,FMn,FMx,FAv", "s--b---", "F--0---" }, \
     { LOG_RALLY_MSG, sizeof(log_Rally), \
-      "RALY", "QBBLLh", "TimeUS,Tot,Seq,Lat,Lng,Alt", "s--DUm", "F--GGB" },  \
+      "RALY", "QBBLLhB", "TimeUS,Tot,Seq,Lat,Lng,Alt,Flags", "s--DUm-", "F--GGB-" },  \
     { LOG_MAV_MSG, sizeof(log_MAV),   \
       "MAV", "QBHHHBHH",   "TimeUS,chan,txp,rxp,rxdp,flags,ss,tf", "s#----s-", "F-000-C-" },   \
 LOG_STRUCTURE_FROM_VISUALODOM \

libraries/AP_Rally/AP_Rally.cpp

@@ -129,13 +129,9 @@ Location AP_Rally::rally_location_to_location(const RallyLocation &rally_loc) co
         rally_loc.lat,
         rally_loc.lng,
         rally_loc.alt * 100,
-        Location::AltFrame::ABOVE_HOME
+        (rally_loc.alt_frame_valid == 1) ? Location::AltFrame(rally_loc.alt_frame) : Location::AltFrame::ABOVE_HOME
     };
 
-    // notionally the following call can fail, but we have no facility
-    // to return that fact here:
-    ret.change_alt_frame(Location::AltFrame::ABSOLUTE);
-
     return ret;
 }
 

libraries/AP_Rally/AP_Rally.h

@@ -30,9 +30,16 @@ struct PACKED RallyLocation {
     int16_t alt;        //transit altitude (and loiter altitude) in meters (absolute);
     int16_t break_alt;  //when autolanding, break out of loiter at this alt (meters)
     uint16_t land_dir;   //when the time comes to auto-land, try to land in this direction (centidegrees)
-    uint8_t flags;      //bit 0 = seek favorable winds when choosing a landing poi
-                        //bit 1 = do auto land after arriving
-                        //all other bits are for future use.
+    union {
+        uint8_t flags; 
+        struct {
+            uint8_t favorable_winds : 1; // bit 0 = seek favorable winds when choosing a landing poi
+            uint8_t do_auto_land    : 1; // bit 1 = do auto land after arriving
+            uint8_t alt_frame_valid : 1; // bit 2 = true if following alt frame value should be used, else Location::AltFrame::ABOVE_HOME
+            uint8_t alt_frame       : 2; // Altitude frame following Location::AltFrame enum
+            uint8_t unused          : 3;
+        };
+    };
 };
 
 /// @class    AP_Rally

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;