Line data    Source code

       1             : #include "Plane.h"
       2             : #include "qautotune.h"
       3             : 
       4             : #if QAUTOTUNE_ENABLED
       5             : 
       6             : /*
       7             :   initialise QAUTOTUNE mode
       8             :  */
       9           1 : bool QAutoTune::init()
      10             : {
      11           1 :     if (!plane.quadplane.available()) {
      12           0 :         return false;
      13             :     }
      14             : 
      15             :     // use position hold while tuning if we were in QLOITER
      16           1 :     bool position_hold = (plane.previous_mode == &plane.mode_qloiter);
      17             : 
      18           1 :     return init_internals(position_hold,
      19           1 :                           plane.quadplane.attitude_control,
      20             :                           plane.quadplane.pos_control,
      21           1 :                           plane.quadplane.ahrs_view);
      22             : }
      23             : 
      24      129524 : float QAutoTune::get_desired_climb_rate_ms(void) const
      25             : {
      26      129524 :     return plane.quadplane.get_pilot_desired_climb_rate_cms() * 0.01;
      27             : }
      28             : 
      29      129524 : void QAutoTune::get_pilot_desired_rp_yrate_rad(float &des_roll_rad, float &des_pitch_rad, float &des_yaw_rate_rads)
      30             : {
      31      129524 :     if (plane.channel_roll->get_control_in() == 0 && plane.channel_pitch->get_control_in() == 0) {
      32      129524 :         des_roll_rad = 0.0;
      33      129524 :         des_pitch_rad = 0.0;
      34             :     } else {
      35           0 :         des_roll_rad = cd_to_rad(plane.nav_roll_cd);
      36           0 :         des_pitch_rad = cd_to_rad(plane.nav_pitch_cd);
      37             :     }
      38      129524 :     des_yaw_rate_rads = cd_to_rad(plane.quadplane.get_desired_yaw_rate_cds());
      39      129524 : }
      40             : 
      41      130154 : void QAutoTune::init_z_limits()
      42             : {
      43             :     // set vertical speed and acceleration limits
      44             :     // All limits must be positive
      45      130154 :     plane.quadplane.pos_control->D_set_max_speed_accel_m(plane.quadplane.get_pilot_velocity_z_max_dn_m(),
      46             :                                                        plane.quadplane.pilot_speed_z_max_up_ms,
      47             :                                                        plane.quadplane.pilot_accel_z_mss);
      48      130154 :     plane.quadplane.pos_control->D_set_correction_speed_accel_m(plane.quadplane.get_pilot_velocity_z_max_dn_m(),
      49             :                                                               plane.quadplane.pilot_speed_z_max_up_ms,
      50             :                                                               plane.quadplane.pilot_accel_z_mss);
      51      130154 : }
      52             : 
      53             : 
      54             : #if HAL_LOGGING_ENABLED
      55             : // log VTOL PIDs for during twitch
      56       41835 : void QAutoTune::log_pids(void)
      57             : {
      58       41835 :     AP::logger().Write_PID(LOG_PIQR_MSG, plane.quadplane.attitude_control->get_rate_roll_pid().get_pid_info());
      59       41835 :     AP::logger().Write_PID(LOG_PIQP_MSG, plane.quadplane.attitude_control->get_rate_pitch_pid().get_pid_info());
      60       41835 :     AP::logger().Write_PID(LOG_PIQY_MSG, plane.quadplane.attitude_control->get_rate_yaw_pid().get_pid_info());
      61       41835 : }
      62             : #endif
      63             : 
      64             : #endif // QAUTOTUNE_ENABLED
      65             :