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 #include "config/simplified_tuning.h"
  44
  45 #include "drivers/pwm_output.h"
  46
  47 #include "config/config.h"
  48 #include "fc/controlrate_profile.h"
  49 #include "fc/core.h"
  50 #include "fc/rc_controls.h"
  51 #include "fc/runtime_config.h"
  52
  53 #include "flight/mixer.h"
  54 #include "flight/pid.h"
  55 #include "flight/pid_init.h"
  56
  57 #include "pg/pg.h"
  58
  59 #include "sensors/battery.h"
  60 #include "sensors/gyro.h"
  61
  62 #include "cli/settings.h"
  63
  64 //
  65 // PID
  66 //
  67
  68 #define PROFILE_INDEX_STRING_ADDITIONAL_SIZE 5 // Additional symbols for setProfileIndexString(): "2 (NAMENAME)\0"
  69
  70 static uint8_t tmpPidProfileIndex;
  71 static uint8_t pidProfileIndex;
  72 static char pidProfileIndexString[MAX_PROFILE_NAME_LENGTH + PROFILE_INDEX_STRING_ADDITIONAL_SIZE];
  73 static uint8_t tempPid[3][3];
  74 static uint16_t tempPidF[3];
  75
  76 static uint8_t tmpRateProfileIndex;
  77 static uint8_t rateProfileIndex;
  78 static char rateProfileIndexString[MAX_RATE_PROFILE_NAME_LENGTH + PROFILE_INDEX_STRING_ADDITIONAL_SIZE];
  79 static controlRateConfig_t rateProfile;
  80
  81 static const char * const osdTableThrottleLimitType[] = {
  82     "OFF", "SCALE", "CLIP"
  83 };
  84
  85 #ifdef USE_MULTI_GYRO
  86 static const char * const osdTableGyroToUse[] = {
  87     "FIRST", "SECOND", "BOTH"
  88 };
  89 #endif
  90
  91 static void setProfileIndexString(char *profileString, int profileIndex, const char *profileName)
  92 {
  93     int charIndex = 0;
  94     profileString[charIndex++] = '1' + profileIndex;
  95
  96 #ifdef USE_PROFILE_NAMES
  97     const int profileNameLen = strlen(profileName);
  98
  99     if (profileNameLen > 0) {
 100         profileString[charIndex++] = ' ';
 101         profileString[charIndex++] = '(';
 102         for (int i = 0; i < profileNameLen; i++) {
 103             profileString[charIndex++] = toupper(profileName[i]);
 104         }
 105         profileString[charIndex++] = ')';
 106     }
 107 #else
 108     UNUSED(profileName);
 109 #endif
 110
 111     profileString[charIndex] = '\0';
 112 }
 113
 114 static char pidProfileNames[PID_PROFILE_COUNT][MAX_PROFILE_NAME_LENGTH + PROFILE_INDEX_STRING_ADDITIONAL_SIZE];
 115 static const char *pidProfileNamePtrs[PID_PROFILE_COUNT];
 116
 117 static char rateProfileNames[CONTROL_RATE_PROFILE_COUNT][MAX_PROFILE_NAME_LENGTH + PROFILE_INDEX_STRING_ADDITIONAL_SIZE];
 118 static const char *rateProfileNamePtrs[CONTROL_RATE_PROFILE_COUNT];
 119
 120 static const void *cmsx_menuImu_onEnter(displayPort_t *pDisp)
 121 {
 122     UNUSED(pDisp);
 123
 124     for (int i = 0; i < PID_PROFILE_COUNT; i++) {
 125         setProfileIndexString(pidProfileNames[i], i, pidProfiles(i)->profileName);
 126         pidProfileNamePtrs[i] = pidProfileNames[i];
 127     }
 128
 129     pidProfileIndex = getCurrentPidProfileIndex();
 130     tmpPidProfileIndex = pidProfileIndex;
 131
 132     for (int i = 0; i < CONTROL_RATE_PROFILE_COUNT; i++) {
 133         setProfileIndexString(rateProfileNames[i], i, controlRateProfilesMutable(i)->profileName);
 134         rateProfileNamePtrs[i] = rateProfileNames[i];
 135     }
 136
 137     rateProfileIndex = getCurrentControlRateProfileIndex();
 138     tmpRateProfileIndex = rateProfileIndex;
 139
 140     return NULL;
 141 }
 142
 143 static const void *cmsx_menuImu_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 144 {
 145     UNUSED(pDisp);
 146     UNUSED(self);
 147
 148     changePidProfile(pidProfileIndex);
 149     changeControlRateProfile(rateProfileIndex);
 150
 151     return NULL;
 152 }
 153
 154 static const void *cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *ptr)
 155 {
 156     UNUSED(displayPort);
 157     UNUSED(ptr);
 158
 159     pidProfileIndex = tmpPidProfileIndex;
 160     changePidProfile(pidProfileIndex);
 161
 162     return NULL;
 163 }
 164
 165 static const void *cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void *ptr)
 166 {
 167     UNUSED(displayPort);
 168     UNUSED(ptr);
 169
 170     rateProfileIndex = tmpRateProfileIndex;
 171     changeControlRateProfile(rateProfileIndex);
 172
 173     return NULL;
 174 }
 175
 176 static const void *cmsx_PidRead(void)
 177 {
 178
 179     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 180     for (uint8_t i = 0; i < 3; i++) {
 181         tempPid[i][0] = pidProfile->pid[i].P;
 182         tempPid[i][1] = pidProfile->pid[i].I;
 183         tempPid[i][2] = pidProfile->pid[i].D;
 184         tempPidF[i] = pidProfile->pid[i].F;
 185     }
 186
 187     return NULL;
 188 }
 189
 190 static const void *cmsx_PidOnEnter(displayPort_t *pDisp)
 191 {
 192     UNUSED(pDisp);
 193
 194     setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName);
 195     cmsx_PidRead();
 196
 197     return NULL;
 198 }
 199
 200 static const void *cmsx_PidWriteback(displayPort_t *pDisp, const OSD_Entry *self)
 201 {
 202     UNUSED(pDisp);
 203     UNUSED(self);
 204
 205     pidProfile_t *pidProfile = currentPidProfile;
 206     for (uint8_t i = 0; i < 3; i++) {
 207         pidProfile->pid[i].P = tempPid[i][0];
 208         pidProfile->pid[i].I = tempPid[i][1];
 209         pidProfile->pid[i].D = tempPid[i][2];
 210         pidProfile->pid[i].F = tempPidF[i];
 211     }
 212     pidInitConfig(currentPidProfile);
 213
 214     return NULL;
 215 }
 216
 217 static OSD_Entry cmsx_menuPidEntries[] =
 218 {
 219     { "-- PID --", OME_Label, NULL, pidProfileIndexString},
 220
 221     { "ROLL  P", OME_UINT8 | SLIDER_RP, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][0],  0, 200, 1 }},
 222     { "ROLL  I", OME_UINT8 | SLIDER_RP, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][1],  0, 200, 1 }},
 223     { "ROLL  D", OME_UINT8 | SLIDER_RP, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][2],  0, 200, 1 }},
 224     { "ROLL  F", OME_UINT16 | SLIDER_RP, NULL, &(OSD_UINT16_t){ &tempPidF[PID_ROLL],  0, 2000, 1 }},
 225
 226     { "PITCH P", OME_UINT8 | SLIDER_RP, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][0], 0, 200, 1 }},
 227     { "PITCH I", OME_UINT8 | SLIDER_RP, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][1], 0, 200, 1 }},
 228     { "PITCH D", OME_UINT8 | SLIDER_RP, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][2], 0, 200, 1 }},
 229     { "PITCH F", OME_UINT16 | SLIDER_RP, NULL, &(OSD_UINT16_t){ &tempPidF[PID_PITCH], 0, 2000, 1 }},
 230
 231     { "YAW   P", OME_UINT8 | SLIDER_RPY, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][0],   0, 200, 1 }},
 232     { "YAW   I", OME_UINT8 | SLIDER_RPY, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][1],   0, 200, 1 }},
 233     { "YAW   D", OME_UINT8 | SLIDER_RPY, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][2],   0, 200, 1 }},
 234     { "YAW   F", OME_UINT16 | SLIDER_RPY, NULL, &(OSD_UINT16_t){ &tempPidF[PID_YAW],   0, 2000, 1 }},
 235
 236     { "BACK", OME_Back, NULL, NULL },
 237     { NULL, OME_END, NULL, NULL}
 238 };
 239
 240 static CMS_Menu cmsx_menuPid = {
 241 #ifdef CMS_MENU_DEBUG
 242     .GUARD_text = "XPID",
 243     .GUARD_type = OME_MENU,
 244 #endif
 245     .onEnter = cmsx_PidOnEnter,
 246     .onExit = cmsx_PidWriteback,
 247     .onDisplayUpdate = NULL,
 248     .entries = cmsx_menuPidEntries
 249 };
 250
 251 #ifdef USE_SIMPLIFIED_TUNING
 252 static uint8_t cmsx_simplified_pids_mode;
 253 static uint8_t cmsx_simplified_master_multiplier;
 254 static uint8_t cmsx_simplified_roll_pitch_ratio;
 255 static uint8_t cmsx_simplified_i_gain;
 256 static uint8_t cmsx_simplified_d_gain;
 257 static uint8_t cmsx_simplified_pi_gain;
 258 #ifdef USE_D_MIN
 259 static uint8_t cmsx_simplified_dmin_ratio;
 260 #endif
 261 static uint8_t cmsx_simplified_feedforward_gain;
 262 static uint8_t cmsx_simplified_pitch_pi_gain;
 263
 264 static uint8_t cmsx_simplified_dterm_filter;
 265 static uint8_t cmsx_simplified_dterm_filter_multiplier;
 266 static uint8_t cmsx_simplified_gyro_filter;
 267 static uint8_t cmsx_simplified_gyro_filter_multiplier;
 268 static uint8_t cmsx_tpa_rate;
 269 static uint16_t cmsx_tpa_breakpoint;
 270
 271 static const void *cmsx_simplifiedTuningOnEnter(displayPort_t *pDisp)
 272 {
 273     UNUSED(pDisp);
 274
 275     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 276
 277     cmsx_simplified_pids_mode = pidProfile->simplified_pids_mode;
 278     cmsx_simplified_master_multiplier = pidProfile->simplified_master_multiplier;
 279     cmsx_simplified_roll_pitch_ratio = pidProfile->simplified_roll_pitch_ratio;
 280     cmsx_simplified_i_gain = pidProfile->simplified_i_gain;
 281     cmsx_simplified_d_gain = pidProfile->simplified_d_gain;
 282     cmsx_simplified_pi_gain = pidProfile->simplified_pi_gain;
 283 #ifdef USE_D_MIN
 284     cmsx_simplified_dmin_ratio = pidProfile->simplified_dmin_ratio;
 285 #endif
 286     cmsx_simplified_feedforward_gain = pidProfile->simplified_feedforward_gain;
 287     cmsx_simplified_pitch_pi_gain = pidProfile->simplified_pitch_pi_gain;
 288
 289     cmsx_simplified_dterm_filter = pidProfile->simplified_dterm_filter;
 290     cmsx_simplified_dterm_filter_multiplier = pidProfile->simplified_dterm_filter_multiplier;
 291     cmsx_simplified_gyro_filter = gyroConfig()->simplified_gyro_filter;
 292     cmsx_simplified_gyro_filter_multiplier = gyroConfig()->simplified_gyro_filter_multiplier;
 293
 294     return 0;
 295 }
 296
 297 static const void *cmsx_simplifiedTuningOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 298 {
 299     UNUSED(pDisp);
 300     UNUSED(self);
 301
 302     pidProfile_t *pidProfile = currentPidProfile;
 303
 304     const bool anySettingChanged = pidProfile->simplified_pids_mode != cmsx_simplified_pids_mode
 305         || pidProfile->simplified_master_multiplier != cmsx_simplified_master_multiplier
 306         || pidProfile->simplified_roll_pitch_ratio != cmsx_simplified_roll_pitch_ratio
 307         || pidProfile->simplified_i_gain != cmsx_simplified_i_gain
 308         || pidProfile->simplified_d_gain != cmsx_simplified_d_gain
 309         || pidProfile->simplified_pi_gain != cmsx_simplified_pi_gain
 310 #ifdef USE_D_MIN
 311         || pidProfile->simplified_dmin_ratio != cmsx_simplified_dmin_ratio
 312 #endif
 313         || pidProfile->simplified_feedforward_gain != cmsx_simplified_feedforward_gain
 314         || pidProfile->simplified_pitch_pi_gain != cmsx_simplified_pitch_pi_gain
 315         || pidProfile->simplified_dterm_filter != cmsx_simplified_dterm_filter
 316         || pidProfile->simplified_dterm_filter_multiplier != cmsx_simplified_dterm_filter_multiplier
 317         || gyroConfigMutable()->simplified_gyro_filter != cmsx_simplified_gyro_filter
 318         || gyroConfigMutable()->simplified_gyro_filter_multiplier != cmsx_simplified_gyro_filter_multiplier;
 319
 320     if (anySettingChanged) {
 321         pidProfile->simplified_pids_mode = cmsx_simplified_pids_mode;
 322         pidProfile->simplified_master_multiplier = cmsx_simplified_master_multiplier;
 323         pidProfile->simplified_roll_pitch_ratio = cmsx_simplified_roll_pitch_ratio;
 324         pidProfile->simplified_i_gain = cmsx_simplified_i_gain;
 325         pidProfile->simplified_d_gain = cmsx_simplified_d_gain;
 326         pidProfile->simplified_pi_gain = cmsx_simplified_pi_gain;
 327 #ifdef USE_D_MIN
 328         pidProfile->simplified_dmin_ratio = cmsx_simplified_dmin_ratio;
 329 #endif
 330         pidProfile->simplified_feedforward_gain = cmsx_simplified_feedforward_gain;
 331         pidProfile->simplified_pitch_pi_gain = cmsx_simplified_pitch_pi_gain;
 332
 333         pidProfile->simplified_dterm_filter = cmsx_simplified_dterm_filter;
 334         pidProfile->simplified_dterm_filter_multiplier = cmsx_simplified_dterm_filter_multiplier;
 335         gyroConfigMutable()->simplified_gyro_filter = cmsx_simplified_gyro_filter;
 336         gyroConfigMutable()->simplified_gyro_filter_multiplier = cmsx_simplified_gyro_filter_multiplier;
 337
 338         applySimplifiedTuning(currentPidProfile, gyroConfigMutable());
 339     }
 340
 341     return 0;
 342 }
 343
 344 static const OSD_Entry cmsx_menuSimplifiedTuningEntries[] =
 345 {
 346     { "-- SIMPLIFIED PID --", OME_Label, NULL, NULL},
 347     { "PID TUNING",        OME_TAB,   NULL, &(OSD_TAB_t)   { &cmsx_simplified_pids_mode, PID_SIMPLIFIED_TUNING_MODE_COUNT - 1, lookupTableSimplifiedTuningPidsMode } },
 348
 349     { "-- BASIC --",       OME_Label, NULL, NULL},
 350     { "D GAINS",           OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_d_gain,            SIMPLIFIED_TUNING_PIDS_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 } },
 351     { "P&I GAINS",         OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_pi_gain,           SIMPLIFIED_TUNING_PIDS_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 } },
 352     { "FF GAINS",          OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_feedforward_gain,  SIMPLIFIED_TUNING_PIDS_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 } },
 353
 354     { "-- EXPERT --",      OME_Label, NULL, NULL},
 355 #ifdef USE_D_MIN
 356     { "D MAX",             OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_dmin_ratio,        SIMPLIFIED_TUNING_PIDS_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 } },
 357 #endif
 358     { "I GAINS",           OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_i_gain,            SIMPLIFIED_TUNING_PIDS_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 } },
 359
 360     { "PITCH:ROLL D",      OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_roll_pitch_ratio,  SIMPLIFIED_TUNING_PIDS_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 } },
 361     { "PITCH:ROLL P,I&FF", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_pitch_pi_gain,     SIMPLIFIED_TUNING_PIDS_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 } },
 362     { "MASTER MULT",       OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_master_multiplier, SIMPLIFIED_TUNING_PIDS_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 } },
 363
 364     { "-- SIMPLIFIED FILTER --", OME_Label, NULL, NULL},
 365     { "GYRO TUNING",       OME_TAB,   NULL, &(OSD_TAB_t)   { &cmsx_simplified_gyro_filter,  1, lookupTableOffOn } },
 366     { "GYRO MULT",         OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_gyro_filter_multiplier,  SIMPLIFIED_TUNING_FILTERS_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 } },
 367     { "DTERM TUNING",      OME_TAB,   NULL, &(OSD_TAB_t)   { &cmsx_simplified_dterm_filter, 1, lookupTableOffOn } },
 368     { "DTERM MULT",        OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_dterm_filter_multiplier, SIMPLIFIED_TUNING_FILTERS_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 } },
 369
 370     { "BACK", OME_Back, NULL, NULL },
 371     { NULL, OME_END, NULL, NULL}
 372 };
 373
 374 static CMS_Menu cmsx_menuSimplifiedTuning = {
 375 #ifdef CMS_MENU_DEBUG
 376     .GUARD_text = "XSIMPLIFIED",
 377     .GUARD_type = OME_MENU,
 378 #endif
 379     .onEnter = cmsx_simplifiedTuningOnEnter,
 380     .onExit = cmsx_simplifiedTuningOnExit,
 381     .entries = cmsx_menuSimplifiedTuningEntries,
 382 };
 383 #endif // USE_SIMPLIFIED_TUNING
 384
 385 //
 386 // Rate & Expo
 387 //
 388
 389 static const void *cmsx_RateProfileRead(void)
 390 {
 391     memcpy(&rateProfile, controlRateProfiles(rateProfileIndex), sizeof(controlRateConfig_t));
 392
 393     return NULL;
 394 }
 395
 396 static const void *cmsx_RateProfileWriteback(displayPort_t *pDisp, const OSD_Entry *self)
 397 {
 398     UNUSED(pDisp);
 399     UNUSED(self);
 400
 401     memcpy(controlRateProfilesMutable(rateProfileIndex), &rateProfile, sizeof(controlRateConfig_t));
 402
 403     return NULL;
 404 }
 405
 406 static const void *cmsx_RateProfileOnEnter(displayPort_t *pDisp)
 407 {
 408     UNUSED(pDisp);
 409
 410     setProfileIndexString(rateProfileIndexString, rateProfileIndex, controlRateProfilesMutable(rateProfileIndex)->profileName);
 411     cmsx_RateProfileRead();
 412
 413     return NULL;
 414 }
 415
 416 static const OSD_Entry cmsx_menuRateProfileEntries[] =
 417 {
 418     { "-- RATE --", OME_Label, NULL, rateProfileIndexString },
 419
 420     { "RC R RATE",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcRates[FD_ROLL],    1, CONTROL_RATE_CONFIG_RC_RATES_MAX, 1, 10 } },
 421     { "RC P RATE",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcRates[FD_PITCH],    1, CONTROL_RATE_CONFIG_RC_RATES_MAX, 1, 10 } },
 422     { "RC Y RATE",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcRates[FD_YAW], 1, CONTROL_RATE_CONFIG_RC_RATES_MAX, 1, 10 } },
 423
 424     { "ROLL SUPER",  OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rates[FD_ROLL],   0, CONTROL_RATE_CONFIG_RATE_MAX, 1, 10 } },
 425     { "PITCH SUPER", OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rates[FD_PITCH],   0, CONTROL_RATE_CONFIG_RATE_MAX, 1, 10 } },
 426     { "YAW SUPER",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rates[FD_YAW],   0, CONTROL_RATE_CONFIG_RATE_MAX, 1, 10 } },
 427
 428     { "RC R EXPO",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo[FD_ROLL],    0, 100, 1, 10 } },
 429     { "RC P EXPO",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo[FD_PITCH],    0, 100, 1, 10 } },
 430     { "RC Y EXPO",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo[FD_YAW], 0, 100, 1, 10 } },
 431
 432     { "THR MID",     OME_UINT8,  NULL, &(OSD_UINT8_t) { &rateProfile.thrMid8,           0,  100,  1} },
 433     { "THR EXPO",    OME_UINT8,  NULL, &(OSD_UINT8_t) { &rateProfile.thrExpo8,          0,  100,  1} },
 434
 435     { "THR LIM TYPE",OME_TAB,    NULL, &(OSD_TAB_t)   { &rateProfile.throttle_limit_type, THROTTLE_LIMIT_TYPE_COUNT - 1, osdTableThrottleLimitType} },
 436     { "THR LIM %",   OME_UINT8,  NULL, &(OSD_UINT8_t) { &rateProfile.throttle_limit_percent, 25,  100,  1} },
 437
 438     { "ROLL LVL EXPO",  OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.levelExpo[FD_ROLL],  0, 100, 1, 10 } },
 439     { "PITCH LVL EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.levelExpo[FD_PITCH], 0, 100, 1, 10 } },
 440
 441     { "BACK", OME_Back, NULL, NULL },
 442     { NULL, OME_END, NULL, NULL}
 443 };
 444
 445 static CMS_Menu cmsx_menuRateProfile = {
 446 #ifdef CMS_MENU_DEBUG
 447     .GUARD_text = "MENURATE",
 448     .GUARD_type = OME_MENU,
 449 #endif
 450     .onEnter = cmsx_RateProfileOnEnter,
 451     .onExit = cmsx_RateProfileWriteback,
 452     .onDisplayUpdate = NULL,
 453     .entries = cmsx_menuRateProfileEntries
 454 };
 455
 456 #ifdef USE_LAUNCH_CONTROL
 457 static uint8_t cmsx_launchControlMode;
 458 static uint8_t cmsx_launchControlAllowTriggerReset;
 459 static uint8_t cmsx_launchControlThrottlePercent;
 460 static uint8_t cmsx_launchControlAngleLimit;
 461 static uint8_t cmsx_launchControlGain;
 462
 463 static const void *cmsx_launchControlOnEnter(displayPort_t *pDisp)
 464 {
 465     UNUSED(pDisp);
 466
 467     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 468
 469     cmsx_launchControlMode  = pidProfile->launchControlMode;
 470     cmsx_launchControlAllowTriggerReset  = pidProfile->launchControlAllowTriggerReset;
 471     cmsx_launchControlThrottlePercent  = pidProfile->launchControlThrottlePercent;
 472     cmsx_launchControlAngleLimit  = pidProfile->launchControlAngleLimit;
 473     cmsx_launchControlGain  = pidProfile->launchControlGain;
 474
 475     return NULL;
 476 }
 477
 478 static const void *cmsx_launchControlOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 479 {
 480     UNUSED(pDisp);
 481     UNUSED(self);
 482
 483     pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
 484
 485     pidProfile->launchControlMode = cmsx_launchControlMode;
 486     pidProfile->launchControlAllowTriggerReset = cmsx_launchControlAllowTriggerReset;
 487     pidProfile->launchControlThrottlePercent = cmsx_launchControlThrottlePercent;
 488     pidProfile->launchControlAngleLimit = cmsx_launchControlAngleLimit;
 489     pidProfile->launchControlGain = cmsx_launchControlGain;
 490
 491     return NULL;
 492 }
 493
 494 static const OSD_Entry cmsx_menuLaunchControlEntries[] = {
 495     { "-- LAUNCH CONTROL --", OME_Label, NULL, pidProfileIndexString },
 496
 497     { "MODE",             OME_TAB,   NULL, &(OSD_TAB_t)   { &cmsx_launchControlMode, LAUNCH_CONTROL_MODE_COUNT - 1, osdLaunchControlModeNames} },
 498     { "ALLOW RESET",      OME_Bool,  NULL, &cmsx_launchControlAllowTriggerReset },
 499     { "TRIGGER THROTTLE", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlThrottlePercent, 0,  LAUNCH_CONTROL_THROTTLE_TRIGGER_MAX, 1 }  },
 500     { "ANGLE LIMIT",      OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlAngleLimit,      0,  80, 1 }  },
 501     { "ITERM GAIN",       OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlGain,            0, 200, 1 }  },
 502
 503     { "BACK", OME_Back, NULL, NULL },
 504     { NULL, OME_END, NULL, NULL}
 505 };
 506
 507 static CMS_Menu cmsx_menuLaunchControl = {
 508 #ifdef CMS_MENU_DEBUG
 509     .GUARD_text = "LAUNCH",
 510     .GUARD_type = OME_MENU,
 511 #endif
 512     .onEnter = cmsx_launchControlOnEnter,
 513     .onExit = cmsx_launchControlOnExit,
 514     .onDisplayUpdate = NULL,
 515     .entries = cmsx_menuLaunchControlEntries,
 516 };
 517 #endif
 518
 519 static uint8_t  cmsx_angleStrength;
 520 static uint8_t  cmsx_horizonStrength;
 521 static uint8_t  cmsx_horizonTransition;
 522 static uint8_t  cmsx_levelAngleLimit;
 523 static uint8_t  cmsx_throttleBoost;
 524 static uint8_t  cmsx_thrustLinearization;
 525 static uint8_t  cmsx_antiGravityGain;
 526 static uint8_t  cmsx_motorOutputLimit;
 527 static int8_t   cmsx_autoProfileCellCount;
 528 #ifdef USE_D_MIN
 529 static uint8_t  cmsx_d_min[XYZ_AXIS_COUNT];
 530 static uint8_t  cmsx_d_min_gain;
 531 static uint8_t  cmsx_d_min_advance;
 532 #endif
 533
 534 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
 535 static uint8_t  cmsx_vbat_sag_compensation;
 536 #endif
 537
 538 #ifdef USE_ITERM_RELAX
 539 static uint8_t cmsx_iterm_relax;
 540 static uint8_t cmsx_iterm_relax_type;
 541 static uint8_t cmsx_iterm_relax_cutoff;
 542 #endif
 543
 544 #ifdef USE_FEEDFORWARD
 545 static uint8_t cmsx_feedforward_transition;
 546 static uint8_t cmsx_feedforward_boost;
 547 static uint8_t cmsx_feedforward_averaging;
 548 static uint8_t cmsx_feedforward_smooth_factor;
 549 static uint8_t cmsx_feedforward_jitter_factor;
 550 #endif
 551
 552 static uint8_t cmsx_tpa_rate;
 553 static uint16_t cmsx_tpa_breakpoint;
 554
 555 static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
 556 {
 557     UNUSED(pDisp);
 558
 559     setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName);
 560
 561     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 562
 563     cmsx_angleStrength =     pidProfile->pid[PID_LEVEL].P;
 564     cmsx_horizonStrength =   pidProfile->pid[PID_LEVEL].I;
 565     cmsx_horizonTransition = pidProfile->pid[PID_LEVEL].D;
 566     cmsx_levelAngleLimit =   pidProfile->levelAngleLimit;
 567
 568     cmsx_antiGravityGain   = pidProfile->anti_gravity_gain;
 569
 570     cmsx_throttleBoost = pidProfile->throttle_boost;
 571     cmsx_thrustLinearization = pidProfile->thrustLinearization;
 572     cmsx_motorOutputLimit = pidProfile->motor_output_limit;
 573     cmsx_autoProfileCellCount = pidProfile->auto_profile_cell_count;
 574
 575 #ifdef USE_D_MIN
 576     for (unsigned i = 0; i < XYZ_AXIS_COUNT; i++) {
 577         cmsx_d_min[i]  = pidProfile->d_min[i];
 578     }
 579     cmsx_d_min_gain = pidProfile->d_min_gain;
 580     cmsx_d_min_advance = pidProfile->d_min_advance;
 581 #endif
 582
 583 #ifdef USE_ITERM_RELAX
 584     cmsx_iterm_relax = pidProfile->iterm_relax;
 585     cmsx_iterm_relax_type = pidProfile->iterm_relax_type;
 586     cmsx_iterm_relax_cutoff = pidProfile->iterm_relax_cutoff;
 587 #endif
 588
 589 #ifdef USE_FEEDFORWARD
 590     cmsx_feedforward_transition  = pidProfile->feedforward_transition;
 591     cmsx_feedforward_averaging = pidProfile->feedforward_averaging;
 592     cmsx_feedforward_boost = pidProfile->feedforward_boost;
 593     cmsx_feedforward_smooth_factor = pidProfile->feedforward_smooth_factor;
 594     cmsx_feedforward_jitter_factor = pidProfile->feedforward_jitter_factor;
 595 #endif
 596
 597 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
 598     cmsx_vbat_sag_compensation = pidProfile->vbat_sag_compensation;
 599 #endif
 600     cmsx_tpa_rate = pidProfile->tpa_rate;
 601     cmsx_tpa_breakpoint = pidProfile->tpa_breakpoint;
 602
 603     return NULL;
 604 }
 605
 606 static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 607 {
 608     UNUSED(pDisp);
 609     UNUSED(self);
 610
 611     pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
 612     pidInitConfig(currentPidProfile);
 613
 614     pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength;
 615     pidProfile->pid[PID_LEVEL].I = cmsx_horizonStrength;
 616     pidProfile->pid[PID_LEVEL].D = cmsx_horizonTransition;
 617     pidProfile->levelAngleLimit  = cmsx_levelAngleLimit;
 618
 619     pidProfile->anti_gravity_gain   = cmsx_antiGravityGain;
 620
 621     pidProfile->throttle_boost = cmsx_throttleBoost;
 622     pidProfile->thrustLinearization = cmsx_thrustLinearization;
 623     pidProfile->motor_output_limit = cmsx_motorOutputLimit;
 624     pidProfile->auto_profile_cell_count = cmsx_autoProfileCellCount;
 625
 626 #ifdef USE_D_MIN
 627     for (unsigned i = 0; i < XYZ_AXIS_COUNT; i++) {
 628         pidProfile->d_min[i] = cmsx_d_min[i];
 629     }
 630     pidProfile->d_min_gain = cmsx_d_min_gain;
 631     pidProfile->d_min_advance = cmsx_d_min_advance;
 632 #endif
 633
 634 #ifdef USE_ITERM_RELAX
 635     pidProfile->iterm_relax = cmsx_iterm_relax;
 636     pidProfile->iterm_relax_type = cmsx_iterm_relax_type;
 637     pidProfile->iterm_relax_cutoff = cmsx_iterm_relax_cutoff;
 638 #endif
 639
 640 #ifdef USE_FEEDFORWARD
 641     pidProfile->feedforward_transition = cmsx_feedforward_transition;
 642     pidProfile->feedforward_averaging = cmsx_feedforward_averaging;
 643     pidProfile->feedforward_boost = cmsx_feedforward_boost;
 644     pidProfile->feedforward_smooth_factor = cmsx_feedforward_smooth_factor;
 645     pidProfile->feedforward_jitter_factor = cmsx_feedforward_jitter_factor;
 646 #endif
 647
 648 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
 649     pidProfile->vbat_sag_compensation = cmsx_vbat_sag_compensation;
 650 #endif
 651     pidProfile->tpa_rate = cmsx_tpa_rate;
 652     pidProfile->tpa_breakpoint = cmsx_tpa_breakpoint;
 653
 654     initEscEndpoints();
 655     return NULL;
 656 }
 657
 658 static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
 659     { "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString },
 660
 661 #ifdef USE_FEEDFORWARD
 662     { "FF TRANSITION", OME_FLOAT,  NULL, &(OSD_FLOAT_t)  { &cmsx_feedforward_transition,        0,    100,   1, 10 } },
 663     { "FF AVERAGING",  OME_TAB,    NULL, &(OSD_TAB_t)    { &cmsx_feedforward_averaging,         4, lookupTableFeedforwardAveraging} },
 664     { "FF SMOOTHNESS", OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_smooth_factor,     0,     95,   1  }    },
 665     { "FF JITTER",     OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_jitter_factor,     0,     20,   1  }    },
 666     { "FF BOOST",      OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_boost,             0,     50,   1  }    },
 667 #endif
 668     { "ANGLE STR",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_angleStrength,          0,    200,   1  }    },
 669     { "HORZN STR",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_horizonStrength,        0,    200,   1  }    },
 670     { "HORZN TRS",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_horizonTransition,      0,    200,   1  }    },
 671     { "ANGLE LIMIT", OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_levelAngleLimit,        10,    90,   1  }    },
 672
 673     { "AG GAIN",     OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &cmsx_antiGravityGain,   ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX, 1, 100 }    },
 674 #ifdef USE_THROTTLE_BOOST
 675     { "THR BOOST",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_throttleBoost,          0,    100,   1  }    },
 676 #endif
 677 #ifdef USE_THRUST_LINEARIZATION
 678     { "THR LINEAR",  OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_thrustLinearization,    0,    150,   1  }    },
 679 #endif
 680 #ifdef USE_ITERM_RELAX
 681     { "I_RELAX",         OME_TAB,    NULL, &(OSD_TAB_t)     { &cmsx_iterm_relax,        ITERM_RELAX_COUNT - 1,      lookupTableItermRelax       } },
 682     { "I_RELAX TYPE",    OME_TAB,    NULL, &(OSD_TAB_t)     { &cmsx_iterm_relax_type,   ITERM_RELAX_TYPE_COUNT - 1, lookupTableItermRelaxType   } },
 683     { "I_RELAX CUTOFF",  OME_UINT8,  NULL, &(OSD_UINT8_t)   { &cmsx_iterm_relax_cutoff, 1, 50, 1 } },
 684 #endif
 685 #ifdef USE_LAUNCH_CONTROL
 686     {"LAUNCH CONTROL", OME_Submenu, cmsMenuChange, &cmsx_menuLaunchControl },
 687 #endif
 688     { "MTR OUT LIM %",OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_motorOutputLimit, MOTOR_OUTPUT_LIMIT_PERCENT_MIN,  MOTOR_OUTPUT_LIMIT_PERCENT_MAX,  1} },
 689
 690     { "AUTO CELL CNT", OME_INT8, NULL, &(OSD_INT8_t) { &cmsx_autoProfileCellCount, AUTO_PROFILE_CELL_COUNT_CHANGE, MAX_AUTO_DETECT_CELL_COUNT, 1} },
 691
 692 #ifdef USE_D_MIN
 693     { "D_MIN ROLL",  OME_UINT8 | SLIDER_RP,  NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_ROLL],      0, 100, 1 } },
 694     { "D_MIN PITCH", OME_UINT8 | SLIDER_RP,  NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_PITCH],     0, 100, 1 } },
 695     { "D_MIN YAW",   OME_UINT8 | SLIDER_RPY,  NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_YAW],       0, 100, 1 } },
 696     { "D_MIN GAIN",  OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_min_gain,          0, 100, 1 } },
 697     { "D_MIN ADV",   OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_min_advance,       0, 200, 1 } },
 698 #endif
 699
 700 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
 701     { "VBAT_SAG_COMP", OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_vbat_sag_compensation, 0, 150, 1 } },
 702 #endif
 703
 704     { "TPA RATE",  OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &cmsx_tpa_rate,          0,  100,  1, 10} },
 705     { "TPA BRKPT",   OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_tpa_breakpoint, 1000, 2000, 10} },
 706
 707     { "BACK", OME_Back, NULL, NULL },
 708     { NULL, OME_END, NULL, NULL}
 709 };
 710
 711 static CMS_Menu cmsx_menuProfileOther = {
 712 #ifdef CMS_MENU_DEBUG
 713     .GUARD_text = "XPROFOTHER",
 714     .GUARD_type = OME_MENU,
 715 #endif
 716     .onEnter = cmsx_profileOtherOnEnter,
 717     .onExit = cmsx_profileOtherOnExit,
 718     .onDisplayUpdate = NULL,
 719     .entries = cmsx_menuProfileOtherEntries,
 720 };
 721
 722
 723 static uint16_t gyroConfig_gyro_lpf1_static_hz;
 724 static uint16_t gyroConfig_gyro_lpf2_static_hz;
 725 static uint16_t gyroConfig_gyro_soft_notch_hz_1;
 726 static uint16_t gyroConfig_gyro_soft_notch_cutoff_1;
 727 static uint16_t gyroConfig_gyro_soft_notch_hz_2;
 728 static uint16_t gyroConfig_gyro_soft_notch_cutoff_2;
 729 static uint8_t  gyroConfig_gyro_to_use;
 730
 731 static const void *cmsx_menuGyro_onEnter(displayPort_t *pDisp)
 732 {
 733     UNUSED(pDisp);
 734
 735     gyroConfig_gyro_lpf1_static_hz =  gyroConfig()->gyro_lpf1_static_hz;
 736     gyroConfig_gyro_lpf2_static_hz =  gyroConfig()->gyro_lpf2_static_hz;
 737     gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
 738     gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
 739     gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
 740     gyroConfig_gyro_soft_notch_cutoff_2 = gyroConfig()->gyro_soft_notch_cutoff_2;
 741     gyroConfig_gyro_to_use = gyroConfig()->gyro_to_use;
 742
 743     return NULL;
 744 }
 745
 746 static const void *cmsx_menuGyro_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 747 {
 748     UNUSED(pDisp);
 749     UNUSED(self);
 750
 751     gyroConfigMutable()->gyro_lpf1_static_hz =  gyroConfig_gyro_lpf1_static_hz;
 752     gyroConfigMutable()->gyro_lpf2_static_hz =  gyroConfig_gyro_lpf2_static_hz;
 753     gyroConfigMutable()->gyro_soft_notch_hz_1 = gyroConfig_gyro_soft_notch_hz_1;
 754     gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
 755     gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
 756     gyroConfigMutable()->gyro_soft_notch_cutoff_2 = gyroConfig_gyro_soft_notch_cutoff_2;
 757     gyroConfigMutable()->gyro_to_use = gyroConfig_gyro_to_use;
 758
 759     return NULL;
 760 }
 761
 762 static const OSD_Entry cmsx_menuFilterGlobalEntries[] =
 763 {
 764     { "-- FILTER GLB  --", OME_Label, NULL, NULL },
 765
 766     { "GYRO LPF1",  OME_UINT16 | SLIDER_GYRO, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lpf1_static_hz, 0, LPF_MAX_HZ, 1 } },
 767 #ifdef USE_GYRO_LPF2
 768     { "GYRO LPF2",  OME_UINT16 | SLIDER_GYRO, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lpf2_static_hz,  0, LPF_MAX_HZ, 1 } },
 769 #endif
 770     { "GYRO NF1",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_1,     0, 500, 1 } },
 771     { "GYRO NF1C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 } },
 772     { "GYRO NF2",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2,     0, 500, 1 } },
 773     { "GYRO NF2C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 } },
 774 #ifdef USE_MULTI_GYRO
 775     { "GYRO TO USE",  OME_TAB | REBOOT_REQUIRED,  NULL, &(OSD_TAB_t)    { &gyroConfig_gyro_to_use,  2, osdTableGyroToUse} },
 776 #endif
 777
 778     { "BACK", OME_Back, NULL, NULL },
 779     { NULL, OME_END, NULL, NULL}
 780 };
 781
 782 static CMS_Menu cmsx_menuFilterGlobal = {
 783 #ifdef CMS_MENU_DEBUG
 784     .GUARD_text = "XFLTGLB",
 785     .GUARD_type = OME_MENU,
 786 #endif
 787     .onEnter = cmsx_menuGyro_onEnter,
 788     .onExit = cmsx_menuGyro_onExit,
 789     .onDisplayUpdate = NULL,
 790     .entries = cmsx_menuFilterGlobalEntries,
 791 };
 792
 793 #if (defined(USE_DYN_NOTCH_FILTER) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
 794
 795 #ifdef USE_DYN_NOTCH_FILTER
 796 static uint16_t dynFiltNotchMaxHz;
 797 static uint8_t  dynFiltNotchCount;
 798 static uint16_t dynFiltNotchQ;
 799 static uint16_t dynFiltNotchMinHz;
 800 #endif
 801 #ifdef USE_DYN_LPF
 802 static uint16_t gyroLpfDynMin;
 803 static uint16_t gyroLpfDynMax;
 804 static uint8_t gyroLpfDynExpo;
 805 static uint16_t dtermLpfDynMin;
 806 static uint16_t dtermLpfDynMax;
 807 static uint8_t dtermLpfDynExpo;
 808 #endif
 809
 810 static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
 811 {
 812     UNUSED(pDisp);
 813
 814 #ifdef USE_DYN_NOTCH_FILTER
 815     dynFiltNotchMaxHz   = dynNotchConfig()->dyn_notch_max_hz;
 816     dynFiltNotchCount   = dynNotchConfig()->dyn_notch_count;
 817     dynFiltNotchQ       = dynNotchConfig()->dyn_notch_q;
 818     dynFiltNotchMinHz   = dynNotchConfig()->dyn_notch_min_hz;
 819 #endif
 820 #ifdef USE_DYN_LPF
 821     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 822     gyroLpfDynMin   = gyroConfig()->gyro_lpf1_dyn_min_hz;
 823     gyroLpfDynMax   = gyroConfig()->gyro_lpf1_dyn_max_hz;
 824     gyroLpfDynExpo  = gyroConfig()->gyro_lpf1_dyn_expo;
 825     dtermLpfDynMin  = pidProfile->dterm_lpf1_dyn_min_hz;
 826     dtermLpfDynMax  = pidProfile->dterm_lpf1_dyn_max_hz;
 827     dtermLpfDynExpo = pidProfile->dterm_lpf1_dyn_expo;
 828 #endif
 829
 830     return NULL;
 831 }
 832
 833 static const void *cmsx_menuDynFilt_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 834 {
 835     UNUSED(pDisp);
 836     UNUSED(self);
 837
 838 #ifdef USE_DYN_NOTCH_FILTER
 839     dynNotchConfigMutable()->dyn_notch_max_hz        = dynFiltNotchMaxHz;
 840     dynNotchConfigMutable()->dyn_notch_count         = dynFiltNotchCount;
 841     dynNotchConfigMutable()->dyn_notch_q             = dynFiltNotchQ;
 842     dynNotchConfigMutable()->dyn_notch_min_hz        = dynFiltNotchMinHz;
 843 #endif
 844 #ifdef USE_DYN_LPF
 845     pidProfile_t *pidProfile = currentPidProfile;
 846     gyroConfigMutable()->gyro_lpf1_dyn_min_hz = gyroLpfDynMin;
 847     gyroConfigMutable()->gyro_lpf1_dyn_max_hz = gyroLpfDynMax;
 848     gyroConfigMutable()->gyro_lpf1_dyn_expo   = gyroLpfDynExpo;
 849     pidProfile->dterm_lpf1_dyn_min_hz         = dtermLpfDynMin;
 850     pidProfile->dterm_lpf1_dyn_max_hz         = dtermLpfDynMax;
 851     pidProfile->dterm_lpf1_dyn_expo           = dtermLpfDynExpo;
 852 #endif
 853
 854     return NULL;
 855 }
 856
 857 static const OSD_Entry cmsx_menuDynFiltEntries[] =
 858 {
 859     { "-- DYN FILT --", OME_Label, NULL, NULL },
 860
 861 #ifdef USE_DYN_NOTCH_FILTER
 862     { "NOTCH COUNT",    OME_UINT8,  NULL, &(OSD_UINT8_t)  { &dynFiltNotchCount,   0, DYN_NOTCH_COUNT_MAX, 1 } },
 863     { "NOTCH Q",        OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ,       1, 1000, 1 } },
 864     { "NOTCH MIN HZ",   OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz,   20, 250, 1 } },
 865     { "NOTCH MAX HZ",   OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMaxHz,   200, 1000, 1 } },
 866 #endif
 867
 868 #ifdef USE_DYN_LPF
 869     { "GYRO DLPF MIN",   OME_UINT16 | SLIDER_GYRO, NULL, &(OSD_UINT16_t) { &gyroLpfDynMin,  0, 1000, 1 } },
 870     { "GYRO DLPF MAX",   OME_UINT16 | SLIDER_GYRO, NULL, &(OSD_UINT16_t) { &gyroLpfDynMax,  0, 1000, 1 } },
 871     { "GYRO DLPF EXPO",  OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroLpfDynExpo,   0, 10, 1 } },
 872     { "DTERM DLPF MIN",  OME_UINT16 | SLIDER_DTERM, NULL, &(OSD_UINT16_t) { &dtermLpfDynMin, 0, 1000, 1 } },
 873     { "DTERM DLPF MAX",  OME_UINT16 | SLIDER_DTERM, NULL, &(OSD_UINT16_t) { &dtermLpfDynMax, 0, 1000, 1 } },
 874     { "DTERM DLPF EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &dtermLpfDynExpo,  0, 10, 1 } },
 875 #endif
 876
 877     { "BACK", OME_Back, NULL, NULL },
 878     { NULL, OME_END, NULL, NULL}
 879 };
 880
 881 static CMS_Menu cmsx_menuDynFilt = {
 882 #ifdef CMS_MENU_DEBUG
 883     .GUARD_text = "XDYNFLT",
 884     .GUARD_type = OME_MENU,
 885 #endif
 886     .onEnter = cmsx_menuDynFilt_onEnter,
 887     .onExit = cmsx_menuDynFilt_onExit,
 888     .onDisplayUpdate = NULL,
 889     .entries = cmsx_menuDynFiltEntries,
 890 };
 891
 892 #endif
 893
 894 static uint16_t cmsx_dterm_lpf1_static_hz;
 895 static uint16_t cmsx_dterm_lpf2_static_hz;
 896 static uint16_t cmsx_dterm_notch_hz;
 897 static uint16_t cmsx_dterm_notch_cutoff;
 898 static uint16_t cmsx_yaw_lowpass_hz;
 899
 900 static const void *cmsx_FilterPerProfileRead(displayPort_t *pDisp)
 901 {
 902     UNUSED(pDisp);
 903
 904     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 905
 906     cmsx_dterm_lpf1_static_hz   = pidProfile->dterm_lpf1_static_hz;
 907     cmsx_dterm_lpf2_static_hz   = pidProfile->dterm_lpf2_static_hz;
 908     cmsx_dterm_notch_hz         = pidProfile->dterm_notch_hz;
 909     cmsx_dterm_notch_cutoff     = pidProfile->dterm_notch_cutoff;
 910     cmsx_yaw_lowpass_hz         = pidProfile->yaw_lowpass_hz;
 911
 912     return NULL;
 913 }
 914
 915 static const void *cmsx_FilterPerProfileWriteback(displayPort_t *pDisp, const OSD_Entry *self)
 916 {
 917     UNUSED(pDisp);
 918     UNUSED(self);
 919
 920     pidProfile_t *pidProfile = currentPidProfile;
 921
 922     pidProfile->dterm_lpf1_static_hz  = cmsx_dterm_lpf1_static_hz;
 923     pidProfile->dterm_lpf2_static_hz  = cmsx_dterm_lpf2_static_hz;
 924     pidProfile->dterm_notch_hz        = cmsx_dterm_notch_hz;
 925     pidProfile->dterm_notch_cutoff    = cmsx_dterm_notch_cutoff;
 926     pidProfile->yaw_lowpass_hz        = cmsx_yaw_lowpass_hz;
 927
 928     return NULL;
 929 }
 930
 931 static const OSD_Entry cmsx_menuFilterPerProfileEntries[] =
 932 {
 933     { "-- FILTER PP  --", OME_Label, NULL, NULL },
 934
 935     { "DTERM LPF1", OME_UINT16 | SLIDER_DTERM, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lpf1_static_hz, 0, LPF_MAX_HZ, 1 } },
 936     { "DTERM LPF2", OME_UINT16 | SLIDER_DTERM, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lpf2_static_hz, 0, LPF_MAX_HZ, 1 } },
 937     { "DTERM NF",   OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz,       0, LPF_MAX_HZ, 1 } },
 938     { "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff,   0, LPF_MAX_HZ, 1 } },
 939     { "YAW LPF",    OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lowpass_hz,       0, 500, 1 } },
 940
 941     { "BACK", OME_Back, NULL, NULL },
 942     { NULL, OME_END, NULL, NULL}
 943 };
 944
 945 static CMS_Menu cmsx_menuFilterPerProfile = {
 946 #ifdef CMS_MENU_DEBUG
 947     .GUARD_text = "XFLTPP",
 948     .GUARD_type = OME_MENU,
 949 #endif
 950     .onEnter = cmsx_FilterPerProfileRead,
 951     .onExit = cmsx_FilterPerProfileWriteback,
 952     .onDisplayUpdate = NULL,
 953     .entries = cmsx_menuFilterPerProfileEntries,
 954 };
 955
 956 #ifdef USE_EXTENDED_CMS_MENUS
 957
 958 static uint8_t cmsx_dstPidProfile;
 959 static uint8_t cmsx_dstControlRateProfile;
 960
 961 static const char * const cmsx_ProfileNames[] = {
 962     "-",
 963     "1",
 964     "2",
 965     "3"
 966 };
 967
 968 static OSD_TAB_t cmsx_PidProfileTable = { &cmsx_dstPidProfile, 3, cmsx_ProfileNames };
 969 static OSD_TAB_t cmsx_ControlRateProfileTable = { &cmsx_dstControlRateProfile, 3, cmsx_ProfileNames };
 970
 971 static const void *cmsx_menuCopyProfile_onEnter(displayPort_t *pDisp)
 972 {
 973     UNUSED(pDisp);
 974
 975     cmsx_dstPidProfile = 0;
 976     cmsx_dstControlRateProfile = 0;
 977
 978     return NULL;
 979 }
 980
 981 static const void *cmsx_CopyPidProfile(displayPort_t *pDisplay, const void *ptr)
 982 {
 983     UNUSED(pDisplay);
 984     UNUSED(ptr);
 985
 986     if (cmsx_dstPidProfile > 0) {
 987         pidCopyProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex());
 988     }
 989
 990     return NULL;
 991 }
 992
 993 static const void *cmsx_CopyControlRateProfile(displayPort_t *pDisplay, const void *ptr)
 994 {
 995     UNUSED(pDisplay);
 996     UNUSED(ptr);
 997
 998     if (cmsx_dstControlRateProfile > 0) {
 999         copyControlRateProfile(cmsx_dstControlRateProfile - 1, getCurrentControlRateProfileIndex());
1000     }
1001
1002     return NULL;
1003 }
1004
1005 static const OSD_Entry cmsx_menuCopyProfileEntries[] =
1006 {
1007     { "-- COPY PROFILE --", OME_Label, NULL, NULL},
1008
1009     { "CPY PID PROF TO",   OME_TAB,      NULL,                        &cmsx_PidProfileTable },
1010     { "COPY PP",           OME_Funcall,  cmsx_CopyPidProfile,         NULL },
1011     { "CPY RATE PROF TO",  OME_TAB,      NULL,                        &cmsx_ControlRateProfileTable },
1012     { "COPY RP",           OME_Funcall,  cmsx_CopyControlRateProfile, NULL },
1013
1014     { "BACK", OME_Back, NULL, NULL },
1015     { NULL, OME_END, NULL, NULL}
1016 };
1017
1018 CMS_Menu cmsx_menuCopyProfile = {
1019 #ifdef CMS_MENU_DEBUG
1020     .GUARD_text = "XCPY",
1021     .GUARD_type = OME_MENU,
1022 #endif
1023     .onEnter = cmsx_menuCopyProfile_onEnter,
1024     .onExit = NULL,
1025     .onDisplayUpdate = NULL,
1026     .entries = cmsx_menuCopyProfileEntries,
1027 };
1028
1029 #endif
1030
1031 static const OSD_Entry cmsx_menuImuEntries[] =
1032 {
1033     { "-- PROFILE --", OME_Label, NULL, NULL},
1034
1035     {"PID PROF",  OME_TAB,     cmsx_profileIndexOnChange,     &(OSD_TAB_t){&tmpPidProfileIndex, PID_PROFILE_COUNT-1, pidProfileNamePtrs}},
1036     {"PID",       OME_Submenu, cmsMenuChange,                 &cmsx_menuPid},
1037 #ifdef USE_SIMPLIFIED_TUNING
1038     {"SIMPLIFIED TUNING",   OME_Submenu, cmsMenuChange,                 &cmsx_menuSimplifiedTuning},
1039 #endif
1040     {"MISC PP",   OME_Submenu, cmsMenuChange,                 &cmsx_menuProfileOther},
1041     {"FILT PP",   OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterPerProfile},
1042
1043     {"RATE PROF", OME_TAB,   cmsx_rateProfileIndexOnChange, &(OSD_TAB_t){&tmpRateProfileIndex, CONTROL_RATE_PROFILE_COUNT-1, rateProfileNamePtrs}},
1044     {"RATE",      OME_Submenu, cmsMenuChange,                 &cmsx_menuRateProfile},
1045
1046     {"FILT GLB",  OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterGlobal},
1047 #if  (defined(USE_DYN_NOTCH_FILTER) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
1048     {"DYN FILT",  OME_Submenu, cmsMenuChange,                 &cmsx_menuDynFilt},
1049 #endif
1050
1051 #ifdef USE_EXTENDED_CMS_MENUS
1052     {"COPY PROF", OME_Submenu, cmsMenuChange,                 &cmsx_menuCopyProfile},
1053 #endif /* USE_EXTENDED_CMS_MENUS */
1054
1055     {"BACK", OME_Back, NULL, NULL},
1056     {NULL, OME_END, NULL, NULL}
1057 };
1058
1059 CMS_Menu cmsx_menuImu = {
1060 #ifdef CMS_MENU_DEBUG
1061     .GUARD_text = "XIMU",
1062     .GUARD_type = OME_MENU,
1063 #endif
1064     .onEnter = cmsx_menuImu_onEnter,
1065     .onExit = cmsx_menuImu_onExit,
1066     .onDisplayUpdate = NULL,
1067     .entries = cmsx_menuImuEntries,
1068 };
1069
1070 #endif // CMS