Line data    Source code

       1             : #include "AP_Rally_config.h"
       2             : 
       3             : #if HAL_RALLY_ENABLED
       4             : 
       5             : #include "AP_Rally.h"
       6             : 
       7             : #include <AP_AHRS/AP_AHRS.h>
       8             : #include <AP_Logger/AP_Logger.h>
       9             : #include <StorageManager/StorageManager.h>
      10             : #include <AP_Vehicle/AP_Vehicle_Type.h>
      11             : 
      12             : // storage object
      13             : StorageAccess AP_Rally::_storage(StorageManager::StorageRally);
      14             : 
      15             : #if APM_BUILD_COPTER_OR_HELI
      16             :   #define RALLY_LIMIT_KM_DEFAULT 0.3f
      17             :   #define RALLY_INCLUDE_HOME_DEFAULT 1
      18             : #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
      19             :   #define RALLY_LIMIT_KM_DEFAULT 5.0f
      20             :   #define RALLY_INCLUDE_HOME_DEFAULT 0
      21             : #elif APM_BUILD_TYPE(APM_BUILD_Rover)
      22             :   #define RALLY_LIMIT_KM_DEFAULT 0.5f
      23             :   #define RALLY_INCLUDE_HOME_DEFAULT 1
      24             : #else
      25             :   #define RALLY_LIMIT_KM_DEFAULT 1.0f
      26             :   #define RALLY_INCLUDE_HOME_DEFAULT 0
      27             : #endif
      28             : 
      29             : const AP_Param::GroupInfo AP_Rally::var_info[] = {
      30             :     // @Param: TOTAL
      31             :     // @DisplayName: Rally Total
      32             :     // @Description: Number of rally points currently loaded
      33             :     // @User: Advanced
      34             :     AP_GROUPINFO("TOTAL", 0, AP_Rally, _rally_point_total_count, 0),
      35             : 
      36             :     // @Param: LIMIT_KM
      37             :     // @DisplayName: Rally Limit
      38             :     // @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.
      39             :     // @User: Advanced
      40             :     // @Units: km
      41             :     // @Increment: 0.1
      42             :     AP_GROUPINFO("LIMIT_KM", 1, AP_Rally, _rally_limit_km, RALLY_LIMIT_KM_DEFAULT),
      43             : 
      44             :     // @Param: INCL_HOME
      45             :     // @DisplayName: Rally Include Home
      46             :     // @Description: Controls if Home is included as a Rally point (i.e. as a safe landing place) for RTL
      47             :     // @User: Standard
      48             :     // @Values: 0:DoNotIncludeHome,1:IncludeHome
      49             :     AP_GROUPINFO("INCL_HOME", 2, AP_Rally, _rally_incl_home, RALLY_INCLUDE_HOME_DEFAULT),
      50             : 
      51             :     AP_GROUPEND
      52             : };
      53             : 
      54             : // constructor
      55        2061 : AP_Rally::AP_Rally()
      56             : {
      57             :     ASSERT_STORAGE_SIZE(RallyLocation, 15);
      58             : 
      59             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
      60        2061 :     if (_singleton != nullptr) {
      61           0 :         AP_HAL::panic("Rally must be singleton");
      62             :     }
      63             : #endif
      64        2061 :     _singleton = this;
      65        2061 :     AP_Param::setup_object_defaults(this, var_info);
      66        2061 : }
      67             : 
      68             : // get a rally point from EEPROM
      69        1777 : bool AP_Rally::get_rally_point_with_index(uint8_t i, RallyLocation &ret) const
      70             : {
      71        1777 :     if (i >= (uint8_t) _rally_point_total_count) {
      72        1753 :         return false;
      73             :     }
      74             : 
      75          24 :     _storage.read_block(&ret, i * sizeof(RallyLocation), sizeof(RallyLocation));
      76             : 
      77          24 :     if (ret.lat == 0 && ret.lng == 0) {
      78           0 :         return false; // sanity check
      79             :     }
      80             : 
      81          24 :     return true; 
      82             : }
      83             : 
      84             : // Get rally point as a location
      85          63 : bool AP_Rally::get_rally_location_with_index(uint8_t i, Location &ret) const
      86             : {
      87             :     RallyLocation rally_loc;
      88          63 :     if (!get_rally_point_with_index(i, rally_loc)) {
      89          60 :         return false;
      90             :     }
      91           3 :     ret = rally_location_to_location(rally_loc);
      92           3 :     return true;
      93             : }
      94             : 
      95         886 : void AP_Rally::truncate(uint8_t num)
      96             : {
      97         886 :     if (num > _rally_point_total_count) {
      98             :         // we never make the space larger this way
      99           5 :         return;
     100             :     }
     101         881 :     _rally_point_total_count.set_and_save_ifchanged(num);
     102             : }
     103             : 
     104           6 : bool AP_Rally::append(const RallyLocation &loc)
     105             : {
     106           6 :     const uint8_t current_total = get_rally_total();
     107           6 :     _rally_point_total_count.set_and_save_ifchanged(current_total + 1);
     108           6 :     if (!set_rally_point_with_index(current_total, loc)) {
     109           0 :         _rally_point_total_count.set_and_save_ifchanged(current_total);
     110           0 :         return false;
     111             :     }
     112           6 :     return true;
     113             : }
     114             : 
     115             : // save a rally point to EEPROM - this assumes that the RALLY_TOTAL param has been incremented beforehand, which is the case in Mission Planner
     116           8 : bool AP_Rally::set_rally_point_with_index(uint8_t i, const RallyLocation &rallyLoc)
     117             : {
     118           8 :     if (i >= (uint8_t) _rally_point_total_count) {
     119           0 :         return false;
     120             :     }
     121             : 
     122           8 :     if (i >= get_rally_max()) {
     123           0 :         return false;
     124             :     }
     125             : 
     126           8 :     _storage.write_block(i * sizeof(RallyLocation), &rallyLoc, sizeof(RallyLocation));
     127             : 
     128           8 :     _last_change_time_ms = AP_HAL::millis();
     129             : 
     130             : #if HAL_LOGGING_ENABLED
     131           8 :     AP::logger().Write_RallyPoint(_rally_point_total_count, i, rallyLoc);
     132             : #endif
     133             : 
     134           8 :     return true;
     135             : }
     136             : 
     137             : // helper function to translate a RallyLocation to a Location
     138          16 : Location AP_Rally::rally_location_to_location(const RallyLocation &rally_loc) const
     139             : {
     140             :     //Relative altitudes are relative to HOME point's altitude:
     141             :     Location ret {
     142          16 :         rally_loc.lat,
     143          16 :         rally_loc.lng,
     144          16 :         rally_loc.alt * 100,
     145          32 :         (rally_loc.alt_frame_valid == 1) ? Location::AltFrame(rally_loc.alt_frame) : Location::AltFrame::ABOVE_HOME
     146          16 :     };
     147             : 
     148          16 :     return ret;
     149             : }
     150             : 
     151             : // returns true if a valid rally point is found, otherwise returns false to indicate home position should be used
     152         443 : bool AP_Rally::find_nearest_rally_point(const Location &current_loc, RallyLocation &return_loc) const
     153             : {
     154         443 :     float min_dis = -1;
     155             : 
     156         448 :     for (uint8_t i = 0; i < (uint8_t) _rally_point_total_count; i++) {
     157             :         RallyLocation next_rally;
     158           5 :         if (!get_rally_point_with_index(i, next_rally)) {
     159           0 :             continue;
     160             :         }
     161           5 :         Location rally_loc = rally_location_to_location(next_rally);
     162           5 :         float dis = current_loc.get_distance(rally_loc);
     163             : 
     164           5 :         if (is_valid(rally_loc) && (dis < min_dis || min_dis < 0)) {
     165           4 :             min_dis = dis;
     166           4 :             return_loc = next_rally;
     167             :         }
     168             :     }
     169             : 
     170             :     // if a limit is defined and all rally points are beyond that limit, use home if it is closer
     171         443 :     if ((_rally_limit_km > 0) && (min_dis > _rally_limit_km*1000.0f)) {
     172           0 :         return false; // use home position
     173             :     }
     174             : 
     175             :     // use home if no rally points found
     176         443 :     return min_dis >= 0;
     177             : }
     178             : 
     179             : // return best RTL location from current position
     180         443 : Location AP_Rally::calc_best_rally_or_home_location(const Location &current_loc, float rtl_home_alt_amsl_cm) const
     181             : {
     182             :     // if no valid rally point, return home position:
     183         443 :     Location return_loc { AP::ahrs().get_home() };
     184         443 :     return_loc.set_alt_cm(rtl_home_alt_amsl_cm, Location::AltFrame::ABSOLUTE);
     185             : 
     186             :     RallyLocation ral_loc;
     187         443 :     if (find_nearest_rally_point(current_loc, ral_loc)) {
     188           4 :         Location loc = rally_location_to_location(ral_loc);
     189             :         // use the rally point if it's closer then home, or we aren't generally considering home as acceptable
     190           4 :         if (!_rally_incl_home  || (current_loc.get_distance(loc) < current_loc.get_distance(return_loc))) {
     191           4 :             return_loc = rally_location_to_location(ral_loc);
     192             :         }
     193             :     }
     194             : 
     195         443 :     return return_loc;
     196             : }
     197             : 
     198             : // singleton instance
     199             : AP_Rally *AP_Rally::_singleton;
     200             : 
     201             : namespace AP {
     202             : 
     203     1298042 : AP_Rally *rally()
     204             : {
     205     1298042 :     return AP_Rally::get_singleton();
     206             : }
     207             : 
     208             : }
     209             : #endif //HAL_RALLY_ENABLED