ArduCopter/mode_acro_heli.cpp

   1 #include "Copter.h"
   2
   3 #if MODE_ACRO_ENABLED
   4
   5 #if FRAME_CONFIG == HELI_FRAME
   6 /*
   7  * Init and run calls for acro flight mode for trad heli
   8  */
   9
  10 // heli_acro_init - initialise acro controller
  11 bool ModeAcro_Heli::init(bool ignore_checks)
  12 {
  13     // if heli is equipped with a flybar, then tell the attitude controller to pass through controls directly to servos
  14     attitude_control->use_flybar_passthrough(motors->has_flybar(), motors->supports_yaw_passthrough());
  15
  16     motors->set_acro_tail(true);
  17
  18     // set stab collective false to use full collective pitch range
  19     copter.input_manager.set_use_stab_col(false);
  20
  21     // always successfully enter acro
  22     return true;
  23 }
  24
  25 // heli_acro_run - runs the acro controller
  26 // should be called at 100hz or more
  27 void ModeAcro_Heli::run()
  28 {
  29     float target_roll, target_pitch, target_yaw;
  30     float pilot_throttle_scaled;
  31
  32     // Tradheli should not reset roll, pitch, yaw targets when motors are not runup while flying, because
  33     // we may be in autorotation flight.  This is so that the servos move in a realistic fashion while disarmed
  34     // for operational checks. Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero
  35     // so the swash servos move.
  36
  37     if (!motors->armed()) {
  38         // Motors should be Stopped
  39         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
  40     } else {
  41         // heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled
  42         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
  43     }
  44
  45     switch (motors->get_spool_state()) {
  46     case AP_Motors::SpoolState::SHUT_DOWN:
  47         // Motors Stopped
  48         attitude_control->reset_target_and_rate(false);
  49         attitude_control->reset_rate_controller_I_terms();
  50         break;
  51     case AP_Motors::SpoolState::GROUND_IDLE:
  52         // If aircraft is landed, set target heading to current and reset the integrator
  53         // Otherwise motors could be at ground idle for practice autorotation
  54         if ((motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) {
  55             attitude_control->reset_target_and_rate(false);
  56             attitude_control->reset_rate_controller_I_terms_smoothly();
  57         }
  58         break;
  59     case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
  60         if (copter.ap.land_complete && !motors->using_leaky_integrator()) {
  61             attitude_control->reset_rate_controller_I_terms_smoothly();
  62         }
  63         break;
  64     case AP_Motors::SpoolState::SPOOLING_UP:
  65     case AP_Motors::SpoolState::SPOOLING_DOWN:
  66         // do nothing
  67         break;
  68     }
  69
  70     if (!motors->has_flybar()){
  71         // convert the input to the desired body frame rate
  72         get_pilot_desired_angle_rates(channel_roll->norm_input_dz(), channel_pitch->norm_input_dz(), channel_yaw->norm_input_dz(), target_roll, target_pitch, target_yaw);
  73         // only mimic flybar response when trainer mode is disabled
  74         if ((Trainer)g.acro_trainer.get() == Trainer::OFF) {
  75             // while landed always leak off target attitude to current attitude
  76             if (copter.ap.land_complete) {
  77                 virtual_flybar(target_roll, target_pitch, target_yaw, 3.0f, 3.0f);
  78             // while flying use acro balance parameters for leak rate
  79             } else {
  80                 virtual_flybar(target_roll, target_pitch, target_yaw, g.acro_balance_pitch, g.acro_balance_roll);
  81             }
  82         }
  83         if (motors->supports_yaw_passthrough()) {
  84             // if the tail on a flybar heli has an external gyro then
  85             // also use no deadzone for the yaw control and
  86             // pass-through the input direct to output.
  87             target_yaw = channel_yaw->get_control_in_zero_dz();
  88         }
  89
  90         // run attitude controller
  91         if (g2.acro_options.get() & uint8_t(AcroOptions::RATE_LOOP_ONLY)) {
  92             attitude_control->input_rate_bf_roll_pitch_yaw_2(target_roll, target_pitch, target_yaw);
  93         } else {
  94             attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
  95         }
  96     }else{
  97         /*
  98           for fly-bar passthrough use control_in values with no
  99           deadzone. This gives true pass-through.
 100          */
 101         float roll_in = channel_roll->get_control_in_zero_dz();
 102         float pitch_in = channel_pitch->get_control_in_zero_dz();
 103         float yaw_in;
 104
 105         if (motors->supports_yaw_passthrough()) {
 106             // if the tail on a flybar heli has an external gyro then
 107             // also use no deadzone for the yaw control and
 108             // pass-through the input direct to output.
 109             yaw_in = channel_yaw->get_control_in_zero_dz();
 110         } else {
 111             // if there is no external gyro then run the usual
 112             // ACRO_YAW_P gain on the input control, including
 113             // deadzone
 114             yaw_in = get_pilot_desired_yaw_rate();
 115         }
 116
 117         // run attitude controller
 118         attitude_control->passthrough_bf_roll_pitch_rate_yaw(roll_in, pitch_in, yaw_in);
 119     }
 120
 121     // get pilot's desired throttle
 122     pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
 123
 124     // output pilot's throttle without angle boost
 125     attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
 126 }
 127
 128
 129 // virtual_flybar - acts like a flybar by leaking target atttitude back to current attitude
 130 void ModeAcro_Heli::virtual_flybar( float &roll_out, float &pitch_out, float &yaw_out, float pitch_leak, float roll_leak)
 131 {
 132     Vector3f rate_ef_level, rate_bf_level;
 133
 134     // get attitude targets
 135     const Vector3f att_target = attitude_control->get_att_target_euler_cd();
 136
 137     // Calculate earth frame rate command for roll leak to current attitude
 138     rate_ef_level.x = -wrap_180_cd(att_target.x - ahrs.roll_sensor) * roll_leak;
 139
 140     // Calculate earth frame rate command for pitch leak to current attitude
 141     rate_ef_level.y = -wrap_180_cd(att_target.y - ahrs.pitch_sensor) * pitch_leak;
 142
 143     // Calculate earth frame rate command for yaw
 144     rate_ef_level.z = 0;
 145
 146     // convert earth-frame leak rates to body-frame leak rates
 147     attitude_control->euler_rate_to_ang_vel(attitude_control->get_attitude_target_quat(), rate_ef_level, rate_bf_level);
 148
 149     // combine earth frame rate corrections with rate requests
 150     roll_out += rate_bf_level.x;
 151     pitch_out += rate_bf_level.y;
 152     yaw_out += rate_bf_level.z;
 153
 154 }
 155 #endif  //HELI_FRAME
 156 #endif  //MODE_ACRO_ENABLED