Line data    Source code

       1             : /// @file    AP_Rally.h
       2             : /// @brief   Handles rally point storage, retrieval and lookup
       3             : 
       4             : /*
       5             :  * The AP_Rally library:
       6             :  * 
       7             :  * Initial implementation: Michael Day, September 2013
       8             :  * Moved to AP_Rally lib:  Andrew Chapman April 2014
       9             :  * 
      10             :  * - responsible for managing a list of rally points
      11             :  * - reads and writes the rally points to storage
      12             :  * - provides access to the rally points, including logic to find the nearest one
      13             :  *
      14             :  */
      15             : #pragma once
      16             : 
      17             : #include "AP_Rally_config.h"
      18             : 
      19             : #if HAL_RALLY_ENABLED
      20             : 
      21             : #include <AP_HAL/AP_HAL_Boards.h>
      22             : 
      23             : #include <AP_Common/AP_Common.h>
      24             : #include <AP_Common/Location.h>
      25             : #include <AP_Param/AP_Param.h>
      26             : 
      27             : struct PACKED RallyLocation {
      28             :     int32_t lat;        //Latitude * 10^7
      29             :     int32_t lng;        //Longitude * 10^7
      30             :     int16_t alt;        //transit altitude (and loiter altitude) in meters (absolute);
      31             :     int16_t break_alt;  //when autolanding, break out of loiter at this alt (meters)
      32             :     uint16_t land_dir;   //when the time comes to auto-land, try to land in this direction (centidegrees)
      33             :     union {
      34             :         uint8_t flags; 
      35             :         struct {
      36             :             uint8_t favorable_winds : 1; // bit 0 = seek favorable winds when choosing a landing poi
      37             :             uint8_t do_auto_land    : 1; // bit 1 = do auto land after arriving
      38             :             uint8_t alt_frame_valid : 1; // bit 2 = true if following alt frame value should be used, else Location::AltFrame::ABOVE_HOME
      39             :             uint8_t alt_frame       : 2; // Altitude frame following Location::AltFrame enum
      40             :             uint8_t unused          : 3;
      41             :         };
      42             :     };
      43             : };
      44             : 
      45             : /// @class    AP_Rally
      46             : /// @brief    Object managing Rally Points
      47             : class AP_Rally {
      48             : public:
      49             :     AP_Rally();
      50             : 
      51             :     /* Do not allow copies */
      52             :     CLASS_NO_COPY(AP_Rally);
      53             : 
      54             :     // data handling
      55             :     bool get_rally_point_with_index(uint8_t i, RallyLocation &ret) const;
      56             :     bool get_rally_location_with_index(uint8_t i, Location &ret) const;
      57             :     bool set_rally_point_with_index(uint8_t i, const RallyLocation &rallyLoc);
      58        2913 :     uint8_t get_rally_total() const {
      59        2913 :         return (uint8_t)_rally_point_total_count;
      60             :     }
      61         894 :     uint8_t get_rally_max(void) const {
      62         894 :         const uint16_t ret = _storage.size() / uint16_t(sizeof(RallyLocation));
      63         894 :         if (ret > 255) {
      64           0 :             return 255;
      65             :         }
      66         894 :         return (uint8_t)ret;
      67             :     }
      68             :     // reduce point count:
      69             :     void truncate(uint8_t num);
      70             :     // append a rally point to the list
      71             :     bool append(const RallyLocation &loc) WARN_IF_UNUSED;
      72             : 
      73             :     float get_rally_limit_km() const { return _rally_limit_km; }
      74             : 
      75             :     Location rally_location_to_location(const RallyLocation &ret) const;
      76             : 
      77             :     // logic handling
      78             :     Location calc_best_rally_or_home_location(const Location &current_loc, float rtl_home_alt_amsl_cm) const;
      79             :     bool find_nearest_rally_point(const Location &myloc, RallyLocation &ret) const;
      80             : 
      81             :     // last time rally points changed
      82     1294706 :     uint32_t last_change_time_ms(void) const { return _last_change_time_ms; }
      83             : 
      84             :     // parameter block
      85             :     static const struct AP_Param::GroupInfo var_info[];
      86             : 
      87             :     // get singleton instance
      88     1298105 :     static AP_Rally *get_singleton() { return _singleton; }
      89             : 
      90             : 
      91             : private:
      92             :     static AP_Rally *_singleton;
      93             : 
      94           2 :     virtual bool is_valid(const Location &rally_point) const { return true; }
      95             : 
      96             :     static StorageAccess _storage;
      97             : 
      98             :     // parameters
      99             :     AP_Int8  _rally_point_total_count;
     100             :     AP_Float _rally_limit_km;
     101             :     AP_Int8  _rally_incl_home;
     102             : 
     103             :     uint32_t _last_change_time_ms = 0xFFFFFFFF;
     104             : };
     105             : 
     106             : namespace AP {
     107             :     AP_Rally *rally();
     108             : };
     109             : 
     110             : #endif  // HAL_RALLY_ENABLED