ArduPlane/tuning.cpp

   1 #include <AP_Tuning/AP_Tuning_config.h>
   2
   3 #if AP_TUNING_ENABLED
   4
   5 #include "Plane.h"
   6
   7 /*
   8   the vehicle class has its own var table for TUNE_PARAM so it can
   9   have separate parameter docs for the list of available parameters
  10  */
  11 const AP_Param::GroupInfo AP_Tuning_Plane::var_info[] = {
  12     // @Param: PARAM
  13     // @DisplayName: Transmitter tuning parameter or set of parameters
  14     // @Description: This sets which parameter or set of parameters will be tuned. Values greater than 100 indicate a set of parameters rather than a single parameter. Parameters less than 50 are for QuadPlane vertical lift motors only.
  15     // @Values: 0:None,1:RateRollPI,2:RateRollP,3:RateRollI,4:RateRollD,5:RatePitchPI,6:RatePitchP,7:RatePitchI,8:RatePitchD,9:RateYawPI,10:RateYawP,11:RateYawI,12:RateYawD,13:AngleRollP,14:AnglePitchP,15:AngleYawP,16:PosXYP,17:PosZP,18:VelXYP,19:VelXYI,20:VelZP,21:AccelZP,22:AccelZI,23:AccelZD,24:RatePitchFF,25:RateRollFF,26:RateYawFF,50:FixedWingRollP,51:FixedWingRollI,52:FixedWingRollD,53:FixedWingRollFF,54:FixedWingPitchP,55:FixedWingPitchI,56:FixedWingPitchD,57:FixedWingPitchFF,101:Set_RateRollPitch,102:Set_RateRoll,103:Set_RatePitch,104:Set_RateYaw,105:Set_AngleRollPitch,106:Set_VelXY,107:Set_AccelZ,108:Set_RatePitchDP,109:Set_RateRollDP,110:Set_RateYawDP
  16     // @User: Standard
  17     AP_GROUPINFO("PARAM", 1, AP_Tuning_Plane, parmset, 0),
  18
  19     // the rest of the parameters are from AP_Tuning
  20     AP_NESTEDGROUPINFO(AP_Tuning, 0),
  21
  22     AP_GROUPEND
  23 };
  24
  25
  26 /*
  27   tables of tuning sets
  28  */
  29 const uint8_t AP_Tuning_Plane::tuning_set_rate_roll_pitch[] =  { TUNING_RATE_ROLL_D, TUNING_RATE_ROLL_PI,
  30                                                                  TUNING_RATE_PITCH_D, TUNING_RATE_PITCH_PI};
  31 const uint8_t AP_Tuning_Plane::tuning_set_rate_roll[] =        { TUNING_RATE_ROLL_D, TUNING_RATE_ROLL_PI };
  32 const uint8_t AP_Tuning_Plane::tuning_set_rate_pitch[] =       { TUNING_RATE_PITCH_D, TUNING_RATE_PITCH_PI };
  33 const uint8_t AP_Tuning_Plane::tuning_set_rate_yaw[] =         { TUNING_RATE_YAW_P, TUNING_RATE_YAW_I, TUNING_RATE_YAW_D };
  34 const uint8_t AP_Tuning_Plane::tuning_set_ang_roll_pitch[] =   { TUNING_ANG_ROLL_P, TUNING_ANG_PITCH_P };
  35 const uint8_t AP_Tuning_Plane::tuning_set_vxy[] =              { TUNING_VXY_P, TUNING_VXY_I };
  36 const uint8_t AP_Tuning_Plane::tuning_set_az[] =               { TUNING_AZ_P, TUNING_AZ_I, TUNING_AZ_D };
  37 const uint8_t AP_Tuning_Plane::tuning_set_rate_pitchDP[]=      { TUNING_RATE_PITCH_D, TUNING_RATE_PITCH_P };
  38 const uint8_t AP_Tuning_Plane::tuning_set_rate_rollDP[]=       { TUNING_RATE_ROLL_D, TUNING_RATE_ROLL_P };
  39 const uint8_t AP_Tuning_Plane::tuning_set_rate_yawDP[]=        { TUNING_RATE_YAW_D, TUNING_RATE_YAW_P };
  40
  41 // macro to prevent getting the array length wrong
  42 #define TUNING_ARRAY(v) ARRAY_SIZE(v), v
  43
  44 // list of tuning sets
  45 const AP_Tuning_Plane::tuning_set AP_Tuning_Plane::tuning_sets[] = {
  46     { TUNING_SET_RATE_ROLL_PITCH,       TUNING_ARRAY(tuning_set_rate_roll_pitch) },
  47     { TUNING_SET_RATE_ROLL,             TUNING_ARRAY(tuning_set_rate_roll) },
  48     { TUNING_SET_RATE_PITCH,            TUNING_ARRAY(tuning_set_rate_pitch) },
  49     { TUNING_SET_RATE_YAW,              TUNING_ARRAY(tuning_set_rate_yaw) },
  50     { TUNING_SET_ANG_ROLL_PITCH,        TUNING_ARRAY(tuning_set_ang_roll_pitch) },
  51     { TUNING_SET_VXY,                   TUNING_ARRAY(tuning_set_vxy) },
  52     { TUNING_SET_AZ,                    TUNING_ARRAY(tuning_set_az) },
  53     { TUNING_SET_RATE_PITCHDP,          TUNING_ARRAY(tuning_set_rate_pitchDP) },
  54     { TUNING_SET_RATE_ROLLDP,           TUNING_ARRAY(tuning_set_rate_rollDP) },
  55     { TUNING_SET_RATE_YAWDP,            TUNING_ARRAY(tuning_set_rate_yawDP) },
  56     { 0, 0, nullptr }
  57 };
  58
  59 /*
  60   table of tuning names
  61  */
  62 const AP_Tuning_Plane::tuning_name AP_Tuning_Plane::tuning_names[] = {
  63     { TUNING_RATE_ROLL_PI, "RateRollPI" },
  64     { TUNING_RATE_ROLL_P,  "RateRollP" },
  65     { TUNING_RATE_ROLL_I,  "RateRollI" },
  66     { TUNING_RATE_ROLL_D,  "RateRollD" },
  67     { TUNING_RATE_PITCH_PI,"RatePitchPI" },
  68     { TUNING_RATE_PITCH_P, "RatePitchP" },
  69     { TUNING_RATE_PITCH_I, "RatePitchI" },
  70     { TUNING_RATE_PITCH_D, "RatePitchD" },
  71     { TUNING_RATE_YAW_PI,  "RateYawPI" },
  72     { TUNING_RATE_YAW_P,   "RateYawP" },
  73     { TUNING_RATE_YAW_I,   "RateYawI" },
  74     { TUNING_RATE_YAW_D,   "RateYawD" },
  75     { TUNING_ANG_ROLL_P,   "AngRollP" },
  76     { TUNING_ANG_PITCH_P,  "AngPitchP" },
  77     { TUNING_ANG_YAW_P,    "AngYawP" },
  78     { TUNING_RATE_PITCH_FF, "RatePitchFF" },
  79     { TUNING_RATE_ROLL_FF, "RateRollFF" },
  80     { TUNING_RATE_YAW_FF, "RateYawFF" },
  81     { TUNING_PXY_P,        "PXY_P" },
  82     { TUNING_PZ_P,         "PZ_P" },
  83     { TUNING_VXY_P,        "VXY_P" },
  84     { TUNING_VXY_I,        "VXY_I" },
  85     { TUNING_VZ_P,         "VZ_P" },
  86     { TUNING_AZ_P,         "RateAZ_P" },
  87     { TUNING_AZ_I,         "RateAZ_I" },
  88     { TUNING_AZ_D,         "RateAZ_D" },
  89     { TUNING_RLL_P,        "RollP" },
  90     { TUNING_RLL_I,        "RollI" },
  91     { TUNING_RLL_D,        "RollD" },
  92     { TUNING_RLL_FF,       "RollFF" },
  93     { TUNING_PIT_P,        "PitchP" },
  94     { TUNING_PIT_I,        "PitchI" },
  95     { TUNING_PIT_D,        "PitchD" },
  96     { TUNING_PIT_FF,       "PitchFF" },
  97     { TUNING_Q_FWD_THR,    "QModeFwdThr" },
  98     { TUNING_NONE, nullptr }
  99 };
 100
 101 /*
 102   get a pointer to an AP_Float for a parameter, or nullptr on fail
 103  */
 104 AP_Float *AP_Tuning_Plane::get_param_pointer(uint8_t parm)
 105 {
 106 #if HAL_QUADPLANE_ENABLED
 107     if (parm < TUNING_FIXED_WING_BASE && !plane.quadplane.available()) {
 108         // quadplane tuning options not available
 109         return nullptr;
 110     }
 111 #endif
 112
 113     switch(parm) {
 114
 115 #if HAL_QUADPLANE_ENABLED
 116     case TUNING_RATE_ROLL_PI:
 117         // use P for initial value when tuning PI
 118         return &plane.quadplane.attitude_control->get_rate_roll_pid().kP();
 119
 120     case TUNING_RATE_ROLL_P:
 121         return &plane.quadplane.attitude_control->get_rate_roll_pid().kP();
 122
 123     case TUNING_RATE_ROLL_I:
 124         return &plane.quadplane.attitude_control->get_rate_roll_pid().kI();
 125
 126     case TUNING_RATE_ROLL_D:
 127         return &plane.quadplane.attitude_control->get_rate_roll_pid().kD();
 128
 129     case TUNING_RATE_PITCH_PI:
 130         return &plane.quadplane.attitude_control->get_rate_pitch_pid().kP();
 131
 132     case TUNING_RATE_PITCH_P:
 133         return &plane.quadplane.attitude_control->get_rate_pitch_pid().kP();
 134
 135     case TUNING_RATE_PITCH_I:
 136         return &plane.quadplane.attitude_control->get_rate_pitch_pid().kI();
 137
 138     case TUNING_RATE_PITCH_D:
 139         return &plane.quadplane.attitude_control->get_rate_pitch_pid().kD();
 140
 141     case TUNING_RATE_YAW_PI:
 142         return &plane.quadplane.attitude_control->get_rate_yaw_pid().kP();
 143
 144     case TUNING_RATE_YAW_P:
 145         return &plane.quadplane.attitude_control->get_rate_yaw_pid().kP();
 146
 147     case TUNING_RATE_YAW_I:
 148         return &plane.quadplane.attitude_control->get_rate_yaw_pid().kI();
 149
 150     case TUNING_RATE_YAW_D:
 151         return &plane.quadplane.attitude_control->get_rate_yaw_pid().kD();
 152
 153     case TUNING_ANG_ROLL_P:
 154         return &plane.quadplane.attitude_control->get_angle_roll_p().kP();
 155
 156     case TUNING_ANG_PITCH_P:
 157         return &plane.quadplane.attitude_control->get_angle_pitch_p().kP();
 158
 159     case TUNING_ANG_YAW_P:
 160         return &plane.quadplane.attitude_control->get_angle_yaw_p().kP();
 161
 162     case TUNING_PXY_P:
 163         return &plane.quadplane.pos_control->get_pos_xy_p().kP();
 164
 165     case TUNING_PZ_P:
 166         return &plane.quadplane.pos_control->get_pos_z_p().kP();
 167
 168     case TUNING_VXY_P:
 169         return &plane.quadplane.pos_control->get_vel_xy_pid().kP();
 170
 171     case TUNING_VXY_I:
 172         return &plane.quadplane.pos_control->get_vel_xy_pid().kI();
 173
 174     case TUNING_VZ_P:
 175         return &plane.quadplane.pos_control->get_vel_z_pid().kP();
 176
 177     case TUNING_AZ_P:
 178         return &plane.quadplane.pos_control->get_accel_z_pid().kP();
 179
 180     case TUNING_AZ_I:
 181         return &plane.quadplane.pos_control->get_accel_z_pid().kI();
 182
 183     case TUNING_AZ_D:
 184         return &plane.quadplane.pos_control->get_accel_z_pid().kD();
 185
 186     case TUNING_RATE_PITCH_FF:
 187         return &plane.quadplane.attitude_control->get_rate_pitch_pid().ff();
 188
 189     case TUNING_RATE_ROLL_FF:
 190         return &plane.quadplane.attitude_control->get_rate_roll_pid().ff();
 191
 192     case TUNING_RATE_YAW_FF:
 193         return &plane.quadplane.attitude_control->get_rate_yaw_pid().ff();
 194
 195     case TUNING_Q_FWD_THR:
 196         return &plane.quadplane.q_fwd_thr_gain;
 197 #endif // HAL_QUADPLANE_ENABLED
 198
 199     // fixed wing tuning parameters
 200     case TUNING_RLL_P:
 201         return &plane.rollController.kP();
 202
 203     case TUNING_RLL_I:
 204         return &plane.rollController.kI();
 205
 206     case TUNING_RLL_D:
 207         return &plane.rollController.kD();
 208
 209     case TUNING_RLL_FF:
 210         return &plane.rollController.kFF();
 211
 212     case TUNING_PIT_P:
 213         return &plane.pitchController.kP();
 214
 215     case TUNING_PIT_I:
 216         return &plane.pitchController.kI();
 217
 218     case TUNING_PIT_D:
 219         return &plane.pitchController.kD();
 220
 221     case TUNING_PIT_FF:
 222         return &plane.pitchController.kFF();
 223     }
 224     return nullptr;
 225 }
 226
 227
 228 /*
 229   save a parameter
 230  */
 231 void AP_Tuning_Plane::save_value(uint8_t parm)
 232 {
 233     switch(parm) {
 234     // special handling of dual-parameters
 235     case TUNING_RATE_ROLL_PI:
 236         save_value(TUNING_RATE_ROLL_P);
 237         save_value(TUNING_RATE_ROLL_I);
 238         break;
 239     case TUNING_RATE_PITCH_PI:
 240         save_value(TUNING_RATE_PITCH_P);
 241         save_value(TUNING_RATE_PITCH_I);
 242         break;
 243     default:
 244         AP_Float *f = get_param_pointer(parm);
 245         if (f != nullptr) {
 246             f->save();
 247         }
 248         break;
 249     }
 250 }
 251
 252 /*
 253   set a parameter
 254  */
 255 void AP_Tuning_Plane::set_value(uint8_t parm, float value)
 256 {
 257     switch(parm) {
 258     // special handling of dual-parameters
 259     case TUNING_RATE_ROLL_PI:
 260         set_value(TUNING_RATE_ROLL_P, value);
 261         set_value(TUNING_RATE_ROLL_I, value);
 262         break;
 263     case TUNING_RATE_PITCH_PI:
 264         set_value(TUNING_RATE_PITCH_P, value);
 265         set_value(TUNING_RATE_PITCH_I, value);
 266         break;
 267     default:
 268         AP_Float *f = get_param_pointer(parm);
 269         if (f != nullptr) {
 270             uint64_t param_bit = (1ULL << parm);
 271             if (!(param_bit & have_set)) {
 272                 // first time this param has been set by tuning. We
 273                 // need to see if a reversion value is available in
 274                 // FRAM, and if not then save one
 275                 float current_value = f->get();
 276                 if (!f->load()) {
 277                     // there is no value in FRAM, set one
 278                     f->set_and_save(current_value);
 279                 }
 280                 have_set |= param_bit;
 281             }
 282             f->set_and_notify(value);
 283         }
 284         break;
 285     }
 286 }
 287
 288 /*
 289   reload a parameter
 290  */
 291 void AP_Tuning_Plane::reload_value(uint8_t parm)
 292 {
 293     switch(parm) {
 294     // special handling of dual-parameters
 295     case TUNING_RATE_ROLL_PI:
 296         reload_value(TUNING_RATE_ROLL_P);
 297         reload_value(TUNING_RATE_ROLL_I);
 298         break;
 299     case TUNING_RATE_PITCH_PI:
 300         reload_value(TUNING_RATE_PITCH_P);
 301         reload_value(TUNING_RATE_PITCH_I);
 302         break;
 303     default:
 304         AP_Float *f = get_param_pointer(parm);
 305         if (f != nullptr) {
 306             uint64_t param_bit = (1ULL << parm);
 307             // only reload if we have set this parameter at some point
 308             if (param_bit & have_set) {
 309                 f->load();
 310             }
 311         }
 312         break;
 313     }
 314 }
 315
 316 #endif  // AP_TUNING_ENABLED