2 * This file is part of Cleanflight and Betaflight.
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)
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.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
21 // Menu contents for PID, RATES, RC preview, misc
22 // Should be part of the relevant .c file.
33 #include "build/version.h"
34 #include "build/build_config.h"
37 #include "cms/cms_types.h"
38 #include "cms/cms_menu_imu.h"
40 #include "common/utils.h"
42 #include "config/feature.h"
44 #include "drivers/pwm_output.h"
46 #include "config/config.h"
47 #include "fc/controlrate_profile.h"
49 #include "fc/rc_controls.h"
50 #include "fc/runtime_config.h"
52 #include "flight/mixer.h"
53 #include "flight/pid.h"
57 #include "sensors/battery.h"
58 #include "sensors/gyro.h"
60 #include "cli/settings.h"
65 static uint8_t tmpPidProfileIndex
;
66 static uint8_t pidProfileIndex
;
67 static char pidProfileIndexString
[MAX_PROFILE_NAME_LENGTH
+ 5];
68 static uint8_t tempPid
[3][3];
69 static uint16_t tempPidF
[3];
71 static uint8_t tmpRateProfileIndex
;
72 static uint8_t rateProfileIndex
;
73 static char rateProfileIndexString
[MAX_RATE_PROFILE_NAME_LENGTH
+ 5];
74 static controlRateConfig_t rateProfile
;
76 static const char * const osdTableThrottleLimitType
[] = {
77 "OFF", "SCALE", "CLIP"
81 static const char * const osdTableGyroToUse
[] = {
82 "FIRST", "SECOND", "BOTH"
86 static void setProfileIndexString(char *profileString
, int profileIndex
, char *profileName
)
89 profileString
[charIndex
++] = '1' + profileIndex
;
91 #ifdef USE_PROFILE_NAMES
92 const int profileNameLen
= strlen(profileName
);
94 if (profileNameLen
> 0) {
95 profileString
[charIndex
++] = ' ';
96 profileString
[charIndex
++] = '(';
97 for (int i
= 0; i
< profileNameLen
; i
++) {
98 profileString
[charIndex
++] = toupper(profileName
[i
]);
100 profileString
[charIndex
++] = ')';
106 profileString
[charIndex
] = '\0';
109 static const void *cmsx_menuImu_onEnter(displayPort_t
*pDisp
)
113 pidProfileIndex
= getCurrentPidProfileIndex();
114 tmpPidProfileIndex
= pidProfileIndex
+ 1;
116 rateProfileIndex
= getCurrentControlRateProfileIndex();
117 tmpRateProfileIndex
= rateProfileIndex
+ 1;
122 static const void *cmsx_menuImu_onExit(displayPort_t
*pDisp
, const OSD_Entry
*self
)
127 changePidProfile(pidProfileIndex
);
128 changeControlRateProfile(rateProfileIndex
);
133 static const void *cmsx_profileIndexOnChange(displayPort_t
*displayPort
, const void *ptr
)
138 pidProfileIndex
= tmpPidProfileIndex
- 1;
139 changePidProfile(pidProfileIndex
);
144 static const void *cmsx_rateProfileIndexOnChange(displayPort_t
*displayPort
, const void *ptr
)
149 rateProfileIndex
= tmpRateProfileIndex
- 1;
150 changeControlRateProfile(rateProfileIndex
);
155 static const void *cmsx_PidRead(void)
158 const pidProfile_t
*pidProfile
= pidProfiles(pidProfileIndex
);
159 for (uint8_t i
= 0; i
< 3; i
++) {
160 tempPid
[i
][0] = pidProfile
->pid
[i
].P
;
161 tempPid
[i
][1] = pidProfile
->pid
[i
].I
;
162 tempPid
[i
][2] = pidProfile
->pid
[i
].D
;
163 tempPidF
[i
] = pidProfile
->pid
[i
].F
;
169 static const void *cmsx_PidOnEnter(displayPort_t
*pDisp
)
173 setProfileIndexString(pidProfileIndexString
, pidProfileIndex
, currentPidProfile
->profileName
);
179 static const void *cmsx_PidWriteback(displayPort_t
*pDisp
, const OSD_Entry
*self
)
184 pidProfile_t
*pidProfile
= currentPidProfile
;
185 for (uint8_t i
= 0; i
< 3; i
++) {
186 pidProfile
->pid
[i
].P
= tempPid
[i
][0];
187 pidProfile
->pid
[i
].I
= tempPid
[i
][1];
188 pidProfile
->pid
[i
].D
= tempPid
[i
][2];
189 pidProfile
->pid
[i
].F
= tempPidF
[i
];
191 pidInitConfig(currentPidProfile
);
196 static const OSD_Entry cmsx_menuPidEntries
[] =
198 { "-- PID --", OME_Label
, NULL
, pidProfileIndexString
, 0},
200 { "ROLL P", OME_UINT8
, NULL
, &(OSD_UINT8_t
){ &tempPid
[PID_ROLL
][0], 0, 200, 1 }, 0 },
201 { "ROLL I", OME_UINT8
, NULL
, &(OSD_UINT8_t
){ &tempPid
[PID_ROLL
][1], 0, 200, 1 }, 0 },
202 { "ROLL D", OME_UINT8
, NULL
, &(OSD_UINT8_t
){ &tempPid
[PID_ROLL
][2], 0, 200, 1 }, 0 },
203 { "ROLL F", OME_UINT16
, NULL
, &(OSD_UINT16_t
){ &tempPidF
[PID_ROLL
], 0, 2000, 1 }, 0 },
205 { "PITCH P", OME_UINT8
, NULL
, &(OSD_UINT8_t
){ &tempPid
[PID_PITCH
][0], 0, 200, 1 }, 0 },
206 { "PITCH I", OME_UINT8
, NULL
, &(OSD_UINT8_t
){ &tempPid
[PID_PITCH
][1], 0, 200, 1 }, 0 },
207 { "PITCH D", OME_UINT8
, NULL
, &(OSD_UINT8_t
){ &tempPid
[PID_PITCH
][2], 0, 200, 1 }, 0 },
208 { "PITCH F", OME_UINT16
, NULL
, &(OSD_UINT16_t
){ &tempPidF
[PID_PITCH
], 0, 2000, 1 }, 0 },
210 { "YAW P", OME_UINT8
, NULL
, &(OSD_UINT8_t
){ &tempPid
[PID_YAW
][0], 0, 200, 1 }, 0 },
211 { "YAW I", OME_UINT8
, NULL
, &(OSD_UINT8_t
){ &tempPid
[PID_YAW
][1], 0, 200, 1 }, 0 },
212 { "YAW D", OME_UINT8
, NULL
, &(OSD_UINT8_t
){ &tempPid
[PID_YAW
][2], 0, 200, 1 }, 0 },
213 { "YAW F", OME_UINT16
, NULL
, &(OSD_UINT16_t
){ &tempPidF
[PID_YAW
], 0, 2000, 1 }, 0 },
215 { "BACK", OME_Back
, NULL
, NULL
, 0 },
216 { NULL
, OME_END
, NULL
, NULL
, 0 }
219 static CMS_Menu cmsx_menuPid
= {
220 #ifdef CMS_MENU_DEBUG
221 .GUARD_text
= "XPID",
222 .GUARD_type
= OME_MENU
,
224 .onEnter
= cmsx_PidOnEnter
,
225 .onExit
= cmsx_PidWriteback
,
226 .onDisplayUpdate
= NULL
,
227 .entries
= cmsx_menuPidEntries
234 static const void *cmsx_RateProfileRead(void)
236 memcpy(&rateProfile
, controlRateProfiles(rateProfileIndex
), sizeof(controlRateConfig_t
));
241 static const void *cmsx_RateProfileWriteback(displayPort_t
*pDisp
, const OSD_Entry
*self
)
246 memcpy(controlRateProfilesMutable(rateProfileIndex
), &rateProfile
, sizeof(controlRateConfig_t
));
251 static const void *cmsx_RateProfileOnEnter(displayPort_t
*pDisp
)
255 setProfileIndexString(rateProfileIndexString
, rateProfileIndex
, controlRateProfilesMutable(rateProfileIndex
)->profileName
);
256 cmsx_RateProfileRead();
261 static const OSD_Entry cmsx_menuRateProfileEntries
[] =
263 { "-- RATE --", OME_Label
, NULL
, rateProfileIndexString
, 0 },
265 { "RC R RATE", OME_FLOAT
, NULL
, &(OSD_FLOAT_t
) { &rateProfile
.rcRates
[FD_ROLL
], 1, CONTROL_RATE_CONFIG_RC_RATES_MAX
, 1, 10 }, 0 },
266 { "RC P RATE", OME_FLOAT
, NULL
, &(OSD_FLOAT_t
) { &rateProfile
.rcRates
[FD_PITCH
], 1, CONTROL_RATE_CONFIG_RC_RATES_MAX
, 1, 10 }, 0 },
267 { "RC Y RATE", OME_FLOAT
, NULL
, &(OSD_FLOAT_t
) { &rateProfile
.rcRates
[FD_YAW
], 1, CONTROL_RATE_CONFIG_RC_RATES_MAX
, 1, 10 }, 0 },
269 { "ROLL SUPER", OME_FLOAT
, NULL
, &(OSD_FLOAT_t
) { &rateProfile
.rates
[FD_ROLL
], 0, CONTROL_RATE_CONFIG_RATE_MAX
, 1, 10 }, 0 },
270 { "PITCH SUPER", OME_FLOAT
, NULL
, &(OSD_FLOAT_t
) { &rateProfile
.rates
[FD_PITCH
], 0, CONTROL_RATE_CONFIG_RATE_MAX
, 1, 10 }, 0 },
271 { "YAW SUPER", OME_FLOAT
, NULL
, &(OSD_FLOAT_t
) { &rateProfile
.rates
[FD_YAW
], 0, CONTROL_RATE_CONFIG_RATE_MAX
, 1, 10 }, 0 },
273 { "RC R EXPO", OME_FLOAT
, NULL
, &(OSD_FLOAT_t
) { &rateProfile
.rcExpo
[FD_ROLL
], 0, 100, 1, 10 }, 0 },
274 { "RC P EXPO", OME_FLOAT
, NULL
, &(OSD_FLOAT_t
) { &rateProfile
.rcExpo
[FD_PITCH
], 0, 100, 1, 10 }, 0 },
275 { "RC Y EXPO", OME_FLOAT
, NULL
, &(OSD_FLOAT_t
) { &rateProfile
.rcExpo
[FD_YAW
], 0, 100, 1, 10 }, 0 },
277 { "THR MID", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &rateProfile
.thrMid8
, 0, 100, 1}, 0 },
278 { "THR EXPO", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &rateProfile
.thrExpo8
, 0, 100, 1}, 0 },
279 { "THRPID ATT", OME_FLOAT
, NULL
, &(OSD_FLOAT_t
) { &rateProfile
.dynThrPID
, 0, 100, 1, 10}, 0 },
280 { "TPA BRKPT", OME_UINT16
, NULL
, &(OSD_UINT16_t
){ &rateProfile
.tpa_breakpoint
, 1000, 2000, 10}, 0 },
282 { "THR LIM TYPE",OME_TAB
, NULL
, &(OSD_TAB_t
) { &rateProfile
.throttle_limit_type
, THROTTLE_LIMIT_TYPE_COUNT
- 1, osdTableThrottleLimitType
}, 0 },
283 { "THR LIM %", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &rateProfile
.throttle_limit_percent
, 25, 100, 1}, 0 },
285 { "BACK", OME_Back
, NULL
, NULL
, 0 },
286 { NULL
, OME_END
, NULL
, NULL
, 0 }
289 static CMS_Menu cmsx_menuRateProfile
= {
290 #ifdef CMS_MENU_DEBUG
291 .GUARD_text
= "MENURATE",
292 .GUARD_type
= OME_MENU
,
294 .onEnter
= cmsx_RateProfileOnEnter
,
295 .onExit
= cmsx_RateProfileWriteback
,
296 .onDisplayUpdate
= NULL
,
297 .entries
= cmsx_menuRateProfileEntries
300 #ifdef USE_LAUNCH_CONTROL
301 static uint8_t cmsx_launchControlMode
;
302 static uint8_t cmsx_launchControlAllowTriggerReset
;
303 static uint8_t cmsx_launchControlThrottlePercent
;
304 static uint8_t cmsx_launchControlAngleLimit
;
305 static uint8_t cmsx_launchControlGain
;
307 static const void *cmsx_launchControlOnEnter(displayPort_t
*pDisp
)
311 const pidProfile_t
*pidProfile
= pidProfiles(pidProfileIndex
);
313 cmsx_launchControlMode
= pidProfile
->launchControlMode
;
314 cmsx_launchControlAllowTriggerReset
= pidProfile
->launchControlAllowTriggerReset
;
315 cmsx_launchControlThrottlePercent
= pidProfile
->launchControlThrottlePercent
;
316 cmsx_launchControlAngleLimit
= pidProfile
->launchControlAngleLimit
;
317 cmsx_launchControlGain
= pidProfile
->launchControlGain
;
322 static const void *cmsx_launchControlOnExit(displayPort_t
*pDisp
, const OSD_Entry
*self
)
327 pidProfile_t
*pidProfile
= pidProfilesMutable(pidProfileIndex
);
329 pidProfile
->launchControlMode
= cmsx_launchControlMode
;
330 pidProfile
->launchControlAllowTriggerReset
= cmsx_launchControlAllowTriggerReset
;
331 pidProfile
->launchControlThrottlePercent
= cmsx_launchControlThrottlePercent
;
332 pidProfile
->launchControlAngleLimit
= cmsx_launchControlAngleLimit
;
333 pidProfile
->launchControlGain
= cmsx_launchControlGain
;
338 static const OSD_Entry cmsx_menuLaunchControlEntries
[] = {
339 { "-- LAUNCH CONTROL --", OME_Label
, NULL
, pidProfileIndexString
, 0 },
341 { "MODE", OME_TAB
, NULL
, &(OSD_TAB_t
) { &cmsx_launchControlMode
, LAUNCH_CONTROL_MODE_COUNT
- 1, osdLaunchControlModeNames
}, 0 },
342 { "ALLOW RESET", OME_Bool
, NULL
, &cmsx_launchControlAllowTriggerReset
, 0 },
343 { "TRIGGER THROTTLE", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_launchControlThrottlePercent
, 0, LAUNCH_CONTROL_THROTTLE_TRIGGER_MAX
, 1 } , 0 },
344 { "ANGLE LIMIT", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_launchControlAngleLimit
, 0, 80, 1 } , 0 },
345 { "ITERM GAIN", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_launchControlGain
, 0, 200, 1 } , 0 },
347 { "BACK", OME_Back
, NULL
, NULL
, 0 },
348 { NULL
, OME_END
, NULL
, NULL
, 0 }
351 static CMS_Menu cmsx_menuLaunchControl
= {
352 #ifdef CMS_MENU_DEBUG
353 .GUARD_text
= "LAUNCH",
354 .GUARD_type
= OME_MENU
,
356 .onEnter
= cmsx_launchControlOnEnter
,
357 .onExit
= cmsx_launchControlOnExit
,
358 .onDisplayUpdate
= NULL
,
359 .entries
= cmsx_menuLaunchControlEntries
,
363 static uint8_t cmsx_feedForwardTransition
;
364 static uint8_t cmsx_ff_boost
;
365 static uint8_t cmsx_angleStrength
;
366 static uint8_t cmsx_horizonStrength
;
367 static uint8_t cmsx_horizonTransition
;
368 static uint8_t cmsx_throttleBoost
;
369 static uint16_t cmsx_itermAcceleratorGain
;
370 static uint16_t cmsx_itermThrottleThreshold
;
371 static uint8_t cmsx_motorOutputLimit
;
372 static int8_t cmsx_autoProfileCellCount
;
374 static uint8_t cmsx_d_min
[XYZ_AXIS_COUNT
];
375 static uint8_t cmsx_d_min_gain
;
376 static uint8_t cmsx_d_min_advance
;
379 #ifdef USE_ITERM_RELAX
380 static uint8_t cmsx_iterm_relax
;
381 static uint8_t cmsx_iterm_relax_type
;
382 static uint8_t cmsx_iterm_relax_cutoff
;
385 #ifdef USE_INTERPOLATED_SP
386 static uint8_t cmsx_ff_interpolate_sp
;
387 static uint8_t cmsx_ff_smooth_factor
;
390 static const void *cmsx_profileOtherOnEnter(displayPort_t
*pDisp
)
394 setProfileIndexString(pidProfileIndexString
, pidProfileIndex
, currentPidProfile
->profileName
);
396 const pidProfile_t
*pidProfile
= pidProfiles(pidProfileIndex
);
398 cmsx_feedForwardTransition
= pidProfile
->feedForwardTransition
;
399 cmsx_ff_boost
= pidProfile
->ff_boost
;
401 cmsx_angleStrength
= pidProfile
->pid
[PID_LEVEL
].P
;
402 cmsx_horizonStrength
= pidProfile
->pid
[PID_LEVEL
].I
;
403 cmsx_horizonTransition
= pidProfile
->pid
[PID_LEVEL
].D
;
405 cmsx_itermAcceleratorGain
= pidProfile
->itermAcceleratorGain
;
406 cmsx_itermThrottleThreshold
= pidProfile
->itermThrottleThreshold
;
408 cmsx_throttleBoost
= pidProfile
->throttle_boost
;
409 cmsx_motorOutputLimit
= pidProfile
->motor_output_limit
;
410 cmsx_autoProfileCellCount
= pidProfile
->auto_profile_cell_count
;
413 for (unsigned i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
414 cmsx_d_min
[i
] = pidProfile
->d_min
[i
];
416 cmsx_d_min_gain
= pidProfile
->d_min_gain
;
417 cmsx_d_min_advance
= pidProfile
->d_min_advance
;
420 #ifdef USE_ITERM_RELAX
421 cmsx_iterm_relax
= pidProfile
->iterm_relax
;
422 cmsx_iterm_relax_type
= pidProfile
->iterm_relax_type
;
423 cmsx_iterm_relax_cutoff
= pidProfile
->iterm_relax_cutoff
;
426 #ifdef USE_INTERPOLATED_SP
427 cmsx_ff_interpolate_sp
= pidProfile
->ff_interpolate_sp
;
428 cmsx_ff_smooth_factor
= pidProfile
->ff_smooth_factor
;
434 static const void *cmsx_profileOtherOnExit(displayPort_t
*pDisp
, const OSD_Entry
*self
)
439 pidProfile_t
*pidProfile
= pidProfilesMutable(pidProfileIndex
);
440 pidProfile
->feedForwardTransition
= cmsx_feedForwardTransition
;
441 pidInitConfig(currentPidProfile
);
442 pidProfile
->ff_boost
= cmsx_ff_boost
;
444 pidProfile
->pid
[PID_LEVEL
].P
= cmsx_angleStrength
;
445 pidProfile
->pid
[PID_LEVEL
].I
= cmsx_horizonStrength
;
446 pidProfile
->pid
[PID_LEVEL
].D
= cmsx_horizonTransition
;
448 pidProfile
->itermAcceleratorGain
= cmsx_itermAcceleratorGain
;
449 pidProfile
->itermThrottleThreshold
= cmsx_itermThrottleThreshold
;
451 pidProfile
->throttle_boost
= cmsx_throttleBoost
;
452 pidProfile
->motor_output_limit
= cmsx_motorOutputLimit
;
453 pidProfile
->auto_profile_cell_count
= cmsx_autoProfileCellCount
;
456 for (unsigned i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
457 pidProfile
->d_min
[i
] = cmsx_d_min
[i
];
459 pidProfile
->d_min_gain
= cmsx_d_min_gain
;
460 pidProfile
->d_min_advance
= cmsx_d_min_advance
;
463 #ifdef USE_ITERM_RELAX
464 pidProfile
->iterm_relax
= cmsx_iterm_relax
;
465 pidProfile
->iterm_relax_type
= cmsx_iterm_relax_type
;
466 pidProfile
->iterm_relax_cutoff
= cmsx_iterm_relax_cutoff
;
469 #ifdef USE_INTERPOLATED_SP
470 pidProfile
->ff_interpolate_sp
= cmsx_ff_interpolate_sp
;
471 pidProfile
->ff_smooth_factor
= cmsx_ff_smooth_factor
;
478 static const OSD_Entry cmsx_menuProfileOtherEntries
[] = {
479 { "-- OTHER PP --", OME_Label
, NULL
, pidProfileIndexString
, 0 },
481 { "FF TRANS", OME_FLOAT
, NULL
, &(OSD_FLOAT_t
) { &cmsx_feedForwardTransition
, 0, 100, 1, 10 }, 0 },
482 #ifdef USE_INTERPOLATED_SP
483 { "FF MODE", OME_TAB
, NULL
, &(OSD_TAB_t
) { &cmsx_ff_interpolate_sp
, 4, lookupTableInterpolatedSetpoint
}, 0 },
484 { "FF SMOOTHNESS", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_ff_smooth_factor
, 0, 75, 1 } , 0 },
486 { "FF BOOST", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_ff_boost
, 0, 50, 1 } , 0 },
487 { "ANGLE STR", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_angleStrength
, 0, 200, 1 } , 0 },
488 { "HORZN STR", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_horizonStrength
, 0, 200, 1 } , 0 },
489 { "HORZN TRS", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_horizonTransition
, 0, 200, 1 } , 0 },
490 { "AG GAIN", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &cmsx_itermAcceleratorGain
, ITERM_ACCELERATOR_GAIN_OFF
, ITERM_ACCELERATOR_GAIN_MAX
, 10 } , 0 },
491 { "AG THR", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &cmsx_itermThrottleThreshold
, 20, 1000, 1 } , 0 },
492 #ifdef USE_THROTTLE_BOOST
493 { "THR BOOST", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_throttleBoost
, 0, 100, 1 } , 0 },
495 #ifdef USE_ITERM_RELAX
496 { "I_RELAX", OME_TAB
, NULL
, &(OSD_TAB_t
) { &cmsx_iterm_relax
, ITERM_RELAX_COUNT
- 1, lookupTableItermRelax
}, 0 },
497 { "I_RELAX TYPE", OME_TAB
, NULL
, &(OSD_TAB_t
) { &cmsx_iterm_relax_type
, ITERM_RELAX_TYPE_COUNT
- 1, lookupTableItermRelaxType
}, 0 },
498 { "I_RELAX CUTOFF", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_iterm_relax_cutoff
, 1, 50, 1 }, 0 },
500 #ifdef USE_LAUNCH_CONTROL
501 {"LAUNCH CONTROL", OME_Submenu
, cmsMenuChange
, &cmsx_menuLaunchControl
, 0 },
503 { "MTR OUT LIM %",OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_motorOutputLimit
, MOTOR_OUTPUT_LIMIT_PERCENT_MIN
, MOTOR_OUTPUT_LIMIT_PERCENT_MAX
, 1}, 0 },
505 { "AUTO CELL CNT", OME_INT8
, NULL
, &(OSD_INT8_t
) { &cmsx_autoProfileCellCount
, AUTO_PROFILE_CELL_COUNT_CHANGE
, MAX_AUTO_DETECT_CELL_COUNT
, 1}, 0 },
508 { "D_MIN ROLL", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_d_min
[FD_ROLL
], 0, 100, 1 }, 0 },
509 { "D_MIN PITCH", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_d_min
[FD_PITCH
], 0, 100, 1 }, 0 },
510 { "D_MIN YAW", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_d_min
[FD_YAW
], 0, 100, 1 }, 0 },
511 { "D_MIN GAIN", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_d_min_gain
, 0, 100, 1 }, 0 },
512 { "D_MIN ADV", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &cmsx_d_min_advance
, 0, 200, 1 }, 0 },
515 { "BACK", OME_Back
, NULL
, NULL
, 0 },
516 { NULL
, OME_END
, NULL
, NULL
, 0 }
519 static CMS_Menu cmsx_menuProfileOther
= {
520 #ifdef CMS_MENU_DEBUG
521 .GUARD_text
= "XPROFOTHER",
522 .GUARD_type
= OME_MENU
,
524 .onEnter
= cmsx_profileOtherOnEnter
,
525 .onExit
= cmsx_profileOtherOnExit
,
526 .onDisplayUpdate
= NULL
,
527 .entries
= cmsx_menuProfileOtherEntries
,
531 static uint16_t gyroConfig_gyro_lowpass_hz
;
532 static uint16_t gyroConfig_gyro_lowpass2_hz
;
533 static uint16_t gyroConfig_gyro_soft_notch_hz_1
;
534 static uint16_t gyroConfig_gyro_soft_notch_cutoff_1
;
535 static uint16_t gyroConfig_gyro_soft_notch_hz_2
;
536 static uint16_t gyroConfig_gyro_soft_notch_cutoff_2
;
537 static uint8_t gyroConfig_gyro_to_use
;
539 static const void *cmsx_menuGyro_onEnter(displayPort_t
*pDisp
)
543 gyroConfig_gyro_lowpass_hz
= gyroConfig()->gyro_lowpass_hz
;
544 gyroConfig_gyro_lowpass2_hz
= gyroConfig()->gyro_lowpass2_hz
;
545 gyroConfig_gyro_soft_notch_hz_1
= gyroConfig()->gyro_soft_notch_hz_1
;
546 gyroConfig_gyro_soft_notch_cutoff_1
= gyroConfig()->gyro_soft_notch_cutoff_1
;
547 gyroConfig_gyro_soft_notch_hz_2
= gyroConfig()->gyro_soft_notch_hz_2
;
548 gyroConfig_gyro_soft_notch_cutoff_2
= gyroConfig()->gyro_soft_notch_cutoff_2
;
549 gyroConfig_gyro_to_use
= gyroConfig()->gyro_to_use
;
554 static const void *cmsx_menuGyro_onExit(displayPort_t
*pDisp
, const OSD_Entry
*self
)
559 gyroConfigMutable()->gyro_lowpass_hz
= gyroConfig_gyro_lowpass_hz
;
560 gyroConfigMutable()->gyro_lowpass2_hz
= gyroConfig_gyro_lowpass2_hz
;
561 gyroConfigMutable()->gyro_soft_notch_hz_1
= gyroConfig_gyro_soft_notch_hz_1
;
562 gyroConfigMutable()->gyro_soft_notch_cutoff_1
= gyroConfig_gyro_soft_notch_cutoff_1
;
563 gyroConfigMutable()->gyro_soft_notch_hz_2
= gyroConfig_gyro_soft_notch_hz_2
;
564 gyroConfigMutable()->gyro_soft_notch_cutoff_2
= gyroConfig_gyro_soft_notch_cutoff_2
;
565 gyroConfigMutable()->gyro_to_use
= gyroConfig_gyro_to_use
;
570 static const OSD_Entry cmsx_menuFilterGlobalEntries
[] =
572 { "-- FILTER GLB --", OME_Label
, NULL
, NULL
, 0 },
574 { "GYRO LPF", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &gyroConfig_gyro_lowpass_hz
, 0, FILTER_FREQUENCY_MAX
, 1 }, 0 },
576 { "GYRO LPF2", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &gyroConfig_gyro_lowpass2_hz
, 0, FILTER_FREQUENCY_MAX
, 1 }, 0 },
578 { "GYRO NF1", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &gyroConfig_gyro_soft_notch_hz_1
, 0, 500, 1 }, 0 },
579 { "GYRO NF1C", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &gyroConfig_gyro_soft_notch_cutoff_1
, 0, 500, 1 }, 0 },
580 { "GYRO NF2", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &gyroConfig_gyro_soft_notch_hz_2
, 0, 500, 1 }, 0 },
581 { "GYRO NF2C", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &gyroConfig_gyro_soft_notch_cutoff_2
, 0, 500, 1 }, 0 },
582 #ifdef USE_MULTI_GYRO
583 { "GYRO TO USE", OME_TAB
, NULL
, &(OSD_TAB_t
) { &gyroConfig_gyro_to_use
, 2, osdTableGyroToUse
}, REBOOT_REQUIRED
},
586 { "BACK", OME_Back
, NULL
, NULL
, 0 },
587 { NULL
, OME_END
, NULL
, NULL
, 0 }
590 static CMS_Menu cmsx_menuFilterGlobal
= {
591 #ifdef CMS_MENU_DEBUG
592 .GUARD_text
= "XFLTGLB",
593 .GUARD_type
= OME_MENU
,
595 .onEnter
= cmsx_menuGyro_onEnter
,
596 .onExit
= cmsx_menuGyro_onExit
,
597 .onDisplayUpdate
= NULL
,
598 .entries
= cmsx_menuFilterGlobalEntries
,
601 #if (defined(USE_GYRO_DATA_ANALYSE) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
603 #ifdef USE_GYRO_DATA_ANALYSE
604 static uint16_t dynFiltNotchMaxHz
;
605 static uint8_t dynFiltWidthPercent
;
606 static uint16_t dynFiltNotchQ
;
607 static uint16_t dynFiltNotchMinHz
;
610 static uint16_t dynFiltGyroMin
;
611 static uint16_t dynFiltGyroMax
;
612 static uint16_t dynFiltDtermMin
;
613 static uint16_t dynFiltDtermMax
;
614 static uint8_t dynFiltDtermExpo
;
617 static const void *cmsx_menuDynFilt_onEnter(displayPort_t
*pDisp
)
621 #ifdef USE_GYRO_DATA_ANALYSE
622 dynFiltNotchMaxHz
= gyroConfig()->dyn_notch_max_hz
;
623 dynFiltWidthPercent
= gyroConfig()->dyn_notch_width_percent
;
624 dynFiltNotchQ
= gyroConfig()->dyn_notch_q
;
625 dynFiltNotchMinHz
= gyroConfig()->dyn_notch_min_hz
;
628 const pidProfile_t
*pidProfile
= pidProfiles(pidProfileIndex
);
629 dynFiltGyroMin
= gyroConfig()->dyn_lpf_gyro_min_hz
;
630 dynFiltGyroMax
= gyroConfig()->dyn_lpf_gyro_max_hz
;
631 dynFiltDtermMin
= pidProfile
->dyn_lpf_dterm_min_hz
;
632 dynFiltDtermMax
= pidProfile
->dyn_lpf_dterm_max_hz
;
633 dynFiltDtermExpo
= pidProfile
->dyn_lpf_curve_expo
;
639 static const void *cmsx_menuDynFilt_onExit(displayPort_t
*pDisp
, const OSD_Entry
*self
)
644 #ifdef USE_GYRO_DATA_ANALYSE
645 gyroConfigMutable()->dyn_notch_max_hz
= dynFiltNotchMaxHz
;
646 gyroConfigMutable()->dyn_notch_width_percent
= dynFiltWidthPercent
;
647 gyroConfigMutable()->dyn_notch_q
= dynFiltNotchQ
;
648 gyroConfigMutable()->dyn_notch_min_hz
= dynFiltNotchMinHz
;
651 pidProfile_t
*pidProfile
= currentPidProfile
;
652 gyroConfigMutable()->dyn_lpf_gyro_min_hz
= dynFiltGyroMin
;
653 gyroConfigMutable()->dyn_lpf_gyro_max_hz
= dynFiltGyroMax
;
654 pidProfile
->dyn_lpf_dterm_min_hz
= dynFiltDtermMin
;
655 pidProfile
->dyn_lpf_dterm_max_hz
= dynFiltDtermMax
;
656 pidProfile
->dyn_lpf_curve_expo
= dynFiltDtermExpo
;
662 static const OSD_Entry cmsx_menuDynFiltEntries
[] =
664 { "-- DYN FILT --", OME_Label
, NULL
, NULL
, 0 },
666 #ifdef USE_GYRO_DATA_ANALYSE
667 { "NOTCH WIDTH %", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &dynFiltWidthPercent
, 0, 20, 1 }, 0 },
668 { "NOTCH Q", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &dynFiltNotchQ
, 0, 1000, 1 }, 0 },
669 { "NOTCH MIN HZ", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &dynFiltNotchMinHz
, 0, 1000, 1 }, 0 },
670 { "NOTCH MAX HZ", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &dynFiltNotchMaxHz
, 0, 1000, 1 }, 0 },
674 { "LPF GYRO MIN", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &dynFiltGyroMin
, 0, 1000, 1 }, 0 },
675 { "LPF GYRO MAX", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &dynFiltGyroMax
, 0, 1000, 1 }, 0 },
676 { "DTERM DLPF MIN", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &dynFiltDtermMin
, 0, 1000, 1 }, 0 },
677 { "DTERM DLPF MAX", OME_UINT16
, NULL
, &(OSD_UINT16_t
) { &dynFiltDtermMax
, 0, 1000, 1 }, 0 },
678 { "DTERM DLPF EXPO", OME_UINT8
, NULL
, &(OSD_UINT8_t
) { &dynFiltDtermExpo
, 0, 10, 1 }, 0 },
681 { "BACK", OME_Back
, NULL
, NULL
, 0 },
682 { NULL
, OME_END
, NULL
, NULL
, 0 }
685 static CMS_Menu cmsx_menuDynFilt
= {
686 #ifdef CMS_MENU_DEBUG
687 .GUARD_text
= "XDYNFLT",
688 .GUARD_type
= OME_MENU
,
690 .onEnter
= cmsx_menuDynFilt_onEnter
,
691 .onExit
= cmsx_menuDynFilt_onExit
,
692 .onDisplayUpdate
= NULL
,
693 .entries
= cmsx_menuDynFiltEntries
,
698 static uint16_t cmsx_dterm_lowpass_hz
;
699 static uint16_t cmsx_dterm_lowpass2_hz
;
700 static uint16_t cmsx_dterm_notch_hz
;
701 static uint16_t cmsx_dterm_notch_cutoff
;
702 static uint16_t cmsx_yaw_lowpass_hz
;
704 static const void *cmsx_FilterPerProfileRead(displayPort_t
*pDisp
)
708 const pidProfile_t
*pidProfile
= pidProfiles(pidProfileIndex
);
710 cmsx_dterm_lowpass_hz
= pidProfile
->dterm_lowpass_hz
;
711 cmsx_dterm_lowpass2_hz
= pidProfile
->dterm_lowpass2_hz
;
712 cmsx_dterm_notch_hz
= pidProfile
->dterm_notch_hz
;
713 cmsx_dterm_notch_cutoff
= pidProfile
->dterm_notch_cutoff
;
714 cmsx_yaw_lowpass_hz
= pidProfile
->yaw_lowpass_hz
;
719 static const void *cmsx_FilterPerProfileWriteback(displayPort_t
*pDisp
, const OSD_Entry
*self
)
724 pidProfile_t
*pidProfile
= currentPidProfile
;
726 pidProfile
->dterm_lowpass_hz
= cmsx_dterm_lowpass_hz
;
727 pidProfile
->dterm_lowpass2_hz
= cmsx_dterm_lowpass2_hz
;
728 pidProfile
->dterm_notch_hz
= cmsx_dterm_notch_hz
;
729 pidProfile
->dterm_notch_cutoff
= cmsx_dterm_notch_cutoff
;
730 pidProfile
->yaw_lowpass_hz
= cmsx_yaw_lowpass_hz
;
735 static const OSD_Entry cmsx_menuFilterPerProfileEntries
[] =
737 { "-- FILTER PP --", OME_Label
, NULL
, NULL
, 0 },
739 { "DTERM LPF", OME_UINT16
, NULL
, &(OSD_UINT16_t
){ &cmsx_dterm_lowpass_hz
, 0, FILTER_FREQUENCY_MAX
, 1 }, 0 },
740 { "DTERM LPF2", OME_UINT16
, NULL
, &(OSD_UINT16_t
){ &cmsx_dterm_lowpass2_hz
, 0, FILTER_FREQUENCY_MAX
, 1 }, 0 },
741 { "DTERM NF", OME_UINT16
, NULL
, &(OSD_UINT16_t
){ &cmsx_dterm_notch_hz
, 0, FILTER_FREQUENCY_MAX
, 1 }, 0 },
742 { "DTERM NFCO", OME_UINT16
, NULL
, &(OSD_UINT16_t
){ &cmsx_dterm_notch_cutoff
, 0, FILTER_FREQUENCY_MAX
, 1 }, 0 },
743 { "YAW LPF", OME_UINT16
, NULL
, &(OSD_UINT16_t
){ &cmsx_yaw_lowpass_hz
, 0, 500, 1 }, 0 },
745 { "BACK", OME_Back
, NULL
, NULL
, 0 },
746 { NULL
, OME_END
, NULL
, NULL
, 0 }
749 static CMS_Menu cmsx_menuFilterPerProfile
= {
750 #ifdef CMS_MENU_DEBUG
751 .GUARD_text
= "XFLTPP",
752 .GUARD_type
= OME_MENU
,
754 .onEnter
= cmsx_FilterPerProfileRead
,
755 .onExit
= cmsx_FilterPerProfileWriteback
,
756 .onDisplayUpdate
= NULL
,
757 .entries
= cmsx_menuFilterPerProfileEntries
,
760 #ifdef USE_EXTENDED_CMS_MENUS
762 static uint8_t cmsx_dstPidProfile
;
763 static uint8_t cmsx_dstControlRateProfile
;
765 static const char * const cmsx_ProfileNames
[] = {
772 static OSD_TAB_t cmsx_PidProfileTable
= { &cmsx_dstPidProfile
, 3, cmsx_ProfileNames
};
773 static OSD_TAB_t cmsx_ControlRateProfileTable
= { &cmsx_dstControlRateProfile
, 3, cmsx_ProfileNames
};
775 static const void *cmsx_menuCopyProfile_onEnter(displayPort_t
*pDisp
)
779 cmsx_dstPidProfile
= 0;
780 cmsx_dstControlRateProfile
= 0;
785 static const void *cmsx_CopyPidProfile(displayPort_t
*pDisplay
, const void *ptr
)
790 if (cmsx_dstPidProfile
> 0) {
791 pidCopyProfile(cmsx_dstPidProfile
- 1, getCurrentPidProfileIndex());
797 static const void *cmsx_CopyControlRateProfile(displayPort_t
*pDisplay
, const void *ptr
)
802 if (cmsx_dstControlRateProfile
> 0) {
803 copyControlRateProfile(cmsx_dstControlRateProfile
- 1, getCurrentControlRateProfileIndex());
809 static const OSD_Entry cmsx_menuCopyProfileEntries
[] =
811 { "-- COPY PROFILE --", OME_Label
, NULL
, NULL
, 0},
813 { "CPY PID PROF TO", OME_TAB
, NULL
, &cmsx_PidProfileTable
, 0 },
814 { "COPY PP", OME_Funcall
, cmsx_CopyPidProfile
, NULL
, 0 },
815 { "CPY RATE PROF TO", OME_TAB
, NULL
, &cmsx_ControlRateProfileTable
, 0 },
816 { "COPY RP", OME_Funcall
, cmsx_CopyControlRateProfile
, NULL
, 0 },
818 { "BACK", OME_Back
, NULL
, NULL
, 0 },
819 { NULL
, OME_END
, NULL
, NULL
, 0 }
822 CMS_Menu cmsx_menuCopyProfile
= {
823 #ifdef CMS_MENU_DEBUG
824 .GUARD_text
= "XCPY",
825 .GUARD_type
= OME_MENU
,
827 .onEnter
= cmsx_menuCopyProfile_onEnter
,
829 .onDisplayUpdate
= NULL
,
830 .entries
= cmsx_menuCopyProfileEntries
,
835 static const OSD_Entry cmsx_menuImuEntries
[] =
837 { "-- PROFILE --", OME_Label
, NULL
, NULL
, 0},
839 {"PID PROF", OME_UINT8
, cmsx_profileIndexOnChange
, &(OSD_UINT8_t
){ &tmpPidProfileIndex
, 1, PID_PROFILE_COUNT
, 1}, 0},
840 {"PID", OME_Submenu
, cmsMenuChange
, &cmsx_menuPid
, 0},
841 {"MISC PP", OME_Submenu
, cmsMenuChange
, &cmsx_menuProfileOther
, 0},
842 {"FILT PP", OME_Submenu
, cmsMenuChange
, &cmsx_menuFilterPerProfile
, 0},
844 {"RATE PROF", OME_UINT8
, cmsx_rateProfileIndexOnChange
, &(OSD_UINT8_t
){ &tmpRateProfileIndex
, 1, CONTROL_RATE_PROFILE_COUNT
, 1}, 0},
845 {"RATE", OME_Submenu
, cmsMenuChange
, &cmsx_menuRateProfile
, 0},
847 {"FILT GLB", OME_Submenu
, cmsMenuChange
, &cmsx_menuFilterGlobal
, 0},
848 #if (defined(USE_GYRO_DATA_ANALYSE) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
849 {"DYN FILT", OME_Submenu
, cmsMenuChange
, &cmsx_menuDynFilt
, 0},
852 #ifdef USE_EXTENDED_CMS_MENUS
853 {"COPY PROF", OME_Submenu
, cmsMenuChange
, &cmsx_menuCopyProfile
, 0},
854 #endif /* USE_EXTENDED_CMS_MENUS */
856 {"BACK", OME_Back
, NULL
, NULL
, 0},
857 {NULL
, OME_END
, NULL
, NULL
, 0}
860 CMS_Menu cmsx_menuImu
= {
861 #ifdef CMS_MENU_DEBUG
862 .GUARD_text
= "XIMU",
863 .GUARD_type
= OME_MENU
,
865 .onEnter
= cmsx_menuImu_onEnter
,
866 .onExit
= cmsx_menuImu_onExit
,
867 .onDisplayUpdate
= NULL
,
868 .entries
= cmsx_menuImuEntries
,