?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Remove deprecated CLI name command
[betaflight.git] / src / main / cms / cms_menu_imu.c
blob13d089c1fb1bea2472e13baf0f3388b71e2fecda
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
44 #include "drivers/pwm_output.h"
45
46 #include "fc/config.h"
47 #include "fc/controlrate_profile.h"
48 #include "fc/core.h"
49 #include "fc/rc_controls.h"
50 #include "fc/runtime_config.h"
51
52 #include "flight/mixer.h"
53 #include "flight/pid.h"
54
55 #include "pg/pg.h"
56
57 #include "sensors/battery.h"
58 #include "sensors/gyro.h"
59
60 #include "cli/settings.h"
61
62 //
63 // PID
64 //
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];
70
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;
75
76 static const char * const osdTableThrottleLimitType[] = {
77 "OFF", "SCALE", "CLIP"
78 };
79
80 #if defined(USE_GYRO_DATA_ANALYSE) && defined(USE_EXTENDED_CMS_MENUS)
81 static const char * const osdTableDynNotchRangeType[] = {
82 "HIGH", "MED", "LOW", "AUTO"
83 };
84 #endif
85
86 #ifdef USE_MULTI_GYRO
87 static const char * const osdTableGyroToUse[] = {
88 "FIRST", "SECOND", "BOTH"
89 };
90 #endif
91
92 static void setProfileIndexString(char *profileString, int profileIndex, char *profileName)
93 {
94 int charIndex = 0;
95 profileString[charIndex++] = '1' + profileIndex;
96
97 #ifdef USE_PROFILE_NAMES
98 const int profileNameLen = strlen(profileName);
99
100 if (profileNameLen > 0) {
101 profileString[charIndex++] = ' ';
102 profileString[charIndex++] = '(';
103 for (int i = 0; i < profileNameLen; i++) {
104 profileString[charIndex++] = toupper(profileName[i]);
105 }
106 profileString[charIndex++] = ')';
107 }
108 #else
109 UNUSED(profileName);
110 #endif
111
112 profileString[charIndex] = '\0';
113 }
114
115 static long cmsx_menuImu_onEnter(void)
116 {
117 pidProfileIndex = getCurrentPidProfileIndex();
118 tmpPidProfileIndex = pidProfileIndex + 1;
119
120 rateProfileIndex = getCurrentControlRateProfileIndex();
121 tmpRateProfileIndex = rateProfileIndex + 1;
122
123 return 0;
124 }
125
126 static long cmsx_menuImu_onExit(const OSD_Entry *self)
127 {
128 UNUSED(self);
129
130 changePidProfile(pidProfileIndex);
131 changeControlRateProfile(rateProfileIndex);
132
133 return 0;
134 }
135
136 static long cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *ptr)
137 {
138 UNUSED(displayPort);
139 UNUSED(ptr);
140
141 pidProfileIndex = tmpPidProfileIndex - 1;
142 changePidProfile(pidProfileIndex);
143
144 return 0;
145 }
146
147 static long cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void *ptr)
148 {
149 UNUSED(displayPort);
150 UNUSED(ptr);
151
152 rateProfileIndex = tmpRateProfileIndex - 1;
153 changeControlRateProfile(rateProfileIndex);
154
155 return 0;
156 }
157
158 static long cmsx_PidRead(void)
159 {
160
161 const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
162 for (uint8_t i = 0; i < 3; i++) {
163 tempPid[i][0] = pidProfile->pid[i].P;
164 tempPid[i][1] = pidProfile->pid[i].I;
165 tempPid[i][2] = pidProfile->pid[i].D;
166 tempPidF[i] = pidProfile->pid[i].F;
167 }
168
169 return 0;
170 }
171
172 static long cmsx_PidOnEnter(void)
173 {
174 setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName);
175 cmsx_PidRead();
176
177 return 0;
178 }
179
180 static long cmsx_PidWriteback(const OSD_Entry *self)
181 {
182 UNUSED(self);
183
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];
190 }
191 pidInitConfig(currentPidProfile);
192
193 return 0;
194 }
195
196 static const OSD_Entry cmsx_menuPidEntries[] =
197 {
198 { "-- PID --", OME_Label, NULL, pidProfileIndexString, 0},
199
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 },
204
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 },
209
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 },
214
215 { "BACK", OME_Back, NULL, NULL, 0 },
216 { NULL, OME_END, NULL, NULL, 0 }
217 };
218
219 static CMS_Menu cmsx_menuPid = {
220 #ifdef CMS_MENU_DEBUG
221 .GUARD_text = "XPID",
222 .GUARD_type = OME_MENU,
223 #endif
224 .onEnter = cmsx_PidOnEnter,
225 .onExit = cmsx_PidWriteback,
226 .entries = cmsx_menuPidEntries
227 };
228
229 //
230 // Rate & Expo
231 //
232
233 static long cmsx_RateProfileRead(void)
234 {
235 memcpy(&rateProfile, controlRateProfiles(rateProfileIndex), sizeof(controlRateConfig_t));
236
237 return 0;
238 }
239
240 static long cmsx_RateProfileWriteback(const OSD_Entry *self)
241 {
242 UNUSED(self);
243
244 memcpy(controlRateProfilesMutable(rateProfileIndex), &rateProfile, sizeof(controlRateConfig_t));
245
246 return 0;
247 }
248
249 static long cmsx_RateProfileOnEnter(void)
250 {
251 setProfileIndexString(rateProfileIndexString, rateProfileIndex, controlRateProfilesMutable(rateProfileIndex)->profileName);
252 cmsx_RateProfileRead();
253
254 return 0;
255 }
256
257 static const OSD_Entry cmsx_menuRateProfileEntries[] =
258 {
259 { "-- RATE --", OME_Label, NULL, rateProfileIndexString, 0 },
260
261 { "RC R RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcRates[FD_ROLL], 1, CONTROL_RATE_CONFIG_RC_RATES_MAX, 1, 10 }, 0 },
262 { "RC P RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcRates[FD_PITCH], 1, CONTROL_RATE_CONFIG_RC_RATES_MAX, 1, 10 }, 0 },
263 { "RC Y RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcRates[FD_YAW], 1, CONTROL_RATE_CONFIG_RC_RATES_MAX, 1, 10 }, 0 },
264
265 { "ROLL SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[FD_ROLL], 0, 100, 1, 10 }, 0 },
266 { "PITCH SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[FD_PITCH], 0, 100, 1, 10 }, 0 },
267 { "YAW SUPER", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rates[FD_YAW], 0, 100, 1, 10 }, 0 },
268
269 { "RC R EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo[FD_ROLL], 0, 100, 1, 10 }, 0 },
270 { "RC P EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo[FD_PITCH], 0, 100, 1, 10 }, 0 },
271 { "RC Y EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.rcExpo[FD_YAW], 0, 100, 1, 10 }, 0 },
272
273 { "THR MID", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.thrMid8, 0, 100, 1}, 0 },
274 { "THR EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.thrExpo8, 0, 100, 1}, 0 },
275 { "THRPID ATT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.dynThrPID, 0, 100, 1, 10}, 0 },
276 { "TPA BRKPT", OME_UINT16, NULL, &(OSD_UINT16_t){ &rateProfile.tpa_breakpoint, 1000, 2000, 10}, 0 },
277
278 { "THR LIM TYPE",OME_TAB, NULL, &(OSD_TAB_t) { &rateProfile.throttle_limit_type, THROTTLE_LIMIT_TYPE_COUNT - 1, osdTableThrottleLimitType}, 0 },
279 { "THR LIM %", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.throttle_limit_percent, 25, 100, 1}, 0 },
280
281 { "BACK", OME_Back, NULL, NULL, 0 },
282 { NULL, OME_END, NULL, NULL, 0 }
283 };
284
285 static CMS_Menu cmsx_menuRateProfile = {
286 #ifdef CMS_MENU_DEBUG
287 .GUARD_text = "MENURATE",
288 .GUARD_type = OME_MENU,
289 #endif
290 .onEnter = cmsx_RateProfileOnEnter,
291 .onExit = cmsx_RateProfileWriteback,
292 .entries = cmsx_menuRateProfileEntries
293 };
294
295 #ifdef USE_LAUNCH_CONTROL
296 static uint8_t cmsx_launchControlMode;
297 static uint8_t cmsx_launchControlAllowTriggerReset;
298 static uint8_t cmsx_launchControlThrottlePercent;
299 static uint8_t cmsx_launchControlAngleLimit;
300 static uint8_t cmsx_launchControlGain;
301
302 static long cmsx_launchControlOnEnter(void)
303 {
304 const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
305
306 cmsx_launchControlMode = pidProfile->launchControlMode;
307 cmsx_launchControlAllowTriggerReset = pidProfile->launchControlAllowTriggerReset;
308 cmsx_launchControlThrottlePercent = pidProfile->launchControlThrottlePercent;
309 cmsx_launchControlAngleLimit = pidProfile->launchControlAngleLimit;
310 cmsx_launchControlGain = pidProfile->launchControlGain;
311
312 return 0;
313 }
314
315 static long cmsx_launchControlOnExit(const OSD_Entry *self)
316 {
317 UNUSED(self);
318
319 pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
320
321 pidProfile->launchControlMode = cmsx_launchControlMode;
322 pidProfile->launchControlAllowTriggerReset = cmsx_launchControlAllowTriggerReset;
323 pidProfile->launchControlThrottlePercent = cmsx_launchControlThrottlePercent;
324 pidProfile->launchControlAngleLimit = cmsx_launchControlAngleLimit;
325 pidProfile->launchControlGain = cmsx_launchControlGain;
326
327 return 0;
328 }
329
330 static const OSD_Entry cmsx_menuLaunchControlEntries[] = {
331 { "-- LAUNCH CONTROL --", OME_Label, NULL, pidProfileIndexString, 0 },
332
333 { "MODE", OME_TAB, NULL, &(OSD_TAB_t) { &cmsx_launchControlMode, LAUNCH_CONTROL_MODE_COUNT - 1, osdLaunchControlModeNames}, 0 },
334 { "ALLOW RESET", OME_Bool, NULL, &cmsx_launchControlAllowTriggerReset, 0 },
335 { "TRIGGER THROTTLE", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlThrottlePercent, 0, LAUNCH_CONTROL_THROTTLE_TRIGGER_MAX, 1 } , 0 },
336 { "ANGLE LIMIT", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlAngleLimit, 0, 80, 1 } , 0 },
337 { "ITERM GAIN", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlGain, 0, 200, 1 } , 0 },
338
339 { "BACK", OME_Back, NULL, NULL, 0 },
340 { NULL, OME_END, NULL, NULL, 0 }
341 };
342
343 static CMS_Menu cmsx_menuLaunchControl = {
344 #ifdef CMS_MENU_DEBUG
345 .GUARD_text = "LAUNCH",
346 .GUARD_type = OME_MENU,
347 #endif
348 .onEnter = cmsx_launchControlOnEnter,
349 .onExit = cmsx_launchControlOnExit,
350 .entries = cmsx_menuLaunchControlEntries,
351 };
352 #endif
353
354 static uint8_t cmsx_feedForwardTransition;
355 static uint8_t cmsx_ff_boost;
356 static uint8_t cmsx_angleStrength;
357 static uint8_t cmsx_horizonStrength;
358 static uint8_t cmsx_horizonTransition;
359 static uint8_t cmsx_throttleBoost;
360 static uint16_t cmsx_itermAcceleratorGain;
361 static uint16_t cmsx_itermThrottleThreshold;
362 static uint8_t cmsx_motorOutputLimit;
363 static int8_t cmsx_autoProfileCellCount;
364 #ifdef USE_D_MIN
365 static uint8_t cmsx_d_min[XYZ_AXIS_COUNT];
366 static uint8_t cmsx_d_min_gain;
367 static uint8_t cmsx_d_min_advance;
368 #endif
369
370 #ifdef USE_ITERM_RELAX
371 static uint8_t cmsx_iterm_relax;
372 static uint8_t cmsx_iterm_relax_type;
373 static uint8_t cmsx_iterm_relax_cutoff;
374 #endif
375
376 static long cmsx_profileOtherOnEnter(void)
377 {
378 setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName);
379
380 const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
381
382 cmsx_feedForwardTransition = pidProfile->feedForwardTransition;
383 cmsx_ff_boost = pidProfile->ff_boost;
384
385 cmsx_angleStrength = pidProfile->pid[PID_LEVEL].P;
386 cmsx_horizonStrength = pidProfile->pid[PID_LEVEL].I;
387 cmsx_horizonTransition = pidProfile->pid[PID_LEVEL].D;
388
389 cmsx_itermAcceleratorGain = pidProfile->itermAcceleratorGain;
390 cmsx_itermThrottleThreshold = pidProfile->itermThrottleThreshold;
391
392 cmsx_throttleBoost = pidProfile->throttle_boost;
393 cmsx_motorOutputLimit = pidProfile->motor_output_limit;
394 cmsx_autoProfileCellCount = pidProfile->auto_profile_cell_count;
395
396 #ifdef USE_D_MIN
397 for (unsigned i = 0; i < XYZ_AXIS_COUNT; i++) {
398 cmsx_d_min[i] = pidProfile->d_min[i];
399 }
400 cmsx_d_min_gain = pidProfile->d_min_gain;
401 cmsx_d_min_advance = pidProfile->d_min_advance;
402 #endif
403
404 #ifdef USE_ITERM_RELAX
405 cmsx_iterm_relax = pidProfile->iterm_relax;
406 cmsx_iterm_relax_type = pidProfile->iterm_relax_type;
407 cmsx_iterm_relax_cutoff = pidProfile->iterm_relax_cutoff;
408 #endif
409
410 return 0;
411 }
412
413 static long cmsx_profileOtherOnExit(const OSD_Entry *self)
414 {
415 UNUSED(self);
416
417 pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
418 pidProfile->feedForwardTransition = cmsx_feedForwardTransition;
419 pidInitConfig(currentPidProfile);
420 pidProfile->ff_boost = cmsx_ff_boost;
421
422 pidProfile->pid[PID_LEVEL].P = cmsx_angleStrength;
423 pidProfile->pid[PID_LEVEL].I = cmsx_horizonStrength;
424 pidProfile->pid[PID_LEVEL].D = cmsx_horizonTransition;
425
426 pidProfile->itermAcceleratorGain = cmsx_itermAcceleratorGain;
427 pidProfile->itermThrottleThreshold = cmsx_itermThrottleThreshold;
428
429 pidProfile->throttle_boost = cmsx_throttleBoost;
430 pidProfile->motor_output_limit = cmsx_motorOutputLimit;
431 pidProfile->auto_profile_cell_count = cmsx_autoProfileCellCount;
432
433 #ifdef USE_D_MIN
434 for (unsigned i = 0; i < XYZ_AXIS_COUNT; i++) {
435 pidProfile->d_min[i] = cmsx_d_min[i];
436 }
437 pidProfile->d_min_gain = cmsx_d_min_gain;
438 pidProfile->d_min_advance = cmsx_d_min_advance;
439 #endif
440
441 #ifdef USE_ITERM_RELAX
442 pidProfile->iterm_relax = cmsx_iterm_relax;
443 pidProfile->iterm_relax_type = cmsx_iterm_relax_type;
444 pidProfile->iterm_relax_cutoff = cmsx_iterm_relax_cutoff;
445 #endif
446
447 initEscEndpoints();
448 return 0;
449 }
450
451 static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
452 { "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 },
453
454 { "FF TRANS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_feedForwardTransition, 0, 100, 1, 10 }, 0 },
455 { "FF BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_boost, 0, 50, 1 } , 0 },
456 { "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 },
457 { "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } , 0 },
458 { "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } , 0 },
459 { "AG GAIN", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_itermAcceleratorGain, 1000, 30000, 10 } , 0 },
460 { "AG THR", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_itermThrottleThreshold, 20, 1000, 1 } , 0 },
461 #ifdef USE_THROTTLE_BOOST
462 { "THR BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_throttleBoost, 0, 100, 1 } , 0 },
463 #endif
464 #ifdef USE_ITERM_RELAX
465 { "I_RELAX", OME_TAB, NULL, &(OSD_TAB_t) { &cmsx_iterm_relax, ITERM_RELAX_COUNT - 1, lookupTableItermRelax }, 0 },
466 { "I_RELAX TYPE", OME_TAB, NULL, &(OSD_TAB_t) { &cmsx_iterm_relax_type, ITERM_RELAX_TYPE_COUNT - 1, lookupTableItermRelaxType }, 0 },
467 { "I_RELAX CUTOFF", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_iterm_relax_cutoff, 1, 100, 1 }, 0 },
468 #endif
469 #ifdef USE_LAUNCH_CONTROL
470 {"LAUNCH CONTROL", OME_Submenu, cmsMenuChange, &cmsx_menuLaunchControl, 0 },
471 #endif
472 { "MTR OUT LIM %",OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_motorOutputLimit, MOTOR_OUTPUT_LIMIT_PERCENT_MIN, MOTOR_OUTPUT_LIMIT_PERCENT_MAX, 1}, 0 },
473
474 { "AUTO CELL CNT", OME_INT8, NULL, &(OSD_INT8_t) { &cmsx_autoProfileCellCount, AUTO_PROFILE_CELL_COUNT_CHANGE, MAX_AUTO_DETECT_CELL_COUNT, 1}, 0 },
475
476 #ifdef USE_D_MIN
477 { "D_MIN ROLL", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_ROLL], 0, 100, 1 }, 0 },
478 { "D_MIN PITCH", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_PITCH], 0, 100, 1 }, 0 },
479 { "D_MIN YAW", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_YAW], 0, 100, 1 }, 0 },
480 { "D_MIN GAIN", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_d_min_gain, 0, 100, 1 }, 0 },
481 { "D_MIN ADV", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_d_min_advance, 0, 200, 1 }, 0 },
482 #endif
483
484 { "BACK", OME_Back, NULL, NULL, 0 },
485 { NULL, OME_END, NULL, NULL, 0 }
486 };
487
488 static CMS_Menu cmsx_menuProfileOther = {
489 #ifdef CMS_MENU_DEBUG
490 .GUARD_text = "XPROFOTHER",
491 .GUARD_type = OME_MENU,
492 #endif
493 .onEnter = cmsx_profileOtherOnEnter,
494 .onExit = cmsx_profileOtherOnExit,
495 .entries = cmsx_menuProfileOtherEntries,
496 };
497
498
499 static uint16_t gyroConfig_gyro_lowpass_hz;
500 static uint16_t gyroConfig_gyro_lowpass2_hz;
501 static uint16_t gyroConfig_gyro_soft_notch_hz_1;
502 static uint16_t gyroConfig_gyro_soft_notch_cutoff_1;
503 static uint16_t gyroConfig_gyro_soft_notch_hz_2;
504 static uint16_t gyroConfig_gyro_soft_notch_cutoff_2;
505 static uint8_t gyroConfig_gyro_to_use;
506
507 static long cmsx_menuGyro_onEnter(void)
508 {
509 gyroConfig_gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz;
510 gyroConfig_gyro_lowpass2_hz = gyroConfig()->gyro_lowpass2_hz;
511 gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
512 gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
513 gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
514 gyroConfig_gyro_soft_notch_cutoff_2 = gyroConfig()->gyro_soft_notch_cutoff_2;
515 gyroConfig_gyro_to_use = gyroConfig()->gyro_to_use;
516
517 return 0;
518 }
519
520 static long cmsx_menuGyro_onExit(const OSD_Entry *self)
521 {
522 UNUSED(self);
523
524 gyroConfigMutable()->gyro_lowpass_hz = gyroConfig_gyro_lowpass_hz;
525 gyroConfigMutable()->gyro_lowpass2_hz = gyroConfig_gyro_lowpass2_hz;
526 gyroConfigMutable()->gyro_soft_notch_hz_1 = gyroConfig_gyro_soft_notch_hz_1;
527 gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
528 gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
529 gyroConfigMutable()->gyro_soft_notch_cutoff_2 = gyroConfig_gyro_soft_notch_cutoff_2;
530 gyroConfigMutable()->gyro_to_use = gyroConfig_gyro_to_use;
531
532 return 0;
533 }
534
535 static const OSD_Entry cmsx_menuFilterGlobalEntries[] =
536 {
537 { "-- FILTER GLB --", OME_Label, NULL, NULL, 0 },
538
539 { "GYRO LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass_hz, 0, FILTER_FREQUENCY_MAX, 1 }, 0 },
540 #ifdef USE_GYRO_LPF2
541 { "GYRO LPF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz, 0, FILTER_FREQUENCY_MAX, 1 }, 0 },
542 #endif
543 { "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_1, 0, 500, 1 }, 0 },
544 { "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
545 { "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2, 0, 500, 1 }, 0 },
546 { "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
547 #ifdef USE_MULTI_GYRO
548 { "GYRO TO USE", OME_TAB, NULL, &(OSD_TAB_t) { &gyroConfig_gyro_to_use, 2, osdTableGyroToUse}, REBOOT_REQUIRED },
549 #endif
550
551 { "BACK", OME_Back, NULL, NULL, 0 },
552 { NULL, OME_END, NULL, NULL, 0 }
553 };
554
555 static CMS_Menu cmsx_menuFilterGlobal = {
556 #ifdef CMS_MENU_DEBUG
557 .GUARD_text = "XFLTGLB",
558 .GUARD_type = OME_MENU,
559 #endif
560 .onEnter = cmsx_menuGyro_onEnter,
561 .onExit = cmsx_menuGyro_onExit,
562 .entries = cmsx_menuFilterGlobalEntries,
563 };
564
565 #if (defined(USE_GYRO_DATA_ANALYSE) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
566
567 #ifdef USE_GYRO_DATA_ANALYSE
568 static uint8_t dynFiltNotchRange;
569 static uint8_t dynFiltWidthPercent;
570 static uint16_t dynFiltNotchQ;
571 static uint16_t dynFiltNotchMinHz;
572 #endif
573 #ifdef USE_DYN_LPF
574 static uint16_t dynFiltGyroMin;
575 static uint16_t dynFiltGyroMax;
576 static uint16_t dynFiltDtermMin;
577 static uint16_t dynFiltDtermMax;
578 #endif
579
580 static long cmsx_menuDynFilt_onEnter(void)
581 {
582 #ifdef USE_GYRO_DATA_ANALYSE
583 dynFiltNotchRange = gyroConfig()->dyn_notch_range;
584 dynFiltWidthPercent = gyroConfig()->dyn_notch_width_percent;
585 dynFiltNotchQ = gyroConfig()->dyn_notch_q;
586 dynFiltNotchMinHz = gyroConfig()->dyn_notch_min_hz;
587 #endif
588 #ifdef USE_DYN_LPF
589 const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
590 dynFiltGyroMin = gyroConfig()->dyn_lpf_gyro_min_hz;
591 dynFiltGyroMax = gyroConfig()->dyn_lpf_gyro_max_hz;
592 dynFiltDtermMin = pidProfile->dyn_lpf_dterm_min_hz;
593 dynFiltDtermMax = pidProfile->dyn_lpf_dterm_max_hz;
594 #endif
595
596 return 0;
597 }
598
599 static long cmsx_menuDynFilt_onExit(const OSD_Entry *self)
600 {
601 UNUSED(self);
602
603 #ifdef USE_GYRO_DATA_ANALYSE
604 gyroConfigMutable()->dyn_notch_range = dynFiltNotchRange;
605 gyroConfigMutable()->dyn_notch_width_percent = dynFiltWidthPercent;
606 gyroConfigMutable()->dyn_notch_q = dynFiltNotchQ;
607 gyroConfigMutable()->dyn_notch_min_hz = dynFiltNotchMinHz;
608 #endif
609 #ifdef USE_DYN_LPF
610 pidProfile_t *pidProfile = currentPidProfile;
611 gyroConfigMutable()->dyn_lpf_gyro_min_hz = dynFiltGyroMin;
612 gyroConfigMutable()->dyn_lpf_gyro_max_hz = dynFiltGyroMax;
613 pidProfile->dyn_lpf_dterm_min_hz = dynFiltDtermMin;
614 pidProfile->dyn_lpf_dterm_max_hz = dynFiltDtermMax;
615 #endif
616
617 return 0;
618 }
619
620 static const OSD_Entry cmsx_menuDynFiltEntries[] =
621 {
622 { "-- DYN FILT --", OME_Label, NULL, NULL, 0 },
623
624 #ifdef USE_GYRO_DATA_ANALYSE
625 { "NOTCH RANGE", OME_TAB, NULL, &(OSD_TAB_t) { &dynFiltNotchRange, 3, osdTableDynNotchRangeType}, 0 },
626 { "NOTCH WIDTH %", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltWidthPercent, 0, 20, 1 }, 0 },
627 { "NOTCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ, 0, 1000, 1 }, 0 },
628 { "NOTCH MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz, 0, 1000, 1 }, 0 },
629 #endif
630
631 #ifdef USE_DYN_LPF
632 { "LPF GYRO MIN", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltGyroMin, 0, 1000, 1 }, 0 },
633 { "LPF GYRO MAX", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltGyroMax, 0, 1000, 1 }, 0 },
634 { "DTERM DLPF MIN", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltDtermMin, 0, 1000, 1 }, 0 },
635 { "DTERM DLPF MAX", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltDtermMax, 0, 1000, 1 }, 0 },
636 #endif
637
638 { "BACK", OME_Back, NULL, NULL, 0 },
639 { NULL, OME_END, NULL, NULL, 0 }
640 };
641
642 static CMS_Menu cmsx_menuDynFilt = {
643 #ifdef CMS_MENU_DEBUG
644 .GUARD_text = "XDYNFLT",
645 .GUARD_type = OME_MENU,
646 #endif
647 .onEnter = cmsx_menuDynFilt_onEnter,
648 .onExit = cmsx_menuDynFilt_onExit,
649 .entries = cmsx_menuDynFiltEntries,
650 };
651
652 #endif
653
654 static uint16_t cmsx_dterm_lowpass_hz;
655 static uint16_t cmsx_dterm_lowpass2_hz;
656 static uint16_t cmsx_dterm_notch_hz;
657 static uint16_t cmsx_dterm_notch_cutoff;
658 static uint16_t cmsx_yaw_lowpass_hz;
659
660 static long cmsx_FilterPerProfileRead(void)
661 {
662 const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
663
664 cmsx_dterm_lowpass_hz = pidProfile->dterm_lowpass_hz;
665 cmsx_dterm_lowpass2_hz = pidProfile->dterm_lowpass2_hz;
666 cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz;
667 cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff;
668 cmsx_yaw_lowpass_hz = pidProfile->yaw_lowpass_hz;
669
670 return 0;
671 }
672
673 static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self)
674 {
675 UNUSED(self);
676
677 pidProfile_t *pidProfile = currentPidProfile;
678
679 pidProfile->dterm_lowpass_hz = cmsx_dterm_lowpass_hz;
680 pidProfile->dterm_lowpass2_hz = cmsx_dterm_lowpass2_hz;
681 pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz;
682 pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
683 pidProfile->yaw_lowpass_hz = cmsx_yaw_lowpass_hz;
684
685 return 0;
686 }
687
688 static const OSD_Entry cmsx_menuFilterPerProfileEntries[] =
689 {
690 { "-- FILTER PP --", OME_Label, NULL, NULL, 0 },
691
692 { "DTERM LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass_hz, 0, FILTER_FREQUENCY_MAX, 1 }, 0 },
693 { "DTERM LPF2", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz, 0, FILTER_FREQUENCY_MAX, 1 }, 0 },
694 { "DTERM NF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz, 0, FILTER_FREQUENCY_MAX, 1 }, 0 },
695 { "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff, 0, FILTER_FREQUENCY_MAX, 1 }, 0 },
696 { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lowpass_hz, 0, 500, 1 }, 0 },
697
698 { "BACK", OME_Back, NULL, NULL, 0 },
699 { NULL, OME_END, NULL, NULL, 0 }
700 };
701
702 static CMS_Menu cmsx_menuFilterPerProfile = {
703 #ifdef CMS_MENU_DEBUG
704 .GUARD_text = "XFLTPP",
705 .GUARD_type = OME_MENU,
706 #endif
707 .onEnter = cmsx_FilterPerProfileRead,
708 .onExit = cmsx_FilterPerProfileWriteback,
709 .entries = cmsx_menuFilterPerProfileEntries,
710 };
711
712 #ifdef USE_EXTENDED_CMS_MENUS
713
714 static uint8_t cmsx_dstPidProfile;
715 static uint8_t cmsx_dstControlRateProfile;
716
717 static const char * const cmsx_ProfileNames[] = {
718 "-",
719 "1",
720 "2",
721 "3"
722 };
723
724 static OSD_TAB_t cmsx_PidProfileTable = { &cmsx_dstPidProfile, 3, cmsx_ProfileNames };
725 static OSD_TAB_t cmsx_ControlRateProfileTable = { &cmsx_dstControlRateProfile, 3, cmsx_ProfileNames };
726
727 static long cmsx_menuCopyProfile_onEnter(void)
728 {
729 cmsx_dstPidProfile = 0;
730 cmsx_dstControlRateProfile = 0;
731
732 return 0;
733 }
734
735 static long cmsx_CopyPidProfile(displayPort_t *pDisplay, const void *ptr)
736 {
737 UNUSED(pDisplay);
738 UNUSED(ptr);
739
740 if (cmsx_dstPidProfile > 0) {
741 pidCopyProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex());
742 }
743
744 return 0;
745 }
746
747 static long cmsx_CopyControlRateProfile(displayPort_t *pDisplay, const void *ptr)
748 {
749 UNUSED(pDisplay);
750 UNUSED(ptr);
751
752 if (cmsx_dstControlRateProfile > 0) {
753 copyControlRateProfile(cmsx_dstControlRateProfile - 1, getCurrentControlRateProfileIndex());
754 }
755
756 return 0;
757 }
758
759 static const OSD_Entry cmsx_menuCopyProfileEntries[] =
760 {
761 { "-- COPY PROFILE --", OME_Label, NULL, NULL, 0},
762
763 { "CPY PID PROF TO", OME_TAB, NULL, &cmsx_PidProfileTable, 0 },
764 { "COPY PP", OME_Funcall, cmsx_CopyPidProfile, NULL, 0 },
765 { "CPY RATE PROF TO", OME_TAB, NULL, &cmsx_ControlRateProfileTable, 0 },
766 { "COPY RP", OME_Funcall, cmsx_CopyControlRateProfile, NULL, 0 },
767
768 { "BACK", OME_Back, NULL, NULL, 0 },
769 { NULL, OME_END, NULL, NULL, 0 }
770 };
771
772 CMS_Menu cmsx_menuCopyProfile = {
773 #ifdef CMS_MENU_DEBUG
774 .GUARD_text = "XCPY",
775 .GUARD_type = OME_MENU,
776 #endif
777 .onEnter = cmsx_menuCopyProfile_onEnter,
778 .onExit = NULL,
779 .entries = cmsx_menuCopyProfileEntries,
780 };
781
782 #endif
783
784 static const OSD_Entry cmsx_menuImuEntries[] =
785 {
786 { "-- PROFILE --", OME_Label, NULL, NULL, 0},
787
788 {"PID PROF", OME_UINT8, cmsx_profileIndexOnChange, &(OSD_UINT8_t){ &tmpPidProfileIndex, 1, PID_PROFILE_COUNT, 1}, 0},
789 {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0},
790 {"MISC PP", OME_Submenu, cmsMenuChange, &cmsx_menuProfileOther, 0},
791 {"FILT PP", OME_Submenu, cmsMenuChange, &cmsx_menuFilterPerProfile, 0},
792
793 {"RATE PROF", OME_UINT8, cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, CONTROL_RATE_PROFILE_COUNT, 1}, 0},
794 {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0},
795
796 {"FILT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0},
797 #if (defined(USE_GYRO_DATA_ANALYSE) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
798 {"DYN FILT", OME_Submenu, cmsMenuChange, &cmsx_menuDynFilt, 0},
799 #endif
800
801 #ifdef USE_EXTENDED_CMS_MENUS
802 {"COPY PROF", OME_Submenu, cmsMenuChange, &cmsx_menuCopyProfile, 0},
803 #endif /* USE_EXTENDED_CMS_MENUS */
804
805 {"BACK", OME_Back, NULL, NULL, 0},
806 {NULL, OME_END, NULL, NULL, 0}
807 };
808
809 CMS_Menu cmsx_menuImu = {
810 #ifdef CMS_MENU_DEBUG
811 .GUARD_text = "XIMU",
812 .GUARD_type = OME_MENU,
813 #endif
814 .onEnter = cmsx_menuImu_onEnter,
815 .onExit = cmsx_menuImu_onExit,
816 .entries = cmsx_menuImuEntries,
817 };
818
819 #endif // CMS