Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #if MODE_SMARTRTL_ENABLED
       4             : 
       5             : /*
       6             :  * Init and run calls for Smart_RTL flight mode
       7             :  *
       8             :  * This code uses the SmartRTL path that is already in memory, and feeds it into WPNav, one point at a time.
       9             :  * Once the copter is close to home, it will run a standard land controller.
      10             :  */
      11             : 
      12             : // Return true if this mode is enabled, used by MAVLink available modes
      13       75239 : bool ModeSmartRTL::enabled() const
      14             : {
      15       75239 :     return g2.smart_rtl.enabled();
      16             : }
      17             : 
      18          16 : bool ModeSmartRTL::init(bool ignore_checks)
      19             : {
      20          16 :     if (enabled() && g2.smart_rtl.is_active()) {
      21             :         // initialise waypoint and spline controller
      22          14 :         wp_nav->wp_and_spline_init_m();
      23             : 
      24             :         // set current target to a reasonable stopping point
      25          14 :         Vector3p stopping_point_ned_m;
      26          14 :         pos_control->get_stopping_point_NE_m(stopping_point_ned_m.xy());
      27          14 :         pos_control->get_stopping_point_D_m(stopping_point_ned_m.z);
      28          14 :         wp_nav->set_wp_destination_NED_m(stopping_point_ned_m);
      29             : 
      30             :         // initialise yaw to obey user parameter
      31          14 :         auto_yaw.set_mode_to_default(true);
      32             : 
      33             :         // wait for cleanup of return path
      34          14 :         smart_rtl_state = SubMode::WAIT_FOR_PATH_CLEANUP;
      35          14 :         return true;
      36             :     }
      37             : 
      38           2 :     return false;
      39             : }
      40             : 
      41             : // perform cleanup required when leaving smart_rtl
      42          14 : void ModeSmartRTL::exit()
      43             : {
      44             :     // restore last point if we hadn't reached it
      45          14 :     if (smart_rtl_state == SubMode::PATH_FOLLOW && !dest_NED_backup.is_zero()) {
      46           4 :         if (!g2.smart_rtl.add_point(dest_NED_backup)) {
      47           0 :             GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "SmartRTL: lost one point");
      48             :         }
      49             :     }
      50          14 :     dest_NED_backup.zero();
      51             : 
      52          14 :     g2.smart_rtl.cancel_request_for_thorough_cleanup();
      53          14 : }
      54             : 
      55      197500 : void ModeSmartRTL::run()
      56             : {
      57      197500 :     switch (smart_rtl_state) {
      58          28 :         case SubMode::WAIT_FOR_PATH_CLEANUP:
      59          28 :             wait_cleanup_run();
      60          28 :             break;
      61       70671 :         case SubMode::PATH_FOLLOW:
      62       70671 :             path_follow_run();
      63       70671 :             break;
      64       98237 :         case SubMode::PRELAND_POSITION:
      65       98237 :             pre_land_position_run();
      66       98237 :             break;
      67           0 :         case SubMode::DESCEND:
      68           0 :             descent_run(); // Re-using the descend method from normal rtl mode.
      69           0 :             break;
      70       28564 :         case SubMode::LAND:
      71       28564 :             land_run(true); // Re-using the land method from normal rtl mode.
      72       28564 :             break;
      73             :     }
      74      197500 : }
      75             : 
      76       60439 : bool ModeSmartRTL::is_landing() const
      77             : {
      78       60439 :     return smart_rtl_state == SubMode::LAND;
      79             : }
      80             : 
      81          28 : void ModeSmartRTL::wait_cleanup_run()
      82             : {
      83             :     // hover at current target position
      84          28 :     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
      85          28 :     wp_nav->update_wpnav();
      86          28 :     pos_control->D_update_controller();
      87          28 :     attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
      88             : 
      89             :     // check if return path is computed and if yes, begin journey home
      90          28 :     if (g2.smart_rtl.request_thorough_cleanup()) {
      91          14 :         path_follow_last_pop_fail_ms = 0;
      92          14 :         smart_rtl_state = SubMode::PATH_FOLLOW;
      93             :     }
      94          28 : }
      95             : 
      96       70671 : void ModeSmartRTL::path_follow_run()
      97             : {
      98             :     // if we are close to current target point, switch the next point to be our target.
      99       70671 :     if (wp_nav->reached_wp_destination()) {
     100             : 
     101             :         // clear destination backup so that it cannot be restored
     102          42 :         dest_NED_backup.zero();
     103             : 
     104             :         // this pop_point can fail if the IO task currently has the
     105             :         // path semaphore.
     106          42 :         Vector3p dest_NED;
     107          42 :         if (g2.smart_rtl.pop_point(dest_NED)) {
     108             :             // backup destination in case we exit smart_rtl mode and need to restore it to the path
     109          42 :             dest_NED_backup = dest_NED;
     110          42 :             path_follow_last_pop_fail_ms = 0;
     111          42 :             if (g2.smart_rtl.get_num_points() == 0) {
     112             :                 // this is the very last point, add 2m to the target alt and move to pre-land state
     113          10 :                 dest_NED.z -= 2.0f;
     114          10 :                 smart_rtl_state = SubMode::PRELAND_POSITION;
     115          10 :                 wp_nav->set_wp_destination_NED_m(dest_NED);
     116             :             } else {
     117             :                 // peek at the next point.  this can fail if the IO task currently has the path semaphore
     118          32 :                 Vector3p next_dest_NED;
     119          32 :                 if (g2.smart_rtl.peek_point(next_dest_NED)) {
     120          32 :                     wp_nav->set_wp_destination_NED_m(dest_NED);
     121          32 :                     if (g2.smart_rtl.get_num_points() == 1) {
     122             :                         // this is the very last point, add 2m to the target alt
     123           9 :                         next_dest_NED.z -= 2.0f;
     124             :                     }
     125          32 :                     wp_nav->set_wp_destination_next_NED_m(next_dest_NED);
     126             :                 } else {
     127             :                     // this can only happen if peek failed to take the semaphore
     128             :                     // send next point anyway which will cause the vehicle to slow at the next point
     129           0 :                     wp_nav->set_wp_destination_NED_m(dest_NED);
     130           0 :                     INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     131             :                 }
     132             :             }
     133           0 :         } else if (g2.smart_rtl.get_num_points() == 0) {
     134             :             // We should never get here; should always have at least
     135             :             // two points and the "zero points left" is handled above.
     136           0 :             INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     137           0 :             smart_rtl_state = SubMode::PRELAND_POSITION;
     138           0 :         } else if (path_follow_last_pop_fail_ms == 0) {
     139             :             // first time we've failed to pop off (ever, or after a success)
     140           0 :             path_follow_last_pop_fail_ms = AP_HAL::millis();
     141           0 :         } else if (AP_HAL::millis() - path_follow_last_pop_fail_ms > 10000) {
     142             :             // we failed to pop a point off for 10 seconds.  This is
     143             :             // almost certainly a bug.
     144           0 :             INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     145           0 :             smart_rtl_state = SubMode::PRELAND_POSITION;
     146             :         }
     147             :     }
     148             : 
     149             :     // update controllers
     150       70671 :     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     151       70671 :     wp_nav->update_wpnav();
     152       70671 :     pos_control->D_update_controller();
     153             : 
     154             :     // call attitude controller with auto yaw
     155       70671 :     attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
     156       70671 : }
     157             : 
     158       98237 : void ModeSmartRTL::pre_land_position_run()
     159             : {
     160             :     // if we are close to 2m above start point, we are ready to land.
     161       98237 :     if (wp_nav->reached_wp_destination()) {
     162             :         // choose descend and hold, or land based on user parameter rtl_alt_final_cm
     163           8 :         if (get_alt_final_m() <= 0 || copter.failsafe.radio) {
     164           8 :             land_start();
     165           8 :             smart_rtl_state = SubMode::LAND;
     166             :         } else {
     167           0 :             set_descent_target_alt(get_alt_final_m() * 100);
     168           0 :             descent_start();
     169           0 :             smart_rtl_state = SubMode::DESCEND;
     170             :         }
     171             :     }
     172             : 
     173             :     // update controllers
     174       98237 :     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     175       98237 :     wp_nav->update_wpnav();
     176       98237 :     pos_control->D_update_controller();
     177       98237 :     attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
     178       98237 : }
     179             : 
     180             : // save current position for use by the smart_rtl flight mode
     181      228158 : void ModeSmartRTL::save_position()
     182             : {
     183      228158 :     const bool should_save_position = motors->armed() && (copter.flightmode->mode_number() != Mode::Number::SMART_RTL);
     184             : 
     185      228158 :     copter.g2.smart_rtl.update(copter.position_ok(), should_save_position);
     186      228158 : }
     187             : 
     188        2464 : bool ModeSmartRTL::get_wp(Location& destination) const
     189             : {
     190             :     // provide target in states which use wp_nav
     191        2464 :     switch (smart_rtl_state) {
     192        2110 :     case SubMode::WAIT_FOR_PATH_CLEANUP:
     193             :     case SubMode::PATH_FOLLOW:
     194             :     case SubMode::PRELAND_POSITION:
     195             :     case SubMode::DESCEND:
     196        2110 :         return wp_nav->get_wp_destination_loc(destination);
     197         354 :     case SubMode::LAND:
     198         354 :         return false;
     199             :     }
     200             : 
     201             :     // we should never get here but just in case
     202           0 :     return false;
     203             : }
     204             : 
     205        2956 : float ModeSmartRTL::wp_distance_m() const
     206             : {
     207        2956 :     return wp_nav->get_wp_distance_to_destination_m();
     208             : }
     209             : 
     210        2956 : float ModeSmartRTL::wp_bearing_deg() const
     211             : {
     212        2956 :     return degrees(wp_nav->get_wp_bearing_to_destination_rad());
     213             : }
     214             : 
     215      141536 : bool ModeSmartRTL::use_pilot_yaw() const
     216             : {
     217      141536 :     const bool land_repositioning = g.land_repositioning && (smart_rtl_state == SubMode::DESCEND);
     218      141536 :     const bool final_landing = smart_rtl_state == SubMode::LAND;
     219      141536 :     return g2.smart_rtl.use_pilot_yaw() || land_repositioning || final_landing;
     220             : }
     221             : 
     222             : #endif