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

2


3


4


5
#include "Sub.h"

6


7


8
bool ModeAcro::init(bool ignore_checks) {

9
    // set target altitude to zero for reporting

10
    position_control->set_pos_desired_z_cm(0);

11


12
    // attitude hold inputs become thrust inputs in acro mode

13
    // set to neutral to prevent chaotic behavior (esp. roll/pitch)

14
    sub.set_neutral_controls();

15


16
    return true;

17
}

18


19
void ModeAcro::run()

20
{

21
    float target_roll, target_pitch, target_yaw;

22


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

24
    if (!motors.armed()) {

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

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

27
        attitude_control->relax_attitude_controllers();

28
        return;

29
    }

30


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

32


33
    // convert the input to the desired body frame rate

34
    get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);

35


36
    // run attitude controller

37
    attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);

38


39
    // output pilot's throttle without angle boost

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

41


42
    //control_in is range 0-1000

43
    //radio_in is raw pwm value

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

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

46
}

47


48