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/Rover/mode_rtl.cpp
Views: 1798
1
#include "Rover.h"

2


3
bool ModeRTL::_enter()

4
{

5
    // refuse RTL if home has not been set

6
    if (!AP::ahrs().home_is_set()) {

7
        return false;

8
    }

9


10
    // initialise waypoint navigation library

11
    g2.wp_nav.init(MAX(0.0f, g2.rtl_speed));

12


13
    // set target to the closest rally point or home

14
#if HAL_RALLY_ENABLED

15
    if (!g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt))) {

16
        return false;

17
    }

18
#else

19
    // set destination

20
    if (!g2.wp_nav.set_desired_location(ahrs.get_home())) {

21
        return false;

22
    }

23
#endif

24


25
    send_notification = true;

26
    _loitering = false;

27
    return true;

28
}

29


30
void ModeRTL::update()

31
{

32
    // determine if we should keep navigating

33
    if (!g2.wp_nav.reached_destination()) {

34
        // update navigation controller

35
        navigate_to_waypoint();

36
    } else {

37
        // send notification

38
        if (send_notification) {

39
            send_notification = false;

40
            gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");

41
        }

42


43
        // we have reached the destination

44
        // boats loiter, rovers stop

45
        if (!rover.is_boat()) {

46
            stop_vehicle();

47
        } else {

48
            // if not loitering yet, start loitering

49
            if (!_loitering) {

50
                _loitering = rover.mode_loiter.enter();

51
            }

52
            // update stop or loiter

53
            if (_loitering) {

54
                rover.mode_loiter.update();

55
            } else {

56
                stop_vehicle();

57
            }

58
        }

59


60
        // update distance to destination

61
        _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());

62
    }

63
}

64


65
// get desired location

66
bool ModeRTL::get_desired_location(Location& destination) const

67
{

68
    if (g2.wp_nav.is_destination_valid()) {

69
        destination = g2.wp_nav.get_oa_destination();

70
        return true;

71
    }

72
    return false;

73
}

74


75
bool ModeRTL::reached_destination() const

76
{

77
    return g2.wp_nav.reached_destination();

78
}

79


80
// set desired speed in m/s

81
bool ModeRTL::set_desired_speed(float speed)

82
{

83
    return g2.wp_nav.set_speed_max(speed);

84
}

85


86