Line data    Source code

       1             : #include "mode.h"
       2             : #include "Plane.h"
       3             : 
       4             : #if HAL_QUADPLANE_ENABLED
       5             : 
       6           2 : bool ModeLoiterAltQLand::_enter()
       7             : {
       8           2 :     if (plane.previous_mode->is_vtol_mode() || plane.quadplane.in_vtol_mode()) {
       9           0 :         plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_IN_VTOL);
      10           0 :         return true;
      11             :     }
      12             : 
      13             :     // If we were already in a loiter then use that waypoint. Else, use the current point
      14           2 :     const bool already_in_a_loiter = plane.nav_controller->reached_loiter_target() && !plane.nav_controller->data_is_stale();
      15           2 :     const Location loiter_wp = already_in_a_loiter ? plane.next_WP_loc : plane.current_loc;
      16             : 
      17           2 :     ModeLoiter::_enter();
      18             : 
      19           2 :     handle_guided_request(loiter_wp);
      20             : 
      21           2 :     switch_qland();
      22             : 
      23           2 :     return true;
      24             : }
      25             : 
      26         519 : void ModeLoiterAltQLand::navigate()
      27             : {
      28         519 :     switch_qland();
      29             : 
      30         519 :     ModeLoiter::navigate();
      31         519 : }
      32             : 
      33         521 : void ModeLoiterAltQLand::switch_qland()
      34             : {
      35             :     ftype dist;
      36         521 :     if ((!plane.current_loc.get_height_above(plane.next_WP_loc, dist) || is_negative(dist)) && plane.nav_controller->reached_loiter_target()) {
      37           2 :         plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_REACHED_QLAND);
      38             :     }
      39         521 : }
      40             : 
      41           2 : bool ModeLoiterAltQLand::handle_guided_request(Location target_loc)
      42             : {
      43             :     // setup altitude
      44             : #if AP_TERRAIN_AVAILABLE
      45           2 :     if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) {
      46           1 :         target_loc.set_alt_m(quadplane.qrtl_alt_m, Location::AltFrame::ABOVE_TERRAIN);
      47             :     } else {
      48           1 :         target_loc.set_alt_m(quadplane.qrtl_alt_m, Location::AltFrame::ABOVE_HOME);
      49             :     }
      50             : #else
      51             :     target_loc.set_alt_m(quadplane.qrtl_alt_m, Location::AltFrame::ABOVE_HOME);
      52             : #endif
      53             : 
      54           2 :     plane.set_guided_WP(target_loc);
      55             : 
      56           2 :     return true;
      57             : }
      58             : 
      59             : #endif