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 const OSD_Entry cmsx_menuPidEntries[] =
 199 {
 200     { "-- PID --", OME_Label, NULL, pidProfileIndexString, 0},
 201
 202     { "ROLL  P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][0],  0, 200, 1 }, 0 },
 203     { "ROLL  I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][1],  0, 200, 1 }, 0 },
 204     { "ROLL  D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][2],  0, 200, 1 }, 0 },
 205     { "ROLL  F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_ROLL],  0, 2000, 1 }, 0 },
 206
 207     { "PITCH P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][0], 0, 200, 1 }, 0 },
 208     { "PITCH I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][1], 0, 200, 1 }, 0 },
 209     { "PITCH D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][2], 0, 200, 1 }, 0 },
 210     { "PITCH F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_PITCH], 0, 2000, 1 }, 0 },
 211
 212     { "YAW   P", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][0],   0, 200, 1 }, 0 },
 213     { "YAW   I", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][1],   0, 200, 1 }, 0 },
 214     { "YAW   D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][2],   0, 200, 1 }, 0 },
 215     { "YAW   F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_YAW],   0, 2000, 1 }, 0 },
 216
 217     { "BACK", OME_Back, NULL, NULL, 0 },
 218     { NULL, OME_END, NULL, NULL, 0 }
 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);
 318     }
 319
 320     return 0;
 321 }
 322
 323 static const OSD_Entry cmsx_menuSimplifiedTuningEntries[] =
 324 {
 325     { "-- SIMPLIFIED PID --", OME_Label, NULL, NULL, 0},
 326     { "PID TUNING",        OME_TAB,   NULL, &(OSD_TAB_t)   { &cmsx_simplified_pids_mode, PID_SIMPLIFIED_TUNING_MODE_COUNT - 1, lookupTableSimplifiedTuningPidsMode }, 0 },
 327
 328     { "-- BASIC --",       OME_Label, NULL, NULL, 0},
 329     { "D GAINS",           OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_d_gain,            SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 },
 330     { "P&I GAINS",         OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_pi_gain,           SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 },
 331     { "FF GAINS",          OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_feedforward_gain,  SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 },
 332
 333     { "-- EXPERT --",      OME_Label, NULL, NULL, 0},
 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 }, 0 },
 336 #endif
 337     { "I GAINS",           OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_i_gain,            SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 },
 338
 339     { "PITCH:ROLL D",      OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_roll_pitch_ratio,  SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 },
 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 }, 0 },
 341     { "MASTER MULT",       OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_master_multiplier, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 },
 342
 343     { "-- SIMPLIFIED FILTER --", OME_Label, NULL, NULL, 0},
 344     { "GYRO TUNING",       OME_TAB,   NULL, &(OSD_TAB_t)   { &cmsx_simplified_gyro_filter,  1, lookupTableOffOn }, 0 },
 345     { "GYRO MULT",         OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_gyro_filter_multiplier,  SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 },
 346     { "DTERM TUNING",      OME_TAB,   NULL, &(OSD_TAB_t)   { &cmsx_simplified_dterm_filter, 1, lookupTableOffOn }, 0 },
 347     { "DTERM MULT",        OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_dterm_filter_multiplier, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 },
 348
 349     { "BACK", OME_Back, NULL, NULL, 0 },
 350     { NULL, OME_END, NULL, NULL, 0 }
 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, 0 },
 398
 399     { "RC R RATE",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcRates[FD_ROLL],    1, CONTROL_RATE_CONFIG_RC_RATES_MAX, 1, 10 }, 0 },
 400     { "RC P RATE",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcRates[FD_PITCH],    1, CONTROL_RATE_CONFIG_RC_RATES_MAX, 1, 10 }, 0 },
 401     { "RC Y RATE",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcRates[FD_YAW], 1, CONTROL_RATE_CONFIG_RC_RATES_MAX, 1, 10 }, 0 },
 402
 403     { "ROLL SUPER",  OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rates[FD_ROLL],   0, CONTROL_RATE_CONFIG_RATE_MAX, 1, 10 }, 0 },
 404     { "PITCH SUPER", OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rates[FD_PITCH],   0, CONTROL_RATE_CONFIG_RATE_MAX, 1, 10 }, 0 },
 405     { "YAW SUPER",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rates[FD_YAW],   0, CONTROL_RATE_CONFIG_RATE_MAX, 1, 10 }, 0 },
 406
 407     { "RC R EXPO",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo[FD_ROLL],    0, 100, 1, 10 }, 0 },
 408     { "RC P EXPO",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo[FD_PITCH],    0, 100, 1, 10 }, 0 },
 409     { "RC Y EXPO",   OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo[FD_YAW], 0, 100, 1, 10 }, 0 },
 410
 411     { "THR MID",     OME_UINT8,  NULL, &(OSD_UINT8_t) { &rateProfile.thrMid8,           0,  100,  1}, 0 },
 412     { "THR EXPO",    OME_UINT8,  NULL, &(OSD_UINT8_t) { &rateProfile.thrExpo8,          0,  100,  1}, 0 },
 413     { "THRPID ATT",  OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &rateProfile.dynThrPID,         0,  100,  1, 10}, 0 },
 414     { "TPA BRKPT",   OME_UINT16, NULL, &(OSD_UINT16_t){ &rateProfile.tpa_breakpoint, 1000, 2000, 10}, 0 },
 415
 416     { "THR LIM TYPE",OME_TAB,    NULL, &(OSD_TAB_t)   { &rateProfile.throttle_limit_type, THROTTLE_LIMIT_TYPE_COUNT - 1, osdTableThrottleLimitType}, 0 },
 417     { "THR LIM %",   OME_UINT8,  NULL, &(OSD_UINT8_t) { &rateProfile.throttle_limit_percent, 25,  100,  1}, 0 },
 418
 419     { "ROLL LVL EXPO",  OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.levelExpo[FD_ROLL],  0, 100, 1, 10 }, 0 },
 420     { "PITCH LVL EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.levelExpo[FD_PITCH], 0, 100, 1, 10 }, 0 },
 421
 422     { "BACK", OME_Back, NULL, NULL, 0 },
 423     { NULL, OME_END, NULL, NULL, 0 }
 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, 0 },
 477
 478     { "MODE",             OME_TAB,   NULL, &(OSD_TAB_t)   { &cmsx_launchControlMode, LAUNCH_CONTROL_MODE_COUNT - 1, osdLaunchControlModeNames}, 0 },
 479     { "ALLOW RESET",      OME_Bool,  NULL, &cmsx_launchControlAllowTriggerReset, 0 },
 480     { "TRIGGER THROTTLE", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlThrottlePercent, 0,  LAUNCH_CONTROL_THROTTLE_TRIGGER_MAX, 1 } , 0 },
 481     { "ANGLE LIMIT",      OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlAngleLimit,      0,  80, 1 } , 0 },
 482     { "ITERM GAIN",       OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlGain,            0, 200, 1 } , 0 },
 483
 484     { "BACK", OME_Back, NULL, NULL, 0 },
 485     { NULL, OME_END, NULL, NULL, 0 }
 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     return NULL;
 579 }
 580
 581 static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 582 {
 583     UNUSED(pDisp);
 584     UNUSED(self);
 585
 586     pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
 587     pidInitConfig(currentPidProfile);
 588
 589     pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength;
 590     pidProfile->pid[PID_LEVEL].I = cmsx_horizonStrength;
 591     pidProfile->pid[PID_LEVEL].D = cmsx_horizonTransition;
 592
 593     pidProfile->itermAcceleratorGain   = cmsx_itermAcceleratorGain;
 594     pidProfile->itermThrottleThreshold = cmsx_itermThrottleThreshold;
 595
 596     pidProfile->throttle_boost = cmsx_throttleBoost;
 597     pidProfile->thrustLinearization = cmsx_thrustLinearization;
 598     pidProfile->motor_output_limit = cmsx_motorOutputLimit;
 599     pidProfile->auto_profile_cell_count = cmsx_autoProfileCellCount;
 600
 601 #ifdef USE_D_MIN
 602     for (unsigned i = 0; i < XYZ_AXIS_COUNT; i++) {
 603         pidProfile->d_min[i] = cmsx_d_min[i];
 604     }
 605     pidProfile->d_min_gain = cmsx_d_min_gain;
 606     pidProfile->d_min_advance = cmsx_d_min_advance;
 607 #endif
 608
 609 #ifdef USE_ITERM_RELAX
 610     pidProfile->iterm_relax = cmsx_iterm_relax;
 611     pidProfile->iterm_relax_type = cmsx_iterm_relax_type;
 612     pidProfile->iterm_relax_cutoff = cmsx_iterm_relax_cutoff;
 613 #endif
 614
 615 #ifdef USE_FEEDFORWARD
 616     pidProfile->feedforward_transition = cmsx_feedforward_transition;
 617     pidProfile->feedforward_averaging = cmsx_feedforward_averaging;
 618     pidProfile->feedforward_boost = cmsx_feedforward_boost;
 619     pidProfile->feedforward_smooth_factor = cmsx_feedforward_smooth_factor;
 620     pidProfile->feedforward_jitter_factor = cmsx_feedforward_jitter_factor;
 621 #endif
 622
 623 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
 624     pidProfile->vbat_sag_compensation = cmsx_vbat_sag_compensation;
 625 #endif
 626
 627     initEscEndpoints();
 628     return NULL;
 629 }
 630
 631 static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
 632     { "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 },
 633
 634 #ifdef USE_FEEDFORWARD
 635     { "FF TRANSITION", OME_FLOAT,  NULL, &(OSD_FLOAT_t)  { &cmsx_feedforward_transition,        0,    100,   1, 10 }, 0 },
 636     { "FF AVERAGING",  OME_TAB,    NULL, &(OSD_TAB_t)    { &cmsx_feedforward_averaging,         4, lookupTableFeedforwardAveraging}, 0 },
 637     { "FF SMOOTHNESS", OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_smooth_factor,     0,     75,   1  }   , 0 },
 638     { "FF JITTER",     OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_jitter_factor,     0,     20,   1  }   , 0 },
 639     { "FF BOOST",      OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_boost,             0,     50,   1  }   , 0 },
 640 #endif
 641     { "ANGLE STR",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_angleStrength,          0,    200,   1  }   , 0 },
 642     { "HORZN STR",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_horizonStrength,        0,    200,   1  }   , 0 },
 643     { "HORZN TRS",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_horizonTransition,      0,    200,   1  }   , 0 },
 644     { "AG GAIN",     OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_itermAcceleratorGain,   ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX, 10 }   , 0 },
 645     { "AG THR",      OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_itermThrottleThreshold, 20,   1000,  1  }   , 0 },
 646 #ifdef USE_THROTTLE_BOOST
 647     { "THR BOOST",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_throttleBoost,          0,    100,   1  }   , 0 },
 648 #endif
 649 #ifdef USE_THRUST_LINEARIZATION
 650     { "THR LINEAR",  OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_thrustLinearization,    0,    150,   1  }   , 0 },
 651 #endif
 652 #ifdef USE_ITERM_RELAX
 653     { "I_RELAX",         OME_TAB,    NULL, &(OSD_TAB_t)     { &cmsx_iterm_relax,        ITERM_RELAX_COUNT - 1,      lookupTableItermRelax       }, 0 },
 654     { "I_RELAX TYPE",    OME_TAB,    NULL, &(OSD_TAB_t)     { &cmsx_iterm_relax_type,   ITERM_RELAX_TYPE_COUNT - 1, lookupTableItermRelaxType   }, 0 },
 655     { "I_RELAX CUTOFF",  OME_UINT8,  NULL, &(OSD_UINT8_t)   { &cmsx_iterm_relax_cutoff, 1, 50, 1 }, 0 },
 656 #endif
 657 #ifdef USE_LAUNCH_CONTROL
 658     {"LAUNCH CONTROL", OME_Submenu, cmsMenuChange, &cmsx_menuLaunchControl, 0 },
 659 #endif
 660     { "MTR OUT LIM %",OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_motorOutputLimit, MOTOR_OUTPUT_LIMIT_PERCENT_MIN,  MOTOR_OUTPUT_LIMIT_PERCENT_MAX,  1}, 0 },
 661
 662     { "AUTO CELL CNT", OME_INT8, NULL, &(OSD_INT8_t) { &cmsx_autoProfileCellCount, AUTO_PROFILE_CELL_COUNT_CHANGE, MAX_AUTO_DETECT_CELL_COUNT, 1}, 0 },
 663
 664 #ifdef USE_D_MIN
 665     { "D_MIN ROLL",  OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_ROLL],      0, 100, 1 }, 0 },
 666     { "D_MIN PITCH", OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_PITCH],     0, 100, 1 }, 0 },
 667     { "D_MIN YAW",   OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_YAW],       0, 100, 1 }, 0 },
 668     { "D_MIN GAIN",  OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_min_gain,          0, 100, 1 }, 0 },
 669     { "D_MIN ADV",   OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_min_advance,       0, 200, 1 }, 0 },
 670 #endif
 671
 672 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
 673     { "VBAT_SAG_COMP", OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_vbat_sag_compensation, 0, 150, 1 }, 0 },
 674 #endif
 675
 676     { "BACK", OME_Back, NULL, NULL, 0 },
 677     { NULL, OME_END, NULL, NULL, 0 }
 678 };
 679
 680 static CMS_Menu cmsx_menuProfileOther = {
 681 #ifdef CMS_MENU_DEBUG
 682     .GUARD_text = "XPROFOTHER",
 683     .GUARD_type = OME_MENU,
 684 #endif
 685     .onEnter = cmsx_profileOtherOnEnter,
 686     .onExit = cmsx_profileOtherOnExit,
 687     .onDisplayUpdate = NULL,
 688     .entries = cmsx_menuProfileOtherEntries,
 689 };
 690
 691
 692 static uint16_t gyroConfig_gyro_lpf1_static_hz;
 693 static uint16_t gyroConfig_gyro_lpf2_static_hz;
 694 static uint16_t gyroConfig_gyro_soft_notch_hz_1;
 695 static uint16_t gyroConfig_gyro_soft_notch_cutoff_1;
 696 static uint16_t gyroConfig_gyro_soft_notch_hz_2;
 697 static uint16_t gyroConfig_gyro_soft_notch_cutoff_2;
 698 static uint8_t  gyroConfig_gyro_to_use;
 699
 700 static const void *cmsx_menuGyro_onEnter(displayPort_t *pDisp)
 701 {
 702     UNUSED(pDisp);
 703
 704     gyroConfig_gyro_lpf1_static_hz =  gyroConfig()->gyro_lpf1_static_hz;
 705     gyroConfig_gyro_lpf2_static_hz =  gyroConfig()->gyro_lpf2_static_hz;
 706     gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
 707     gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
 708     gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
 709     gyroConfig_gyro_soft_notch_cutoff_2 = gyroConfig()->gyro_soft_notch_cutoff_2;
 710     gyroConfig_gyro_to_use = gyroConfig()->gyro_to_use;
 711
 712     return NULL;
 713 }
 714
 715 static const void *cmsx_menuGyro_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 716 {
 717     UNUSED(pDisp);
 718     UNUSED(self);
 719
 720     gyroConfigMutable()->gyro_lpf1_static_hz =  gyroConfig_gyro_lpf1_static_hz;
 721     gyroConfigMutable()->gyro_lpf2_static_hz =  gyroConfig_gyro_lpf2_static_hz;
 722     gyroConfigMutable()->gyro_soft_notch_hz_1 = gyroConfig_gyro_soft_notch_hz_1;
 723     gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
 724     gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
 725     gyroConfigMutable()->gyro_soft_notch_cutoff_2 = gyroConfig_gyro_soft_notch_cutoff_2;
 726     gyroConfigMutable()->gyro_to_use = gyroConfig_gyro_to_use;
 727
 728     return NULL;
 729 }
 730
 731 static const OSD_Entry cmsx_menuFilterGlobalEntries[] =
 732 {
 733     { "-- FILTER GLB  --", OME_Label, NULL, NULL, 0 },
 734
 735     { "GYRO LPF1",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lpf1_static_hz, 0, LPF_MAX_HZ, 1 }, 0 },
 736 #ifdef USE_GYRO_LPF2
 737     { "GYRO LPF2",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lpf2_static_hz,  0, LPF_MAX_HZ, 1 }, 0 },
 738 #endif
 739     { "GYRO NF1",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_1,     0, 500, 1 }, 0 },
 740     { "GYRO NF1C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
 741     { "GYRO NF2",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2,     0, 500, 1 }, 0 },
 742     { "GYRO NF2C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
 743 #ifdef USE_MULTI_GYRO
 744     { "GYRO TO USE",  OME_TAB,  NULL, &(OSD_TAB_t)    { &gyroConfig_gyro_to_use,  2, osdTableGyroToUse}, REBOOT_REQUIRED },
 745 #endif
 746
 747     { "BACK", OME_Back, NULL, NULL, 0 },
 748     { NULL, OME_END, NULL, NULL, 0 }
 749 };
 750
 751 static CMS_Menu cmsx_menuFilterGlobal = {
 752 #ifdef CMS_MENU_DEBUG
 753     .GUARD_text = "XFLTGLB",
 754     .GUARD_type = OME_MENU,
 755 #endif
 756     .onEnter = cmsx_menuGyro_onEnter,
 757     .onExit = cmsx_menuGyro_onExit,
 758     .onDisplayUpdate = NULL,
 759     .entries = cmsx_menuFilterGlobalEntries,
 760 };
 761
 762 #if (defined(USE_DYN_NOTCH_FILTER) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
 763
 764 #ifdef USE_DYN_NOTCH_FILTER
 765 static uint16_t dynFiltNotchMaxHz;
 766 static uint8_t  dynFiltNotchCount;
 767 static uint16_t dynFiltNotchQ;
 768 static uint16_t dynFiltNotchMinHz;
 769 #endif
 770 #ifdef USE_DYN_LPF
 771 static uint16_t gyroLpfDynMin;
 772 static uint16_t gyroLpfDynMax;
 773 static uint8_t gyroLpfDynExpo;
 774 static uint16_t dtermLpfDynMin;
 775 static uint16_t dtermLpfDynMax;
 776 static uint8_t dtermLpfDynExpo;
 777 #endif
 778
 779 static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
 780 {
 781     UNUSED(pDisp);
 782
 783 #ifdef USE_DYN_NOTCH_FILTER
 784     dynFiltNotchMaxHz   = dynNotchConfig()->dyn_notch_max_hz;
 785     dynFiltNotchCount   = dynNotchConfig()->dyn_notch_count;
 786     dynFiltNotchQ       = dynNotchConfig()->dyn_notch_q;
 787     dynFiltNotchMinHz   = dynNotchConfig()->dyn_notch_min_hz;
 788 #endif
 789 #ifdef USE_DYN_LPF
 790     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 791     gyroLpfDynMin   = gyroConfig()->gyro_lpf1_dyn_min_hz;
 792     gyroLpfDynMax   = gyroConfig()->gyro_lpf1_dyn_max_hz;
 793     gyroLpfDynExpo  = gyroConfig()->gyro_lpf1_dyn_expo;
 794     dtermLpfDynMin  = pidProfile->dterm_lpf1_dyn_min_hz;
 795     dtermLpfDynMax  = pidProfile->dterm_lpf1_dyn_max_hz;
 796     dtermLpfDynExpo = pidProfile->dterm_lpf1_dyn_expo;
 797 #endif
 798
 799     return NULL;
 800 }
 801
 802 static const void *cmsx_menuDynFilt_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 803 {
 804     UNUSED(pDisp);
 805     UNUSED(self);
 806
 807 #ifdef USE_DYN_NOTCH_FILTER
 808     dynNotchConfigMutable()->dyn_notch_max_hz        = dynFiltNotchMaxHz;
 809     dynNotchConfigMutable()->dyn_notch_count         = dynFiltNotchCount;
 810     dynNotchConfigMutable()->dyn_notch_q             = dynFiltNotchQ;
 811     dynNotchConfigMutable()->dyn_notch_min_hz        = dynFiltNotchMinHz;
 812 #endif
 813 #ifdef USE_DYN_LPF
 814     pidProfile_t *pidProfile = currentPidProfile;
 815     gyroConfigMutable()->gyro_lpf1_dyn_min_hz = gyroLpfDynMin;
 816     gyroConfigMutable()->gyro_lpf1_dyn_max_hz = gyroLpfDynMax;
 817     gyroConfigMutable()->gyro_lpf1_dyn_expo   = gyroLpfDynExpo;
 818     pidProfile->dterm_lpf1_dyn_min_hz         = dtermLpfDynMin;
 819     pidProfile->dterm_lpf1_dyn_max_hz         = dtermLpfDynMax;
 820     pidProfile->dterm_lpf1_dyn_expo           = dtermLpfDynExpo;
 821 #endif
 822
 823     return NULL;
 824 }
 825
 826 static const OSD_Entry cmsx_menuDynFiltEntries[] =
 827 {
 828     { "-- DYN FILT --", OME_Label, NULL, NULL, 0 },
 829
 830 #ifdef USE_DYN_NOTCH_FILTER
 831     { "NOTCH COUNT",    OME_UINT8,  NULL, &(OSD_UINT8_t)  { &dynFiltNotchCount,   0, DYN_NOTCH_COUNT_MAX, 1 }, 0 },
 832     { "NOTCH Q",        OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ,       1, 1000, 1 }, 0 },
 833     { "NOTCH MIN HZ",   OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz,   60, 250, 1 }, 0 },
 834     { "NOTCH MAX HZ",   OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMaxHz,   200, 1000, 1 }, 0 },
 835 #endif
 836
 837 #ifdef USE_DYN_LPF
 838     { "GYRO DLPF MIN",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroLpfDynMin,  0, 1000, 1 }, 0 },
 839     { "GYRO DLPF MAX",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroLpfDynMax,  0, 1000, 1 }, 0 },
 840     { "GYRO DLPF EXPO",  OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroLpfDynExpo,   0, 10, 1 }, 0 },
 841     { "DTERM DLPF MIN",  OME_UINT16, NULL, &(OSD_UINT16_t) { &dtermLpfDynMin, 0, 1000, 1 }, 0 },
 842     { "DTERM DLPF MAX",  OME_UINT16, NULL, &(OSD_UINT16_t) { &dtermLpfDynMax, 0, 1000, 1 }, 0 },
 843     { "DTERM DLPF EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &dtermLpfDynExpo,  0, 10, 1 }, 0 },
 844 #endif
 845
 846     { "BACK", OME_Back, NULL, NULL, 0 },
 847     { NULL, OME_END, NULL, NULL, 0 }
 848 };
 849
 850 static CMS_Menu cmsx_menuDynFilt = {
 851 #ifdef CMS_MENU_DEBUG
 852     .GUARD_text = "XDYNFLT",
 853     .GUARD_type = OME_MENU,
 854 #endif
 855     .onEnter = cmsx_menuDynFilt_onEnter,
 856     .onExit = cmsx_menuDynFilt_onExit,
 857     .onDisplayUpdate = NULL,
 858     .entries = cmsx_menuDynFiltEntries,
 859 };
 860
 861 #endif
 862
 863 static uint16_t cmsx_dterm_lpf1_static_hz;
 864 static uint16_t cmsx_dterm_lpf2_static_hz;
 865 static uint16_t cmsx_dterm_notch_hz;
 866 static uint16_t cmsx_dterm_notch_cutoff;
 867 static uint16_t cmsx_yaw_lowpass_hz;
 868
 869 static const void *cmsx_FilterPerProfileRead(displayPort_t *pDisp)
 870 {
 871     UNUSED(pDisp);
 872
 873     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 874
 875     cmsx_dterm_lpf1_static_hz   = pidProfile->dterm_lpf1_static_hz;
 876     cmsx_dterm_lpf2_static_hz   = pidProfile->dterm_lpf2_static_hz;
 877     cmsx_dterm_notch_hz         = pidProfile->dterm_notch_hz;
 878     cmsx_dterm_notch_cutoff     = pidProfile->dterm_notch_cutoff;
 879     cmsx_yaw_lowpass_hz         = pidProfile->yaw_lowpass_hz;
 880
 881     return NULL;
 882 }
 883
 884 static const void *cmsx_FilterPerProfileWriteback(displayPort_t *pDisp, const OSD_Entry *self)
 885 {
 886     UNUSED(pDisp);
 887     UNUSED(self);
 888
 889     pidProfile_t *pidProfile = currentPidProfile;
 890
 891     pidProfile->dterm_lpf1_static_hz  = cmsx_dterm_lpf1_static_hz;
 892     pidProfile->dterm_lpf2_static_hz  = cmsx_dterm_lpf2_static_hz;
 893     pidProfile->dterm_notch_hz        = cmsx_dterm_notch_hz;
 894     pidProfile->dterm_notch_cutoff    = cmsx_dterm_notch_cutoff;
 895     pidProfile->yaw_lowpass_hz        = cmsx_yaw_lowpass_hz;
 896
 897     return NULL;
 898 }
 899
 900 static const OSD_Entry cmsx_menuFilterPerProfileEntries[] =
 901 {
 902     { "-- FILTER PP  --", OME_Label, NULL, NULL, 0 },
 903
 904     { "DTERM LPF1", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lpf1_static_hz, 0, LPF_MAX_HZ, 1 }, 0 },
 905     { "DTERM LPF2", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lpf2_static_hz, 0, LPF_MAX_HZ, 1 }, 0 },
 906     { "DTERM NF",   OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz,       0, LPF_MAX_HZ, 1 }, 0 },
 907     { "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff,   0, LPF_MAX_HZ, 1 }, 0 },
 908     { "YAW LPF",    OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lowpass_hz,       0, 500, 1 }, 0 },
 909
 910     { "BACK", OME_Back, NULL, NULL, 0 },
 911     { NULL, OME_END, NULL, NULL, 0 }
 912 };
 913
 914 static CMS_Menu cmsx_menuFilterPerProfile = {
 915 #ifdef CMS_MENU_DEBUG
 916     .GUARD_text = "XFLTPP",
 917     .GUARD_type = OME_MENU,
 918 #endif
 919     .onEnter = cmsx_FilterPerProfileRead,
 920     .onExit = cmsx_FilterPerProfileWriteback,
 921     .onDisplayUpdate = NULL,
 922     .entries = cmsx_menuFilterPerProfileEntries,
 923 };
 924
 925 #ifdef USE_EXTENDED_CMS_MENUS
 926
 927 static uint8_t cmsx_dstPidProfile;
 928 static uint8_t cmsx_dstControlRateProfile;
 929
 930 static const char * const cmsx_ProfileNames[] = {
 931     "-",
 932     "1",
 933     "2",
 934     "3"
 935 };
 936
 937 static OSD_TAB_t cmsx_PidProfileTable = { &cmsx_dstPidProfile, 3, cmsx_ProfileNames };
 938 static OSD_TAB_t cmsx_ControlRateProfileTable = { &cmsx_dstControlRateProfile, 3, cmsx_ProfileNames };
 939
 940 static const void *cmsx_menuCopyProfile_onEnter(displayPort_t *pDisp)
 941 {
 942     UNUSED(pDisp);
 943
 944     cmsx_dstPidProfile = 0;
 945     cmsx_dstControlRateProfile = 0;
 946
 947     return NULL;
 948 }
 949
 950 static const void *cmsx_CopyPidProfile(displayPort_t *pDisplay, const void *ptr)
 951 {
 952     UNUSED(pDisplay);
 953     UNUSED(ptr);
 954
 955     if (cmsx_dstPidProfile > 0) {
 956         pidCopyProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex());
 957     }
 958
 959     return NULL;
 960 }
 961
 962 static const void *cmsx_CopyControlRateProfile(displayPort_t *pDisplay, const void *ptr)
 963 {
 964     UNUSED(pDisplay);
 965     UNUSED(ptr);
 966
 967     if (cmsx_dstControlRateProfile > 0) {
 968         copyControlRateProfile(cmsx_dstControlRateProfile - 1, getCurrentControlRateProfileIndex());
 969     }
 970
 971     return NULL;
 972 }
 973
 974 static const OSD_Entry cmsx_menuCopyProfileEntries[] =
 975 {
 976     { "-- COPY PROFILE --", OME_Label, NULL, NULL, 0},
 977
 978     { "CPY PID PROF TO",   OME_TAB,      NULL,                        &cmsx_PidProfileTable, 0 },
 979     { "COPY PP",           OME_Funcall,  cmsx_CopyPidProfile,         NULL, 0 },
 980     { "CPY RATE PROF TO",  OME_TAB,      NULL,                        &cmsx_ControlRateProfileTable, 0 },
 981     { "COPY RP",           OME_Funcall,  cmsx_CopyControlRateProfile, NULL, 0 },
 982
 983     { "BACK", OME_Back, NULL, NULL, 0 },
 984     { NULL, OME_END, NULL, NULL, 0 }
 985 };
 986
 987 CMS_Menu cmsx_menuCopyProfile = {
 988 #ifdef CMS_MENU_DEBUG
 989     .GUARD_text = "XCPY",
 990     .GUARD_type = OME_MENU,
 991 #endif
 992     .onEnter = cmsx_menuCopyProfile_onEnter,
 993     .onExit = NULL,
 994     .onDisplayUpdate = NULL,
 995     .entries = cmsx_menuCopyProfileEntries,
 996 };
 997
 998 #endif
 999
