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 #ifdef USE_MULTI_GYRO
  82 static const char * const osdTableGyroToUse[] = {
  83     "FIRST", "SECOND", "BOTH"
  84 };
  85 #endif
  86
  87 static void setProfileIndexString(char *profileString, int profileIndex, const char *profileName)
  88 {
  89     int charIndex = 0;
  90     profileString[charIndex++] = '1' + profileIndex;
  91
  92 #ifdef USE_PROFILE_NAMES
  93     const int profileNameLen = strlen(profileName);
  94
  95     if (profileNameLen > 0) {
  96         profileString[charIndex++] = ' ';
  97         profileString[charIndex++] = '(';
  98         for (int i = 0; i < profileNameLen; i++) {
  99             profileString[charIndex++] = toupper(profileName[i]);
 100         }
 101         profileString[charIndex++] = ')';
 102     }
 103 #else
 104     UNUSED(profileName);
 105 #endif
 106
 107     profileString[charIndex] = '\0';
 108 }
 109
 110 static char pidProfileNames[PID_PROFILE_COUNT][MAX_PROFILE_NAME_LENGTH + PROFILE_INDEX_STRING_ADDITIONAL_SIZE];
 111 static const char *pidProfileNamePtrs[PID_PROFILE_COUNT];
 112
 113 static char rateProfileNames[CONTROL_RATE_PROFILE_COUNT][MAX_PROFILE_NAME_LENGTH + PROFILE_INDEX_STRING_ADDITIONAL_SIZE];
 114 static const char *rateProfileNamePtrs[CONTROL_RATE_PROFILE_COUNT];
 115
 116 static const void *cmsx_menuImu_onEnter(displayPort_t *pDisp)
 117 {
 118     UNUSED(pDisp);
 119
 120     for (int i = 0; i < PID_PROFILE_COUNT; i++) {
 121         setProfileIndexString(pidProfileNames[i], i, pidProfiles(i)->profileName);
 122         pidProfileNamePtrs[i] = pidProfileNames[i];
 123     }
 124
 125     pidProfileIndex = getCurrentPidProfileIndex();
 126     tmpPidProfileIndex = pidProfileIndex;
 127
 128     for (int i = 0; i < CONTROL_RATE_PROFILE_COUNT; i++) {
 129         setProfileIndexString(rateProfileNames[i], i, controlRateProfilesMutable(i)->profileName);
 130         rateProfileNamePtrs[i] = rateProfileNames[i];
 131     }
 132
 133     rateProfileIndex = getCurrentControlRateProfileIndex();
 134     tmpRateProfileIndex = rateProfileIndex;
 135
 136     return NULL;
 137 }
 138
 139 static const void *cmsx_menuImu_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 140 {
 141     UNUSED(pDisp);
 142     UNUSED(self);
 143
 144     changePidProfile(pidProfileIndex);
 145     changeControlRateProfile(rateProfileIndex);
 146
 147     return NULL;
 148 }
 149
 150 static const void *cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *ptr)
 151 {
 152     UNUSED(displayPort);
 153     UNUSED(ptr);
 154
 155     pidProfileIndex = tmpPidProfileIndex;
 156     changePidProfile(pidProfileIndex);
 157
 158     return NULL;
 159 }
 160
 161 static const void *cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void *ptr)
 162 {
 163     UNUSED(displayPort);
 164     UNUSED(ptr);
 165
 166     rateProfileIndex = tmpRateProfileIndex;
 167     changeControlRateProfile(rateProfileIndex);
 168
 169     return NULL;
 170 }
 171
 172 static const void *cmsx_PidRead(void)
 173 {
 174
 175     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 176     for (uint8_t i = 0; i < 3; i++) {
 177         tempPid[i][0] = pidProfile->pid[i].P;
 178         tempPid[i][1] = pidProfile->pid[i].I;
 179         tempPid[i][2] = pidProfile->pid[i].D;
 180         tempPidF[i] = pidProfile->pid[i].F;
 181     }
 182
 183     return NULL;
 184 }
 185
 186 static const void *cmsx_PidOnEnter(displayPort_t *pDisp)
 187 {
 188     UNUSED(pDisp);
 189
 190     setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName);
 191     cmsx_PidRead();
 192
 193     return NULL;
 194 }
 195
 196 static const void *cmsx_PidWriteback(displayPort_t *pDisp, const OSD_Entry *self)
 197 {
 198     UNUSED(pDisp);
 199     UNUSED(self);
 200
 201     pidProfile_t *pidProfile = currentPidProfile;
 202     for (uint8_t i = 0; i < 3; i++) {
 203         pidProfile->pid[i].P = tempPid[i][0];
 204         pidProfile->pid[i].I = tempPid[i][1];
 205         pidProfile->pid[i].D = tempPid[i][2];
 206         pidProfile->pid[i].F = tempPidF[i];
 207     }
 208     pidInitConfig(currentPidProfile);
 209
 210     return NULL;
 211 }
 212
 213 static OSD_Entry cmsx_menuPidEntries[] =
 214 {
 215     { "-- PID --", OME_Label, NULL, pidProfileIndexString},
 216
 217     { "ROLL  P", OME_UINT8 | SLIDER_RP, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][0],  0, 200, 1 }},
 218     { "ROLL  I", OME_UINT8 | SLIDER_RP, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][1],  0, 200, 1 }},
 219     { "ROLL  D", OME_UINT8 | SLIDER_RP, NULL, &(OSD_UINT8_t){ &tempPid[PID_ROLL][2],  0, 200, 1 }},
 220     { "ROLL  F", OME_UINT16 | SLIDER_RP, NULL, &(OSD_UINT16_t){ &tempPidF[PID_ROLL],  0, 2000, 1 }},
 221
 222     { "PITCH P", OME_UINT8 | SLIDER_RP, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][0], 0, 200, 1 }},
 223     { "PITCH I", OME_UINT8 | SLIDER_RP, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][1], 0, 200, 1 }},
 224     { "PITCH D", OME_UINT8 | SLIDER_RP, NULL, &(OSD_UINT8_t){ &tempPid[PID_PITCH][2], 0, 200, 1 }},
 225     { "PITCH F", OME_UINT16 | SLIDER_RP, NULL, &(OSD_UINT16_t){ &tempPidF[PID_PITCH], 0, 2000, 1 }},
 226
 227     { "YAW   P", OME_UINT8 | SLIDER_RPY, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][0],   0, 200, 1 }},
 228     { "YAW   I", OME_UINT8 | SLIDER_RPY, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][1],   0, 200, 1 }},
 229     { "YAW   D", OME_UINT8 | SLIDER_RPY, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][2],   0, 200, 1 }},
 230     { "YAW   F", OME_UINT16 | SLIDER_RPY, NULL, &(OSD_UINT16_t){ &tempPidF[PID_YAW],   0, 2000, 1 }},
 231
 232     { "BACK", OME_Back, NULL, NULL },
 233     { NULL, OME_END, NULL, NULL}
 234 };
 235
 236 static CMS_Menu cmsx_menuPid = {
 237 #ifdef CMS_MENU_DEBUG
 238     .GUARD_text = "XPID",
 239     .GUARD_type = OME_MENU,
 240 #endif
 241     .onEnter = cmsx_PidOnEnter,
 242     .onExit = cmsx_PidWriteback,
 243     .onDisplayUpdate = NULL,
 244     .entries = cmsx_menuPidEntries
 245 };
 246
 247 #ifdef USE_SIMPLIFIED_TUNING
 248 static uint8_t cmsx_simplified_pids_mode;
 249 static uint8_t cmsx_simplified_master_multiplier;
 250 static uint8_t cmsx_simplified_roll_pitch_ratio;
 251 static uint8_t cmsx_simplified_i_gain;
 252 static uint8_t cmsx_simplified_d_gain;
 253 static uint8_t cmsx_simplified_pi_gain;
 254 #ifdef USE_D_MAX
 255 static uint8_t cmsx_simplified_d_max_gain;
 256 #endif
 257 static uint8_t cmsx_simplified_feedforward_gain;
 258 static uint8_t cmsx_simplified_pitch_pi_gain;
 259
 260 static uint8_t cmsx_simplified_dterm_filter;
 261 static uint8_t cmsx_simplified_dterm_filter_multiplier;
 262 static uint8_t cmsx_simplified_gyro_filter;
 263 static uint8_t cmsx_simplified_gyro_filter_multiplier;
 264 static uint8_t cmsx_tpa_rate;
 265 static uint16_t cmsx_tpa_breakpoint;
 266 static int8_t cmsx_tpa_low_rate;
 267 static uint16_t cmsx_tpa_low_breakpoint;
 268 static uint8_t cmsx_tpa_low_always;
 269 static uint8_t cmsx_landing_disarm_threshold;
 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_MAX
 284     cmsx_simplified_d_max_gain = pidProfile->simplified_d_max_gain;
 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_MAX
 311         || pidProfile->simplified_d_max_gain != cmsx_simplified_d_max_gain
 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_MAX
 328         pidProfile->simplified_d_max_gain = cmsx_simplified_d_max_gain;
 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_MAX
 356     { "D MAX",             OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_d_max_gain,       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, lookupTableThrottleLimitType} },
 436     { "THR LIM %",   OME_UINT8,  NULL, &(OSD_UINT8_t) { &rateProfile.throttle_limit_percent, 25,  100,  1} },
 437
 438     { "BACK", OME_Back, NULL, NULL },
 439     { NULL, OME_END, NULL, NULL}
 440 };
 441
 442 static CMS_Menu cmsx_menuRateProfile = {
 443 #ifdef CMS_MENU_DEBUG
 444     .GUARD_text = "MENURATE",
 445     .GUARD_type = OME_MENU,
 446 #endif
 447     .onEnter = cmsx_RateProfileOnEnter,
 448     .onExit = cmsx_RateProfileWriteback,
 449     .onDisplayUpdate = NULL,
 450     .entries = cmsx_menuRateProfileEntries
 451 };
 452
 453 #ifdef USE_LAUNCH_CONTROL
 454 static uint8_t cmsx_launchControlMode;
 455 static uint8_t cmsx_launchControlAllowTriggerReset;
 456 static uint8_t cmsx_launchControlThrottlePercent;
 457 static uint8_t cmsx_launchControlAngleLimit;
 458 static uint8_t cmsx_launchControlGain;
 459
 460 static const void *cmsx_launchControlOnEnter(displayPort_t *pDisp)
 461 {
 462     UNUSED(pDisp);
 463
 464     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 465
 466     cmsx_launchControlMode  = pidProfile->launchControlMode;
 467     cmsx_launchControlAllowTriggerReset  = pidProfile->launchControlAllowTriggerReset;
 468     cmsx_launchControlThrottlePercent  = pidProfile->launchControlThrottlePercent;
 469     cmsx_launchControlAngleLimit  = pidProfile->launchControlAngleLimit;
 470     cmsx_launchControlGain  = pidProfile->launchControlGain;
 471
 472     return NULL;
 473 }
 474
 475 static const void *cmsx_launchControlOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 476 {
 477     UNUSED(pDisp);
 478     UNUSED(self);
 479
 480     pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
 481
 482     pidProfile->launchControlMode = cmsx_launchControlMode;
 483     pidProfile->launchControlAllowTriggerReset = cmsx_launchControlAllowTriggerReset;
 484     pidProfile->launchControlThrottlePercent = cmsx_launchControlThrottlePercent;
 485     pidProfile->launchControlAngleLimit = cmsx_launchControlAngleLimit;
 486     pidProfile->launchControlGain = cmsx_launchControlGain;
 487
 488     return NULL;
 489 }
 490
 491 static const OSD_Entry cmsx_menuLaunchControlEntries[] = {
 492     { "-- LAUNCH CONTROL --", OME_Label, NULL, pidProfileIndexString },
 493
 494     { "MODE",             OME_TAB,   NULL, &(OSD_TAB_t)   { &cmsx_launchControlMode, LAUNCH_CONTROL_MODE_COUNT - 1, osdLaunchControlModeNames} },
 495     { "ALLOW RESET",      OME_Bool,  NULL, &cmsx_launchControlAllowTriggerReset },
 496     { "TRIGGER THROTTLE", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlThrottlePercent, 0,  LAUNCH_CONTROL_THROTTLE_TRIGGER_MAX, 1 }  },
 497     { "ANGLE LIMIT",      OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlAngleLimit,      0,  80, 1 }  },
 498     { "ITERM GAIN",       OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlGain,            0, 200, 1 }  },
 499
 500     { "BACK", OME_Back, NULL, NULL },
 501     { NULL, OME_END, NULL, NULL}
 502 };
 503
 504 static CMS_Menu cmsx_menuLaunchControl = {
 505 #ifdef CMS_MENU_DEBUG
 506     .GUARD_text = "LAUNCH",
 507     .GUARD_type = OME_MENU,
 508 #endif
 509     .onEnter = cmsx_launchControlOnEnter,
 510     .onExit = cmsx_launchControlOnExit,
 511     .onDisplayUpdate = NULL,
 512     .entries = cmsx_menuLaunchControlEntries,
 513 };
 514 #endif
 515
 516 static uint8_t  cmsx_angleP;
 517 static uint8_t  cmsx_angleFF;
 518 static uint8_t  cmsx_angleLimit;
 519 static uint8_t  cmsx_angleEarthRef;
 520
 521 static uint8_t  cmsx_horizonStrength;
 522 static uint8_t  cmsx_horizonLimitSticks;
 523 static uint8_t  cmsx_horizonLimitDegrees;
 524
 525 static uint8_t  cmsx_throttleBoost;
 526 static uint8_t  cmsx_thrustLinearization;
 527 static uint8_t  cmsx_antiGravityGain;
 528 static uint8_t  cmsx_motorOutputLimit;
 529 static int8_t   cmsx_autoProfileCellCount;
 530 #ifdef USE_D_MAX
 531 static uint8_t  cmsx_d_max[XYZ_AXIS_COUNT];
 532 static uint8_t  cmsx_d_max_gain;
 533 static uint8_t  cmsx_d_max_advance;
 534 #endif
 535
 536 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
 537 static uint8_t  cmsx_vbat_sag_compensation;
 538 #endif
 539
 540 #ifdef USE_ITERM_RELAX
 541 static uint8_t cmsx_iterm_relax;
 542 static uint8_t cmsx_iterm_relax_type;
 543 static uint8_t cmsx_iterm_relax_cutoff;
 544 #endif
 545
 546 #ifdef USE_FEEDFORWARD
 547 static uint8_t cmsx_feedforward_transition;
 548 static uint8_t cmsx_feedforward_boost;
 549 static uint8_t cmsx_feedforward_averaging;
 550 static uint8_t cmsx_feedforward_smooth_factor;
 551 static uint8_t cmsx_feedforward_jitter_factor;
 552 #endif
 553
 554 static uint8_t cmsx_tpa_rate;
 555 static uint16_t cmsx_tpa_breakpoint;
 556 static int8_t cmsx_tpa_low_rate;
 557 static uint16_t cmsx_tpa_low_breakpoint;
 558 static uint8_t cmsx_tpa_low_always;
 559
 560 static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
 561 {
 562     UNUSED(pDisp);
 563
 564     setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName);
 565
 566     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 567
 568     cmsx_angleP =             pidProfile->pid[PID_LEVEL].P;
 569     cmsx_angleFF =            pidProfile->pid[PID_LEVEL].F;
 570     cmsx_angleLimit =         pidProfile->angle_limit;
 571     cmsx_angleEarthRef =      pidProfile->angle_earth_ref;
 572
 573     cmsx_horizonStrength =    pidProfile->pid[PID_LEVEL].I;
 574     cmsx_horizonLimitSticks = pidProfile->pid[PID_LEVEL].D;
 575     cmsx_horizonLimitDegrees = pidProfile->horizon_limit_degrees;
 576
 577     cmsx_antiGravityGain   = pidProfile->anti_gravity_gain;
 578
 579     cmsx_throttleBoost = pidProfile->throttle_boost;
 580     cmsx_thrustLinearization = pidProfile->thrustLinearization;
 581     cmsx_motorOutputLimit = pidProfile->motor_output_limit;
 582     cmsx_autoProfileCellCount = pidProfile->auto_profile_cell_count;
 583
 584 #ifdef USE_D_MAX
 585     for (unsigned i = 0; i < XYZ_AXIS_COUNT; i++) {
 586         cmsx_d_max[i]  = pidProfile->d_max[i];
 587     }
 588     cmsx_d_max_gain = pidProfile->d_max_gain;
 589     cmsx_d_max_advance = pidProfile->d_max_advance;
 590 #endif
 591
 592 #ifdef USE_ITERM_RELAX
 593     cmsx_iterm_relax = pidProfile->iterm_relax;
 594     cmsx_iterm_relax_type = pidProfile->iterm_relax_type;
 595     cmsx_iterm_relax_cutoff = pidProfile->iterm_relax_cutoff;
 596 #endif
 597
 598 #ifdef USE_FEEDFORWARD
 599     cmsx_feedforward_transition  = pidProfile->feedforward_transition;
 600     cmsx_feedforward_averaging = pidProfile->feedforward_averaging;
 601     cmsx_feedforward_boost = pidProfile->feedforward_boost;
 602     cmsx_feedforward_smooth_factor = pidProfile->feedforward_smooth_factor;
 603     cmsx_feedforward_jitter_factor = pidProfile->feedforward_jitter_factor;
 604 #endif
 605
 606 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
 607     cmsx_vbat_sag_compensation = pidProfile->vbat_sag_compensation;
 608 #endif
 609     cmsx_tpa_rate = pidProfile->tpa_rate;
 610     cmsx_tpa_breakpoint = pidProfile->tpa_breakpoint;
 611     cmsx_tpa_low_rate = pidProfile->tpa_low_rate;
 612     cmsx_tpa_low_breakpoint = pidProfile->tpa_low_breakpoint;
 613     cmsx_tpa_low_always = pidProfile->tpa_low_always;
 614     cmsx_landing_disarm_threshold = pidProfile->landing_disarm_threshold;
 615     return NULL;
 616 }
 617
 618 static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 619 {
 620     UNUSED(pDisp);
 621     UNUSED(self);
 622
 623     pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
 624     pidInitConfig(currentPidProfile);
 625
 626     pidProfile->pid[PID_LEVEL].P = cmsx_angleP;
 627     pidProfile->pid[PID_LEVEL].F = cmsx_angleFF;
 628     pidProfile->angle_limit = cmsx_angleLimit;
 629     pidProfile->angle_earth_ref = cmsx_angleEarthRef;
 630
 631     pidProfile->pid[PID_LEVEL].I = cmsx_horizonStrength;
 632     pidProfile->pid[PID_LEVEL].D = cmsx_horizonLimitSticks;
 633     pidProfile->horizon_limit_degrees = cmsx_horizonLimitDegrees;
 634
 635     pidProfile->anti_gravity_gain   = cmsx_antiGravityGain;
 636
 637     pidProfile->throttle_boost = cmsx_throttleBoost;
 638     pidProfile->thrustLinearization = cmsx_thrustLinearization;
 639     pidProfile->motor_output_limit = cmsx_motorOutputLimit;
 640     pidProfile->auto_profile_cell_count = cmsx_autoProfileCellCount;
 641
 642 #ifdef USE_D_MAX
 643     for (unsigned i = 0; i < XYZ_AXIS_COUNT; i++) {
 644         pidProfile->d_max[i] = cmsx_d_max[i];
 645     }
 646     pidProfile->d_max_gain = cmsx_d_max_gain;
 647     pidProfile->d_max_advance = cmsx_d_max_advance;
 648 #endif
 649
 650 #ifdef USE_ITERM_RELAX
 651     pidProfile->iterm_relax = cmsx_iterm_relax;
 652     pidProfile->iterm_relax_type = cmsx_iterm_relax_type;
 653     pidProfile->iterm_relax_cutoff = cmsx_iterm_relax_cutoff;
 654 #endif
 655
 656 #ifdef USE_FEEDFORWARD
 657     pidProfile->feedforward_transition = cmsx_feedforward_transition;
 658     pidProfile->feedforward_averaging = cmsx_feedforward_averaging;
 659     pidProfile->feedforward_boost = cmsx_feedforward_boost;
 660     pidProfile->feedforward_smooth_factor = cmsx_feedforward_smooth_factor;
 661     pidProfile->feedforward_jitter_factor = cmsx_feedforward_jitter_factor;
 662 #endif
 663
 664 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
 665     pidProfile->vbat_sag_compensation = cmsx_vbat_sag_compensation;
 666 #endif
 667     pidProfile->tpa_rate = cmsx_tpa_rate;
 668     pidProfile->tpa_breakpoint = cmsx_tpa_breakpoint;
 669     pidProfile->tpa_low_rate = cmsx_tpa_low_rate;
 670     pidProfile->tpa_low_breakpoint = cmsx_tpa_low_breakpoint;
 671     pidProfile->tpa_low_always = cmsx_tpa_low_always;
 672     pidProfile->landing_disarm_threshold = cmsx_landing_disarm_threshold;
 673
 674     initEscEndpoints();
 675     return NULL;
 676 }
 677
 678 static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
 679     { "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString },
 680
 681 #ifdef USE_FEEDFORWARD
 682     { "FF TRANSITION", OME_FLOAT,  NULL, &(OSD_FLOAT_t)  { &cmsx_feedforward_transition,        0,    100,   1, 10 } },
 683     { "FF AVERAGING",  OME_TAB,    NULL, &(OSD_TAB_t)    { &cmsx_feedforward_averaging,         4, lookupTableFeedforwardAveraging} },
 684     { "FF SMOOTHNESS", OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_smooth_factor,     0,     95,   1  }    },
 685     { "FF JITTER",     OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_jitter_factor,     0,     20,   1  }    },
 686     { "FF BOOST",      OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_boost,             0,     50,   1  }    },
 687 #endif
 688     { "ANGLE P",         OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_angleP,                     0,    200,   1  }    },
 689     { "ANGLE FF",        OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_angleFF,                    0,    200,   1  }    },
 690     { "ANGLE LIMIT",     OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_angleLimit,                10,     90,   1  }    },
 691     { "ANGLE E_REF",     OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_angleEarthRef,              0,    100,   1  }    },
 692
 693     { "HORZN STR",       OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_horizonStrength,            0,    100,   1  }    },
 694     { "HORZN LIM_STK",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_horizonLimitSticks,        10,    200,   1  }    },
 695     { "HORZN LIM_DEG",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_horizonLimitDegrees,       10,    250,   1  }    },
 696
 697     { "AG GAIN",     OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &cmsx_antiGravityGain,   ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX, 1, 100 }    },
 698 #ifdef USE_THROTTLE_BOOST
 699     { "THR BOOST",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_throttleBoost,          0,    100,   1  }    },
 700 #endif
 701 #ifdef USE_THRUST_LINEARIZATION
 702     { "THR LINEAR",  OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_thrustLinearization,    0,    150,   1  }    },
 703 #endif
 704 #ifdef USE_ITERM_RELAX
 705     { "I_RELAX",         OME_TAB,    NULL, &(OSD_TAB_t)     { &cmsx_iterm_relax,        ITERM_RELAX_COUNT - 1,      lookupTableItermRelax       } },
 706     { "I_RELAX TYPE",    OME_TAB,    NULL, &(OSD_TAB_t)     { &cmsx_iterm_relax_type,   ITERM_RELAX_TYPE_COUNT - 1, lookupTableItermRelaxType   } },
 707     { "I_RELAX CUTOFF",  OME_UINT8,  NULL, &(OSD_UINT8_t)   { &cmsx_iterm_relax_cutoff, 1, 50, 1 } },
 708 #endif
 709 #ifdef USE_LAUNCH_CONTROL
 710     {"LAUNCH CONTROL", OME_Submenu, cmsMenuChange, &cmsx_menuLaunchControl },
 711 #endif
 712     { "MTR OUT LIM %",OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_motorOutputLimit, MOTOR_OUTPUT_LIMIT_PERCENT_MIN,  MOTOR_OUTPUT_LIMIT_PERCENT_MAX,  1} },
 713
 714     { "AUTO CELL CNT", OME_INT8, NULL, &(OSD_INT8_t) { &cmsx_autoProfileCellCount, AUTO_PROFILE_CELL_COUNT_CHANGE, MAX_AUTO_DETECT_CELL_COUNT, 1} },
 715
 716 #ifdef USE_D_MAX
 717     { "D_MAX ROLL",  OME_UINT8 | SLIDER_RP,  NULL, &(OSD_UINT8_t) { &cmsx_d_max[FD_ROLL],      0, 100, 1 } },
 718     { "D_MAX PITCH", OME_UINT8 | SLIDER_RP,  NULL, &(OSD_UINT8_t) { &cmsx_d_max[FD_PITCH],     0, 100, 1 } },
 719     { "D_MAX YAW",   OME_UINT8 | SLIDER_RPY,  NULL, &(OSD_UINT8_t) { &cmsx_d_max[FD_YAW],       0, 100, 1 } },
 720     { "D_MAX GAIN",  OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_max_gain,          0, 100, 1 } },
 721     { "D_MAX ADV",   OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_d_max_advance,       0, 200, 1 } },
 722 #endif
 723
 724 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
 725     { "VBAT_SAG_COMP", OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_vbat_sag_compensation, 0, 150, 1 } },
 726 #endif
 727
 728     { "TPA RATE",      OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &cmsx_tpa_rate, 0, 100, 1, 10} },
 729     { "TPA BRKPT",     OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_tpa_breakpoint, 1000, 2000, 10} },
 730     { "TPA LOW RATE",  OME_INT8,   NULL, &(OSD_INT8_t) { &cmsx_tpa_low_rate, TPA_LOW_RATE_MIN, TPA_MAX , 1} },
 731     { "TPA LOW BRKPT", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_tpa_low_breakpoint, 1000, 2000, 10} },
 732     { "TPA LOW ALWYS", OME_Bool,   NULL, &cmsx_tpa_low_always },
 733     { "EZDISARM THR",  OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_landing_disarm_threshold, 0, 150, 1} },
 734
 735     { "BACK", OME_Back, NULL, NULL },
 736     { NULL, OME_END, NULL, NULL}
 737 };
 738
 739 static CMS_Menu cmsx_menuProfileOther = {
 740 #ifdef CMS_MENU_DEBUG
 741     .GUARD_text = "XPROFOTHER",
 742     .GUARD_type = OME_MENU,
 743 #endif
 744     .onEnter = cmsx_profileOtherOnEnter,
 745     .onExit = cmsx_profileOtherOnExit,
 746     .onDisplayUpdate = NULL,
 747     .entries = cmsx_menuProfileOtherEntries,
 748 };
 749
 750 static uint16_t gyroConfig_gyro_lpf1_static_hz;
 751 static uint16_t gyroConfig_gyro_lpf2_static_hz;
 752 static uint16_t gyroConfig_gyro_soft_notch_hz_1;
 753 static uint16_t gyroConfig_gyro_soft_notch_cutoff_1;
 754 static uint16_t gyroConfig_gyro_soft_notch_hz_2;
 755 static uint16_t gyroConfig_gyro_soft_notch_cutoff_2;
 756 static uint8_t  gyroConfig_gyro_to_use;
 757
 758 static const void *cmsx_menuGyro_onEnter(displayPort_t *pDisp)
 759 {
 760     UNUSED(pDisp);
 761
 762     gyroConfig_gyro_lpf1_static_hz =  gyroConfig()->gyro_lpf1_static_hz;
 763     gyroConfig_gyro_lpf2_static_hz =  gyroConfig()->gyro_lpf2_static_hz;
 764     gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
 765     gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
 766     gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
 767     gyroConfig_gyro_soft_notch_cutoff_2 = gyroConfig()->gyro_soft_notch_cutoff_2;
 768     gyroConfig_gyro_to_use = gyroConfig()->gyro_to_use;
 769
 770     return NULL;
 771 }
 772
 773 static const void *cmsx_menuGyro_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 774 {
 775     UNUSED(pDisp);
 776     UNUSED(self);
 777
 778     gyroConfigMutable()->gyro_lpf1_static_hz =  gyroConfig_gyro_lpf1_static_hz;
 779     gyroConfigMutable()->gyro_lpf2_static_hz =  gyroConfig_gyro_lpf2_static_hz;
 780     gyroConfigMutable()->gyro_soft_notch_hz_1 = gyroConfig_gyro_soft_notch_hz_1;
 781     gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
 782     gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
 783     gyroConfigMutable()->gyro_soft_notch_cutoff_2 = gyroConfig_gyro_soft_notch_cutoff_2;
 784     gyroConfigMutable()->gyro_to_use = gyroConfig_gyro_to_use;
 785
 786     return NULL;
 787 }
 788
 789 static const OSD_Entry cmsx_menuFilterGlobalEntries[] =
 790 {
 791     { "-- FILTER GLB  --", OME_Label, NULL, NULL },
 792
 793     { "GYRO LPF1",  OME_UINT16 | SLIDER_GYRO, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lpf1_static_hz, 0, LPF_MAX_HZ, 1 } },
 794 #ifdef USE_GYRO_LPF2
 795     { "GYRO LPF2",  OME_UINT16 | SLIDER_GYRO, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lpf2_static_hz,  0, LPF_MAX_HZ, 1 } },
 796 #endif
 797     { "GYRO NF1",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_1,     0, 500, 1 } },
 798     { "GYRO NF1C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 } },
 799     { "GYRO NF2",   OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2,     0, 500, 1 } },
 800     { "GYRO NF2C",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 } },
 801 #ifdef USE_MULTI_GYRO
 802     { "GYRO TO USE",  OME_TAB | REBOOT_REQUIRED,  NULL, &(OSD_TAB_t)    { &gyroConfig_gyro_to_use,  2, osdTableGyroToUse} },
 803 #endif
 804
 805     { "BACK", OME_Back, NULL, NULL },
 806     { NULL, OME_END, NULL, NULL}
 807 };
 808
 809 static CMS_Menu cmsx_menuFilterGlobal = {
 810 #ifdef CMS_MENU_DEBUG
 811     .GUARD_text = "XFLTGLB",
 812     .GUARD_type = OME_MENU,
 813 #endif
 814     .onEnter = cmsx_menuGyro_onEnter,
 815     .onExit = cmsx_menuGyro_onExit,
 816     .onDisplayUpdate = NULL,
 817     .entries = cmsx_menuFilterGlobalEntries,
 818 };
 819
 820 #if (defined(USE_DYN_NOTCH_FILTER) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
 821
 822 #ifdef USE_DYN_NOTCH_FILTER
 823 static uint16_t dynFiltNotchMaxHz;
 824 static uint8_t  dynFiltNotchCount;
 825 static uint16_t dynFiltNotchQ;
 826 static uint16_t dynFiltNotchMinHz;
 827 #endif
 828 #ifdef USE_DYN_LPF
 829 static uint16_t gyroLpfDynMin;
 830 static uint16_t gyroLpfDynMax;
 831 static uint8_t gyroLpfDynExpo;
 832 static uint16_t dtermLpfDynMin;
 833 static uint16_t dtermLpfDynMax;
 834 static uint8_t dtermLpfDynExpo;
 835 #endif
 836
 837 static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
 838 {
 839     UNUSED(pDisp);
 840
 841 #ifdef USE_DYN_NOTCH_FILTER
 842     dynFiltNotchMaxHz   = dynNotchConfig()->dyn_notch_max_hz;
 843     dynFiltNotchCount   = dynNotchConfig()->dyn_notch_count;
 844     dynFiltNotchQ       = dynNotchConfig()->dyn_notch_q;
 845     dynFiltNotchMinHz   = dynNotchConfig()->dyn_notch_min_hz;
 846 #endif
 847 #ifdef USE_DYN_LPF
 848     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 849     gyroLpfDynMin   = gyroConfig()->gyro_lpf1_dyn_min_hz;
 850     gyroLpfDynMax   = gyroConfig()->gyro_lpf1_dyn_max_hz;
 851     gyroLpfDynExpo  = gyroConfig()->gyro_lpf1_dyn_expo;
 852     dtermLpfDynMin  = pidProfile->dterm_lpf1_dyn_min_hz;
 853     dtermLpfDynMax  = pidProfile->dterm_lpf1_dyn_max_hz;
 854     dtermLpfDynExpo = pidProfile->dterm_lpf1_dyn_expo;
 855 #endif
 856
 857     return NULL;
 858 }
 859
 860 static const void *cmsx_menuDynFilt_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 861 {
 862     UNUSED(pDisp);
 863     UNUSED(self);
 864
 865 #ifdef USE_DYN_NOTCH_FILTER
 866     dynNotchConfigMutable()->dyn_notch_max_hz        = dynFiltNotchMaxHz;
 867     dynNotchConfigMutable()->dyn_notch_count         = dynFiltNotchCount;
 868     dynNotchConfigMutable()->dyn_notch_q             = dynFiltNotchQ;
 869     dynNotchConfigMutable()->dyn_notch_min_hz        = dynFiltNotchMinHz;
 870 #endif
 871 #ifdef USE_DYN_LPF
 872     pidProfile_t *pidProfile = currentPidProfile;
 873     gyroConfigMutable()->gyro_lpf1_dyn_min_hz = gyroLpfDynMin;
 874     gyroConfigMutable()->gyro_lpf1_dyn_max_hz = gyroLpfDynMax;
 875     gyroConfigMutable()->gyro_lpf1_dyn_expo   = gyroLpfDynExpo;
 876     pidProfile->dterm_lpf1_dyn_min_hz         = dtermLpfDynMin;
 877     pidProfile->dterm_lpf1_dyn_max_hz         = dtermLpfDynMax;
 878     pidProfile->dterm_lpf1_dyn_expo           = dtermLpfDynExpo;
 879 #endif
 880
 881     return NULL;
 882 }
 883
 884 static const OSD_Entry cmsx_menuDynFiltEntries[] =
 885 {
 886     { "-- DYN FILT --", OME_Label, NULL, NULL },
 887
 888 #ifdef USE_DYN_NOTCH_FILTER
 889     { "NOTCH COUNT",    OME_UINT8,  NULL, &(OSD_UINT8_t)  { &dynFiltNotchCount,   0, DYN_NOTCH_COUNT_MAX, 1 } },
 890     { "NOTCH Q",        OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ,       1, 1000, 1 } },
 891     { "NOTCH MIN HZ",   OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz,   20, 250, 1 } },
 892     { "NOTCH MAX HZ",   OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMaxHz,   200, 1000, 1 } },
 893 #endif
 894
 895 #ifdef USE_DYN_LPF
 896     { "GYRO DLPF MIN",   OME_UINT16 | SLIDER_GYRO, NULL, &(OSD_UINT16_t) { &gyroLpfDynMin,  0, 1000, 1 } },
 897     { "GYRO DLPF MAX",   OME_UINT16 | SLIDER_GYRO, NULL, &(OSD_UINT16_t) { &gyroLpfDynMax,  0, 1000, 1 } },
 898     { "GYRO DLPF EXPO",  OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroLpfDynExpo,   0, 10, 1 } },
 899     { "DTERM DLPF MIN",  OME_UINT16 | SLIDER_DTERM, NULL, &(OSD_UINT16_t) { &dtermLpfDynMin, 0, 1000, 1 } },
 900     { "DTERM DLPF MAX",  OME_UINT16 | SLIDER_DTERM, NULL, &(OSD_UINT16_t) { &dtermLpfDynMax, 0, 1000, 1 } },
 901     { "DTERM DLPF EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &dtermLpfDynExpo,  0, 10, 1 } },
 902 #endif
 903
 904     { "BACK", OME_Back, NULL, NULL },
 905     { NULL, OME_END, NULL, NULL}
 906 };
 907
 908 static CMS_Menu cmsx_menuDynFilt = {
 909 #ifdef CMS_MENU_DEBUG
 910     .GUARD_text = "XDYNFLT",
 911     .GUARD_type = OME_MENU,
 912 #endif
 913     .onEnter = cmsx_menuDynFilt_onEnter,
 914     .onExit = cmsx_menuDynFilt_onExit,
 915     .onDisplayUpdate = NULL,
 916     .entries = cmsx_menuDynFiltEntries,
 917 };
 918
 919 #endif
 920
 921 static uint16_t cmsx_dterm_lpf1_static_hz;
 922 static uint16_t cmsx_dterm_lpf2_static_hz;
 923 static uint16_t cmsx_dterm_notch_hz;
 924 static uint16_t cmsx_dterm_notch_cutoff;
 925 static uint16_t cmsx_yaw_lowpass_hz;
 926
 927 static const void *cmsx_FilterPerProfileRead(displayPort_t *pDisp)
 928 {
 929     UNUSED(pDisp);
 930
 931     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 932
 933     cmsx_dterm_lpf1_static_hz   = pidProfile->dterm_lpf1_static_hz;
 934     cmsx_dterm_lpf2_static_hz   = pidProfile->dterm_lpf2_static_hz;
 935     cmsx_dterm_notch_hz         = pidProfile->dterm_notch_hz;
 936     cmsx_dterm_notch_cutoff     = pidProfile->dterm_notch_cutoff;
 937     cmsx_yaw_lowpass_hz         = pidProfile->yaw_lowpass_hz;
 938
 939     return NULL;
 940 }
 941
 942 static const void *cmsx_FilterPerProfileWriteback(displayPort_t *pDisp, const OSD_Entry *self)
 943 {
 944     UNUSED(pDisp);
 945     UNUSED(self);
 946
 947     pidProfile_t *pidProfile = currentPidProfile;
 948
 949     pidProfile->dterm_lpf1_static_hz  = cmsx_dterm_lpf1_static_hz;
 950     pidProfile->dterm_lpf2_static_hz  = cmsx_dterm_lpf2_static_hz;
 951     pidProfile->dterm_notch_hz        = cmsx_dterm_notch_hz;
 952     pidProfile->dterm_notch_cutoff    = cmsx_dterm_notch_cutoff;
 953     pidProfile->yaw_lowpass_hz        = cmsx_yaw_lowpass_hz;
 954
 955     return NULL;
 956 }
 957
 958 static const OSD_Entry cmsx_menuFilterPerProfileEntries[] =
 959 {
 960     { "-- FILTER PP  --", OME_Label, NULL, NULL },
 961
 962     { "DTERM LPF1", OME_UINT16 | SLIDER_DTERM, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lpf1_static_hz, 0, LPF_MAX_HZ, 1 } },
 963     { "DTERM LPF2", OME_UINT16 | SLIDER_DTERM, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lpf2_static_hz, 0, LPF_MAX_HZ, 1 } },
 964     { "DTERM NF",   OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz,       0, LPF_MAX_HZ, 1 } },
 965     { "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff,   0, LPF_MAX_HZ, 1 } },
 966     { "YAW LPF",    OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lowpass_hz,       0, 500, 1 } },
 967
 968     { "BACK", OME_Back, NULL, NULL },
 969     { NULL, OME_END, NULL, NULL}
 970 };
 971
 972 static CMS_Menu cmsx_menuFilterPerProfile = {
 973 #ifdef CMS_MENU_DEBUG
 974     .GUARD_text = "XFLTPP",
 975     .GUARD_type = OME_MENU,
 976 #endif
 977     .onEnter = cmsx_FilterPerProfileRead,
 978     .onExit = cmsx_FilterPerProfileWriteback,
 979     .onDisplayUpdate = NULL,
 980     .entries = cmsx_menuFilterPerProfileEntries,
 981 };
 982
 983 #ifdef USE_EXTENDED_CMS_MENUS
 984
 985 static uint8_t cmsx_dstPidProfile;
 986 static uint8_t cmsx_dstControlRateProfile;
 987
 988 static const char * const cmsx_ProfileNames[] = {
 989     "-",
 990     "1",
 991     "2",
 992     "3"
 993 };
 994
 995 static OSD_TAB_t cmsx_PidProfileTable = { &cmsx_dstPidProfile, 3, cmsx_ProfileNames };
 996 static OSD_TAB_t cmsx_ControlRateProfileTable = { &cmsx_dstControlRateProfile, 3, cmsx_ProfileNames };
 997
 998 static const void *cmsx_menuCopyProfile_onEnter(displayPort_t *pDisp)
 999 {
1000     UNUSED(pDisp);
1001
1002     cmsx_dstPidProfile = 0;
1003     cmsx_dstControlRateProfile = 0;
1004
1005     return NULL;
1006 }
1007
1008 static const void *cmsx_CopyPidProfile(displayPort_t *pDisplay, const void *ptr)
1009 {
1010     UNUSED(pDisplay);
1011     UNUSED(ptr);
1012
1013     if (cmsx_dstPidProfile > 0) {
1014         pidCopyProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex());
1015     }
1016
1017     return NULL;
1018 }
1019
1020 static const void *cmsx_CopyControlRateProfile(displayPort_t *pDisplay, const void *ptr)
1021 {
1022     UNUSED(pDisplay);
1023     UNUSED(ptr);
1024
1025     if (cmsx_dstControlRateProfile > 0) {
1026         copyControlRateProfile(cmsx_dstControlRateProfile - 1, getCurrentControlRateProfileIndex());
1027     }
1028
1029     return NULL;
1030 }
1031
1032 static const OSD_Entry cmsx_menuCopyProfileEntries[] =
1033 {
1034     { "-- COPY PROFILE --", OME_Label, NULL, NULL},
1035
1036     { "CPY PID PROF TO",   OME_TAB,      NULL,                        &cmsx_PidProfileTable },
1037     { "COPY PP",           OME_Funcall,  cmsx_CopyPidProfile,         NULL },
1038     { "CPY RATE PROF TO",  OME_TAB,      NULL,                        &cmsx_ControlRateProfileTable },
1039     { "COPY RP",           OME_Funcall,  cmsx_CopyControlRateProfile, NULL },
1040
1041     { "BACK", OME_Back, NULL, NULL },
1042     { NULL, OME_END, NULL, NULL}
1043 };
1044
1045 CMS_Menu cmsx_menuCopyProfile = {
1046 #ifdef CMS_MENU_DEBUG
1047     .GUARD_text = "XCPY",
1048     .GUARD_type = OME_MENU,
1049 #endif
1050     .onEnter = cmsx_menuCopyProfile_onEnter,
1051     .onExit = NULL,
1052     .onDisplayUpdate = NULL,
1053     .entries = cmsx_menuCopyProfileEntries,
1054 };
1055
1056 #endif
1057
1058 static const OSD_Entry cmsx_menuImuEntries[] =
1059 {
1060     { "-- PROFILE --", OME_Label, NULL, NULL},
1061
1062     {"PID PROF",  OME_TAB,     cmsx_profileIndexOnChange,     &(OSD_TAB_t){&tmpPidProfileIndex, PID_PROFILE_COUNT-1, pidProfileNamePtrs}},
1063     {"PID",       OME_Submenu, cmsMenuChange,                 &cmsx_menuPid},
1064 #ifdef USE_SIMPLIFIED_TUNING
1065     {"SIMPLIFIED TUNING",   OME_Submenu, cmsMenuChange,                 &cmsx_menuSimplifiedTuning},
1066 #endif
1067     {"MISC PP",   OME_Submenu, cmsMenuChange,                 &cmsx_menuProfileOther},
1068     {"FILT PP",   OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterPerProfile},
1069
1070     {"RATE PROF", OME_TAB,   cmsx_rateProfileIndexOnChange, &(OSD_TAB_t){&tmpRateProfileIndex, CONTROL_RATE_PROFILE_COUNT-1, rateProfileNamePtrs}},
1071     {"RATE",      OME_Submenu, cmsMenuChange,                 &cmsx_menuRateProfile},
1072
1073     {"FILT GLB",  OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterGlobal},
1074 #if  (defined(USE_DYN_NOTCH_FILTER) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
1075     {"DYN FILT",  OME_Submenu, cmsMenuChange,                 &cmsx_menuDynFilt},
1076 #endif
1077
1078 #ifdef USE_EXTENDED_CMS_MENUS
1079     {"COPY PROF", OME_Submenu, cmsMenuChange,                 &cmsx_menuCopyProfile},
1080 #endif /* USE_EXTENDED_CMS_MENUS */
1081
1082     {"BACK", OME_Back, NULL, NULL},
1083     {NULL, OME_END, NULL, NULL}
1084 };
1085
1086 CMS_Menu cmsx_menuImu = {
1087 #ifdef CMS_MENU_DEBUG
1088     .GUARD_text = "XIMU",
1089     .GUARD_type = OME_MENU,
1090 #endif
1091     .onEnter = cmsx_menuImu_onEnter,
1092     .onExit = cmsx_menuImu_onExit,
1093     .onDisplayUpdate = NULL,
1094     .entries = cmsx_menuImuEntries,
1095 };
1096
1097 #endif // CMS