Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3             : // Return true if this mode is enabled, used by MAVLink available mode
       4       12529 : bool ModeSmartRTL::enabled() const
       5             : {
       6             :     // Smart RTL mode requires smart RTL lib
       7       12529 :     return g2.smart_rtl.enabled();
       8             : }
       9             : 
      10           3 : bool ModeSmartRTL::_enter()
      11             : {
      12             :     // SmartRTL requires EKF (not DCM)
      13           3 :     Location ekf_origin;
      14           3 :     if (!ahrs.get_origin(ekf_origin)) {
      15           0 :         return false;
      16             :     }
      17             : 
      18             :     // refuse to enter SmartRTL if smart RTL's home has not been set
      19           3 :     if (!enabled() || !g2.smart_rtl.is_active()) {
      20           0 :         return false;
      21             :     }
      22             : 
      23             :     // initialise waypoint navigation library
      24           3 :     g2.wp_nav.init(MAX(0, g2.rtl_speed));
      25             : 
      26             :     // set desired location to reasonable stopping point
      27           3 :     if (!g2.wp_nav.set_desired_location_to_stopping_location()) {
      28           0 :         return false;
      29             :     }
      30             : 
      31             :     // init state
      32           3 :     smart_rtl_state = SmartRTLState::WaitForPathCleanup;
      33           3 :     _loitering = false;
      34             : 
      35           3 :     return true;
      36             : }
      37             : 
      38        2054 : void ModeSmartRTL::update()
      39             : {
      40        2054 :     switch (smart_rtl_state) {
      41           6 :         case SmartRTLState::WaitForPathCleanup:
      42             :             // check if return path is computed and if yes, begin journey home
      43           6 :             if (g2.smart_rtl.request_thorough_cleanup()) {
      44           3 :                 smart_rtl_state = SmartRTLState::PathFollow;
      45           3 :                 _load_point = true;
      46             :             }
      47             :             // Note: this may lead to an unnecessary 20ms slow down of the vehicle (but it is unlikely)
      48           6 :             stop_vehicle();
      49           6 :             break;
      50             : 
      51        2048 :         case SmartRTLState::PathFollow:
      52             :             // load point if required
      53        2048 :             if (_load_point) {
      54           7 :                 Vector3p dest_NED;
      55           7 :                 if (!g2.smart_rtl.pop_point(dest_NED)) {
      56             :                     // if not more points, we have reached home
      57           0 :                     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Reached destination");
      58           0 :                     smart_rtl_state = SmartRTLState::StopAtHome;
      59           0 :                     break;
      60             :                 } else {
      61             :                     // peek at the next point.  this can fail if the IO task currently has the path semaphore
      62           7 :                     Vector3p next_dest_NED;
      63           7 :                     if (g2.smart_rtl.peek_point(next_dest_NED)) {
      64           5 :                         if (!g2.wp_nav.set_desired_location_NED(dest_NED.tofloat(), next_dest_NED.tofloat())) {
      65             :                             // this should never happen because the EKF origin should already be set
      66           0 :                             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
      67           0 :                             smart_rtl_state = SmartRTLState::Failure;
      68           0 :                             INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
      69             :                         }
      70             :                     } else {
      71             :                         // no next point so add only immediate point
      72           2 :                         if (!g2.wp_nav.set_desired_location_NED(dest_NED.tofloat())) {
      73             :                             // this should never happen because the EKF origin should already be set
      74           0 :                             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
      75           0 :                             smart_rtl_state = SmartRTLState::Failure;
      76           0 :                             INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
      77             :                         }
      78             :                     }
      79             :                 }
      80           7 :                 _load_point = false;
      81             :             }
      82             :             // update navigation controller
      83        2048 :             navigate_to_waypoint();
      84             : 
      85             :             // check if we've reached the next point
      86        2048 :             if (g2.wp_nav.reached_destination()) {
      87           4 :                 _load_point = true;
      88             :             }
      89        2048 :             break;
      90             : 
      91           0 :         case SmartRTLState::StopAtHome:
      92             :         case SmartRTLState::Failure:
      93           0 :             _reached_destination = true;
      94             :             // we have reached the destination
      95             :             // boats loiters, rovers stop
      96           0 :             if (!rover.is_boat()) {
      97           0 :                stop_vehicle();
      98             :             } else {
      99             :                 // if not loitering yet, start loitering
     100           0 :                 if (!_loitering) {
     101           0 :                     _loitering = rover.mode_loiter.enter();
     102             :                 }
     103           0 :                 if (_loitering) {
     104           0 :                     rover.mode_loiter.update();
     105             :                 } else {
     106           0 :                     stop_vehicle();
     107             :                }
     108             :             }
     109           0 :             break;
     110             :     }
     111        2054 : }
     112             : 
     113             : // get desired location
     114         408 : bool ModeSmartRTL::get_desired_location(Location& destination) const
     115             : {
     116         408 :     switch (smart_rtl_state) {
     117           0 :     case SmartRTLState::WaitForPathCleanup:
     118           0 :         return false;
     119         408 :     case SmartRTLState::PathFollow:
     120         408 :         if (g2.wp_nav.is_destination_valid()) {
     121         408 :             destination = g2.wp_nav.get_destination();
     122         408 :             return true;
     123             :         }
     124           0 :         return false;
     125           0 :     case SmartRTLState::StopAtHome:
     126             :     case SmartRTLState::Failure:
     127           0 :         return false;
     128             :     }
     129             :     // should never reach here but just in case
     130           0 :     return false;
     131             : }
     132             : 
     133             : // set desired speed in m/s
     134           0 : bool ModeSmartRTL::set_desired_speed(float speed_ms)
     135             : {
     136           0 :     return g2.wp_nav.set_speed_max(speed_ms);
     137             : }
     138             : 
     139             : // save current position for use by the smart_rtl flight mode
     140       39175 : void ModeSmartRTL::save_position()
     141             : {
     142       39175 :     const bool save_pos = (rover.control_mode != &rover.mode_smartrtl);
     143       39175 :     g2.smart_rtl.update(true, save_pos);
     144       39175 : }