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_cruise.cpp
Views: 1798
1
#include "mode.h"

2
#include "Plane.h"

3


4
bool ModeCruise::_enter()

5
{

6
    locked_heading = false;

7
    lock_timer_ms = 0;

8


9
#if HAL_SOARING_ENABLED

10
    // for ArduSoar soaring_controller

11
    plane.g2.soaring_controller.init_cruising();

12
#endif

13


14
    plane.set_target_altitude_current();

15


16
    return true;

17
}

18


19
void ModeCruise::update()

20
{

21
    /*

22
      in CRUISE mode we use the navigation code to control

23
      roll when heading is locked. Heading becomes unlocked on

24
      any aileron or rudder input

25
    */

26
    if (plane.channel_roll->get_control_in() != 0 || plane.channel_rudder->get_control_in() != 0) {

27
        locked_heading = false;

28
        lock_timer_ms = 0;

29
    }

30


31
#if AP_SCRIPTING_ENABLED

32
    if (plane.nav_scripting_active()) {

33
        // while a trick is running unlock heading and zero altitude offset

34
        locked_heading = false;

35
        lock_timer_ms = 0;

36
        plane.set_target_altitude_current();

37
    }

38
#endif

39
    

40
    if (!locked_heading) {

41
        plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd;

42
        plane.update_load_factor();

43
    } else {

44
        plane.calc_nav_roll();

45
    }

46
    plane.update_fbwb_speed_height();

47
}

48


49
/*

50
  handle CRUISE mode, locking heading to GPS course when we have

51
  sufficient ground speed, and no aileron or rudder input

52
 */

53
void ModeCruise::navigate()

54
{

55
#if AP_SCRIPTING_ENABLED

56
    if (plane.nav_scripting_active()) {

57
        // don't try to navigate while running trick

58
        return;

59
    }

60
#endif

61


62
    // check if we are moving in the direction of the front of the vehicle

63
    const int32_t ground_course_cd = plane.gps.ground_course_cd();

64
    const bool moving_forwards = fabsf(wrap_PI(radians(ground_course_cd * 0.01) - plane.ahrs.get_yaw())) < M_PI_2;

65


66
    if (!locked_heading &&

67
        plane.channel_roll->get_control_in() == 0 &&

68
        plane.rudder_input() == 0 &&

69
        plane.gps.status() >= AP_GPS::GPS_OK_FIX_2D &&

70
        plane.gps.ground_speed() >= 3 &&

71
        moving_forwards &&

72
        lock_timer_ms == 0) {

73
        // user wants to lock the heading - start the timer

74
        lock_timer_ms = millis();

75
    }

76
    if (lock_timer_ms != 0 &&

77
        (millis() - lock_timer_ms) > 500) {

78
        // lock the heading after 0.5 seconds of zero heading input

79
        // from user

80
        locked_heading = true;

81
        lock_timer_ms = 0;

82
        locked_heading_cd = ground_course_cd;

83
        plane.prev_WP_loc = plane.current_loc;

84
    }

85
    if (locked_heading) {

86
        plane.next_WP_loc = plane.prev_WP_loc;

87
        // always look 1km ahead

88
        plane.next_WP_loc.offset_bearing(locked_heading_cd*0.01f, plane.prev_WP_loc.get_distance(plane.current_loc) + 1000);

89
        plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);

90
    }

91
}

92


93
bool ModeCruise::get_target_heading_cd(int32_t &target_heading) const

94
{

95
    target_heading = locked_heading_cd;

96
    return locked_heading;

97
}

98


99