src/main/cms/cms_menu_imu.c

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