Plane: Add support for rally points.

Added parameter RALLY_TOTAL.

Added handlers for new MAVLink messages RALLY_POINT and
RALLY_FETCH_POINT.

defines.h modified to make room in EEPROM to store rally points.

rally.pde added and is responsible for ensuring rally points get
stored in the correct spot in EEPROM.

Multiple Rally/RTL point support now done.  If rally points have
been defined, then when RTL mode is entered, the closest Rally
point is chosend and the plane loiters at that point.

Note only 10 rally points can be defined; this is to save space in
the APM's EEPROM.

ArduPlane/GCS_Mavlink.pde

@@ -1809,6 +1809,44 @@ mission_failed:
     }
 #endif // GEOFENCE_ENABLED
 
+    // receive a rally point from GCS and store in EEPROM
+    case MAVLINK_MSG_ID_RALLY_POINT: {
+        mavlink_rally_point_t packet;
+        mavlink_msg_rally_point_decode(msg, &packet);
+        if (mavlink_check_target(packet.target_system, packet.target_component))
+            break;
+        
+        if (packet.count != g.rally_total) {
+            send_text_P(SEVERITY_LOW,PSTR("bad rally point message"));
+        } else {
+            RallyLocation rally_point;
+            rally_point.lat = packet.lat;
+            rally_point.lng = packet.lng;
+            rally_point.alt = packet.alt;
+            rally_point.break_alt = packet.break_alt;
+            rally_point.land_dir = packet.land_dir;
+            rally_point.flags = packet.flags;
+            set_rally_point_with_index(rally_point, packet.idx);
+        }
+        break;
+    }
+
+    //send a rally point to the GCS
+    case MAVLINK_MSG_ID_RALLY_FETCH_POINT: {
+        mavlink_rally_fetch_point_t packet;
+        mavlink_msg_rally_fetch_point_decode(msg, &packet);
+        if (mavlink_check_target(packet.target_system, packet.target_component))
+            break;
+        if (packet.idx > g.rally_total) {
+            send_text_P(SEVERITY_LOW, PSTR("bad rally point"));   
+        } else {
+            RallyLocation rally_point = get_rally_point_with_index(packet.idx);
+            mavlink_msg_rally_point_send(chan, msg->sysid, msg->compid, packet.idx, g.rally_total, rally_point.lat, rally_point.lng, rally_point.alt, rally_point.break_alt, rally_point.land_dir, rally_point.flags);
+        }
+
+        break;
+    }    
+
     case MAVLINK_MSG_ID_PARAM_SET:
     {
         AP_Param                  *vp;

ArduPlane/Parameters.h

@@ -242,6 +242,8 @@ public:
         k_param_rcmap,
         k_param_TECS_controller,
         
+        k_param_rally_total,
+
         //
         // 240: PID Controllers
         k_param_pidNavRoll = 240, // unused
@@ -308,6 +310,8 @@ public:
     AP_Int16 fence_maxalt;    // meters
 #endif
 
+    AP_Int8 rally_total;
+
     // Fly-by-wire
     //
     AP_Int8 flybywire_elev_reverse;

ArduPlane/Parameters.pde

@@ -259,6 +259,12 @@ const AP_Param::Info var_info[] PROGMEM = {
     GSCALAR(fence_maxalt,           "FENCE_MAXALT",   0),
 #endif
 
+    // @Param: RALLY_TOTAL
+    // @DisplayName: Rally Total
+    // @Description: Number of rally points currently loaded
+    // @User: Standard
+    GSCALAR(rally_total,            "RALLY_TOTAL",    0),
+
     // @Param: ARSPD_FBW_MIN
     // @DisplayName: Fly By Wire Minimum Airspeed
     // @Description: Airspeed corresponding to minimum throttle in auto throttle modes (FBWB, CRUISE, AUTO, GUIDED, LOITER, CIRCLE and RTL). This is a calibrated (apparent) airspeed.

ArduPlane/commands_logic.pde

@@ -233,19 +233,27 @@ static void do_RTL(void)
 {
     control_mode    = RTL;
     prev_WP = current_loc;
+
+    if ((unsigned) g.rally_total.get() < 1) {
         next_WP = home;
 
+        // Altitude to hold over home
+        // Set by configuration tool
+        // -------------------------
+        next_WP.alt = read_alt_to_hold();
+
+    } else { //we have setup Rally points: use them instead of Home for RTL
+        RallyLocation ral_loc = find_best_rally_point();
+
+        next_WP = rally_location_to_location(ral_loc);
+    }
+
     if (g.loiter_radius < 0) {
         loiter.direction = -1;
     } else {
         loiter.direction = 1;
     }
 
-    // Altitude to hold over home
-    // Set by configuration tool
-    // -------------------------
-    next_WP.alt = read_alt_to_hold();
-
     setup_glide_slope();
 
     if (g.log_bitmask & MASK_LOG_MODE)

ArduPlane/defines.h

@@ -219,7 +219,12 @@ enum log_messages {
 #define FENCE_WP_SIZE sizeof(Vector2l)
 #define FENCE_START_BYTE (EEPROM_MAX_ADDR-(MAX_FENCEPOINTS*FENCE_WP_SIZE))
 
-#define MAX_WAYPOINTS  ((FENCE_START_BYTE - WP_START_BYTE) / WP_SIZE) - 1 // -
+// rally points shoehorned between fence points and waypoints
+#define MAX_RALLYPOINTS 10
+#define RALLY_WP_SIZE 15
+#define RALLY_START_BYTE (FENCE_START_BYTE-(MAX_RALLYPOINTS*RALLY_WP_SIZE))
+
+#define MAX_WAYPOINTS  ((RALLY_START_BYTE - WP_START_BYTE) / WP_SIZE) - 1 // -
                                                                           // 1
                                                                           // to
                                                                           // be

ArduPlane/rally.pde

@@ -0,0 +1,104 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ * rally point support
+ * Michael Day, September 2013
+ */
+
+//get a rally point
+static RallyLocation get_rally_point_with_index(unsigned i)
+{
+    uint16_t mem;
+    RallyLocation ret;
+
+    if (i > (unsigned) g.rally_total.get()) {
+        ret.lat = 0; ret.lng = 0; ret.alt = 0;
+        ret.land_dir = 0; ret.flags = 0;
+        return ret;
+    }
+
+    //read rally point
+    mem = RALLY_START_BYTE + (i * RALLY_WP_SIZE);
+    ret.lat = hal.storage->read_dword(mem);
+    mem += sizeof(uint32_t);
+    ret.lng = hal.storage->read_dword(mem);
+    mem += sizeof(uint32_t);
+    ret.alt = hal.storage->read_word(mem);
+    mem += sizeof(int16_t);
+    ret.break_alt = hal.storage->read_word(mem);
+    mem += sizeof(int16_t);
+    ret.land_dir = hal.storage->read_word(mem);
+    mem += sizeof(uint16_t);
+    ret.flags = hal.storage->read_byte(mem);
+
+    return ret;    
+}
+
+//save a rally point
+static void set_rally_point_with_index(RallyLocation &rallyLoc, unsigned i)
+{
+    uint16_t mem;
+
+    if (i >= (unsigned) g.rally_total.get()) {
+        //not allowed
+        return;
+    }
+
+    if (i >= MAX_RALLYPOINTS) {
+        //also not allowed
+        return;
+    }
+
+    mem = RALLY_START_BYTE + (i * RALLY_WP_SIZE);
+
+    hal.storage->write_dword(mem, rallyLoc.lat);
+    mem += sizeof(uint32_t);
+    hal.storage->write_dword(mem, rallyLoc.lng);
+    mem += sizeof(uint32_t);
+    hal.storage->write_word(mem, rallyLoc.alt);
+    mem += sizeof(int16_t);
+    hal.storage->write_word(mem, rallyLoc.break_alt);
+    mem += sizeof(int16_t);
+    hal.storage->write_word(mem, rallyLoc.land_dir);
+    mem += sizeof(uint32_t);
+    hal.storage->write_byte(mem, rallyLoc.flags);
+}
+
+//'best' means 'closest to my current location' for now.
+RallyLocation find_best_rally_point() {
+    RallyLocation ret;
+    RallyLocation next_rally;
+    Location rally_loc;
+    float dis;
+    float min_dis = 999999999.9f;
+
+    for (unsigned i = 0; i < (unsigned) g.rally_total.get(); i++) {
+        next_rally = get_rally_point_with_index(i);
+        rally_loc = rally_location_to_location(next_rally);
+        dis = get_distance(current_loc, rally_loc);
+
+        if (dis < min_dis) {
+            min_dis = dis;
+            ret = next_rally;
+        }
+    }
+
+    return ret;
+}
+
+//translate a RallyLocation to a Location
+Location rally_location_to_location (const RallyLocation &r_loc) {
+    Location ret;
+
+    ret.id = MAV_CMD_NAV_LOITER_UNLIM;
+    ret.options = MASK_OPTIONS_RELATIVE_ALT;
+
+    //Currently can't do true AGL on the APM.  Relative altitudes are
+    //relative to HOME point's altitude.  Terrain on the board is inbound
+    //for the PX4, though.  This line will need to be updated when that happens:
+    ret.alt = r_loc.alt + home.alt;
+
+    ret.lat = r_loc.lat;
+    ret.lng = r_loc.lng;
+
+    return ret;
+}