ArduCopter/mode_smart_rtl.cpp

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