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

2


3
void ModeAcro::update()

4
{

5
    // get speed forward

6
    float speed, desired_steering;

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

8
        float desired_throttle;

9
        // convert pilot stick input into desired steering and throttle

10
        get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);

11


12
        // if vehicle is balance bot, calculate actual throttle required for balancing

13
        if (rover.is_balancebot()) {

14
            rover.balancebot_pitch_control(desired_throttle);

15
        }

16


17
        // no valid speed, just use the provided throttle

18
        g2.motors.set_throttle(desired_throttle);

19
    } else {

20
        float desired_speed;

21
        // convert pilot stick input into desired steering and speed

22
        get_pilot_desired_steering_and_speed(desired_steering, desired_speed);

23
        calc_throttle(desired_speed, true);

24
    }

25


26
    float steering_out;

27


28
    // handle sailboats

29
    if (!is_zero(desired_steering)) {

30
        // steering input return control to user

31
        g2.sailboat.clear_tack();

32
    }

33
    if (g2.sailboat.tacking()) {

34
        // call heading controller during tacking

35


36
        steering_out = attitude_control.get_steering_out_heading(g2.sailboat.get_tack_heading_rad(),

37
                                                                 g2.wp_nav.get_pivot_rate(),

38
                                                                 g2.motors.limit.steer_left,

39
                                                                 g2.motors.limit.steer_right,

40
                                                                 rover.G_Dt);

41
    } else {

42
        // convert pilot steering input to desired turn rate in radians/sec

43
        const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);

44


45
        // run steering turn rate controller and throttle controller

46
        steering_out = attitude_control.get_steering_out_rate(target_turn_rate,

47
                                                              g2.motors.limit.steer_left,

48
                                                              g2.motors.limit.steer_right,

49
                                                              rover.G_Dt);

50
    }

51


52
    set_steering(steering_out * 4500.0f);

53
}

54


55
bool ModeAcro::requires_velocity() const

56
{

57
    return !g2.motors.have_skid_steering();

58
}

59


60
// sailboats in acro mode support user manually initiating tacking from transmitter

61
void ModeAcro::handle_tack_request()

62
{

63
    g2.sailboat.handle_tack_request_acro();

64
}

65


66