Line data    Source code

       1             : #include "Sub.h"
       2             : 
       3             : 
       4           1 : bool ModeAcro::init(bool ignore_checks) {
       5             :     // set target altitude to zero for reporting
       6           1 :     position_control->set_pos_desired_U_cm(0);
       7             : 
       8             :     // attitude hold inputs become thrust inputs in acro mode
       9             :     // set to neutral to prevent chaotic behavior (esp. roll/pitch)
      10           1 :     sub.set_neutral_controls();
      11             : 
      12           1 :     return true;
      13             : }
      14             : 
      15         799 : void ModeAcro::run()
      16             : {
      17             :     float target_roll, target_pitch, target_yaw;
      18             : 
      19             :     // if not armed set throttle to zero and exit immediately
      20         799 :     if (!motors.armed()) {
      21         439 :         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
      22         439 :         attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
      23         439 :         attitude_control->relax_attitude_controllers();
      24         439 :         return;
      25             :     }
      26             : 
      27         360 :     motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
      28             : 
      29             :     // convert the input to the desired body frame rate
      30         360 :     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);
      31             : 
      32             :     // run attitude controller
      33         360 :     attitude_control->input_rate_bf_roll_pitch_yaw_cds(target_roll, target_pitch, target_yaw);
      34             : 
      35             :     // output pilot's throttle without angle boost
      36         360 :     attitude_control->set_throttle_out((channel_throttle->norm_input() + 1.0f) / 2.0f, false, g.throttle_filt);
      37             : 
      38             :     //control_in is range 0-1000
      39             :     //radio_in is raw pwm value
      40         360 :     motors.set_forward(channel_forward->norm_input());
      41         360 :     motors.set_lateral(channel_lateral->norm_input());
      42             : }