Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : /*************************************************************
       4             :  *  Attitude Rate controllers and timing
       5             :  ****************************************************************/
       6             : 
       7             : /*
       8             :   update rate controller when run from main thread (normal operation)
       9             : */
      10    30412448 : void Copter::run_rate_controller_main()
      11             : {
      12             :     // set attitude and position controller loop time
      13    30412448 :     const float last_loop_time_s = AP::scheduler().get_last_loop_time_s();
      14    30412448 :     pos_control->set_dt_s(last_loop_time_s);
      15    30412448 :     attitude_control->set_dt_s(last_loop_time_s);
      16             : 
      17    30412448 :     if (!using_rate_thread) {
      18    30297322 :         motors->set_dt_s(last_loop_time_s);
      19             :         // only run the rate controller if we are not using the rate thread
      20    30297322 :         attitude_control->rate_controller_run();
      21             :     }
      22             :     // reset sysid and other temporary inputs
      23    30412448 :     attitude_control->rate_controller_target_reset();
      24    30412448 : }
      25             : 
      26             : /*************************************************************
      27             :  *  throttle control
      28             :  ****************************************************************/
      29             : 
      30             : // update estimated throttle required to hover (if necessary)
      31             : //  called at 100hz
      32     7602701 : void Copter::update_throttle_hover()
      33             : {
      34             :     // if not armed or landed or on standby then exit
      35     7602701 :     if (!motors->armed() || ap.land_complete || standby_active) {
      36     5642654 :         return;
      37             :     }
      38             : 
      39             :     // do not update in manual throttle modes or Drift
      40     4600455 :     if (flightmode->has_manual_throttle() || (copter.flightmode->mode_number() == Mode::Number::DRIFT)) {
      41       83132 :         return;
      42             :     }
      43             : 
      44             :     // do not update while climbing or descending
      45     4517323 :     if (!is_zero(pos_control->get_vel_desired_U_ms())) {
      46     2557276 :         return;
      47             :     }
      48             : 
      49             :     // do not update if no vertical velocity estimate
      50             :     float vel_d_ms;
      51     1960047 :     if (!AP::ahrs().get_velocity_D(vel_d_ms, vibration_check.high_vibes)) {
      52           0 :         return;
      53             :     }
      54             : 
      55             :     // get throttle output
      56     1960047 :     float throttle = motors->get_throttle();
      57             : 
      58             :     // calc average throttle if we are in a level hover.  accounts for heli hover roll trim
      59     3916758 :     if ((throttle > 0.0f) && (fabsf(vel_d_ms) < 0.6) &&
      60     5876805 :         (fabsf(ahrs.get_roll_rad() - attitude_control->get_roll_trim_rad()) < radians(5)) && (fabsf(ahrs.get_pitch_rad()) < radians(5))) {
      61             :         // Can we set the time constant automatically
      62     1434604 :         motors->update_throttle_hover(0.01f);
      63             : #if HAL_GYROFFT_ENABLED
      64     1434604 :         gyro_fft.update_freq_hover(0.01f, motors->get_throttle_out());
      65             : #endif
      66             :     }
      67             : }
      68             : 
      69             : // get_pilot_desired_climb_rate_ms - transform pilot's throttle input to climb rate in m/s
      70             : // without any deadzone at the bottom.
      71             : // The max climb and descent rates are adjusted for any climb/descent rate currently being
      72             : // commanded by surface tracking, so full stick deflection always yields net vehicle climb
      73             : // or descent at PILOT_SPD_UP  / PILOT_SPD_DN  (see get_pilot_speed_up_adjusted_ms()).
      74     8006604 : float Copter::get_pilot_desired_climb_rate_ms()
      75             : {
      76             :     // throttle failsafe check
      77     8006604 :     if (!rc().has_valid_input()) {
      78       13681 :         return 0.0f;
      79             :     }
      80             : 
      81     7992923 :     float throttle_control = copter.channel_throttle->get_control_in();
      82             : 
      83             : #if TOY_MODE_ENABLED
      84             :     if (g2.toy_mode.enabled()) {
      85             :         // allow throttle to be reduced after throttle arming and for
      86             :         // slower descent close to the ground
      87             :         g2.toy_mode.throttle_adjust(throttle_control);
      88             :     }
      89             : #endif
      90             : 
      91             :     // ensure a reasonable throttle value
      92     7992923 :     throttle_control = constrain_float(throttle_control, 0.0f, 1000.0f);
      93             : 
      94             :     // ensure a reasonable deadzone
      95     7992923 :     g.throttle_deadzone.set(constrain_int16(g.throttle_deadzone, 0, 400));
      96             : 
      97     7992923 :     float desired_rate_ms = 0.0f;
      98     7992923 :     const float mid_stick = get_throttle_mid();
      99     7992923 :     const float deadband_top = mid_stick + g.throttle_deadzone;
     100     7992923 :     const float deadband_bottom = mid_stick - g.throttle_deadzone;
     101             : 
     102             :     // check throttle is above, below or in the deadband
     103     7992923 :     if (throttle_control < deadband_bottom) {
     104             :         // below the deadband
     105     1762650 :         desired_rate_ms = get_pilot_speed_dn_adjusted_ms() * (throttle_control - deadband_bottom) / deadband_bottom;
     106     6230273 :     } else if (throttle_control > deadband_top) {
     107             :         // above the deadband
     108     1756174 :         desired_rate_ms = get_pilot_speed_up_adjusted_ms() * (throttle_control - deadband_top) / (1000.0 - deadband_top);
     109             :     } else {
     110             :         // must be in the deadband
     111     4474099 :         desired_rate_ms = 0.0f;
     112             :     }
     113             : 
     114     7992923 :     return desired_rate_ms;
     115             : }
     116             : 
     117             : // get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff
     118      548756 : float Copter::get_non_takeoff_throttle()
     119             : {
     120      548756 :     return MAX(0,motors->get_throttle_hover() / 2.0);
     121             : }
     122             : 
     123             : // set_accel_throttle_I_from_pilot_throttle - smoothes transition from pilot controlled throttle to autopilot throttle
     124          57 : void Copter::set_accel_throttle_I_from_pilot_throttle()
     125             : {
     126             :     // get last throttle input sent to attitude controller
     127          57 :     float pilot_throttle = constrain_float(attitude_control->get_throttle_in(), 0.0, 1.0);
     128             :     // shift difference between pilot's throttle and hover throttle into accelerometer I
     129          57 :     pos_control->D_get_accel_pid().set_integrator(-(pilot_throttle - motors->get_throttle_hover()));
     130          57 : }
     131             : 
     132             : // It will return the PILOT_SPD_DN value if non zero, otherwise if zero it returns the PILOT_SPD_UP value.
     133    11545438 : float Copter::get_pilot_speed_dn_ms() const
     134             : {
     135    11545438 :     if (is_zero(g2.pilot_speed_dn_ms)) {
     136    11545438 :         return fabsf(g2.pilot_speed_up_ms);
     137             :     } else {
     138           0 :         return fabsf(g2.pilot_speed_dn_ms);
     139             :     }
     140             : }
     141             : 
     142             : // Returns the maximum pilot climb rate (m/s) adjusted for the climb/descent rate currently
     143             : // being commanded by surface tracking, to always result in at least PILOT_SPD_UP on full up stick.
     144     1756174 : float Copter::get_pilot_speed_up_adjusted_ms() const
     145             : {
     146             :     // terrain velocity in Up-positive frame (m/s)
     147     1756174 :     const float terrain_climb_ms = -pos_control->get_vel_terrain_D_ms();
     148             :     // floored at zero to prevent sign flip if terrain velocity exceeds PILOT_SPD_UP
     149     1756174 :     return MAX(0.0f, g2.pilot_speed_up_ms - terrain_climb_ms);
     150             : }
     151             : 
     152             : // Returns the maximum pilot descent rate (m/s, positive magnitude) adjusted for the
     153             : // climb/descent rate currently being commanded by surface tracking, to always result in
     154             : // at least PILOT_SPD_DN on full down stick.
     155     1762650 : float Copter::get_pilot_speed_dn_adjusted_ms() const
     156             : {
     157             :     // terrain velocity in Up-positive frame (m/s)
     158     1762650 :     const float terrain_climb_ms = -pos_control->get_vel_terrain_D_ms();
     159             :     // floored at zero to prevent sign flip if terrain velocity exceeds PILOT_SPD_DN
     160     1762650 :     return MAX(0.0f, get_pilot_speed_dn_ms() + terrain_climb_ms);
     161             : }