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/ArduSub/mode_stabilize.cpp
Views: 1798
1
#include "Sub.h"

2


3


4
bool ModeStabilize::init(bool ignore_checks) {

5
    // set target altitude to zero for reporting

6
    position_control->set_pos_desired_z_cm(0);

7
    sub.last_pilot_heading = ahrs.yaw_sensor;

8


9
    return true;

10
    return true;

11
}

12


13
void ModeStabilize::run()

14
{

15
  uint32_t tnow = AP_HAL::millis();

16
    float target_roll, target_pitch;

17


18
    // if not armed set throttle to zero and exit immediately

19
    if (!motors.armed()) {

20
        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);

21
        attitude_control->set_throttle_out(0,true,g.throttle_filt);

22
        attitude_control->relax_attitude_controllers();

23
        sub.last_pilot_heading = ahrs.yaw_sensor;

24
        return;

25
    }

26


27
    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

28


29
    // convert pilot input to lean angles

30
    // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats

31
    // TODO2: move into mode.h

32
    sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max);

33


34
    // get pilot's desired yaw rate

35
    float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim());

36
    float target_yaw_rate = sub.get_pilot_desired_yaw_rate(yaw_input);

37


38
    // call attitude controller

39
    // update attitude controller targets

40


41
    if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input

42
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);

43
        sub.last_pilot_heading = ahrs.yaw_sensor;

44
        sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading

45


46
    } else { // hold current heading

47


48
        // this check is required to prevent bounce back after very fast yaw maneuvers

49
        // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped

50
        if (tnow < sub.last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading

51
            target_yaw_rate = 0;  // Stop rotation on yaw axis

52


53
            // call attitude controller with target yaw rate = 0 to decelerate on yaw axis

54
            attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);

55
            sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold

56


57
        } else { // call attitude controller holding absolute absolute bearing

58
            attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true);

59
        }

60
    }

61


62
    // output pilot's throttle

63
    attitude_control->set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);

64


65
    //control_in is range -1000-1000

66
    //radio_in is raw pwm value

67
    motors.set_forward(channel_forward->norm_input());

68
    motors.set_lateral(channel_lateral->norm_input());

69
}

70


71