Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3          49 : bool ModeRTL::_enter()
       4             : {
       5             :     // refuse RTL if home has not been set
       6          49 :     if (!AP::ahrs().home_is_set()) {
       7           0 :         return false;
       8             :     }
       9             : 
      10             :     // initialise waypoint navigation library
      11          49 :     g2.wp_nav.init(MAX(0.0f, g2.rtl_speed));
      12             : 
      13             :     // set target to the closest rally point or home
      14             : #if HAL_RALLY_ENABLED
      15          49 :     if (!g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt))) {
      16           0 :         return false;
      17             :     }
      18             : #else
      19             :     // set destination
      20             :     if (!g2.wp_nav.set_desired_location(ahrs.get_home())) {
      21             :         return false;
      22             :     }
      23             : #endif
      24             : 
      25          49 :     send_notification = true;
      26          49 :     _loitering = false;
      27          49 :     return true;
      28             : }
      29             : 
      30       55640 : void ModeRTL::update()
      31             : {
      32             :     // determine if we should keep navigating
      33       55640 :     if (!g2.wp_nav.reached_destination()) {
      34             :         // update navigation controller
      35       54773 :         navigate_to_waypoint();
      36             :     } else {
      37             :         // send notification
      38         867 :         if (send_notification) {
      39          16 :             send_notification = false;
      40          16 :             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Reached destination");
      41             :         }
      42             : 
      43             :         // we have reached the destination
      44             :         // boats loiter, rovers stop
      45         867 :         if (!rover.is_boat()) {
      46         863 :             stop_vehicle();
      47             :         } else {
      48             :             // if not loitering yet, start loitering
      49           4 :             if (!_loitering) {
      50           4 :                 _loitering = rover.mode_loiter.enter();
      51             :             }
      52             :             // update stop or loiter
      53           4 :             if (_loitering) {
      54           4 :                 rover.mode_loiter.update();
      55             :             } else {
      56           0 :                 stop_vehicle();
      57             :             }
      58             :         }
      59             : 
      60             :         // update distance to destination
      61         867 :         _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
      62             :     }
      63       55640 : }
      64             : 
      65             : // get desired location
      66       11111 : bool ModeRTL::get_desired_location(Location& destination) const
      67             : {
      68       11111 :     if (g2.wp_nav.is_destination_valid()) {
      69       11111 :         destination = g2.wp_nav.get_oa_destination();
      70       11111 :         return true;
      71             :     }
      72           0 :     return false;
      73             : }
      74             : 
      75       22896 : bool ModeRTL::reached_destination() const
      76             : {
      77       22896 :     return g2.wp_nav.reached_destination();
      78             : }
      79             : 
      80             : // set desired speed in m/s
      81           0 : bool ModeRTL::set_desired_speed(float speed_ms)
      82             : {
      83           0 :     return g2.wp_nav.set_speed_max(speed_ms);
      84             : }