src/main/cms/cms_menu_imu.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 // Menu contents for PID, RATES, RC preview, misc
  22 // Should be part of the relevant .c file.
  23
  24 #include <stdbool.h>
  25 #include <stdint.h>
  26 #include <string.h>
  27 #include <ctype.h>
  28
  29 #include "platform.h"
  30
  31 #ifdef USE_CMS
  32
  33 #include "build/version.h"
  34 #include "build/build_config.h"
  35
  36 #include "cms/cms.h"
  37 #include "cms/cms_types.h"
  38 #include "cms/cms_menu_imu.h"
  39
  40 #include "common/utils.h"
  41
  42 #include "config/feature.h"
  43
  44 #include "drivers/pwm_output.h"
  45
  46 #include "config/config.h"
  47 #include "fc/controlrate_profile.h"
  48 #include "fc/core.h"
  49 #include "fc/rc_controls.h"
  50 #include "fc/runtime_config.h"
  51
  52 #include "flight/mixer.h"
  53 #include "flight/pid.h"
  54
  55 #include "pg/pg.h"
  56
  57 #include "sensors/battery.h"
  58 #include "sensors/gyro.h"
  59
  60 #include "cli/settings.h"
  61
  62 //
  63 // PID
  64 //
  65 static uint8_t tmpPidProfileIndex;
  66 static uint8_t pidProfileIndex;
  67 static char pidProfileIndexString[MAX_PROFILE_NAME_LENGTH + 5];
  68 static uint8_t tempPid[3][3];
  69 static uint16_t tempPidF[3];
  70
  71 static uint8_t tmpRateProfileIndex;
  72 static uint8_t rateProfileIndex;
  73 static char rateProfileIndexString[MAX_RATE_PROFILE_NAME_LENGTH + 5];
  74 static controlRateConfig_t rateProfile;
  75
  76 static const char * const osdTableThrottleLimitType[] = {
  77     "OFF", "SCALE", "CLIP"
  78 };
  79
  80 #ifdef USE_MULTI_GYRO
  81 static const char * const osdTableGyroToUse[] = {
  82     "FIRST", "SECOND", "BOTH"
  83 };
  84 #endif
  85
  86 static void setProfileIndexString(char *profileString, int profileIndex, char *profileName)
  87 {
  88     int charIndex = 0;
  89     profileString[charIndex++] = '1' + profileIndex;
  90
  91 #ifdef USE_PROFILE_NAMES
  92     const int profileNameLen = strlen(profileName);
  93
  94     if (profileNameLen > 0) {
  95         profileString[charIndex++] = ' ';
  96         profileString[charIndex++] = '(';
  97         for (int i = 0; i < profileNameLen; i++) {
  98             profileString[charIndex++] = toupper(profileName[i]);
  99         }
 100         profileString[charIndex++] = ')';
 101     }
 102 #else
 103     UNUSED(profileName);
 104 #endif
 105
 106     profileString[charIndex] = '\0';
 107 }
 108
 109 static const void *cmsx_menuImu_onEnter(displayPort_t *pDisp)
 110 {
 111     UNUSED(pDisp);
 112
 113     pidProfileIndex = getCurrentPidProfileIndex();
 114     tmpPidProfileIndex = pidProfileIndex + 1;
 115
 116     rateProfileIndex = getCurrentControlRateProfileIndex();
 117     tmpRateProfileIndex = rateProfileIndex + 1;
 118
 119     return NULL;
 120 }
 121
 122 static const void *cmsx_menuImu_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 123 {
 124     UNUSED(pDisp);
 125     UNUSED(self);
 126
 127     changePidProfile(pidProfileIndex);
 128     changeControlRateProfile(rateProfileIndex);
 129
 130     return NULL;
 131 }
 132
 133 static const void *cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *ptr)
 134 {
 135     UNUSED(displayPort);
 136     UNUSED(ptr);
 137
 138     pidProfileIndex = tmpPidProfileIndex - 1;
 139     changePidProfile(pidProfileIndex);
 140
 141     return NULL;
 142 }
 143
 144 static const void *cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void *ptr)
 145 {
 146     UNUSED(displayPort);
 147     UNUSED(ptr);
 148
 149     rateProfileIndex = tmpRateProfileIndex - 1;
 150     changeControlRateProfile(rateProfileIndex);
 151
 152     return NULL;
 153 }
 154
 155 static const void *cmsx_PidRead(void)
 156 {
 157
 158     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 159     for (uint8_t i = 0; i < 3; i++) {
 160         tempPid[i][0] = pidProfile->pid[i].P;
 161         tempPid[i][1] = pidProfile->pid[i].I;
 162         tempPid[i][2] = pidProfile->pid[i].D;
 163         tempPidF[i] = pidProfile->pid[i].F;
 164     }
 165
 166     return NULL;
 167 }
 168
 169 static const void *cmsx_PidOnEnter(displayPort_t *pDisp)
 170 {
 171     UNUSED(pDisp);
 172
 173     setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName);
 174     cmsx_PidRead();
 175
 176     return NULL;
 177 }
 178
 179 static const void *cmsx_PidWriteback(displayPort_t *pDisp, const OSD_Entry *self)
 180 {
 181     UNUSED(pDisp);
 182     UNUSED(self);
 183
 184     pidProfile_t *pidProfile = currentPidProfile;
 185     for (uint8_t i = 0; i < 3; i++) {
 186         pidProfile->pid[i].P = tempPid[i][0];
 187         pidProfile->pid[i].I = tempPid[i][1];
 188         pidProfile->pid[i].D = tempPid[i][2];
 189         pidProfile->pid[i].F = tempPidF[i];
 190     }
 191     pidInitConfig(currentPidProfile);
 192
 193     return NULL;
 194 }
 195
 196 static const OSD_Entry cmsx_menuPidEntries[] =
 197 {
 198     { "-- PID --", OME_Label, NULL, pidProfileIndexString, 0},
 199
 200     { "ROLL  P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][0],  0, 200, 1 }, 0 },
 201     { "ROLL  I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][1],  0, 200, 1 }, 0 },
 202     { "ROLL  D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][2],  0, 200, 1 }, 0 },
 203     { "ROLL  F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_ROLL],  0, 2000, 1 }, 0 },
 204
 205     { "PITCH P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][0], 0, 200, 1 }, 0 },
 206     { "PITCH I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][1], 0, 200, 1 }, 0 },
 207     { "PITCH D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][2], 0, 200, 1 }, 0 },
 208     { "PITCH F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_PITCH], 0, 2000, 1 }, 0 },
 209
 210     { "YAW   P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][0],   0, 200, 1 }, 0 },
 211     { "YAW   I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][1],   0, 200, 1 }, 0 },
 212     { "YAW   D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][2],   0, 200, 1 }, 0 },
 213     { "YAW   F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_YAW],   0, 2000, 1 }, 0 },
 214
 215     { "BACK", OME_Back, NULL, NULL, 0 },
 216     { NULL, OME_END, NULL, NULL, 0 }
 217 };
 218
 219 static CMS_Menu cmsx_menuPid = {
 220 #ifdef CMS_MENU_DEBUG
 221     .GUARD_text = "XPID",
 222     .GUARD_type = OME_MENU,
 223 #endif
 224     .onEnter = cmsx_PidOnEnter,
 225     .onExit = cmsx_PidWriteback,
 226     .onDisplayUpdate = NULL,
 227     .entries = cmsx_menuPidEntries
 228 };
 229
 230 //
 231 // Rate & Expo
 232 //
 233
 234 static const void *cmsx_RateProfileRead(void)
 235 {
 236     memcpy(&rateProfile, controlRateProfiles(rateProfileIndex), sizeof(controlRateConfig_t));
 237
 238     return NULL;
 239 }
 240
 241 static const void *cmsx_RateProfileWriteback(displayPort_t *pDisp, const OSD_Entry *self)
 242 {
 243     UNUSED(pDisp);
 244     UNUSED(self);
 245
 246     memcpy(controlRateProfilesMutable(rateProfileIndex), &rateProfile, sizeof(controlRateConfig_t));
 247
 248     return NULL;
 249 }
 250
 251 static const void *cmsx_RateProfileOnEnter(displayPort_t *pDisp)
 252 {
 253     UNUSED(pDisp);
 254
 255     setProfileIndexString(rateProfileIndexString, rateProfileIndex, controlRateProfilesMutable(rateProfileIndex)->profileName);
 256     cmsx_RateProfileRead();
 257
 258     return NULL;
 259 }
 260
 261 static const OSD_Entry cmsx_menuRateProfileEntries[] =
 262 {
 263     { "-- RATE --", OME_Label, NULL, rateProfileIndexString, 0 },
 264
 265     { "RC R RATE",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcRates[FD_ROLL],    1, CONTROL_RATE_CONFIG_RC_RATES_MAX, 1, 10 }, 0 },
 266     { "RC P RATE",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcRates[FD_PITCH],    1, CONTROL_RATE_CONFIG_RC_RATES_MAX, 1, 10 }, 0 },
 267     { "RC Y RATE",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcRates[FD_YAW], 1, CONTROL_RATE_CONFIG_RC_RATES_MAX, 1, 10 }, 0 },
 268
 269     { "ROLL SUPER",  OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rates[FD_ROLL],   0, CONTROL_RATE_CONFIG_RATE_MAX, 1, 10 }, 0 },
 270     { "PITCH SUPER", OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rates[FD_PITCH],   0, CONTROL_RATE_CONFIG_RATE_MAX, 1, 10 }, 0 },
 271     { "YAW SUPER",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rates[FD_YAW],   0, CONTROL_RATE_CONFIG_RATE_MAX, 1, 10 }, 0 },
 272
 273     { "RC R EXPO",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo[FD_ROLL],    0, 100, 1, 10 }, 0 },
 274     { "RC P EXPO",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo[FD_PITCH],    0, 100, 1, 10 }, 0 },
 275     { "RC Y EXPO",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo[FD_YAW], 0, 100, 1, 10 }, 0 },
 276
 277     { "THR MID",     OME_UINT8,  NULL, &(OSD_UINT8_t) { &rateProfile.thrMid8,           0,  100,  1}, 0 },
 278     { "THR EXPO",    OME_UINT8,  NULL, &(OSD_UINT8_t) { &rateProfile.thrExpo8,          0,  100,  1}, 0 },
 279     { "THRPID ATT",  OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.dynThrPID,         0,  100,  1, 10}, 0 },
 280     { "TPA BRKPT",   OME_UINT16, NULL, &(OSD_UINT16_t){ &rateProfile.tpa_breakpoint, 1000, 2000, 10}, 0 },
 281
 282     { "THR LIM TYPE",OME_TAB,    NULL, &(OSD_TAB_t)   { &rateProfile.throttle_limit_type, THROTTLE_LIMIT_TYPE_COUNT - 1, osdTableThrottleLimitType}, 0 },
 283     { "THR LIM %",   OME_UINT8,  NULL, &(OSD_UINT8_t) { &rateProfile.throttle_limit_percent, 25,  100,  1}, 0 },
 284
 285     { "BACK", OME_Back, NULL, NULL, 0 },
 286     { NULL, OME_END, NULL, NULL, 0 }
 287 };
 288
 289 static CMS_Menu cmsx_menuRateProfile = {
 290 #ifdef CMS_MENU_DEBUG
 291     .GUARD_text = "MENURATE",
 292     .GUARD_type = OME_MENU,
 293 #endif
 294     .onEnter = cmsx_RateProfileOnEnter,
 295     .onExit = cmsx_RateProfileWriteback,
 296     .onDisplayUpdate = NULL,
 297     .entries = cmsx_menuRateProfileEntries
 298 };
 299
 300 #ifdef USE_LAUNCH_CONTROL
 301 static uint8_t cmsx_launchControlMode;
 302 static uint8_t cmsx_launchControlAllowTriggerReset;
 303 static uint8_t cmsx_launchControlThrottlePercent;
 304 static uint8_t cmsx_launchControlAngleLimit;
 305 static uint8_t cmsx_launchControlGain;
 306
 307 static const void *cmsx_launchControlOnEnter(displayPort_t *pDisp)
 308 {
 309     UNUSED(pDisp);
 310
 311     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 312
 313     cmsx_launchControlMode  = pidProfile->launchControlMode;
 314     cmsx_launchControlAllowTriggerReset  = pidProfile->launchControlAllowTriggerReset;
 315     cmsx_launchControlThrottlePercent  = pidProfile->launchControlThrottlePercent;
 316     cmsx_launchControlAngleLimit  = pidProfile->launchControlAngleLimit;
 317     cmsx_launchControlGain  = pidProfile->launchControlGain;
 318
 319     return NULL;
 320 }
 321
 322 static const void *cmsx_launchControlOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 323 {
 324     UNUSED(pDisp);
 325     UNUSED(self);
 326
 327     pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
 328
 329     pidProfile->launchControlMode = cmsx_launchControlMode;
 330     pidProfile->launchControlAllowTriggerReset = cmsx_launchControlAllowTriggerReset;
 331     pidProfile->launchControlThrottlePercent = cmsx_launchControlThrottlePercent;
 332     pidProfile->launchControlAngleLimit = cmsx_launchControlAngleLimit;
 333     pidProfile->launchControlGain = cmsx_launchControlGain;
 334
 335     return NULL;
 336 }
 337
 338 static const OSD_Entry cmsx_menuLaunchControlEntries[] = {
 339     { "-- LAUNCH CONTROL --", OME_Label, NULL, pidProfileIndexString, 0 },
 340
 341     { "MODE",             OME_TAB,   NULL, &(OSD_TAB_t)   { &cmsx_launchControlMode, LAUNCH_CONTROL_MODE_COUNT - 1, osdLaunchControlModeNames}, 0 },
 342     { "ALLOW RESET",      OME_Bool,  NULL, &cmsx_launchControlAllowTriggerReset, 0 },
 343     { "TRIGGER THROTTLE", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlThrottlePercent, 0,  LAUNCH_CONTROL_THROTTLE_TRIGGER_MAX, 1 } , 0 },
 344     { "ANGLE LIMIT",      OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlAngleLimit,      0,  80, 1 } , 0 },
 345     { "ITERM GAIN",       OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlGain,            0, 200, 1 } , 0 },
 346
 347     { "BACK", OME_Back, NULL, NULL, 0 },
 348     { NULL, OME_END, NULL, NULL, 0 }
 349 };
 350
 351 static CMS_Menu cmsx_menuLaunchControl = {
 352 #ifdef CMS_MENU_DEBUG
 353     .GUARD_text = "LAUNCH",
 354     .GUARD_type = OME_MENU,
 355 #endif
 356     .onEnter = cmsx_launchControlOnEnter,
 357     .onExit = cmsx_launchControlOnExit,
 358     .onDisplayUpdate = NULL,
 359     .entries = cmsx_menuLaunchControlEntries,
 360 };
 361 #endif
 362
 363 static uint8_t  cmsx_feedForwardTransition;
 364 static uint8_t  cmsx_ff_boost;
 365 static uint8_t  cmsx_angleStrength;
 366 static uint8_t  cmsx_horizonStrength;
 367 static uint8_t  cmsx_horizonTransition;
 368 static uint8_t  cmsx_throttleBoost;
 369 static uint16_t cmsx_itermAcceleratorGain;
 370 static uint16_t cmsx_itermThrottleThreshold;
 371 static uint8_t  cmsx_motorOutputLimit;
 372 static int8_t   cmsx_autoProfileCellCount;
 373 #ifdef USE_D_MIN
 374 static uint8_t  cmsx_d_min[XYZ_AXIS_COUNT];
 375 static uint8_t  cmsx_d_min_gain;
 376 static uint8_t  cmsx_d_min_advance;
 377 #endif
 378
 379 #ifdef USE_ITERM_RELAX
 380 static uint8_t cmsx_iterm_relax;
 381 static uint8_t cmsx_iterm_relax_type;
 382 static uint8_t cmsx_iterm_relax_cutoff;
 383 #endif
 384
 385 #ifdef USE_INTERPOLATED_SP
 386 static uint8_t cmsx_ff_interpolate_sp;
 387 static uint8_t cmsx_ff_smooth_factor;
 388 #endif
 389
 390 static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
 391 {
 392     UNUSED(pDisp);
 393
 394     setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName);
 395
 396     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 397
 398     cmsx_feedForwardTransition  = pidProfile->feedForwardTransition;
 399     cmsx_ff_boost = pidProfile->ff_boost;
 400
 401     cmsx_angleStrength =     pidProfile->pid[PID_LEVEL].P;
 402     cmsx_horizonStrength =   pidProfile->pid[PID_LEVEL].I;
 403     cmsx_horizonTransition = pidProfile->pid[PID_LEVEL].D;
 404
 405     cmsx_itermAcceleratorGain   = pidProfile->itermAcceleratorGain;
 406     cmsx_itermThrottleThreshold = pidProfile->itermThrottleThreshold;
 407
 408     cmsx_throttleBoost = pidProfile->throttle_boost;
 409     cmsx_motorOutputLimit = pidProfile->motor_output_limit;
 410     cmsx_autoProfileCellCount = pidProfile->auto_profile_cell_count;
 411
 412 #ifdef USE_D_MIN
 413     for (unsigned i = 0; i < XYZ_AXIS_COUNT; i++) {
 414         cmsx_d_min[i]  = pidProfile->d_min[i];
 415     }
 416     cmsx_d_min_gain = pidProfile->d_min_gain;
 417     cmsx_d_min_advance = pidProfile->d_min_advance;
 418 #endif
 419
 420 #ifdef USE_ITERM_RELAX
 421     cmsx_iterm_relax = pidProfile->iterm_relax;
 422     cmsx_iterm_relax_type = pidProfile->iterm_relax_type;
 423     cmsx_iterm_relax_cutoff = pidProfile->iterm_relax_cutoff;
 424 #endif
 425
 426 #ifdef USE_INTERPOLATED_SP
 427     cmsx_ff_interpolate_sp = pidProfile->ff_interpolate_sp;
 428     cmsx_ff_smooth_factor = pidProfile->ff_smooth_factor;
 429 #endif
 430
 431     return NULL;
 432 }
 433
 434 static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 435 {
 436     UNUSED(pDisp);
 437     UNUSED(self);
 438
 439     pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
 440     pidProfile->feedForwardTransition = cmsx_feedForwardTransition;
 441     pidInitConfig(currentPidProfile);
 442     pidProfile->ff_boost = cmsx_ff_boost;
 443
 444     pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength;
 445     pidProfile->pid[PID_LEVEL].I = cmsx_horizonStrength;
 446     pidProfile->pid[PID_LEVEL].D = cmsx_horizonTransition;
 447
 448     pidProfile->itermAcceleratorGain   = cmsx_itermAcceleratorGain;
 449     pidProfile->itermThrottleThreshold = cmsx_itermThrottleThreshold;
 450
 451     pidProfile->throttle_boost = cmsx_throttleBoost;
 452     pidProfile->motor_output_limit = cmsx_motorOutputLimit;
 453     pidProfile->auto_profile_cell_count = cmsx_autoProfileCellCount;
 454
 455 #ifdef USE_D_MIN
 456     for (unsigned i = 0; i < XYZ_AXIS_COUNT; i++) {
 457         pidProfile->d_min[i] = cmsx_d_min[i];
 458     }
 459     pidProfile->d_min_gain = cmsx_d_min_gain;
 460     pidProfile->d_min_advance = cmsx_d_min_advance;
 461 #endif
 462
 463 #ifdef USE_ITERM_RELAX
 464     pidProfile->iterm_relax = cmsx_iterm_relax;
 465     pidProfile->iterm_relax_type = cmsx_iterm_relax_type;
 466     pidProfile->iterm_relax_cutoff = cmsx_iterm_relax_cutoff;
 467 #endif
 468
 469 #ifdef USE_INTERPOLATED_SP
 470     pidProfile->ff_interpolate_sp = cmsx_ff_interpolate_sp;
 471     pidProfile->ff_smooth_factor = cmsx_ff_smooth_factor;
 472 #endif
 473
 474     initEscEndpoints();
 475     return NULL;
 476 }
 477
 478 static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
 479     { "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 },
 480
 481     { "FF TRANS",      OME_FLOAT,  NULL, &(OSD_FLOAT_t)  { &cmsx_feedForwardTransition,  0,    100,   1, 10 }, 0 },
 482 #ifdef USE_INTERPOLATED_SP
 483     { "FF MODE",       OME_TAB,    NULL, &(OSD_TAB_t)    { &cmsx_ff_interpolate_sp,  4, lookupTableInterpolatedSetpoint}, 0 },
 484     { "FF SMOOTHNESS", OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_ff_smooth_factor,     0,     75,   1  }   , 0 },
 485 #endif
 486     { "FF BOOST",    OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_ff_boost,               0,     50,   1  }   , 0 },
 487     { "ANGLE STR",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_angleStrength,          0,    200,   1  }   , 0 },
 488     { "HORZN STR",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_horizonStrength,        0,    200,   1  }   , 0 },
 489     { "HORZN TRS",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_horizonTransition,      0,    200,   1  }   , 0 },
 490     { "AG GAIN",     OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_itermAcceleratorGain,   ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX, 10 }   , 0 },
 491     { "AG THR",      OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_itermThrottleThreshold, 20,   1000,  1  }   , 0 },
 492 #ifdef USE_THROTTLE_BOOST
 493     { "THR BOOST",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_throttleBoost,          0,    100,   1  }   , 0 },
 494 #endif
 495 #ifdef USE_ITERM_RELAX
 496     { "I_RELAX",         OME_TAB,    NULL, &(OSD_TAB_t)     { &cmsx_iterm_relax,        ITERM_RELAX_COUNT - 1,      lookupTableItermRelax       }, 0 },
 497     { "I_RELAX TYPE",    OME_TAB,    NULL, &(OSD_TAB_t)     { &cmsx_iterm_relax_type,   ITERM_RELAX_TYPE_COUNT - 1, lookupTableItermRelaxType   }, 0 },
 498     { "I_RELAX CUTOFF",  OME_UINT8,  NULL, &(OSD_UINT8_t)   { &cmsx_iterm_relax_cutoff, 1, 50, 1 }, 0 },
 499 #endif
 500 #ifdef USE_LAUNCH_CONTROL
 501     {"LAUNCH CONTROL", OME_Submenu, cmsMenuChange, &cmsx_menuLaunchControl, 0 },
 502 #endif
 503     { "MTR OUT LIM %",OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_motorOutputLimit, MOTOR_OUTPUT_LIMIT_PERCENT_MIN,  MOTOR_OUTPUT_LIMIT_PERCENT_MAX,  1}, 0 },
 504
 505     { "AUTO CELL CNT", OME_INT8, NULL, &(OSD_INT8_t) { &cmsx_autoProfileCellCount, AUTO_PROFILE_CELL_COUNT_CHANGE, MAX_AUTO_DETECT_CELL_COUNT, 1}, 0 },
 506
 507 #ifdef USE_D_MIN
 508     { "D_MIN ROLL",  OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_ROLL],      0, 100, 1 }, 0 },
 509     { "D_MIN PITCH", OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_PITCH],     0, 100, 1 }, 0 },
 510     { "D_MIN YAW",   OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_YAW],       0, 100, 1 }, 0 },
 511     { "D_MIN GAIN",  OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_min_gain,          0, 100, 1 }, 0 },
 512     { "D_MIN ADV",   OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_min_advance,       0, 200, 1 }, 0 },
 513 #endif
 514
 515     { "BACK", OME_Back, NULL, NULL, 0 },
 516     { NULL, OME_END, NULL, NULL, 0 }
 517 };
 518
 519 static CMS_Menu cmsx_menuProfileOther = {
 520 #ifdef CMS_MENU_DEBUG
 521     .GUARD_text = "XPROFOTHER",
 522     .GUARD_type = OME_MENU,
 523 #endif
 524     .onEnter = cmsx_profileOtherOnEnter,
 525     .onExit = cmsx_profileOtherOnExit,
 526     .onDisplayUpdate = NULL,
 527     .entries = cmsx_menuProfileOtherEntries,
 528 };
 529
 530
 531 static uint16_t gyroConfig_gyro_lowpass_hz;
 532 static uint16_t gyroConfig_gyro_lowpass2_hz;
 533 static uint16_t gyroConfig_gyro_soft_notch_hz_1;
 534 static uint16_t gyroConfig_gyro_soft_notch_cutoff_1;
 535 static uint16_t gyroConfig_gyro_soft_notch_hz_2;
 536 static uint16_t gyroConfig_gyro_soft_notch_cutoff_2;
 537 static uint8_t  gyroConfig_gyro_to_use;
 538
 539 static const void *cmsx_menuGyro_onEnter(displayPort_t *pDisp)
 540 {
 541     UNUSED(pDisp);
 542
 543     gyroConfig_gyro_lowpass_hz =  gyroConfig()->gyro_lowpass_hz;
 544     gyroConfig_gyro_lowpass2_hz =  gyroConfig()->gyro_lowpass2_hz;
 545     gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
 546     gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
 547     gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
 548     gyroConfig_gyro_soft_notch_cutoff_2 = gyroConfig()->gyro_soft_notch_cutoff_2;
 549     gyroConfig_gyro_to_use = gyroConfig()->gyro_to_use;
 550
 551     return NULL;
 552 }
 553
 554 static const void *cmsx_menuGyro_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 555 {
 556     UNUSED(pDisp);
 557     UNUSED(self);
 558
 559     gyroConfigMutable()->gyro_lowpass_hz =  gyroConfig_gyro_lowpass_hz;
 560     gyroConfigMutable()->gyro_lowpass2_hz =  gyroConfig_gyro_lowpass2_hz;
 561     gyroConfigMutable()->gyro_soft_notch_hz_1 = gyroConfig_gyro_soft_notch_hz_1;
 562     gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
 563     gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
 564     gyroConfigMutable()->gyro_soft_notch_cutoff_2 = gyroConfig_gyro_soft_notch_cutoff_2;
 565     gyroConfigMutable()->gyro_to_use = gyroConfig_gyro_to_use;
 566
 567     return NULL;
 568 }
 569
 570 static const OSD_Entry cmsx_menuFilterGlobalEntries[] =
 571 {
 572     { "-- FILTER GLB  --", OME_Label, NULL, NULL, 0 },
 573
 574     { "GYRO LPF",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass_hz, 0, FILTER_FREQUENCY_MAX, 1 }, 0 },
 575 #ifdef USE_GYRO_LPF2
 576     { "GYRO LPF2",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz,  0, FILTER_FREQUENCY_MAX, 1 }, 0 },
 577 #endif
 578     { "GYRO NF1",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_1,     0, 500, 1 }, 0 },
 579     { "GYRO NF1C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
 580     { "GYRO NF2",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2,     0, 500, 1 }, 0 },
 581     { "GYRO NF2C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
 582 #ifdef USE_MULTI_GYRO
 583     { "GYRO TO USE",  OME_TAB,  NULL, &(OSD_TAB_t)    { &gyroConfig_gyro_to_use,  2, osdTableGyroToUse}, REBOOT_REQUIRED },
 584 #endif
 585
 586     { "BACK", OME_Back, NULL, NULL, 0 },
 587     { NULL, OME_END, NULL, NULL, 0 }
 588 };
 589
 590 static CMS_Menu cmsx_menuFilterGlobal = {
 591 #ifdef CMS_MENU_DEBUG
 592     .GUARD_text = "XFLTGLB",
 593     .GUARD_type = OME_MENU,
 594 #endif
 595     .onEnter = cmsx_menuGyro_onEnter,
 596     .onExit = cmsx_menuGyro_onExit,
 597     .onDisplayUpdate = NULL,
 598     .entries = cmsx_menuFilterGlobalEntries,
 599 };
 600
 601 #if (defined(USE_GYRO_DATA_ANALYSE) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
 602
 603 #ifdef USE_GYRO_DATA_ANALYSE
 604 static uint16_t dynFiltNotchMaxHz;
 605 static uint8_t  dynFiltWidthPercent;
 606 static uint16_t dynFiltNotchQ;
 607 static uint16_t dynFiltNotchMinHz;
 608 #endif
 609 #ifdef USE_DYN_LPF
 610 static uint16_t dynFiltGyroMin;
 611 static uint16_t dynFiltGyroMax;
 612 static uint16_t dynFiltDtermMin;
 613 static uint16_t dynFiltDtermMax;
 614 static uint8_t dynFiltDtermExpo;
 615 #endif
 616
 617 static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
 618 {
 619     UNUSED(pDisp);
 620
 621 #ifdef USE_GYRO_DATA_ANALYSE
 622     dynFiltNotchMaxHz   = gyroConfig()->dyn_notch_max_hz;
 623     dynFiltWidthPercent = gyroConfig()->dyn_notch_width_percent;
 624     dynFiltNotchQ       = gyroConfig()->dyn_notch_q;
 625     dynFiltNotchMinHz   = gyroConfig()->dyn_notch_min_hz;
 626 #endif
 627 #ifdef USE_DYN_LPF
 628     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 629     dynFiltGyroMin  = gyroConfig()->dyn_lpf_gyro_min_hz;
 630     dynFiltGyroMax  = gyroConfig()->dyn_lpf_gyro_max_hz;
 631     dynFiltDtermMin = pidProfile->dyn_lpf_dterm_min_hz;
 632     dynFiltDtermMax = pidProfile->dyn_lpf_dterm_max_hz;
 633     dynFiltDtermExpo = pidProfile->dyn_lpf_curve_expo;
 634 #endif
 635
 636     return NULL;
 637 }
 638
 639 static const void *cmsx_menuDynFilt_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 640 {
 641     UNUSED(pDisp);
 642     UNUSED(self);
 643
 644 #ifdef USE_GYRO_DATA_ANALYSE
 645     gyroConfigMutable()->dyn_notch_max_hz        = dynFiltNotchMaxHz;
 646     gyroConfigMutable()->dyn_notch_width_percent = dynFiltWidthPercent;
 647     gyroConfigMutable()->dyn_notch_q             = dynFiltNotchQ;
 648     gyroConfigMutable()->dyn_notch_min_hz        = dynFiltNotchMinHz;
 649 #endif
 650 #ifdef USE_DYN_LPF
 651     pidProfile_t *pidProfile = currentPidProfile;
 652     gyroConfigMutable()->dyn_lpf_gyro_min_hz = dynFiltGyroMin;
 653     gyroConfigMutable()->dyn_lpf_gyro_max_hz = dynFiltGyroMax;
 654     pidProfile->dyn_lpf_dterm_min_hz         = dynFiltDtermMin;
 655     pidProfile->dyn_lpf_dterm_max_hz         = dynFiltDtermMax;
 656     pidProfile->dyn_lpf_curve_expo           = dynFiltDtermExpo;
 657 #endif
 658
 659     return NULL;
 660 }
 661
 662 static const OSD_Entry cmsx_menuDynFiltEntries[] =
 663 {
 664     { "-- DYN FILT --", OME_Label, NULL, NULL, 0 },
 665
 666 #ifdef USE_GYRO_DATA_ANALYSE
 667     { "NOTCH WIDTH %",  OME_UINT8,  NULL, &(OSD_UINT8_t)  { &dynFiltWidthPercent, 0, 20, 1 }, 0 },
 668     { "NOTCH Q",        OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ,       0, 1000, 1 }, 0 },
 669     { "NOTCH MIN HZ",   OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz,   0, 1000, 1 }, 0 },
 670     { "NOTCH MAX HZ",   OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMaxHz,   0, 1000, 1 }, 0 },
 671 #endif
 672
 673 #ifdef USE_DYN_LPF
 674     { "LPF GYRO MIN",   OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltGyroMin,  0, 1000, 1 }, 0 },
 675     { "LPF GYRO MAX",   OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltGyroMax,  0, 1000, 1 }, 0 },
 676     { "DTERM DLPF MIN", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltDtermMin, 0, 1000, 1 }, 0 },
 677     { "DTERM DLPF MAX", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltDtermMax, 0, 1000, 1 }, 0 },
 678     { "DTERM DLPF EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltDtermExpo, 0, 10, 1 }, 0 },
 679 #endif
 680
 681     { "BACK", OME_Back, NULL, NULL, 0 },
 682     { NULL, OME_END, NULL, NULL, 0 }
 683 };
 684
 685 static CMS_Menu cmsx_menuDynFilt = {
 686 #ifdef CMS_MENU_DEBUG
 687     .GUARD_text = "XDYNFLT",
 688     .GUARD_type = OME_MENU,
 689 #endif
 690     .onEnter = cmsx_menuDynFilt_onEnter,
 691     .onExit = cmsx_menuDynFilt_onExit,
 692     .onDisplayUpdate = NULL,
 693     .entries = cmsx_menuDynFiltEntries,
 694 };
 695
 696 #endif
 697
 698 static uint16_t cmsx_dterm_lowpass_hz;
 699 static uint16_t cmsx_dterm_lowpass2_hz;
 700 static uint16_t cmsx_dterm_notch_hz;
 701 static uint16_t cmsx_dterm_notch_cutoff;
 702 static uint16_t cmsx_yaw_lowpass_hz;
 703
 704 static const void *cmsx_FilterPerProfileRead(displayPort_t *pDisp)
 705 {
 706     UNUSED(pDisp);
 707
 708     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 709
 710     cmsx_dterm_lowpass_hz   = pidProfile->dterm_lowpass_hz;
 711     cmsx_dterm_lowpass2_hz  = pidProfile->dterm_lowpass2_hz;
 712     cmsx_dterm_notch_hz     = pidProfile->dterm_notch_hz;
 713     cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff;
 714     cmsx_yaw_lowpass_hz     = pidProfile->yaw_lowpass_hz;
 715
 716     return NULL;
 717 }
 718
 719 static const void *cmsx_FilterPerProfileWriteback(displayPort_t *pDisp, const OSD_Entry *self)
 720 {
 721     UNUSED(pDisp);
 722     UNUSED(self);
 723
 724     pidProfile_t *pidProfile = currentPidProfile;
 725
 726     pidProfile->dterm_lowpass_hz   = cmsx_dterm_lowpass_hz;
 727     pidProfile->dterm_lowpass2_hz  = cmsx_dterm_lowpass2_hz;
 728     pidProfile->dterm_notch_hz     = cmsx_dterm_notch_hz;
 729     pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
 730     pidProfile->yaw_lowpass_hz     = cmsx_yaw_lowpass_hz;
 731
 732     return NULL;
 733 }
 734
 735 static const OSD_Entry cmsx_menuFilterPerProfileEntries[] =
 736 {
 737     { "-- FILTER PP  --", OME_Label, NULL, NULL, 0 },
 738
 739     { "DTERM LPF",  OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass_hz,     0, FILTER_FREQUENCY_MAX, 1 }, 0 },
 740     { "DTERM LPF2", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz,    0, FILTER_FREQUENCY_MAX, 1 }, 0 },
 741     { "DTERM NF",   OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz,       0, FILTER_FREQUENCY_MAX, 1 }, 0 },
 742     { "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff,   0, FILTER_FREQUENCY_MAX, 1 }, 0 },
 743     { "YAW LPF",    OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lowpass_hz,       0, 500, 1 }, 0 },
 744
 745     { "BACK", OME_Back, NULL, NULL, 0 },
 746     { NULL, OME_END, NULL, NULL, 0 }
 747 };
 748
 749 static CMS_Menu cmsx_menuFilterPerProfile = {
 750 #ifdef CMS_MENU_DEBUG
 751     .GUARD_text = "XFLTPP",
 752     .GUARD_type = OME_MENU,
 753 #endif
 754     .onEnter = cmsx_FilterPerProfileRead,
 755     .onExit = cmsx_FilterPerProfileWriteback,
 756     .onDisplayUpdate = NULL,
 757     .entries = cmsx_menuFilterPerProfileEntries,
 758 };
 759
 760 #ifdef USE_EXTENDED_CMS_MENUS
 761
 762 static uint8_t cmsx_dstPidProfile;
 763 static uint8_t cmsx_dstControlRateProfile;
 764
 765 static const char * const cmsx_ProfileNames[] = {
 766     "-",
 767     "1",
 768     "2",
 769     "3"
 770 };
 771
 772 static OSD_TAB_t cmsx_PidProfileTable = { &cmsx_dstPidProfile, 3, cmsx_ProfileNames };
 773 static OSD_TAB_t cmsx_ControlRateProfileTable = { &cmsx_dstControlRateProfile, 3, cmsx_ProfileNames };
 774
 775 static const void *cmsx_menuCopyProfile_onEnter(displayPort_t *pDisp)
 776 {
 777     UNUSED(pDisp);
 778
 779     cmsx_dstPidProfile = 0;
 780     cmsx_dstControlRateProfile = 0;
 781
 782     return NULL;
 783 }
 784
 785 static const void *cmsx_CopyPidProfile(displayPort_t *pDisplay, const void *ptr)
 786 {
 787     UNUSED(pDisplay);
 788     UNUSED(ptr);
 789
 790     if (cmsx_dstPidProfile > 0) {
 791         pidCopyProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex());
 792     }
 793
 794     return NULL;
 795 }
 796
 797 static const void *cmsx_CopyControlRateProfile(displayPort_t *pDisplay, const void *ptr)
 798 {
 799     UNUSED(pDisplay);
 800     UNUSED(ptr);
 801
 802     if (cmsx_dstControlRateProfile > 0) {
 803         copyControlRateProfile(cmsx_dstControlRateProfile - 1, getCurrentControlRateProfileIndex());
 804     }
 805
 806     return NULL;
 807 }
 808
 809 static const OSD_Entry cmsx_menuCopyProfileEntries[] =
 810 {
 811     { "-- COPY PROFILE --", OME_Label, NULL, NULL, 0},
 812
 813     { "CPY PID PROF TO",   OME_TAB,      NULL,                        &cmsx_PidProfileTable, 0 },
 814     { "COPY PP",           OME_Funcall,  cmsx_CopyPidProfile,         NULL, 0 },
 815     { "CPY RATE PROF TO",  OME_TAB,      NULL,                        &cmsx_ControlRateProfileTable, 0 },
 816     { "COPY RP",           OME_Funcall,  cmsx_CopyControlRateProfile, NULL, 0 },
 817
 818     { "BACK", OME_Back, NULL, NULL, 0 },
 819     { NULL, OME_END, NULL, NULL, 0 }
 820 };
 821
 822 CMS_Menu cmsx_menuCopyProfile = {
 823 #ifdef CMS_MENU_DEBUG
 824     .GUARD_text = "XCPY",
 825     .GUARD_type = OME_MENU,
 826 #endif
 827     .onEnter = cmsx_menuCopyProfile_onEnter,
 828     .onExit = NULL,
 829     .onDisplayUpdate = NULL,
 830     .entries = cmsx_menuCopyProfileEntries,
 831 };
 832
 833 #endif
 834
 835 static const OSD_Entry cmsx_menuImuEntries[] =
 836 {
 837     { "-- PROFILE --", OME_Label, NULL, NULL, 0},
 838
 839     {"PID PROF",  OME_UINT8,   cmsx_profileIndexOnChange,     &(OSD_UINT8_t){ &tmpPidProfileIndex, 1, PID_PROFILE_COUNT, 1},    0},
 840     {"PID",       OME_Submenu, cmsMenuChange,                 &cmsx_menuPid,                                                 0},
 841     {"MISC PP",   OME_Submenu, cmsMenuChange,                 &cmsx_menuProfileOther,                                        0},
 842     {"FILT PP",   OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterPerProfile,                                    0},
 843
 844     {"RATE PROF", OME_UINT8,   cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, CONTROL_RATE_PROFILE_COUNT, 1}, 0},
 845     {"RATE",      OME_Submenu, cmsMenuChange,                 &cmsx_menuRateProfile,                                         0},
 846
 847     {"FILT GLB",  OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterGlobal,                                        0},
 848 #if  (defined(USE_GYRO_DATA_ANALYSE) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
 849     {"DYN FILT",  OME_Submenu, cmsMenuChange,                 &cmsx_menuDynFilt,                                             0},
 850 #endif
 851
 852 #ifdef USE_EXTENDED_CMS_MENUS
 853     {"COPY PROF", OME_Submenu, cmsMenuChange,                 &cmsx_menuCopyProfile,                                         0},
 854 #endif /* USE_EXTENDED_CMS_MENUS */
 855
 856     {"BACK", OME_Back, NULL, NULL, 0},
 857     {NULL, OME_END, NULL, NULL, 0}
 858 };
 859
 860 CMS_Menu cmsx_menuImu = {
 861 #ifdef CMS_MENU_DEBUG
 862     .GUARD_text = "XIMU",
 863     .GUARD_type = OME_MENU,
 864 #endif
 865     .onEnter = cmsx_menuImu_onEnter,
 866     .onExit = cmsx_menuImu_onExit,
 867     .onDisplayUpdate = NULL,
 868     .entries = cmsx_menuImuEntries,
 869 };
 870
 871 #endif // CMS