Line data    Source code

       1             : #include "mode.h"
       2             : #include "Plane.h"
       3             : 
       4          68 : bool ModeRTL::_enter()
       5             : {
       6          68 :     plane.prev_WP_loc = plane.current_loc;
       7          68 :     plane.do_RTL(plane.get_RTL_altitude_cm());
       8          68 :     plane.rtl.done_climb = false;
       9             : #if HAL_QUADPLANE_ENABLED
      10          68 :     plane.vtol_approach_s.approach_stage = Plane::VTOLApproach::Stage::RTL;
      11             : 
      12             :     // Quadplane specific checks
      13          68 :     if (plane.quadplane.available()) {
      14             :         // treat RTL as QLAND if we are in guided wait takeoff state, to cope
      15             :         // with failsafes during GUIDED->AUTO takeoff sequence
      16          28 :         if (plane.quadplane.guided_wait_takeoff_on_mode_enter) {
      17           0 :             plane.set_mode(plane.mode_qland, ModeReason::QLAND_INSTEAD_OF_RTL);
      18           0 :             return true;
      19             :         }
      20             : 
      21             :         // if Q_RTL_MODE is QRTL always, immediately switch to QRTL mode
      22          28 :         if (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::QRTL_ALWAYS) {
      23           0 :             plane.set_mode(plane.mode_qrtl, ModeReason::QRTL_INSTEAD_OF_RTL);
      24           0 :             return true;
      25             :         }
      26             : 
      27             :         // if VTOL landing is expected and quadplane motors are active and within QRTL radius and under QRTL altitude then switch to QRTL
      28          28 :         const bool vtol_landing = (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::SWITCH_QRTL) || (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::VTOL_APPROACH_QRTL);
      29          28 :         if (vtol_landing && (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED)) {
      30             :             int32_t alt_cm;
      31           0 :             if ((plane.current_loc.get_distance(plane.next_WP_loc) < plane.mode_qrtl.get_VTOL_return_radius()) &&
      32           0 :                 plane.current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_cm) && (alt_cm < plane.quadplane.qrtl_alt_m*100)) {
      33           0 :                 plane.set_mode(plane.mode_qrtl, ModeReason::QRTL_INSTEAD_OF_RTL);
      34           0 :                 return true;
      35             :             }
      36             :         }
      37             :     }
      38             : #endif
      39             : 
      40          68 :     return true;
      41             : }
      42             : 
      43       90460 : void ModeRTL::update()
      44             : {
      45       90460 :     plane.calc_nav_roll();
      46       90460 :     plane.calc_nav_pitch();
      47       90460 :     plane.calc_throttle();
      48             : 
      49       90460 :     bool alt_threshold_reached = false;
      50       90460 :     if (plane.flight_option_enabled(FlightOptions::CLIMB_BEFORE_TURN)) {
      51             :         // Climb to RTL_ALTITUDE before turning. This overrides RTL_CLIMB_MIN.
      52         778 :         alt_threshold_reached = plane.current_loc.alt > plane.next_WP_loc.alt;
      53       89682 :     } else if (plane.g2.rtl_climb_min > 0) {
      54             :         /*
      55             :            when RTL first starts limit bank angle to LEVEL_ROLL_LIMIT
      56             :            until we have climbed by RTL_CLIMB_MIN meters
      57             :            */
      58        4289 :         alt_threshold_reached = (plane.current_loc.alt - plane.prev_WP_loc.alt)*0.01 > plane.g2.rtl_climb_min;
      59             :     } else {
      60       85393 :         return;
      61             :     }
      62             : 
      63        5067 :     if (!plane.rtl.done_climb && alt_threshold_reached) {
      64           1 :         plane.prev_WP_loc = plane.current_loc;
      65           1 :         plane.setup_alt_slope();
      66           1 :         plane.rtl.done_climb = true;
      67             :     }
      68        5067 :     if (!plane.rtl.done_climb) {
      69             :         // Constrain the roll limit as a failsafe, that way if something goes wrong the plane will
      70             :         // eventually turn back and go to RTL instead of going perfectly straight. This also leaves
      71             :         // some leeway for fighting wind.
      72        1959 :         plane.roll_limit_cd = MIN(plane.roll_limit_cd, plane.g.level_roll_limit*100);
      73        1959 :         plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -plane.roll_limit_cd, plane.roll_limit_cd);
      74             :     }
      75             : }
      76             : 
      77        7657 : void ModeRTL::navigate()
      78             : {
      79             : #if HAL_QUADPLANE_ENABLED
      80        7657 :     if (plane.quadplane.available()) {
      81        2095 :         if (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::VTOL_APPROACH_QRTL) {
      82             :             // VTOL approach landing
      83           0 :             AP_Mission::Mission_Command cmd;
      84           0 :             cmd.content.location = plane.next_WP_loc;
      85           0 :             plane.verify_landing_vtol_approach(cmd);
      86           0 :             if (plane.vtol_approach_s.approach_stage == Plane::VTOLApproach::Stage::VTOL_LANDING) {
      87           0 :                 plane.set_mode(plane.mode_qrtl, ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL);
      88             :             }
      89           0 :             return;
      90             :         }
      91             : 
      92        2095 :         if ((AP_HAL::millis() - plane.last_mode_change_ms > 1000) && switch_QRTL()) {
      93           1 :             return;
      94             :         }
      95             :     }
      96             : #endif
      97             : 
      98        7656 :     uint16_t radius = abs(plane.g.rtl_radius);
      99        7656 :     if (radius > 0) {
     100         835 :         plane.loiter.direction = (plane.g.rtl_radius < 0) ? -1 : 1;
     101             :     }
     102             : 
     103        7656 :     plane.update_loiter(radius);
     104             : 
     105        7656 :     if (!plane.auto_state.checked_for_autoland) {
     106       16004 :         if ((plane.g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START) ||
     107        8475 :             (plane.g.rtl_autoland == RtlAutoland::RTL_THEN_DO_LAND_START &&
     108         962 :             plane.reached_loiter_target() && 
     109          77 :             labs(plane.calc_altitude_error_cm()) < 1000)) {
     110             :                 // we've reached the RTL point, see if we have a landing sequence
     111          20 :                 if (plane.have_position && plane.mission.jump_to_landing_sequence(plane.current_loc)) {
     112             :                     // switch from RTL -> AUTO
     113          15 :                     plane.mission.set_force_resume(true);
     114          15 :                     if (plane.set_mode(plane.mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND)) {
     115             :                         // return here so we don't change the radius and don't run the rtl update_loiter()
     116          15 :                         return;
     117             :                     }
     118             :                     // mode change failed, revert force resume flag
     119           0 :                     plane.mission.set_force_resume(false);
     120             :                 }
     121             : 
     122             :                 // prevent running the expensive jump_to_landing_sequence
     123             :                 // on every loop
     124           5 :                 plane.auto_state.checked_for_autoland = true;
     125             : 
     126        7509 :         } else if (plane.g.rtl_autoland == RtlAutoland::DO_RETURN_PATH_START) {
     127           0 :             if (plane.have_position && plane.mission.jump_to_closest_mission_leg(plane.current_loc)) {
     128           0 :                 plane.mission.set_force_resume(true);
     129           0 :                 if (plane.set_mode(plane.mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND)) {
     130             :                     // return here so we don't change the radius and don't run the rtl update_loiter()
     131           0 :                     return;
     132             :                 }
     133             :                 // mode change failed, revert force resume flag
     134           0 :                 plane.mission.set_force_resume(false);
     135             :             }
     136           0 :             plane.auto_state.checked_for_autoland = true;
     137             :         }
     138             :     }
     139             : }
     140             : 
     141             : #if HAL_QUADPLANE_ENABLED
     142             : // Switch to QRTL if enabled and within radius
     143        1899 : bool ModeRTL::switch_QRTL()
     144             : {
     145        1899 :     if (plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::SWITCH_QRTL) {
     146        1117 :         return false;
     147             :     }
     148             : 
     149         782 :     uint16_t qrtl_radius = abs(plane.g.rtl_radius);
     150         782 :     if (qrtl_radius == 0) {
     151         782 :         qrtl_radius = abs(plane.aparm.loiter_radius);
     152             :     }
     153             : 
     154        1564 :     if (plane.nav_controller->reached_loiter_target() ||
     155        1564 :          plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc) ||
     156        1564 :          plane.auto_state.wp_distance < MAX(qrtl_radius, plane.quadplane.stopping_distance_m())) {
     157             :         /*
     158             :           for a quadplane in RTL mode we switch to QRTL when we
     159             :           are within the maximum of the stopping distance and the
     160             :           RTL_RADIUS
     161             :          */
     162           1 :         plane.set_mode(plane.mode_qrtl, ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL);
     163           1 :         return true;
     164             :     }
     165             : 
     166         781 :     return false;
     167             : }
     168             : 
     169             : #endif  // HAL_QUADPLANE_ENABLED