1000 static const OSD_Entry cmsx_menuImuEntries[] =
1001 {
1002     { "-- PROFILE --", OME_Label, NULL, NULL, 0},
1003
1004     {"PID PROF",  OME_UINT8,   cmsx_profileIndexOnChange,     &(OSD_UINT8_t){ &tmpPidProfileIndex, 1, PID_PROFILE_COUNT, 1},    0},
1005     {"PID",       OME_Submenu, cmsMenuChange,                 &cmsx_menuPid,                                                 0},
1006 #ifdef USE_SIMPLIFIED_TUNING
1007     {"SIMPLIFIED TUNING",   OME_Submenu, cmsMenuChange,                 &cmsx_menuSimplifiedTuning,                                             0},
1008 #endif
1009     {"MISC PP",   OME_Submenu, cmsMenuChange,                 &cmsx_menuProfileOther,                                        0},
1010     {"FILT PP",   OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterPerProfile,                                    0},
1011
1012     {"RATE PROF", OME_UINT8,   cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, CONTROL_RATE_PROFILE_COUNT, 1}, 0},
1013     {"RATE",      OME_Submenu, cmsMenuChange,                 &cmsx_menuRateProfile,                                         0},
1014
1015     {"FILT GLB",  OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterGlobal,                                        0},
1016 #if  (defined(USE_DYN_NOTCH_FILTER) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
1017     {"DYN FILT",  OME_Submenu, cmsMenuChange,                 &cmsx_menuDynFilt,                                             0},
1018 #endif
1019
1020 #ifdef USE_EXTENDED_CMS_MENUS
1021     {"COPY PROF", OME_Submenu, cmsMenuChange,                 &cmsx_menuCopyProfile,                                         0},
1022 #endif /* USE_EXTENDED_CMS_MENUS */
1023
1024     {"BACK", OME_Back, NULL, NULL, 0},
1025     {NULL, OME_END, NULL, NULL, 0}
1026 };
1027
1028 CMS_Menu cmsx_menuImu = {
1029 #ifdef CMS_MENU_DEBUG
1030     .GUARD_text = "XIMU",
1031     .GUARD_type = OME_MENU,
1032 #endif
1033     .onEnter = cmsx_menuImu_onEnter,
1034     .onExit = cmsx_menuImu_onExit,
1035     .onDisplayUpdate = NULL,
1036     .entries = cmsx_menuImuEntries,
1037 };
1038
1039 #endif // CMS