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