CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/ArduPlane/mode_LoiterAltQLand.cpp
Views: 1798
1
#include "mode.h"

2
#include "Plane.h"

3


4
#if HAL_QUADPLANE_ENABLED

5


6
bool ModeLoiterAltQLand::_enter()

7
{

8
    if (plane.previous_mode->is_vtol_mode() || plane.quadplane.in_vtol_mode()) {

9
        plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_IN_VTOL);

10
        return true;

11
    }

12


13
    // If we were already in a loiter then use that waypoint. Else, use the current point

14
    const bool already_in_a_loiter = plane.nav_controller->reached_loiter_target() && !plane.nav_controller->data_is_stale();

15
    const Location loiter_wp = already_in_a_loiter ? plane.next_WP_loc : plane.current_loc;

16


17
    ModeLoiter::_enter();

18


19
    handle_guided_request(loiter_wp);

20


21
    switch_qland();

22


23
    return true;

24
}

25


26
void ModeLoiterAltQLand::navigate()

27
{

28
    switch_qland();

29


30
    ModeLoiter::navigate();

31
}

32


33
void ModeLoiterAltQLand::switch_qland()

34
{

35
    ftype dist;

36
    if ((!plane.current_loc.get_alt_distance(plane.next_WP_loc, dist) || is_negative(dist)) && plane.nav_controller->reached_loiter_target()) {

37
        plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_REACHED_QLAND);

38
    }

39
}

40


41
bool ModeLoiterAltQLand::handle_guided_request(Location target_loc)

42
{

43
    // setup altitude

44
#if AP_TERRAIN_AVAILABLE

45
    if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) {

46
        target_loc.set_alt_m(quadplane.qrtl_alt, Location::AltFrame::ABOVE_TERRAIN);

47
    } else {

48
        target_loc.set_alt_m(quadplane.qrtl_alt, Location::AltFrame::ABOVE_HOME);

49
    }

50
#else

51
    target_loc.set_alt_m(quadplane.qrtl_alt, Location::AltFrame::ABOVE_HOME);

52
#endif

53


54
    plane.set_guided_WP(target_loc);

55


56
    return true;

57
}

58


59
#endif

60


61