Plane: Now using AP_Rally library.

ArduPlane/ArduPlane.pde

@@ -79,6 +79,8 @@
 #include <AP_BoardConfig.h>
 #include <AP_ServoRelayEvents.h>
 
+#include <AP_Rally.h>
+
 // Pre-AP_HAL compatibility
 #include "compat.h"
 
@@ -320,6 +322,9 @@ static AP_ServoRelayEvents ServoRelayEvents(relay);
 static AP_Camera camera(&relay);
 #endif
 
+//Rally Ponints
+AP_Rally rally(ahrs, MAX_RALLYPOINTS, RALLY_START_BYTE);
+
 ////////////////////////////////////////////////////////////////////////////////
 // Global variables
 ////////////////////////////////////////////////////////////////////////////////

ArduPlane/GCS_Mavlink.pde

@@ -1485,13 +1485,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         if (mavlink_check_target(packet.target_system, packet.target_component))
             break;
         
-        if (packet.idx >= g.rally_total || 
+        if (packet.idx >= rally.get_rally_total() || 
             packet.idx >= MAX_RALLYPOINTS) {
             send_text_P(SEVERITY_LOW,PSTR("bad rally point message ID"));
             break;
         }
 
-        if (packet.count != g.rally_total) {
+        if (packet.count != rally.get_rally_total()) {
             send_text_P(SEVERITY_LOW,PSTR("bad rally point message count"));
             break;
         }
@@ -1503,7 +1503,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         rally_point.break_alt = packet.break_alt;
         rally_point.land_dir = packet.land_dir;
         rally_point.flags = packet.flags;
-        set_rally_point_with_index(packet.idx, rally_point);
+        rally.set_rally_point_with_index(packet.idx, rally_point);
         break;
     }
 
@@ -1513,19 +1513,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         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) {
+        if (packet.idx > rally.get_rally_total()) {
             send_text_P(SEVERITY_LOW, PSTR("bad rally point index"));   
             break;
         }
         RallyLocation rally_point;
-        if (!get_rally_point_with_index(packet.idx, rally_point)) {
+        if (!rally.get_rally_point_with_index(packet.idx, rally_point)) {
             send_text_P(SEVERITY_LOW, PSTR("failed to set rally point"));   
             break;
         }
 
         mavlink_msg_rally_point_send_buf(msg,
                                          chan, msg->sysid, msg->compid, packet.idx, 
-                                         g.rally_total, rally_point.lat, rally_point.lng, 
+                                         rally.get_rally_total(), rally_point.lat, rally_point.lng, 
                                          rally_point.alt, rally_point.break_alt, rally_point.land_dir, 
                                          rally_point.flags);
         break;

ArduPlane/Parameters.h

@@ -96,7 +96,7 @@ public:
         k_param_waypoint_max_radius,
         k_param_ground_steer_alt,        
         k_param_ground_steer_dps,
-        k_param_rally_limit_km,
+        k_param_rally_limit_km_old, //unused anymore -- just holding this index
         k_param_hil_err_limit,
         k_param_sonar,
         k_param_log_bitmask,
@@ -106,6 +106,7 @@ public:
         k_param_flaperon_output,
         k_param_gps,
         k_param_autotune_level,
+        k_param_rally,
 
         // 100: Arming parameters
         k_param_arming = 100,
@@ -268,7 +269,7 @@ public:
         k_param_L1_controller,
         k_param_rcmap,
         k_param_TECS_controller,
-        k_param_rally_total,
+        k_param_rally_total_old,  //unused
         k_param_steerController,
 
         //
@@ -347,9 +348,6 @@ public:
     AP_Int8 fence_ret_rally;
 #endif
 
-    AP_Int8 rally_total;
-    AP_Float rally_limit_km;
-
     // Fly-by-wire
     //
     AP_Int8 flybywire_elev_reverse;

ArduPlane/Parameters.pde

@@ -295,20 +295,6 @@ const AP_Param::Info var_info[] PROGMEM = {
     GSCALAR(fence_ret_rally,        "FENCE_RET_RALLY",  0),     
 #endif
 
-    // @Param: RALLY_TOTAL
-    // @DisplayName: Rally Total
-    // @Description: Number of rally points currently loaded
-    // @User: Advanced
-    GSCALAR(rally_total,            "RALLY_TOTAL",    0),
-
-    // @Param: RALLY_LIMIT_KM
-    // @DisplayName: Rally Limit
-    // @Description: Maximum distance to rally point. If the closest rally point is more than this number of kilometers from the current position and the home location is closer than any of the rally points from the current position then do RTL to home rather than to the closest rally point. This prevents a leftover rally point from a different airfield being used accidentally. If this is set to 0 then the closest rally point is always used.
-    // @User: Advanced
-    // @Units: kilometers
-    // @Increment: 0.1
-    GSCALAR(rally_limit_km,          "RALLY_LIMIT_KM",    5),
-
     // @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.
@@ -1004,6 +990,10 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/AP_Mission/AP_Mission.cpp
     GOBJECT(mission, "MIS_",       AP_Mission),
 
+    // @Group: RALLY_
+    // @Path: ../libraries/AP_Rally/AP_Rally.cpp
+    GOBJECT(rally,  "RALLY_",       AP_Rally),
+
     AP_VAREND
 };
 
@@ -1039,6 +1029,8 @@ const AP_Param::ConversionInfo conversion_table[] PROGMEM = {
     { Parameters::k_param_curr_amp_offset,    0,      AP_PARAM_FLOAT, "BATT_AMP_OFFSET" },
     { Parameters::k_param_pack_capacity,      0,      AP_PARAM_INT32, "BATT_CAPACITY" },
     { Parameters::k_param_log_bitmask_old,    0,      AP_PARAM_INT16, "LOG_BITMASK" },
+    { Parameters::k_param_rally_limit_km_old, 0,      AP_PARAM_FLOAT, "RALLY_LIMIT_KM" },
+    { Parameters::k_param_rally_total_old,    0,      AP_PARAM_INT8, "RALLY_TOTAL" },
 };
 
 static void load_parameters(void)

ArduPlane/commands_logic.pde

@@ -241,7 +241,7 @@ static void do_RTL(void)
 {
     control_mode    = RTL;
     prev_WP_loc = current_loc;
-    next_WP_loc = rally_find_best_location(current_loc, home);
+    next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, read_alt_to_hold());
 
     if (g.loiter_radius < 0) {
         loiter.direction = -1;
@@ -620,7 +620,8 @@ static void exit_mission_callback()
     if (control_mode == AUTO) {
         gcs_send_text_fmt(PSTR("Returning to Home"));
         memset(&auto_rtl_command, 0, sizeof(auto_rtl_command));
-        auto_rtl_command.content.location = rally_find_best_location(current_loc, home);
+        auto_rtl_command.content.location = 
+            rally.calc_best_rally_or_home_location(current_loc, read_alt_to_hold());
         auto_rtl_command.id = MAV_CMD_NAV_LOITER_UNLIM;
         setup_glide_slope();
         start_command(auto_rtl_command);

ArduPlane/geofence.pde

@@ -339,7 +339,7 @@ static void geofence_check(bool altitude_check_only)
     case FENCE_ACTION_GUIDED:
     case FENCE_ACTION_GUIDED_THR_PASS:
         if (g.fence_ret_rally != 0) { //return to a rally point
-            guided_WP_loc = rally_find_best_location(current_loc, home);
+            guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, read_alt_to_hold());
 
         } else { //return to fence return point, not a rally point
             if (g.fence_retalt > 0) {

ArduPlane/rally.pde

@@ -1,101 +0,0 @@
-// -*- 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 bool get_rally_point_with_index(unsigned i, RallyLocation &ret)
-{
-    if (i >= (unsigned)g.rally_total) {
-        return false;
-    }
-    hal.storage->read_block(&ret, RALLY_START_BYTE + (i * sizeof(RallyLocation)), sizeof(RallyLocation));
-    if (ret.lat == 0 && ret.lng == 0) {
-        // sanity check ...
-        return false;
-    }
-    return true; 
-}
-
-//save a rally point
-static bool set_rally_point_with_index(unsigned i, const RallyLocation &rallyLoc)
-{
-    if (i >= (unsigned) g.rally_total) {
-        //not allowed
-        return false;
-    }
-
-    if (i >= MAX_RALLYPOINTS) {
-        //also not allowed
-        return false;
-    }
-    hal.storage->write_block(RALLY_START_BYTE + (i * sizeof(RallyLocation)), &rallyLoc, sizeof(RallyLocation));
-    return true;
-}
-
-// 'best' means 'closest to Location loc' for now.
-static bool find_best_rally_point(const Location &myloc, const Location &homeloc, RallyLocation &ret) 
-{
-    float min_dis = -1;
-
-    for (unsigned i = 0; i < (unsigned) g.rally_total; i++) {
-        RallyLocation next_rally;
-        if (!get_rally_point_with_index(i, next_rally)) {
-            continue;
-        }
-        Location rally_loc = rally_location_to_location(next_rally, homeloc);
-        float dis = get_distance(myloc, rally_loc);
-
-        if (dis < min_dis || min_dis < 0) {
-            min_dis = dis;
-            ret = next_rally;
-        }
-    }
-
-    if (g.rally_limit_km > 0 && min_dis > g.rally_limit_km*1000.0f && 
-        get_distance(myloc, homeloc) < min_dis) {
-        // return false, which makes home be used instead
-        return false;
-    }
-
-    return min_dis >= 0;
-}
-
-// translate a RallyLocation to a Location
-static Location rally_location_to_location(const RallyLocation &r_loc, const Location &homeloc)
-{
-    Location ret = {};
-
-    // we return an absolute altitude, as we add homeloc.alt below
-    ret.flags.relative_alt = false;
-
-    //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*100UL) + homeloc.alt;
-
-    ret.lat = r_loc.lat;
-    ret.lng = r_loc.lng;
-
-    return ret;
-}
-
-// return best RTL location from current position
-static Location rally_find_best_location(const Location &myloc, const Location &homeloc)
-{
-    RallyLocation ral_loc = {};
-    Location ret = {};
-    if (find_best_rally_point(myloc, home, ral_loc)) {
-        //we have setup Rally points: use them instead of Home for RTL
-        ret = rally_location_to_location(ral_loc, home);
-    } else {
-        ret = homeloc;
-        // Altitude to hold over home
-        ret.alt = read_alt_to_hold();
-        // read_alt_to_hold returns an absolute altitude
-        ret.flags.relative_alt = false;
-    }
-    return ret;
-}
-