ArduCopter/tuning.cpp

   1 #include "Copter.h"
   2
   3 /*
   4  * Function to update various parameters in flight using the TRANSMITTER_TUNING channel knob
   5  * This should not be confused with the AutoTune feature which can be found in control_autotune.cpp
   6  */
   7
   8 // tuning - updates parameters based on the ch6 TRANSMITTER_TUNING channel knob's position
   9 //  should be called at 3.3hz
  10 void Copter::tuning()
  11 {
  12     const RC_Channel *rc_tuning = rc().find_channel_for_option(RC_Channel::AUX_FUNC::TRANSMITTER_TUNING);
  13
  14     // exit immediately if tuning channel is not set
  15     if (rc_tuning == nullptr) {
  16         return;
  17     }
  18
  19     // exit immediately if the tuning function is not set or min and max are both zero
  20     if ((g.radio_tuning <= 0) || (is_zero(g2.tuning_min.get()) && is_zero(g2.tuning_max.get()))) {
  21         return;
  22     }
  23
  24     // exit immediately when radio failsafe is invoked or transmitter has not been turned on
  25     if (failsafe.radio || failsafe.radio_counter != 0 || rc_tuning->get_radio_in() == 0) {
  26         return;
  27     }
  28
  29     const uint16_t radio_in = rc_tuning->get_radio_in();
  30     float tuning_value = linear_interpolate(g2.tuning_min, g2.tuning_max, radio_in, rc_tuning->get_radio_min(), rc_tuning->get_radio_max());
  31
  32 #if HAL_LOGGING_ENABLED
  33     Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, g2.tuning_min, g2.tuning_max);
  34 #endif
  35
  36     switch(g.radio_tuning) {
  37
  38     // Roll, Pitch tuning
  39     case TUNING_STABILIZE_ROLL_PITCH_KP:
  40         attitude_control->get_angle_roll_p().set_kP(tuning_value);
  41         attitude_control->get_angle_pitch_p().set_kP(tuning_value);
  42         break;
  43
  44     case TUNING_RATE_ROLL_PITCH_KP:
  45         attitude_control->get_rate_roll_pid().set_kP(tuning_value);
  46         attitude_control->get_rate_pitch_pid().set_kP(tuning_value);
  47         break;
  48
  49     case TUNING_RATE_ROLL_PITCH_KI:
  50         attitude_control->get_rate_roll_pid().set_kI(tuning_value);
  51         attitude_control->get_rate_pitch_pid().set_kI(tuning_value);
  52         break;
  53
  54     case TUNING_RATE_ROLL_PITCH_KD:
  55         attitude_control->get_rate_roll_pid().set_kD(tuning_value);
  56         attitude_control->get_rate_pitch_pid().set_kD(tuning_value);
  57         break;
  58
  59     // Yaw tuning
  60     case TUNING_STABILIZE_YAW_KP:
  61         attitude_control->get_angle_yaw_p().set_kP(tuning_value);
  62         break;
  63
  64     case TUNING_YAW_RATE_KP:
  65         attitude_control->get_rate_yaw_pid().set_kP(tuning_value);
  66         break;
  67
  68     case TUNING_YAW_RATE_KD:
  69         attitude_control->get_rate_yaw_pid().set_kD(tuning_value);
  70         break;
  71
  72     // Altitude and throttle tuning
  73     case TUNING_ALTITUDE_HOLD_KP:
  74         pos_control->get_pos_z_p().set_kP(tuning_value);
  75         break;
  76
  77     case TUNING_THROTTLE_RATE_KP:
  78         pos_control->get_vel_z_pid().set_kP(tuning_value);
  79         break;
  80
  81     case TUNING_ACCEL_Z_KP:
  82         pos_control->get_accel_z_pid().set_kP(tuning_value);
  83         break;
  84
  85     case TUNING_ACCEL_Z_KI:
  86         pos_control->get_accel_z_pid().set_kI(tuning_value);
  87         break;
  88
  89     case TUNING_ACCEL_Z_KD:
  90         pos_control->get_accel_z_pid().set_kD(tuning_value);
  91         break;
  92
  93     // Loiter and navigation tuning
  94     case TUNING_LOITER_POSITION_KP:
  95         pos_control->get_pos_xy_p().set_kP(tuning_value);
  96         break;
  97
  98     case TUNING_VEL_XY_KP:
  99         pos_control->get_vel_xy_pid().set_kP(tuning_value);
 100         break;
 101
 102     case TUNING_VEL_XY_KI:
 103         pos_control->get_vel_xy_pid().set_kI(tuning_value);
 104         break;
 105
 106     case TUNING_WP_SPEED:
 107         wp_nav->set_speed_xy(tuning_value);
 108         break;
 109
 110 #if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
 111     // Acro roll pitch rates
 112     case TUNING_ACRO_RP_RATE:
 113         g2.command_model_acro_rp.set_rate(tuning_value);
 114         break;
 115 #endif
 116
 117 #if MODE_ACRO_ENABLED || MODE_DRIFT_ENABLED
 118     // Acro yaw rate
 119     case TUNING_ACRO_YAW_RATE:
 120         g2.command_model_acro_y.set_rate(tuning_value);
 121         break;
 122 #endif
 123
 124 #if FRAME_CONFIG == HELI_FRAME
 125     case TUNING_HELI_EXTERNAL_GYRO:
 126         motors->ext_gyro_gain(tuning_value);
 127         break;
 128
 129     case TUNING_RATE_PITCH_FF:
 130         attitude_control->get_rate_pitch_pid().set_ff(tuning_value);
 131         break;
 132
 133     case TUNING_RATE_ROLL_FF:
 134         attitude_control->get_rate_roll_pid().set_ff(tuning_value);
 135         break;
 136
 137     case TUNING_RATE_YAW_FF:
 138         attitude_control->get_rate_yaw_pid().set_ff(tuning_value);
 139         break;
 140 #endif
 141
 142     case TUNING_DECLINATION:
 143         compass.set_declination(ToRad(tuning_value), false);     // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact
 144         break;
 145
 146 #if MODE_CIRCLE_ENABLED
 147     case TUNING_CIRCLE_RATE:
 148         circle_nav->set_rate(tuning_value);
 149         break;
 150 #endif
 151
 152     case TUNING_RC_FEEL_RP:
 153         attitude_control->set_input_tc(tuning_value);
 154         break;
 155
 156     case TUNING_RATE_PITCH_KP:
 157         attitude_control->get_rate_pitch_pid().set_kP(tuning_value);
 158         break;
 159
 160     case TUNING_RATE_PITCH_KI:
 161         attitude_control->get_rate_pitch_pid().set_kI(tuning_value);
 162         break;
 163
 164     case TUNING_RATE_PITCH_KD:
 165         attitude_control->get_rate_pitch_pid().set_kD(tuning_value);
 166         break;
 167
 168     case TUNING_RATE_ROLL_KP:
 169         attitude_control->get_rate_roll_pid().set_kP(tuning_value);
 170         break;
 171
 172     case TUNING_RATE_ROLL_KI:
 173         attitude_control->get_rate_roll_pid().set_kI(tuning_value);
 174         break;
 175
 176     case TUNING_RATE_ROLL_KD:
 177         attitude_control->get_rate_roll_pid().set_kD(tuning_value);
 178         break;
 179
 180 #if FRAME_CONFIG != HELI_FRAME
 181     case TUNING_RATE_MOT_YAW_HEADROOM:
 182         motors->set_yaw_headroom(tuning_value);
 183         break;
 184 #endif
 185
 186     case TUNING_RATE_YAW_FILT:
 187         attitude_control->get_rate_yaw_pid().set_filt_E_hz(tuning_value);
 188         break;
 189
 190     case TUNING_SYSTEM_ID_MAGNITUDE:
 191 #if MODE_SYSTEMID_ENABLED
 192         copter.mode_systemid.set_magnitude(tuning_value);
 193 #endif
 194         break;
 195
 196     case TUNING_POS_CONTROL_ANGLE_MAX:
 197         pos_control->set_lean_angle_max_cd(tuning_value * 100.0);
 198         break;
 199     }
 200 }