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

2


3
#if MODE_FOLLOW_ENABLED

4
// initialize follow mode

5
bool ModeFollow::_enter()

6
{

7
    if (!g2.follow.enabled()) {

8
        return false;

9
    }

10


11
    // initialise speed to waypoint speed

12
    _desired_speed = g2.wp_nav.get_default_speed();

13


14
    return true;

15
}

16


17
// exit handling

18
void ModeFollow::_exit()

19
{

20
    g2.follow.clear_offsets_if_required();

21
}

22


23
void ModeFollow::update()

24
{

25
    // stop vehicle if no speed estimate

26
    float speed;

27
    if (!attitude_control.get_forward_speed(speed)) {

28
        // no valid speed so stop

29
        g2.motors.set_throttle(0.0f);

30
        g2.motors.set_steering(0.0f);

31
        return;

32
    }

33


34
    Vector3f dist_vec;  // vector to lead vehicle

35
    Vector3f dist_vec_offs; // vector to lead vehicle + offset

36
    Vector3f vel_of_target; // velocity of lead vehicle

37


38
    // if no target simply stop the vehicle

39
    if (!g2.follow.get_target_dist_and_vel_ned(dist_vec, dist_vec_offs, vel_of_target)) {

40
        _reached_destination = true;

41
        stop_vehicle();

42
        return;

43
    }

44


45
    // calculate desired velocity vector

46
    Vector2f desired_velocity_ne;

47
    const float kp = g2.follow.get_pos_p().kP();

48
    desired_velocity_ne.x = vel_of_target.x + (dist_vec_offs.x * kp);

49
    desired_velocity_ne.y = vel_of_target.y + (dist_vec_offs.y * kp);

50


51
    // if desired velocity is zero stop vehicle

52
    if (is_zero(desired_velocity_ne.x) && is_zero(desired_velocity_ne.y)) {

53
        _reached_destination = true;

54
        stop_vehicle();

55
        return;

56
    }

57


58
    // we have not reached the target

59
    _reached_destination = false;

60


61
    // scale desired velocity to stay within horizontal speed limit

62
    float desired_speed = safe_sqrt(sq(desired_velocity_ne.x) + sq(desired_velocity_ne.y));

63
    if (!is_zero(desired_speed) && (desired_speed > _desired_speed)) {

64
        const float scalar_xy = _desired_speed / desired_speed;

65
        desired_velocity_ne *= scalar_xy;

66
        desired_speed = _desired_speed;

67
    }

68


69
    // calculate vehicle heading

70
    const float desired_yaw_cd = wrap_180_cd(atan2f(desired_velocity_ne.y, desired_velocity_ne.x) * DEGX100);

71


72
    // run steering and throttle controllers

73
    calc_steering_to_heading(desired_yaw_cd);

74
    calc_throttle(desired_speed, true);

75
}

76


77
// return desired heading (in degrees) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)

78
float ModeFollow::wp_bearing() const

79
{

80
    return g2.follow.get_bearing_to_target();

81
}

82


83
// return distance (in meters) to destination

84
float ModeFollow::get_distance_to_destination() const

85
{

86
    return g2.follow.get_distance_to_target();

87
}

88


89
// set desired speed in m/s

90
bool ModeFollow::set_desired_speed(float speed)

91
{

92
    if (is_negative(speed)) {

93
        return false;

94
    }

95
    _desired_speed = speed;

96
    return true;

97
}

98


99
#endif // MODE_FOLLOW_ENABLED

100


101