AP_Rally library

libraries/AP_Rally/AP_Rally.cpp

@@ -0,0 +1,154 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file    AP_Rally.h
+/// @brief   Handles rally point storage and retrieval.
+#include "AP_Rally.h"
+
+#include <AP_HAL.h>
+extern const AP_HAL::HAL& hal;
+
+// ArduCopter/defines.h sets this, and this definition will be moved into ArduPlane/defines.h when that is patched to use the lib
+#ifdef APM_BUILD_DIRECTORY
+  #if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
+    #define RALLY_LIMIT_KM_DEFAULT 2.0
+  #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+    #define RALLY_LIMIT_KM_DEFAULT 5.0
+  #elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
+    #define RALLY_LIMIT_KM_DEFAULT 0.5
+  #endif
+#else 
+    #define RALLY_LIMIT_KM_DEFAULT 1.0
+#endif // APM_BUILD_DIRECTORY
+
+const AP_Param::GroupInfo AP_Rally::var_info[] PROGMEM = {
+    // @Param: TOTAL
+    // @DisplayName: Rally Total
+    // @Description: Number of rally points currently loaded
+    // @User: Advanced
+    AP_GROUPINFO("TOTAL", 0, AP_Rally, _rally_point_total_count, 0),
+
+    // @Param: 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
+    AP_GROUPINFO("LIMIT_KM", 1, AP_Rally, _rally_limit_km, RALLY_LIMIT_KM_DEFAULT),
+
+    AP_GROUPEND
+};
+
+// constructor
+AP_Rally::AP_Rally(AP_AHRS &ahrs, uint16_t max_rally_points, uint16_t rally_wp_size, uint16_t rally_start_byte) 
+    : _ahrs(ahrs)
+    , _max_rally_points(max_rally_points)
+    , _rally_start_byte(rally_start_byte)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+
+    // we only pass rally_point_size through to verify that the value used in defines.h to layout the EEPROM stays correct over time
+    // would be nice if we could reliably check at compile time, but I can't find a solution that works in the Arduino IDE too
+    // if there is a difference in passed size versus sizeof(RallyLocation) we disable rally points by setting the max count to zero
+    if (rally_wp_size != sizeof(RallyLocation)) {
+        _max_rally_points = 0;
+    }
+}
+
+// get a rally point from EEPROM
+bool AP_Rally::get_rally_point_with_index(uint8_t i, RallyLocation &ret) const
+{
+    if (i >= (uint8_t) _rally_point_total_count) {
+        return false;
+    }
+
+    hal.storage->read_block(&ret, _rally_start_byte + (i * sizeof(RallyLocation)), sizeof(RallyLocation));
+
+    if (ret.lat == 0 && ret.lng == 0) {
+        return false; // sanity check
+    }
+
+    return true; 
+}
+
+// save a rally point to EEPROM - this assumes that the RALLY_TOTAL param has been incremented beforehand, which is the case in Mission Planner
+bool AP_Rally::set_rally_point_with_index(uint8_t i, const RallyLocation &rallyLoc)
+{
+    if (i >= (uint8_t) _rally_point_total_count) {
+        return false;
+    }
+
+    if (i >= (uint8_t) _max_rally_points) {
+        return false;
+    }
+
+    hal.storage->write_block(_rally_start_byte + (i * sizeof(RallyLocation)), &rallyLoc, sizeof(RallyLocation));
+
+    return true;
+}
+
+// helper function to translate a RallyLocation to a Location
+Location AP_Rally::rally_location_to_location(const RallyLocation &rally_loc, const Location &home_loc) const
+{
+    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 = (rally_loc.alt*100UL) + home_loc.alt;
+
+    ret.lat = rally_loc.lat;
+    ret.lng = rally_loc.lng;
+
+    return ret;
+}
+
+// returns true if a valid rally point is found, otherwise returns false to indicate home position should be used
+bool AP_Rally::find_nearest_rally_point(const Location &current_loc, RallyLocation &return_loc) const
+{
+    float min_dis = -1;
+    const struct Location &home_loc = _ahrs.get_home();
+
+    for (uint8_t i = 0; i < (uint8_t) _rally_point_total_count; i++) {
+        RallyLocation next_rally;
+        if (!get_rally_point_with_index(i, next_rally)) {
+            continue;
+        }
+        Location rally_loc = rally_location_to_location(next_rally, home_loc);
+        float dis = get_distance(current_loc, rally_loc);
+
+        if (dis < min_dis || min_dis < 0) {
+            min_dis = dis;
+            return_loc = next_rally;
+        }
+    }
+
+    if ((_rally_limit_km > 0) && (min_dis > _rally_limit_km*1000.0f) && (get_distance(current_loc, home_loc) < min_dis)) {
+        return false; // use home position
+    }
+
+    return min_dis >= 0;
+}
+
+// return best RTL location from current position
+Location AP_Rally::calc_best_rally_or_home_location(const Location &current_loc, float rtl_home_alt) const
+{
+    RallyLocation ral_loc = {};
+    Location return_loc = {};
+    const struct Location &home_loc = _ahrs.get_home();
+    
+    if (find_nearest_rally_point(current_loc, ral_loc)) {
+        // valid rally point found
+        return_loc = rally_location_to_location(ral_loc, home_loc);
+        return_loc.alt = max(return_loc.alt, rtl_home_alt);
+    } else {
+        // no valid rally point, return home position
+        return_loc = home_loc;
+        return_loc.alt = rtl_home_alt;
+        return_loc.flags.relative_alt = false; // read_alt_to_hold returns an absolute altitude
+    }
+
+    return return_loc;
+}

libraries/AP_Rally/AP_Rally.h

@@ -0,0 +1,58 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file    AP_Rally.h
+/// @brief   Handles rally point storage, retrieval and lookup
+
+/*
+ * The AP_Rally library:
+ * 
+ * Initial implementation: Michael Day, September 2013
+ * Moved to AP_Rally lib:  Andrew Chapman April 2014
+ * 
+ * - responsible for managing a list of rally points
+ * - reads and writes the rally points to storage
+ * - provides access to the rally points, including logic to find the nearest one
+ *
+ */
+#ifndef AP_Rally_h
+#define AP_Rally_h
+
+#include <AP_Common.h>
+#include <AP_Param.h>
+#include <AP_AHRS.h>
+
+/// @class    AP_Rally
+/// @brief    Object managing Rally Points
+class AP_Rally {
+
+public:
+    AP_Rally(AP_AHRS &ahrs, uint16_t max_rally_points, uint16_t rally_point_size, uint16_t rally_start_byte);
+
+    // data handling
+    bool get_rally_point_with_index(uint8_t i, RallyLocation &ret) const;
+    bool set_rally_point_with_index(uint8_t i, const RallyLocation &rallyLoc);
+    uint8_t get_rally_total() const { return _rally_point_total_count; }
+
+    // logic handling
+    Location calc_best_rally_or_home_location(const Location &current_loc, float rtl_home_alt) const;
+    bool find_nearest_rally_point(const Location &myloc, RallyLocation &ret) const;
+
+    // parameter block
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+    Location rally_location_to_location(const RallyLocation &rally_loc, const Location &home_loc) const;
+
+private:
+    // internal variables
+    const AP_AHRS& _ahrs; // used only for home position
+    uint16_t _max_rally_points;
+    const uint16_t _rally_start_byte;
+
+    // parameters
+    AP_Int8  _rally_point_total_count;
+    AP_Float _rally_limit_km;
+};
+
+
+#endif // AP_Rally_h