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

2


3
void ModeSteering::update()

4
{

5
    // get speed forward

6
    float speed;

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

8
        // no valid speed so stop

9
        g2.motors.set_throttle(0.0f);

10
        g2.motors.set_steering(0.0f);

11
        _desired_lat_accel = 0.0f;

12
        return;

13
    }

14


15
    float desired_steering, desired_speed;

16
    get_pilot_desired_steering_and_speed(desired_steering, desired_speed);

17


18
    bool reversed = is_negative(desired_speed);

19


20
    // determine if pilot is requesting pivot turn

21
    if (g2.motors.have_skid_steering() && is_zero(desired_speed)) {

22
        // pivot turning using turn rate controller

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

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

25
        _desired_lat_accel = 0.0f;

26


27
        // run steering turn rate controller and throttle controller

28
        const float steering_out = attitude_control.get_steering_out_rate(target_turn_rate,

29
                                                                          g2.motors.limit.steer_left,

30
                                                                          g2.motors.limit.steer_right,

31
                                                                          rover.G_Dt);

32
        set_steering(steering_out * 4500.0f);

33
    } else {

34
        // In steering mode we control lateral acceleration directly.

35
        // For regular steering vehicles we use the maximum lateral acceleration

36
        //  at full steering lock for this speed: V^2/R where R is the radius of turn.

37
        float max_g_force = speed * speed / MAX(g2.turn_radius, 0.1f);

38
        max_g_force = constrain_float(max_g_force, 0.1f, attitude_control.get_turn_lat_accel_max());

39


40
        // convert pilot steering input to desired lateral acceleration

41
        _desired_lat_accel = max_g_force * (desired_steering / 4500.0f);

42


43
        // reverse target lateral acceleration if backing up

44
        if (reversed) {

45
            _desired_lat_accel = -_desired_lat_accel;

46
        }

47


48
        // run lateral acceleration to steering controller

49
        calc_steering_from_lateral_acceleration(_desired_lat_accel, reversed);

50
    }

51


52
    // run speed to throttle controller

53
    calc_throttle(desired_speed, true);

54
}

55


56