Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #include "mode.h"
       4             : 
       5             : #if MODE_ACRO_ENABLED
       6             : 
       7             : /*
       8             :  * Init and run calls for acro flight mode
       9             :  */
      10       14963 : void ModeAcro::run()
      11             : {
      12             :     // convert the input to the desired body frame rate
      13             :     float target_roll_rads, target_pitch_rads, target_yaw_rads;
      14       14963 :     get_pilot_desired_rates_rads(target_roll_rads, target_pitch_rads, target_yaw_rads);
      15             : 
      16             :     // Determine desired spool state based on pilot throttle input.
      17             :     // The setter enforces that disarmed aircraft are held at SHUT_DOWN until armed.
      18       14963 :     if (copter.ap.throttle_zero) {
      19       13411 :         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
      20             :     } else {
      21        1552 :         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
      22             :     }
      23             : 
      24       14963 :     float pilot_desired_throttle = get_pilot_desired_throttle();
      25             : 
      26       14963 :     switch (motors->get_spool_state()) {
      27       10568 :     case AP_Motors::SpoolState::SHUT_DOWN:
      28             :         // Motors Stopped
      29       10568 :         attitude_control->reset_target_and_rate(true);
      30       10568 :         attitude_control->reset_rate_controller_I_terms();
      31       10568 :         pilot_desired_throttle = 0.0f;
      32       10568 :         break;
      33             : 
      34        4395 :     case AP_Motors::SpoolState::GROUND_IDLE:
      35             :         // Landed
      36        4395 :         attitude_control->reset_target_and_rate();
      37        4395 :         attitude_control->reset_rate_controller_I_terms_smoothly();
      38        4395 :         pilot_desired_throttle = 0.0f;
      39        4395 :         break;
      40             : 
      41           0 :     case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
      42             :         // clear landing flag above zero throttle
      43           0 :         if (!motors->limit.throttle_lower) {
      44           0 :             set_land_complete(false);
      45             :         }
      46           0 :         break;
      47             : 
      48           0 :     case AP_Motors::SpoolState::SPOOLING_UP:
      49             :     case AP_Motors::SpoolState::SPOOLING_DOWN:
      50             :         // do nothing
      51           0 :         break;
      52             :     }
      53             : 
      54             :     // run attitude controller
      55       14963 :     if (g2.acro_options.get() & uint8_t(AcroOptions::RATE_LOOP_ONLY)) {
      56             :         // scale I by the value of angle P to mimic betaflight tunes
      57           0 :         attitude_control->scale_I_to_angle_P();
      58             : 
      59             :         // send rate commands to attitude controller (RATE_LOOP_ONLY bypasses full attitude stabilization)
      60           0 :         attitude_control->input_rate_bf_roll_pitch_yaw_2_rads(target_roll_rads, target_pitch_rads, target_yaw_rads);
      61             :     } else {
      62             :         // send rate commands to attitude controller with attitude stabilization
      63       14963 :         attitude_control->input_rate_bf_roll_pitch_yaw_rads(target_roll_rads, target_pitch_rads, target_yaw_rads);
      64             :     }
      65             : 
      66             :     // output pilot's throttle without angle boost
      67       14963 :     attitude_control->set_throttle_out(pilot_desired_throttle, false, copter.g.throttle_filt);
      68       14963 : }
      69             : 
      70           8 : bool ModeAcro::init(bool ignore_checks)
      71             : {
      72           8 :     if (g2.acro_options.get() & uint8_t(AcroOptions::AIR_MODE)) {
      73           0 :         disable_air_mode_reset = false;
      74           0 :         copter.air_mode = AirMode::AIRMODE_ENABLED;
      75             :     }
      76             : 
      77           8 :     return true;
      78             : }
      79             : 
      80           9 : void ModeAcro::exit()
      81             : {
      82           9 :     if (!disable_air_mode_reset && (g2.acro_options.get() & uint8_t(AcroOptions::AIR_MODE))) {
      83           0 :         copter.air_mode = AirMode::AIRMODE_DISABLED;
      84             :     }
      85           9 :     disable_air_mode_reset = false;
      86           9 : }
      87             : 
      88          15 : void ModeAcro::air_mode_aux_changed()
      89             : {
      90          15 :     disable_air_mode_reset = true;
      91          15 : }
      92             : 
      93       14965 : float ModeAcro::throttle_hover() const
      94             : {
      95       14965 :     if (is_positive(g2.acro_thr_mid)) {
      96           0 :         return g2.acro_thr_mid;
      97             :     }
      98       14965 :     return Mode::throttle_hover();
      99             : }
     100             : 
     101             : // return desired angular rates (radians/second) created from pilot inputs
     102       16163 : void ModeAcro::get_pilot_desired_rates_rads(float &roll_out_rads, float &pitch_out_rads, float &yaw_out_rads)
     103             : {
     104             :     float rate_delta_max_rads;
     105       16163 :     Vector3f rate_ef_level_rads, rate_bf_level_rads, rate_bf_request_rads;
     106             : 
     107       16163 :     float roll_in_norm = channel_roll->norm_input_dz();
     108       16163 :     float pitch_in_norm = channel_pitch->norm_input_dz();
     109       16163 :     const float yaw_in_norm = channel_yaw->norm_input_dz();
     110             : 
     111             :     // apply circular limit to pitch and roll inputs
     112       16163 :     float norm_in_length = norm(pitch_in_norm, roll_in_norm);
     113             : 
     114       16163 :     if (norm_in_length > 1.0) {
     115           0 :         float ratio = 1.0 / norm_in_length;
     116           0 :         roll_in_norm *= ratio;
     117           0 :         pitch_in_norm *= ratio;
     118             :     }
     119             : 
     120             :     // calculate roll, pitch rate requests
     121             : 
     122             :     // roll rate request with input expo applied
     123       16163 :     rate_bf_request_rads.x = radians(g2.command_model_acro_rp.get_rate()) * input_expo(roll_in_norm, g2.command_model_acro_rp.get_expo());
     124             : 
     125             :     // pitch rate request with input expo applied
     126       16163 :     rate_bf_request_rads.y = radians(g2.command_model_acro_rp.get_rate()) * input_expo(pitch_in_norm, g2.command_model_acro_rp.get_expo());
     127             : 
     128             :     // yaw rate request with input expo applied
     129       16163 :     rate_bf_request_rads.z = radians(g2.command_model_acro_y.get_rate()) * input_expo(yaw_in_norm, g2.command_model_acro_y.get_expo());
     130             : 
     131             :     // calculate earth frame rate corrections to pull the copter back to level while in ACRO mode
     132             : 
     133       16163 :     if (g.acro_trainer != (uint8_t)Trainer::OFF) {
     134             : 
     135             :         // get attitude targets
     136       16163 :         const Vector3f att_target_euler_rad = attitude_control->get_att_target_euler_rad();
     137             : 
     138             :         // Calculate trainer mode earth frame rate command for roll
     139       16163 :         float roll_angle_rad = wrap_PI(att_target_euler_rad.x);
     140       16163 :         rate_ef_level_rads.x = -constrain_float(roll_angle_rad, -ACRO_LEVEL_MAX_ANGLE_RAD, ACRO_LEVEL_MAX_ANGLE_RAD) * g.acro_balance_roll;
     141             : 
     142             :         // Calculate trainer mode earth frame rate command for pitch
     143       16163 :         float pitch_angle_rad = wrap_PI(att_target_euler_rad.y);
     144       16163 :         rate_ef_level_rads.y = -constrain_float(pitch_angle_rad, -ACRO_LEVEL_MAX_ANGLE_RAD, ACRO_LEVEL_MAX_ANGLE_RAD) * g.acro_balance_pitch;
     145             : 
     146             :         // Calculate trainer mode earth frame rate command for yaw
     147       16163 :         rate_ef_level_rads.z = 0;
     148             : 
     149             :         // Calculate angle limiting earth frame rate commands
     150       16163 :         if (g.acro_trainer == (uint8_t)Trainer::LIMITED) {
     151       16163 :             const float angle_max_rad = attitude_control->lean_angle_max_rad();
     152       16163 :             if (roll_angle_rad > angle_max_rad) {
     153           0 :                 rate_ef_level_rads.x += sqrt_controller(angle_max_rad - roll_angle_rad, radians(g2.command_model_acro_rp.get_rate()) / ACRO_LEVEL_MAX_OVERSHOOT_RAD, attitude_control->get_accel_roll_max_radss(), G_Dt);
     154       16163 :             } else if (roll_angle_rad < -angle_max_rad) {
     155           0 :                 rate_ef_level_rads.x += sqrt_controller(-angle_max_rad - roll_angle_rad, radians(g2.command_model_acro_rp.get_rate()) / ACRO_LEVEL_MAX_OVERSHOOT_RAD, attitude_control->get_accel_roll_max_radss(), G_Dt);
     156             :             }
     157             : 
     158       16163 :             if (pitch_angle_rad > angle_max_rad) {
     159           0 :                 rate_ef_level_rads.y += sqrt_controller(angle_max_rad - pitch_angle_rad, radians(g2.command_model_acro_rp.get_rate()) / ACRO_LEVEL_MAX_OVERSHOOT_RAD, attitude_control->get_accel_pitch_max_radss(), G_Dt);
     160       16163 :             } else if (pitch_angle_rad < -angle_max_rad) {
     161           0 :                 rate_ef_level_rads.y += sqrt_controller(-angle_max_rad - pitch_angle_rad, radians(g2.command_model_acro_rp.get_rate()) / ACRO_LEVEL_MAX_OVERSHOOT_RAD, attitude_control->get_accel_pitch_max_radss(), G_Dt);
     162             :             }
     163             :         }
     164             : 
     165             :         // convert earth-frame level rates to body-frame level rates
     166       16163 :         attitude_control->euler_derivative_to_body(attitude_control->get_attitude_target_quat(), rate_ef_level_rads, rate_bf_level_rads);
     167             : 
     168             :         // combine earth frame rate corrections with rate requests
     169       16163 :         if (g.acro_trainer == (uint8_t)Trainer::LIMITED) {
     170       16163 :             rate_bf_request_rads.x += rate_bf_level_rads.x;
     171       16163 :             rate_bf_request_rads.y += rate_bf_level_rads.y;
     172       16163 :             rate_bf_request_rads.z += rate_bf_level_rads.z;
     173             :         } else {
     174           0 :             float acro_level_mix = constrain_float(1-float(MAX(MAX(abs(roll_in_norm), abs(pitch_in_norm)), abs(yaw_in_norm))), 0, 1) * ahrs.cos_pitch();
     175             : 
     176             :             // Scale levelling rates by stick input
     177           0 :             rate_bf_level_rads = rate_bf_level_rads * acro_level_mix;
     178             : 
     179             :             // Calculate the maximum allowed change in rate to prevent reversal through inverted
     180           0 :             rate_delta_max_rads = fabsf(fabsf(rate_bf_request_rads.x)-fabsf(rate_bf_level_rads.x));
     181           0 :             rate_bf_request_rads.x += rate_bf_level_rads.x;
     182           0 :             rate_bf_request_rads.x = constrain_float(rate_bf_request_rads.x, -rate_delta_max_rads, rate_delta_max_rads);
     183             : 
     184             :             // Calculate the maximum allowed change in rate to prevent reversal through inverted
     185           0 :             rate_delta_max_rads = fabsf(fabsf(rate_bf_request_rads.y)-fabsf(rate_bf_level_rads.y));
     186           0 :             rate_bf_request_rads.y += rate_bf_level_rads.y;
     187           0 :             rate_bf_request_rads.y = constrain_float(rate_bf_request_rads.y, -rate_delta_max_rads, rate_delta_max_rads);
     188             : 
     189             :             // Calculate the maximum allowed change in rate to prevent reversal through inverted
     190           0 :             rate_delta_max_rads = fabsf(fabsf(rate_bf_request_rads.z)-fabsf(rate_bf_level_rads.z));
     191           0 :             rate_bf_request_rads.z += rate_bf_level_rads.z;
     192           0 :             rate_bf_request_rads.z = constrain_float(rate_bf_request_rads.z, -rate_delta_max_rads, rate_delta_max_rads);
     193             :         }
     194             :     }
     195             : 
     196             :     // hand back rate request
     197       16163 :     roll_out_rads = rate_bf_request_rads.x;
     198       16163 :     pitch_out_rads = rate_bf_request_rads.y;
     199       16163 :     yaw_out_rads = rate_bf_request_rads.z;
     200       16163 : }
     201             : #endif