Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : /*
       4             :   autotune mode is a wrapper around the AC_AutoTune library
       5             :  */
       6             : 
       7             : #if AUTOTUNE_ENABLED
       8             : 
       9          45 : bool AutoTune::init()
      10             : {
      11             :     // only allow AutoTune from some flight modes, for example Stabilize, AltHold,  PosHold or Loiter modes
      12          45 :     if (!copter.flightmode->allows_autotune()) {
      13          32 :         return false;
      14             :     }
      15             : 
      16             :     // ensure throttle is above zero
      17          13 :     if (copter.ap.throttle_zero) {
      18           1 :         return false;
      19             :     }
      20             : 
      21             :     // ensure we are flying
      22          12 :     if (!copter.motors->armed() || !copter.ap.auto_armed || copter.ap.land_complete) {
      23           0 :         return false;
      24             :     }
      25             : 
      26             :     // use position hold while tuning if we were in QLOITER
      27          12 :     bool position_hold = (copter.flightmode->mode_number() == Mode::Number::LOITER || copter.flightmode->mode_number() == Mode::Number::POSHOLD);
      28             : 
      29          12 :     return init_internals(position_hold,
      30             :                           copter.attitude_control,
      31             :                           copter.pos_control,
      32          12 :                           copter.ahrs_view);
      33             : }
      34             : 
      35     1694072 : void AutoTune::run()
      36             : {
      37             :     // disarm when the landing detector says we've landed and spool state is ground idle
      38     1694072 :     if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
      39           7 :         copter.arming.disarm(AP_Arming::Method::LANDED);
      40             :     }
      41             : 
      42             :     // if not armed set throttle to zero and exit immediately
      43     1694072 :     if (copter.ap.land_complete) {
      44        7428 :         copter.flightmode->make_safe_ground_handling();
      45        7428 :         return;
      46             :     }
      47             : 
      48             :     // run autotune mode
      49     1686644 :     AC_AutoTune::run();
      50             : }
      51             : 
      52             : 
      53             : /*
      54             :   get stick input climb rate
      55             :  */
      56     1663380 : float AutoTune::get_desired_climb_rate_ms(void) const
      57             : {
      58     1663380 :     float target_climb_rate_ms = copter.get_pilot_desired_climb_rate_ms();
      59             : 
      60             :     // get avoidance adjusted climb rate
      61     1663380 :     target_climb_rate_ms = copter.mode_autotune.get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
      62             : 
      63     1663380 :     return target_climb_rate_ms;
      64             : }
      65             : 
      66             : /*
      67             :   get stick roll, pitch and yaw rate
      68             :  */
      69     1663380 : void AutoTune::get_pilot_desired_rp_yrate_rad(float &des_roll_rad, float &des_pitch_rad, float &des_yaw_rate_rads)
      70             : {
      71     1663380 :     copter.mode_autotune.get_pilot_desired_lean_angles_rad(des_roll_rad, des_pitch_rad, attitude_control->lean_angle_max_rad(),
      72     1663380 :                                                        copter.attitude_control->get_althold_lean_angle_max_rad());
      73     1663380 :     des_yaw_rate_rads = copter.mode_autotune.get_pilot_desired_yaw_rate_rads();
      74     1663380 : }
      75             : 
      76             : /*
      77             :   setup z controller velocity and accel limits
      78             :  */
      79     1686656 : void AutoTune::init_z_limits()
      80             : {
      81             :     // set vertical speed and acceleration limits
      82     1686656 :     copter.pos_control->D_set_max_speed_accel_m(copter.flightmode->get_pilot_speed_dn_ms(), copter.flightmode->get_pilot_speed_up_ms(), copter.flightmode->get_pilot_accel_D_mss());
      83     1686656 :     copter.pos_control->D_set_correction_speed_accel_m(copter.flightmode->get_pilot_speed_dn_ms(), copter.flightmode->get_pilot_speed_up_ms(), copter.flightmode->get_pilot_accel_D_mss());
      84     1686656 : }
      85             : 
      86             : #if HAL_LOGGING_ENABLED
      87      813535 : void AutoTune::log_pids()
      88             : {
      89      813535 :     copter.logger.Write_PID(LOG_PIDR_MSG, copter.attitude_control->get_rate_roll_pid().get_pid_info());
      90      813535 :     copter.logger.Write_PID(LOG_PIDP_MSG, copter.attitude_control->get_rate_pitch_pid().get_pid_info());
      91      813535 :     copter.logger.Write_PID(LOG_PIDY_MSG, copter.attitude_control->get_rate_yaw_pid().get_pid_info());
      92      813535 : }
      93             : #endif
      94             : 
      95             : /*
      96             :   check if we have a good position estimate
      97             :  */
      98      357524 : bool AutoTune::position_ok()
      99             : {
     100      357524 :     return copter.position_ok();
     101             : }
     102             : 
     103             : /*
     104             :   initialise autotune mode
     105             : */
     106          45 : bool ModeAutoTune::init(bool ignore_checks)
     107             : {
     108          45 :     return autotune.init();
     109             : }
     110             : 
     111     1694072 : void ModeAutoTune::run()
     112             : {
     113     1694072 :     autotune.run();
     114     1694072 : }
     115             : 
     116          11 : void ModeAutoTune::exit()
     117             : {
     118          11 :     autotune.stop();
     119          11 : }
     120             : 
     121             : #endif  // AUTOTUNE_ENABLED