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_angleP;
 520 static uint8_t  cmsx_angleFF;
 521 static uint8_t  cmsx_angleLimit;
 522
 523 static uint8_t  cmsx_horizonStrength;
 524 static uint8_t  cmsx_horizonLimitSticks;
 525 static uint8_t  cmsx_horizonLimitDegrees;
 526
 527 static uint8_t  cmsx_throttleBoost;
 528 static uint8_t  cmsx_thrustLinearization;
 529 static uint8_t  cmsx_antiGravityGain;
 530 static uint8_t  cmsx_motorOutputLimit;
 531 static int8_t   cmsx_autoProfileCellCount;
 532 #ifdef USE_D_MIN
 533 static uint8_t  cmsx_d_min[XYZ_AXIS_COUNT];
 534 static uint8_t  cmsx_d_min_gain;
 535 static uint8_t  cmsx_d_min_advance;
 536 #endif
 537
 538 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
 539 static uint8_t  cmsx_vbat_sag_compensation;
 540 #endif
 541
 542 #ifdef USE_ITERM_RELAX
 543 static uint8_t cmsx_iterm_relax;
 544 static uint8_t cmsx_iterm_relax_type;
 545 static uint8_t cmsx_iterm_relax_cutoff;
 546 #endif
 547
 548 #ifdef USE_FEEDFORWARD
 549 static uint8_t cmsx_feedforward_transition;
 550 static uint8_t cmsx_feedforward_boost;
 551 static uint8_t cmsx_feedforward_averaging;
 552 static uint8_t cmsx_feedforward_smooth_factor;
 553 static uint8_t cmsx_feedforward_jitter_factor;
 554 #endif
 555
 556 static uint8_t cmsx_tpa_rate;
 557 static uint16_t cmsx_tpa_breakpoint;
 558
 559 static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
 560 {
 561     UNUSED(pDisp);
 562
 563     setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName);
 564
 565     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 566
 567     cmsx_angleP =             pidProfile->pid[PID_LEVEL].P;
 568     cmsx_angleFF =            pidProfile->pid[PID_LEVEL].F;
 569     cmsx_angleLimit =         pidProfile->angle_limit;
 570
 571     cmsx_horizonStrength =    pidProfile->pid[PID_LEVEL].I;
 572     cmsx_horizonLimitSticks = pidProfile->pid[PID_LEVEL].D;
 573     cmsx_horizonLimitDegrees = pidProfile->horizon_limit_degrees;
 574
 575     cmsx_antiGravityGain   = pidProfile->anti_gravity_gain;
 576
 577     cmsx_throttleBoost = pidProfile->throttle_boost;
 578     cmsx_thrustLinearization = pidProfile->thrustLinearization;
 579     cmsx_motorOutputLimit = pidProfile->motor_output_limit;
 580     cmsx_autoProfileCellCount = pidProfile->auto_profile_cell_count;
 581
 582 #ifdef USE_D_MIN
 583     for (unsigned i = 0; i < XYZ_AXIS_COUNT; i++) {
 584         cmsx_d_min[i]  = pidProfile->d_min[i];
 585     }
 586     cmsx_d_min_gain = pidProfile->d_min_gain;
 587     cmsx_d_min_advance = pidProfile->d_min_advance;
 588 #endif
 589
 590 #ifdef USE_ITERM_RELAX
 591     cmsx_iterm_relax = pidProfile->iterm_relax;
 592     cmsx_iterm_relax_type = pidProfile->iterm_relax_type;
 593     cmsx_iterm_relax_cutoff = pidProfile->iterm_relax_cutoff;
 594 #endif
 595
 596 #ifdef USE_FEEDFORWARD
 597     cmsx_feedforward_transition  = pidProfile->feedforward_transition;
 598     cmsx_feedforward_averaging = pidProfile->feedforward_averaging;
 599     cmsx_feedforward_boost = pidProfile->feedforward_boost;
 600     cmsx_feedforward_smooth_factor = pidProfile->feedforward_smooth_factor;
 601     cmsx_feedforward_jitter_factor = pidProfile->feedforward_jitter_factor;
 602 #endif
 603
 604 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
 605     cmsx_vbat_sag_compensation = pidProfile->vbat_sag_compensation;
 606 #endif
 607     cmsx_tpa_rate = pidProfile->tpa_rate;
 608     cmsx_tpa_breakpoint = pidProfile->tpa_breakpoint;
 609
 610     return NULL;
 611 }
 612
 613 static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 614 {
 615     UNUSED(pDisp);
 616     UNUSED(self);
 617
 618     pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
 619     pidInitConfig(currentPidProfile);
 620
 621     pidProfile->pid[PID_LEVEL].P = cmsx_angleP;
 622     pidProfile->pid[PID_LEVEL].F = cmsx_angleFF;
 623     pidProfile->angle_limit = cmsx_angleLimit;
 624
 625     pidProfile->pid[PID_LEVEL].I = cmsx_horizonStrength;
 626     pidProfile->pid[PID_LEVEL].D = cmsx_horizonLimitSticks;
 627     pidProfile->horizon_limit_degrees = cmsx_horizonLimitDegrees;
 628
 629     pidProfile->anti_gravity_gain   = cmsx_antiGravityGain;
 630
 631     pidProfile->throttle_boost = cmsx_throttleBoost;
 632     pidProfile->thrustLinearization = cmsx_thrustLinearization;
 633     pidProfile->motor_output_limit = cmsx_motorOutputLimit;
 634     pidProfile->auto_profile_cell_count = cmsx_autoProfileCellCount;
 635
 636 #ifdef USE_D_MIN
 637     for (unsigned i = 0; i < XYZ_AXIS_COUNT; i++) {
 638         pidProfile->d_min[i] = cmsx_d_min[i];
 639     }
 640     pidProfile->d_min_gain = cmsx_d_min_gain;
 641     pidProfile->d_min_advance = cmsx_d_min_advance;
 642 #endif
 643
 644 #ifdef USE_ITERM_RELAX
 645     pidProfile->iterm_relax = cmsx_iterm_relax;
 646     pidProfile->iterm_relax_type = cmsx_iterm_relax_type;
 647     pidProfile->iterm_relax_cutoff = cmsx_iterm_relax_cutoff;
 648 #endif
 649
 650 #ifdef USE_FEEDFORWARD
 651     pidProfile->feedforward_transition = cmsx_feedforward_transition;
 652     pidProfile->feedforward_averaging = cmsx_feedforward_averaging;
 653     pidProfile->feedforward_boost = cmsx_feedforward_boost;
 654     pidProfile->feedforward_smooth_factor = cmsx_feedforward_smooth_factor;
 655     pidProfile->feedforward_jitter_factor = cmsx_feedforward_jitter_factor;
 656 #endif
 657
 658 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
 659     pidProfile->vbat_sag_compensation = cmsx_vbat_sag_compensation;
 660 #endif
 661     pidProfile->tpa_rate = cmsx_tpa_rate;
 662     pidProfile->tpa_breakpoint = cmsx_tpa_breakpoint;
 663
 664     initEscEndpoints();
 665     return NULL;
 666 }
 667
 668 static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
 669     { "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString },
 670
 671 #ifdef USE_FEEDFORWARD
 672     { "FF TRANSITION", OME_FLOAT,  NULL, &(OSD_FLOAT_t)  { &cmsx_feedforward_transition,        0,    100,   1, 10 } },
 673     { "FF AVERAGING",  OME_TAB,    NULL, &(OSD_TAB_t)    { &cmsx_feedforward_averaging,         4, lookupTableFeedforwardAveraging} },
 674     { "FF SMOOTHNESS", OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_smooth_factor,     0,     95,   1  }    },
 675     { "FF JITTER",     OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_jitter_factor,     0,     20,   1  }    },
 676     { "FF BOOST",      OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_boost,             0,     50,   1  }    },
 677 #endif
 678     { "ANGLE P",         OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_angleP,                     0,    200,   1  }    },
 679     { "ANGLE FF",        OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_angleFF,                    0,    200,   1  }    },
 680     { "ANGLE LIMIT",     OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_angleLimit,                10,     90,   1  }    },
 681
 682     { "HORZN STR",       OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_horizonStrength,            0,    100,   1  }    },
 683     { "HORZN LIM_STK",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_horizonLimitSticks,        10,    200,   1  }    },
 684     { "HORZN LIM_DEG",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_horizonLimitDegrees,       10,    250,   1  }    },
 685
 686     { "AG GAIN",     OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &cmsx_antiGravityGain,   ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX, 1, 100 }    },
 687 #ifdef USE_THROTTLE_BOOST
 688     { "THR BOOST",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_throttleBoost,          0,    100,   1  }    },
 689 #endif
 690 #ifdef USE_THRUST_LINEARIZATION
 691     { "THR LINEAR",  OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_thrustLinearization,    0,    150,   1  }    },
 692 #endif
 693 #ifdef USE_ITERM_RELAX
 694     { "I_RELAX",         OME_TAB,    NULL, &(OSD_TAB_t)     { &cmsx_iterm_relax,        ITERM_RELAX_COUNT - 1,      lookupTableItermRelax       } },
 695     { "I_RELAX TYPE",    OME_TAB,    NULL, &(OSD_TAB_t)     { &cmsx_iterm_relax_type,   ITERM_RELAX_TYPE_COUNT - 1, lookupTableItermRelaxType   } },
 696     { "I_RELAX CUTOFF",  OME_UINT8,  NULL, &(OSD_UINT8_t)   { &cmsx_iterm_relax_cutoff, 1, 50, 1 } },
 697 #endif
 698 #ifdef USE_LAUNCH_CONTROL
 699     {"LAUNCH CONTROL", OME_Submenu, cmsMenuChange, &cmsx_menuLaunchControl },
 700 #endif
 701     { "MTR OUT LIM %",OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_motorOutputLimit, MOTOR_OUTPUT_LIMIT_PERCENT_MIN,  MOTOR_OUTPUT_LIMIT_PERCENT_MAX,  1} },
 702
 703     { "AUTO CELL CNT", OME_INT8, NULL, &(OSD_INT8_t) { &cmsx_autoProfileCellCount, AUTO_PROFILE_CELL_COUNT_CHANGE, MAX_AUTO_DETECT_CELL_COUNT, 1} },
 704
 705 #ifdef USE_D_MIN
 706     { "D_MIN ROLL",  OME_UINT8 | SLIDER_RP,  NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_ROLL],      0, 100, 1 } },
 707     { "D_MIN PITCH", OME_UINT8 | SLIDER_RP,  NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_PITCH],     0, 100, 1 } },
 708     { "D_MIN YAW",   OME_UINT8 | SLIDER_RPY,  NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_YAW],       0, 100, 1 } },
 709     { "D_MIN GAIN",  OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_min_gain,          0, 100, 1 } },
 710     { "D_MIN ADV",   OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_min_advance,       0, 200, 1 } },
 711 #endif
 712
 713 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
 714     { "VBAT_SAG_COMP", OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_vbat_sag_compensation, 0, 150, 1 } },
 715 #endif
 716
 717     { "TPA RATE",  OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &cmsx_tpa_rate,          0,  100,  1, 10} },
 718     { "TPA BRKPT",   OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_tpa_breakpoint, 1000, 2000, 10} },
 719
 720     { "BACK", OME_Back, NULL, NULL },
 721     { NULL, OME_END, NULL, NULL}
 722 };
 723
 724 static CMS_Menu cmsx_menuProfileOther = {
 725 #ifdef CMS_MENU_DEBUG
 726     .GUARD_text = "XPROFOTHER",
 727     .GUARD_type = OME_MENU,
 728 #endif
 729     .onEnter = cmsx_profileOtherOnEnter,
 730     .onExit = cmsx_profileOtherOnExit,
 731     .onDisplayUpdate = NULL,
 732     .entries = cmsx_menuProfileOtherEntries,
 733 };
 734
 735
 736 static uint16_t gyroConfig_gyro_lpf1_static_hz;
 737 static uint16_t gyroConfig_gyro_lpf2_static_hz;
 738 static uint16_t gyroConfig_gyro_soft_notch_hz_1;
 739 static uint16_t gyroConfig_gyro_soft_notch_cutoff_1;
 740 static uint16_t gyroConfig_gyro_soft_notch_hz_2;
 741 static uint16_t gyroConfig_gyro_soft_notch_cutoff_2;
 742 static uint8_t  gyroConfig_gyro_to_use;
 743
 744 static const void *cmsx_menuGyro_onEnter(displayPort_t *pDisp)
 745 {
 746     UNUSED(pDisp);
 747
 748     gyroConfig_gyro_lpf1_static_hz =  gyroConfig()->gyro_lpf1_static_hz;
 749     gyroConfig_gyro_lpf2_static_hz =  gyroConfig()->gyro_lpf2_static_hz;
 750     gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
 751     gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
 752     gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
 753     gyroConfig_gyro_soft_notch_cutoff_2 = gyroConfig()->gyro_soft_notch_cutoff_2;
 754     gyroConfig_gyro_to_use = gyroConfig()->gyro_to_use;
 755
 756     return NULL;
 757 }
 758
 759 static const void *cmsx_menuGyro_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 760 {
 761     UNUSED(pDisp);
 762     UNUSED(self);
 763
 764     gyroConfigMutable()->gyro_lpf1_static_hz =  gyroConfig_gyro_lpf1_static_hz;
 765     gyroConfigMutable()->gyro_lpf2_static_hz =  gyroConfig_gyro_lpf2_static_hz;
 766     gyroConfigMutable()->gyro_soft_notch_hz_1 = gyroConfig_gyro_soft_notch_hz_1;
 767     gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
 768     gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
 769     gyroConfigMutable()->gyro_soft_notch_cutoff_2 = gyroConfig_gyro_soft_notch_cutoff_2;
 770     gyroConfigMutable()->gyro_to_use = gyroConfig_gyro_to_use;
 771
 772     return NULL;
 773 }
 774
 775 static const OSD_Entry cmsx_menuFilterGlobalEntries[] =
 776 {
 777     { "-- FILTER GLB  --", OME_Label, NULL, NULL },
 778
 779     { "GYRO LPF1",  OME_UINT16 | SLIDER_GYRO, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lpf1_static_hz, 0, LPF_MAX_HZ, 1 } },
 780 #ifdef USE_GYRO_LPF2
 781     { "GYRO LPF2",  OME_UINT16 | SLIDER_GYRO, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lpf2_static_hz,  0, LPF_MAX_HZ, 1 } },
 782 #endif
 783     { "GYRO NF1",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_1,     0, 500, 1 } },
 784     { "GYRO NF1C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 } },
 785     { "GYRO NF2",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2,     0, 500, 1 } },
 786     { "GYRO NF2C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 } },
 787 #ifdef USE_MULTI_GYRO
 788     { "GYRO TO USE",  OME_TAB | REBOOT_REQUIRED,  NULL, &(OSD_TAB_t)    { &gyroConfig_gyro_to_use,  2, osdTableGyroToUse} },
 789 #endif
 790
 791     { "BACK", OME_Back, NULL, NULL },
 792     { NULL, OME_END, NULL, NULL}
 793 };
 794
 795 static CMS_Menu cmsx_menuFilterGlobal = {
 796 #ifdef CMS_MENU_DEBUG
 797     .GUARD_text = "XFLTGLB",
 798     .GUARD_type = OME_MENU,
 799 #endif
 800     .onEnter = cmsx_menuGyro_onEnter,
 801     .onExit = cmsx_menuGyro_onExit,
 802     .onDisplayUpdate = NULL,
 803     .entries = cmsx_menuFilterGlobalEntries,
 804 };
 805
 806 #if (defined(USE_DYN_NOTCH_FILTER) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
 807
 808 #ifdef USE_DYN_NOTCH_FILTER
 809 static uint16_t dynFiltNotchMaxHz;
 810 static uint8_t  dynFiltNotchCount;
 811 static uint16_t dynFiltNotchQ;
 812 static uint16_t dynFiltNotchMinHz;
 813 #endif
 814 #ifdef USE_DYN_LPF
 815 static uint16_t gyroLpfDynMin;
 816 static uint16_t gyroLpfDynMax;
 817 static uint8_t gyroLpfDynExpo;
 818 static uint16_t dtermLpfDynMin;
 819 static uint16_t dtermLpfDynMax;
 820 static uint8_t dtermLpfDynExpo;
 821 #endif
 822
 823 static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
 824 {
 825     UNUSED(pDisp);
 826
 827 #ifdef USE_DYN_NOTCH_FILTER
 828     dynFiltNotchMaxHz   = dynNotchConfig()->dyn_notch_max_hz;
 829     dynFiltNotchCount   = dynNotchConfig()->dyn_notch_count;
 830     dynFiltNotchQ       = dynNotchConfig()->dyn_notch_q;
 831     dynFiltNotchMinHz   = dynNotchConfig()->dyn_notch_min_hz;
 832 #endif
 833 #ifdef USE_DYN_LPF
 834     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 835     gyroLpfDynMin   = gyroConfig()->gyro_lpf1_dyn_min_hz;
 836     gyroLpfDynMax   = gyroConfig()->gyro_lpf1_dyn_max_hz;
 837     gyroLpfDynExpo  = gyroConfig()->gyro_lpf1_dyn_expo;
 838     dtermLpfDynMin  = pidProfile->dterm_lpf1_dyn_min_hz;
 839     dtermLpfDynMax  = pidProfile->dterm_lpf1_dyn_max_hz;
 840     dtermLpfDynExpo = pidProfile->dterm_lpf1_dyn_expo;
 841 #endif
 842
 843     return NULL;
 844 }
 845
 846 static const void *cmsx_menuDynFilt_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 847 {
 848     UNUSED(pDisp);
 849     UNUSED(self);
 850
 851 #ifdef USE_DYN_NOTCH_FILTER
 852     dynNotchConfigMutable()->dyn_notch_max_hz        = dynFiltNotchMaxHz;
 853     dynNotchConfigMutable()->dyn_notch_count         = dynFiltNotchCount;
 854     dynNotchConfigMutable()->dyn_notch_q             = dynFiltNotchQ;
 855     dynNotchConfigMutable()->dyn_notch_min_hz        = dynFiltNotchMinHz;
 856 #endif
 857 #ifdef USE_DYN_LPF
 858     pidProfile_t *pidProfile = currentPidProfile;
 859     gyroConfigMutable()->gyro_lpf1_dyn_min_hz = gyroLpfDynMin;
 860     gyroConfigMutable()->gyro_lpf1_dyn_max_hz = gyroLpfDynMax;
 861     gyroConfigMutable()->gyro_lpf1_dyn_expo   = gyroLpfDynExpo;
 862     pidProfile->dterm_lpf1_dyn_min_hz         = dtermLpfDynMin;
 863     pidProfile->dterm_lpf1_dyn_max_hz         = dtermLpfDynMax;
 864     pidProfile->dterm_lpf1_dyn_expo           = dtermLpfDynExpo;
 865 #endif
 866
 867     return NULL;
 868 }
 869
 870 static const OSD_Entry cmsx_menuDynFiltEntries[] =
 871 {
 872     { "-- DYN FILT --", OME_Label, NULL, NULL },
 873
 874 #ifdef USE_DYN_NOTCH_FILTER
 875     { "NOTCH COUNT",    OME_UINT8,  NULL, &(OSD_UINT8_t)  { &dynFiltNotchCount,   0, DYN_NOTCH_COUNT_MAX, 1 } },
 876     { "NOTCH Q",        OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ,       1, 1000, 1 } },
 877     { "NOTCH MIN HZ",   OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz,   20, 250, 1 } },
 878     { "NOTCH MAX HZ",   OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMaxHz,   200, 1000, 1 } },
 879 #endif
 880
 881 #ifdef USE_DYN_LPF
 882     { "GYRO DLPF MIN",   OME_UINT16 | SLIDER_GYRO, NULL, &(OSD_UINT16_t) { &gyroLpfDynMin,  0, 1000, 1 } },
 883     { "GYRO DLPF MAX",   OME_UINT16 | SLIDER_GYRO, NULL, &(OSD_UINT16_t) { &gyroLpfDynMax,  0, 1000, 1 } },
 884     { "GYRO DLPF EXPO",  OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroLpfDynExpo,   0, 10, 1 } },
 885     { "DTERM DLPF MIN",  OME_UINT16 | SLIDER_DTERM, NULL, &(OSD_UINT16_t) { &dtermLpfDynMin, 0, 1000, 1 } },
 886     { "DTERM DLPF MAX",  OME_UINT16 | SLIDER_DTERM, NULL, &(OSD_UINT16_t) { &dtermLpfDynMax, 0, 1000, 1 } },
 887     { "DTERM DLPF EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &dtermLpfDynExpo,  0, 10, 1 } },
 888 #endif
 889
 890     { "BACK", OME_Back, NULL, NULL },
 891     { NULL, OME_END, NULL, NULL}
 892 };
 893
 894 static CMS_Menu cmsx_menuDynFilt = {
 895 #ifdef CMS_MENU_DEBUG
 896     .GUARD_text = "XDYNFLT",
 897     .GUARD_type = OME_MENU,
 898 #endif
 899     .onEnter = cmsx_menuDynFilt_onEnter,
 900     .onExit = cmsx_menuDynFilt_onExit,
 901     .onDisplayUpdate = NULL,
 902     .entries = cmsx_menuDynFiltEntries,
 903 };
 904
 905 #endif
 906
 907 static uint16_t cmsx_dterm_lpf1_static_hz;
 908 static uint16_t cmsx_dterm_lpf2_static_hz;
 909 static uint16_t cmsx_dterm_notch_hz;
 910 static uint16_t cmsx_dterm_notch_cutoff;
 911 static uint16_t cmsx_yaw_lowpass_hz;
 912
 913 static const void *cmsx_FilterPerProfileRead(displayPort_t *pDisp)
 914 {
 915     UNUSED(pDisp);
 916
 917     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 918
 919     cmsx_dterm_lpf1_static_hz   = pidProfile->dterm_lpf1_static_hz;
 920     cmsx_dterm_lpf2_static_hz   = pidProfile->dterm_lpf2_static_hz;
 921     cmsx_dterm_notch_hz         = pidProfile->dterm_notch_hz;
 922     cmsx_dterm_notch_cutoff     = pidProfile->dterm_notch_cutoff;
 923     cmsx_yaw_lowpass_hz         = pidProfile->yaw_lowpass_hz;
 924
 925     return NULL;
 926 }
 927
 928 static const void *cmsx_FilterPerProfileWriteback(displayPort_t *pDisp, const OSD_Entry *self)
 929 {
 930     UNUSED(pDisp);
 931     UNUSED(self);
 932
 933     pidProfile_t *pidProfile = currentPidProfile;
 934
 935     pidProfile->dterm_lpf1_static_hz  = cmsx_dterm_lpf1_static_hz;
 936     pidProfile->dterm_lpf2_static_hz  = cmsx_dterm_lpf2_static_hz;
 937     pidProfile->dterm_notch_hz        = cmsx_dterm_notch_hz;
 938     pidProfile->dterm_notch_cutoff    = cmsx_dterm_notch_cutoff;
 939     pidProfile->yaw_lowpass_hz        = cmsx_yaw_lowpass_hz;
 940
 941     return NULL;
 942 }
 943
 944 static const OSD_Entry cmsx_menuFilterPerProfileEntries[] =
 945 {
 946     { "-- FILTER PP  --", OME_Label, NULL, NULL },
 947
 948     { "DTERM LPF1", OME_UINT16 | SLIDER_DTERM, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lpf1_static_hz, 0, LPF_MAX_HZ, 1 } },
 949     { "DTERM LPF2", OME_UINT16 | SLIDER_DTERM, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lpf2_static_hz, 0, LPF_MAX_HZ, 1 } },
 950     { "DTERM NF",   OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz,       0, LPF_MAX_HZ, 1 } },
 951     { "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff,   0, LPF_MAX_HZ, 1 } },
 952     { "YAW LPF",    OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lowpass_hz,       0, 500, 1 } },
 953
 954     { "BACK", OME_Back, NULL, NULL },
 955     { NULL, OME_END, NULL, NULL}
 956 };
 957
 958 static CMS_Menu cmsx_menuFilterPerProfile = {
 959 #ifdef CMS_MENU_DEBUG
 960     .GUARD_text = "XFLTPP",
 961     .GUARD_type = OME_MENU,
 962 #endif
 963     .onEnter = cmsx_FilterPerProfileRead,
 964     .onExit = cmsx_FilterPerProfileWriteback,
 965     .onDisplayUpdate = NULL,
 966     .entries = cmsx_menuFilterPerProfileEntries,
 967 };
 968
 969 #ifdef USE_EXTENDED_CMS_MENUS
 970
 971 static uint8_t cmsx_dstPidProfile;
 972 static uint8_t cmsx_dstControlRateProfile;
 973
 974 static const char * const cmsx_ProfileNames[] = {
 975     "-",
 976     "1",
 977     "2",
 978     "3"
 979 };
 980
 981 static OSD_TAB_t cmsx_PidProfileTable = { &cmsx_dstPidProfile, 3, cmsx_ProfileNames };
 982 static OSD_TAB_t cmsx_ControlRateProfileTable = { &cmsx_dstControlRateProfile, 3, cmsx_ProfileNames };
 983
 984 static const void *cmsx_menuCopyProfile_onEnter(displayPort_t *pDisp)
 985 {
 986     UNUSED(pDisp);
 987
 988     cmsx_dstPidProfile = 0;
 989     cmsx_dstControlRateProfile = 0;
 990
 991     return NULL;
 992 }
 993
 994 static const void *cmsx_CopyPidProfile(displayPort_t *pDisplay, const void *ptr)
 995 {
 996     UNUSED(pDisplay);
 997     UNUSED(ptr);
 998
 999     if (cmsx_dstPidProfile > 0) {
1000         pidCopyProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex());
1001     }
1002
1003     return NULL;
1004 }
1005
1006 static const void *cmsx_CopyControlRateProfile(displayPort_t *pDisplay, const void *ptr)
1007 {
1008     UNUSED(pDisplay);
1009     UNUSED(ptr);
1010
1011     if (cmsx_dstControlRateProfile > 0) {
1012         copyControlRateProfile(cmsx_dstControlRateProfile - 1, getCurrentControlRateProfileIndex());
1013     }
1014
1015     return NULL;
1016 }
1017
1018 static const OSD_Entry cmsx_menuCopyProfileEntries[] =
1019 {
1020     { "-- COPY PROFILE --", OME_Label, NULL, NULL},
1021
1022     { "CPY PID PROF TO",   OME_TAB,      NULL,                        &cmsx_PidProfileTable },
1023     { "COPY PP",           OME_Funcall,  cmsx_CopyPidProfile,         NULL },
1024     { "CPY RATE PROF TO",  OME_TAB,      NULL,                        &cmsx_ControlRateProfileTable },
1025     { "COPY RP",           OME_Funcall,  cmsx_CopyControlRateProfile, NULL },
1026
1027     { "BACK", OME_Back, NULL, NULL },
1028     { NULL, OME_END, NULL, NULL}
1029 };
1030
1031 CMS_Menu cmsx_menuCopyProfile = {
1032 #ifdef CMS_MENU_DEBUG
1033     .GUARD_text = "XCPY",
1034     .GUARD_type = OME_MENU,
1035 #endif
1036     .onEnter = cmsx_menuCopyProfile_onEnter,
1037     .onExit = NULL,
1038     .onDisplayUpdate = NULL,
1039     .entries = cmsx_menuCopyProfileEntries,
1040 };
1041
1042 #endif
1043
1044 static const OSD_Entry cmsx_menuImuEntries[] =
1045 {
1046     { "-- PROFILE --", OME_Label, NULL, NULL},
1047
1048     {"PID PROF",  OME_TAB,     cmsx_profileIndexOnChange,     &(OSD_TAB_t){&tmpPidProfileIndex, PID_PROFILE_COUNT-1, pidProfileNamePtrs}},
1049     {"PID",       OME_Submenu, cmsMenuChange,                 &cmsx_menuPid},
1050 #ifdef USE_SIMPLIFIED_TUNING
1051     {"SIMPLIFIED TUNING",   OME_Submenu, cmsMenuChange,                 &cmsx_menuSimplifiedTuning},
1052 #endif
1053     {"MISC PP",   OME_Submenu, cmsMenuChange,                 &cmsx_menuProfileOther},
1054     {"FILT PP",   OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterPerProfile},
1055
1056     {"RATE PROF", OME_TAB,   cmsx_rateProfileIndexOnChange, &(OSD_TAB_t){&tmpRateProfileIndex, CONTROL_RATE_PROFILE_COUNT-1, rateProfileNamePtrs}},
1057     {"RATE",      OME_Submenu, cmsMenuChange,                 &cmsx_menuRateProfile},
1058
1059     {"FILT GLB",  OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterGlobal},
1060 #if  (defined(USE_DYN_NOTCH_FILTER) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
1061     {"DYN FILT",  OME_Submenu, cmsMenuChange,                 &cmsx_menuDynFilt},
1062 #endif
1063
1064 #ifdef USE_EXTENDED_CMS_MENUS
1065     {"COPY PROF", OME_Submenu, cmsMenuChange,                 &cmsx_menuCopyProfile},
1066 #endif /* USE_EXTENDED_CMS_MENUS */
1067
1068     {"BACK", OME_Back, NULL, NULL},
1069     {NULL, OME_END, NULL, NULL}
1070 };
1071
1072 CMS_Menu cmsx_menuImu = {
1073 #ifdef CMS_MENU_DEBUG
1074     .GUARD_text = "XIMU",
1075     .GUARD_type = OME_MENU,
1076 #endif
1077     .onEnter = cmsx_menuImu_onEnter,
1078     .onExit = cmsx_menuImu_onExit,
1079     .onDisplayUpdate = NULL,
1080     .entries = cmsx_menuImuEntries,
1081 };
1082
1083 #endif // CMS