Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #if AP_RC_TRANSMITTER_TUNING_ENABLED
       4             : /*
       5             :  * Function to update various parameters in flight using the TRANSMITTER_TUNING channel knob
       6             :  * This should not be confused with the AutoTune feature which can be found in control_autotune.cpp
       7             :  */
       8             : 
       9             : // tuning - updates parameters based on the TRANSMITTER_TUNING channel knob's position
      10             : //  should be called at 3.3hz
      11      228158 : void Copter::tuning()
      12             : {
      13      228158 :     tuning(rc_tuning, g.rc_tuning_param, g2.rc_tuning_min.get(), g2.rc_tuning_max.get());
      14      228158 :     tuning(rc_tuning2, g2.rc_tuning2_param, g2.rc_tuning2_min.get(), g2.rc_tuning2_max.get());
      15      228158 : }
      16             : 
      17      115671 : bool Copter::being_tuned(int8_t tuning_param) const
      18             : {
      19      115671 :     if (g.rc_tuning_param == tuning_param) {
      20           0 :         return true;
      21             :     }
      22      115671 :     if (g2.rc_tuning2_param == tuning_param) {
      23           0 :         return true;
      24             :     }
      25      115671 :     return false;
      26             : }
      27             : 
      28      456316 : void Copter::tuning(const RC_Channel *tuning_ch, int8_t tuning_param, float tuning_min, float tuning_max)
      29             : {
      30      456316 :     if (tuning_ch == nullptr) {
      31             :         // tuning channel is not set - don't know where to take input value from
      32      454930 :         return;
      33             :     }
      34             : 
      35        1398 :     if (tuning_param <= 0) {
      36             :         // no parameter set for tuning
      37          12 :         return;
      38             :     }
      39             : 
      40        1386 :     if (is_equal(tuning_min, tuning_max)) {
      41             :         // endpoints are equal, there is no parameter value range to tune across
      42           0 :         return;
      43             :     }
      44             : 
      45             :     float norm_in;
      46        1386 :     if (!tuning_ch->norm_input_ignore_trim(norm_in)) {
      47             :         // unable to get input for this channel (eg. in RC failsafe)
      48           0 :         return;
      49             :     }
      50             : 
      51        1386 :     const float tuning_value = linear_interpolate(tuning_min, tuning_max, norm_in, -1, 1);
      52             : 
      53             : #if HAL_LOGGING_ENABLED
      54        1386 :     Log_Write_PTUN(tuning_param, tuning_value, tuning_min, tuning_max, norm_in);
      55             : #endif
      56             : 
      57        1386 :     switch(tuning_param) {
      58             : 
      59             :     // Roll, Pitch tuning
      60           0 :     case TUNING_STABILIZE_ROLL_PITCH_KP:
      61           0 :         attitude_control->get_angle_roll_p().set_kP(tuning_value);
      62           0 :         attitude_control->get_angle_pitch_p().set_kP(tuning_value);
      63           0 :         break;
      64             : 
      65           0 :     case TUNING_RATE_ROLL_PITCH_KP:
      66           0 :         attitude_control->get_rate_roll_pid().set_kP(tuning_value);
      67           0 :         attitude_control->get_rate_pitch_pid().set_kP(tuning_value);
      68           0 :         break;
      69             : 
      70           0 :     case TUNING_RATE_ROLL_PITCH_KI:
      71           0 :         attitude_control->get_rate_roll_pid().set_kI(tuning_value);
      72           0 :         attitude_control->get_rate_pitch_pid().set_kI(tuning_value);
      73           0 :         break;
      74             : 
      75           0 :     case TUNING_RATE_ROLL_PITCH_KD:
      76           0 :         attitude_control->get_rate_roll_pid().set_kD(tuning_value);
      77           0 :         attitude_control->get_rate_pitch_pid().set_kD(tuning_value);
      78           0 :         break;
      79             : 
      80             :     // Yaw tuning
      81           0 :     case TUNING_STABILIZE_YAW_KP:
      82           0 :         attitude_control->get_angle_yaw_p().set_kP(tuning_value);
      83           0 :         break;
      84             : 
      85           0 :     case TUNING_YAW_RATE_KP:
      86           0 :         attitude_control->get_rate_yaw_pid().set_kP(tuning_value);
      87           0 :         break;
      88             : 
      89           0 :     case TUNING_YAW_RATE_KD:
      90           0 :         attitude_control->get_rate_yaw_pid().set_kD(tuning_value);
      91           0 :         break;
      92             : 
      93             :     // Altitude and throttle tuning
      94           0 :     case TUNING_ALTITUDE_HOLD_KP:
      95           0 :         pos_control->D_get_pos_p().set_kP(tuning_value);
      96           0 :         break;
      97             : 
      98           0 :     case TUNING_THROTTLE_RATE_KP:
      99           0 :         pos_control->D_get_vel_pid().set_kP(tuning_value);
     100           0 :         break;
     101             : 
     102           0 :     case TUNING_ACCEL_Z_KP:
     103           0 :         pos_control->D_get_accel_pid().set_kP(tuning_value);
     104           0 :         break;
     105             : 
     106           0 :     case TUNING_ACCEL_Z_KI:
     107           0 :         pos_control->D_get_accel_pid().set_kI(tuning_value);
     108           0 :         break;
     109             : 
     110           0 :     case TUNING_ACCEL_Z_KD:
     111           0 :         pos_control->D_get_accel_pid().set_kD(tuning_value);
     112           0 :         break;
     113             : 
     114             :     // Loiter and navigation tuning
     115           0 :     case TUNING_LOITER_POSITION_KP:
     116           0 :         pos_control->NE_get_pos_p().set_kP(tuning_value);
     117           0 :         break;
     118             : 
     119           0 :     case TUNING_VEL_XY_KP:
     120           0 :         pos_control->NE_get_vel_pid().set_kP(tuning_value);
     121           0 :         break;
     122             : 
     123          29 :     case TUNING_VEL_XY_KI:
     124          29 :         pos_control->NE_get_vel_pid().set_kI(tuning_value);
     125          29 :         break;
     126             : 
     127             : #if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
     128             :     // Acro roll pitch rates
     129           0 :     case TUNING_ACRO_RP_RATE:
     130           0 :         g2.command_model_acro_rp.set_rate(tuning_value);
     131           0 :         break;
     132             : #endif
     133             : 
     134             : #if MODE_ACRO_ENABLED || MODE_DRIFT_ENABLED
     135             :     // Acro yaw rate
     136           0 :     case TUNING_ACRO_YAW_RATE:
     137           0 :         g2.command_model_acro_y.set_rate(tuning_value);
     138           0 :         break;
     139             : #endif
     140             : 
     141             : #if FRAME_CONFIG == HELI_FRAME
     142           0 :     case TUNING_RATE_PITCH_FF:
     143           0 :         attitude_control->get_rate_pitch_pid().set_ff(tuning_value);
     144           0 :         break;
     145             : 
     146           0 :     case TUNING_RATE_ROLL_FF:
     147           0 :         attitude_control->get_rate_roll_pid().set_ff(tuning_value);
     148           0 :         break;
     149             : 
     150           0 :     case TUNING_RATE_YAW_FF:
     151           0 :         attitude_control->get_rate_yaw_pid().set_ff(tuning_value);
     152           0 :         break;
     153             : #endif
     154             : 
     155           0 :     case TUNING_DECLINATION:
     156           0 :         compass.set_declination(radians(tuning_value), false);     // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact
     157           0 :         break;
     158             : 
     159             : #if MODE_CIRCLE_ENABLED
     160           0 :     case TUNING_CIRCLE_RATE:
     161           0 :         circle_nav->set_rate_degs(tuning_value);
     162           0 :         break;
     163             : #endif
     164             : 
     165           0 :     case TUNING_RC_FEEL_RP:
     166           0 :         attitude_control->set_input_tc(tuning_value);
     167           0 :         break;
     168             : 
     169           0 :     case TUNING_RATE_PITCH_KP:
     170           0 :         attitude_control->get_rate_pitch_pid().set_kP(tuning_value);
     171           0 :         break;
     172             : 
     173           0 :     case TUNING_RATE_PITCH_KI:
     174           0 :         attitude_control->get_rate_pitch_pid().set_kI(tuning_value);
     175           0 :         break;
     176             : 
     177           0 :     case TUNING_RATE_PITCH_KD:
     178           0 :         attitude_control->get_rate_pitch_pid().set_kD(tuning_value);
     179           0 :         break;
     180             : 
     181           0 :     case TUNING_RATE_ROLL_KP:
     182           0 :         attitude_control->get_rate_roll_pid().set_kP(tuning_value);
     183           0 :         break;
     184             : 
     185           0 :     case TUNING_RATE_ROLL_KI:
     186           0 :         attitude_control->get_rate_roll_pid().set_kI(tuning_value);
     187           0 :         break;
     188             : 
     189           0 :     case TUNING_RATE_ROLL_KD:
     190           0 :         attitude_control->get_rate_roll_pid().set_kD(tuning_value);
     191           0 :         break;
     192             : 
     193             : #if FRAME_CONFIG != HELI_FRAME
     194           0 :     case TUNING_RATE_MOT_YAW_HEADROOM:
     195           0 :         motors->set_yaw_headroom(tuning_value);
     196           0 :         break;
     197             : #endif
     198             : 
     199           0 :     case TUNING_RATE_YAW_FILT:
     200           0 :         attitude_control->get_rate_yaw_pid().set_filt_E_hz(tuning_value);
     201           0 :         break;
     202             : 
     203           0 :     case TUNING_SYSTEM_ID_MAGNITUDE:
     204             : #if MODE_SYSTEMID_ENABLED
     205           0 :         copter.mode_systemid.set_magnitude(tuning_value);
     206             : #endif
     207           0 :         break;
     208             : 
     209           0 :     case TUNING_POS_CONTROL_ANGLE_MAX:
     210           0 :         pos_control->set_lean_angle_max_deg(tuning_value);
     211           0 :         break;
     212             : 
     213         672 :     case TUNING_LOITER_MAX_XY_SPEED:
     214         672 :         loiter_nav->set_speed_max_NE_ms(tuning_value);
     215         672 :         break;
     216             : 
     217         685 :     case TUNING_WP_SPEED_MS:
     218         685 :         flightmode->set_speed_NE_ms(tuning_value);
     219         685 :         break;
     220             :     }
     221             : }
     222             : 
     223             : #endif  // AP_RC_TRANSMITTER_TUNING_ENABLED