ArduCopter/mode_autotune.cpp

   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 bool AutoTune::init()
  10 {
  11     // only allow AutoTune from some flight modes, for example Stabilize, AltHold,  PosHold or Loiter modes
  12     if (!copter.flightmode->allows_autotune()) {
  13         return false;
  14     }
  15
  16     // ensure throttle is above zero
  17     if (copter.ap.throttle_zero) {
  18         return false;
  19     }
  20
  21     // ensure we are flying
  22     if (!copter.motors->armed() || !copter.ap.auto_armed || copter.ap.land_complete) {
  23         return false;
  24     }
  25
  26     // use position hold while tuning if we were in QLOITER
  27     bool position_hold = (copter.flightmode->mode_number() == Mode::Number::LOITER || copter.flightmode->mode_number() == Mode::Number::POSHOLD);
  28
  29     return init_internals(position_hold,
  30                           copter.attitude_control,
  31                           copter.pos_control,
  32                           copter.ahrs_view,
  33                           &copter.inertial_nav);
  34 }
  35
  36 void AutoTune::run()
  37 {
  38     // apply SIMPLE mode transform to pilot inputs
  39     copter.update_simple_mode();
  40
  41     // disarm when the landing detector says we've landed and spool state is ground idle
  42     if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
  43         copter.arming.disarm(AP_Arming::Method::LANDED);
  44     }
  45
  46     // if not armed set throttle to zero and exit immediately
  47     if (copter.ap.land_complete) {
  48         copter.flightmode->make_safe_ground_handling();
  49         return;
  50     }
  51
  52     // run autotune mode
  53     AC_AutoTune::run();
  54
  55 }
  56
  57
  58 /*
  59   get stick input climb rate
  60  */
  61 float AutoTune::get_pilot_desired_climb_rate_cms(void) const
  62 {
  63     float target_climb_rate = copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in());
  64
  65     // get avoidance adjusted climb rate
  66     target_climb_rate = copter.mode_autotune.get_avoidance_adjusted_climbrate(target_climb_rate);
  67
  68     return target_climb_rate;
  69 }
  70
  71 /*
  72   get stick roll, pitch and yaw rate
  73  */
  74 void AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitch_cd, float &yaw_rate_cds)
  75 {
  76     copter.mode_autotune.get_pilot_desired_lean_angles(des_roll_cd, des_pitch_cd, copter.aparm.angle_max,
  77                                                        copter.attitude_control->get_althold_lean_angle_max_cd());
  78     yaw_rate_cds = copter.mode_autotune.get_pilot_desired_yaw_rate();
  79 }
  80
  81 /*
  82   setup z controller velocity and accel limits
  83  */
  84 void AutoTune::init_z_limits()
  85 {
  86     // set vertical speed and acceleration limits
  87     copter.pos_control->set_max_speed_accel_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up, copter.g.pilot_accel_z);
  88     copter.pos_control->set_correction_speed_accel_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up, copter.g.pilot_accel_z);
  89 }
  90
  91 #if HAL_LOGGING_ENABLED
  92 void AutoTune::log_pids()
  93 {
  94     copter.logger.Write_PID(LOG_PIDR_MSG, copter.attitude_control->get_rate_roll_pid().get_pid_info());
  95     copter.logger.Write_PID(LOG_PIDP_MSG, copter.attitude_control->get_rate_pitch_pid().get_pid_info());
  96     copter.logger.Write_PID(LOG_PIDY_MSG, copter.attitude_control->get_rate_yaw_pid().get_pid_info());
  97 }
  98 #endif
  99
 100 /*
 101   check if we have a good position estimate
 102  */
 103 bool AutoTune::position_ok()
 104 {
 105     return copter.position_ok();
 106 }
 107
 108 /*
 109   initialise autotune mode
 110 */
 111 bool ModeAutoTune::init(bool ignore_checks)
 112 {
 113     return autotune.init();
 114 }
 115
 116 void ModeAutoTune::run()
 117 {
 118     autotune.run();
 119 }
 120
 121 void ModeAutoTune::exit()
 122 {
 123     autotune.stop();
 124 }
 125
 126 #endif  // AUTOTUNE_ENABLED