Merge remote-tracking branch 'origin/master' into mmosca-mavlinkrc
[inav.git] / src / main / cms / cms_menu_imu.c
blob5fd9df7d9738a5cf23dcf750d7ede6cd65933865
1 /*
2 * This file is part of Cleanflight.
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
18 // Menu contents for PID, RATES, RC preview, misc
19 // Should be part of the relevant .c file.
21 #include <stdbool.h>
22 #include <stdint.h>
23 #include <string.h>
25 #include "platform.h"
27 #ifdef USE_CMS
29 #include "common/utils.h"
31 #include "cms/cms.h"
32 #include "cms/cms_types.h"
33 #include "cms/cms_menu_imu.h"
35 #include "common/axis.h"
37 #include "flight/pid.h"
39 #include "fc/config.h"
40 #include "fc/controlrate_profile.h"
41 #include "fc/rc_controls.h"
42 #include "fc/settings.h"
44 #include "navigation/navigation.h"
46 #include "sensors/gyro.h"
50 // PIDFF
52 #define PIDFF_MIN 0
53 #define PIDFF_STEP 1
55 #define RPY_PIDFF_MAX 200
56 #define OTHER_PIDDF_MAX 255
58 #define PIDFF_ENTRY(label, ptr, max) OSD_UINT16_ENTRY(label, (&(const OSD_UINT16_t){ ptr, PIDFF_MIN, max, PIDFF_STEP }))
59 #define RPY_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, RPY_PIDFF_MAX)
60 #define OTHER_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, OTHER_PIDDF_MAX)
62 static pid8_t cmsx_pidRoll;
63 static pid8_t cmsx_pidPitch;
64 static pid8_t cmsx_pidYaw;
65 static pid8_t cmsx_pidPosZ;
66 static pid8_t cmsx_pidVelZ;
67 static pid8_t cmsx_pidHead;
68 static pid8_t cmsx_pidPosXY;
69 static pid8_t cmsx_pidVelXY;
71 static uint8_t tmpProfileIndex;
72 static uint8_t profileIndex;
73 static char profileIndexString[] = " p";
75 static void cmsx_ReadPidToArray(pid8_t *dst, int pidIndex)
77 memcpy(dst, &pidBank()->pid[pidIndex], sizeof(*dst));
80 static void cmsx_WritebackPidFromArray(const pid8_t *src, int pidIndex)
82 memcpy(&pidBankMutable()->pid[pidIndex], src, sizeof(*src));
85 static long cmsx_menuImu_onEnter(const OSD_Entry *from)
87 UNUSED(from);
89 profileIndex = getConfigProfile();
90 tmpProfileIndex = profileIndex + 1;
91 profileIndexString[1] = '0' + tmpProfileIndex;
93 return 0;
96 static long cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *ptr)
98 UNUSED(displayPort);
99 UNUSED(ptr);
101 profileIndex = tmpProfileIndex - 1;
102 profileIndexString[1] = '0' + tmpProfileIndex;
103 setConfigProfile(profileIndex);
105 return 0;
108 static long cmsx_PidRead(void)
110 cmsx_ReadPidToArray(&cmsx_pidRoll, PID_ROLL);
111 cmsx_ReadPidToArray(&cmsx_pidPitch, PID_PITCH);
112 cmsx_ReadPidToArray(&cmsx_pidYaw, PID_YAW);
114 return 0;
117 static long cmsx_PidOnEnter(const OSD_Entry *from)
119 UNUSED(from);
121 profileIndexString[1] = '0' + tmpProfileIndex;
122 cmsx_PidRead();
124 return 0;
127 static long cmsx_PidWriteback(const OSD_Entry *self)
129 UNUSED(self);
131 cmsx_WritebackPidFromArray(&cmsx_pidRoll, PID_ROLL);
132 cmsx_WritebackPidFromArray(&cmsx_pidPitch, PID_PITCH);
133 cmsx_WritebackPidFromArray(&cmsx_pidYaw, PID_YAW);
135 schedulePidGainsUpdate();
137 return 0;
140 static const OSD_Entry cmsx_menuEzTuneEntries[] =
142 OSD_LABEL_DATA_ENTRY("-- EZTUNE --", profileIndexString),
144 OSD_SETTING_ENTRY("ENABLED", SETTING_EZ_ENABLED),
145 OSD_SETTING_ENTRY("FILTER HZ", SETTING_EZ_FILTER_HZ),
146 OSD_SETTING_ENTRY("RATIO", SETTING_EZ_AXIS_RATIO),
147 OSD_SETTING_ENTRY("RESP.", SETTING_EZ_RESPONSE),
148 OSD_SETTING_ENTRY("DAMP.", SETTING_EZ_DAMPING),
149 OSD_SETTING_ENTRY("STAB.", SETTING_EZ_STABILITY),
150 OSD_SETTING_ENTRY("AGGR.", SETTING_EZ_AGGRESSIVENESS),
151 OSD_SETTING_ENTRY("RATE", SETTING_EZ_RATE),
152 OSD_SETTING_ENTRY("EXPO", SETTING_EZ_EXPO),
154 OSD_BACK_AND_END_ENTRY,
157 static const CMS_Menu cmsx_menuEzTune = {
158 .onEnter = NULL,
159 .onExit = NULL,
160 .onGlobalExit = NULL,
161 .entries = cmsx_menuEzTuneEntries
164 static const OSD_Entry cmsx_menuPidEntries[] =
166 OSD_LABEL_DATA_ENTRY("-- PID --", profileIndexString),
168 RPY_PIDFF_ENTRY("ROLL P", &cmsx_pidRoll.P),
169 RPY_PIDFF_ENTRY("ROLL I", &cmsx_pidRoll.I),
170 RPY_PIDFF_ENTRY("ROLL D", &cmsx_pidRoll.D),
171 RPY_PIDFF_ENTRY("ROLL FF", &cmsx_pidRoll.FF),
173 RPY_PIDFF_ENTRY("PITCH P", &cmsx_pidPitch.P),
174 RPY_PIDFF_ENTRY("PITCH I", &cmsx_pidPitch.I),
175 RPY_PIDFF_ENTRY("PITCH D", &cmsx_pidPitch.D),
176 RPY_PIDFF_ENTRY("PITCH FF", &cmsx_pidPitch.FF),
178 RPY_PIDFF_ENTRY("YAW P", &cmsx_pidYaw.P),
179 RPY_PIDFF_ENTRY("YAW I", &cmsx_pidYaw.I),
180 RPY_PIDFF_ENTRY("YAW D", &cmsx_pidYaw.D),
181 RPY_PIDFF_ENTRY("YAW FF", &cmsx_pidYaw.FF),
183 OSD_BACK_AND_END_ENTRY,
186 static const CMS_Menu cmsx_menuPid = {
187 #ifdef CMS_MENU_DEBUG
188 .GUARD_text = "XPID",
189 .GUARD_type = OME_MENU,
190 #endif
191 .onEnter = cmsx_PidOnEnter,
192 .onExit = cmsx_PidWriteback,
193 .onGlobalExit = NULL,
194 .entries = cmsx_menuPidEntries
197 static long cmsx_menuPidAltMag_onEnter(const OSD_Entry *from)
199 UNUSED(from);
201 cmsx_ReadPidToArray(&cmsx_pidPosZ, PID_POS_Z);
202 cmsx_ReadPidToArray(&cmsx_pidVelZ, PID_VEL_Z);
203 cmsx_ReadPidToArray(&cmsx_pidHead, PID_HEADING);
205 return 0;
208 static long cmsx_menuPidAltMag_onExit(const OSD_Entry *self)
210 UNUSED(self);
212 cmsx_WritebackPidFromArray(&cmsx_pidPosZ, PID_POS_Z);
213 cmsx_WritebackPidFromArray(&cmsx_pidVelZ, PID_VEL_Z);
214 cmsx_WritebackPidFromArray(&cmsx_pidHead, PID_HEADING);
216 navigationUsePIDs();
218 return 0;
221 static const OSD_Entry cmsx_menuPidAltMagEntries[] =
223 OSD_LABEL_DATA_ENTRY("-- ALT&MAG --", profileIndexString),
225 OSD_SETTING_ENTRY("FW ALT RESPONSE", SETTING_NAV_FW_ALT_CONTROL_RESPONSE),
227 OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P),
228 OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I),
229 OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D),
231 OTHER_PIDFF_ENTRY("VEL P", &cmsx_pidVelZ.P),
232 OTHER_PIDFF_ENTRY("VEL I", &cmsx_pidVelZ.I),
233 OTHER_PIDFF_ENTRY("VEL D", &cmsx_pidVelZ.D),
235 OTHER_PIDFF_ENTRY("MAG P", &cmsx_pidHead.P),
237 OSD_BACK_AND_END_ENTRY,
240 static const CMS_Menu cmsx_menuPidAltMag = {
241 #ifdef CMS_MENU_DEBUG
242 .GUARD_text = "XALTMAG",
243 .GUARD_type = OME_MENU,
244 #endif
245 .onEnter = cmsx_menuPidAltMag_onEnter,
246 .onExit = cmsx_menuPidAltMag_onExit,
247 .onGlobalExit = NULL,
248 .entries = cmsx_menuPidAltMagEntries,
251 static long cmsx_menuPidGpsnav_onEnter(const OSD_Entry *from)
253 UNUSED(from);
255 cmsx_ReadPidToArray(&cmsx_pidPosXY, PID_POS_XY);
256 cmsx_ReadPidToArray(&cmsx_pidVelXY, PID_VEL_XY);
258 return 0;
261 static long cmsx_menuPidGpsnav_onExit(const OSD_Entry *self)
263 UNUSED(self);
265 cmsx_WritebackPidFromArray(&cmsx_pidPosXY, PID_POS_XY);
266 cmsx_WritebackPidFromArray(&cmsx_pidVelXY, PID_VEL_XY);
268 navigationUsePIDs();
270 return 0;
273 static const OSD_Entry cmsx_menuPidGpsnavEntries[] =
275 OSD_LABEL_DATA_ENTRY("-- GPSNAV --", profileIndexString),
277 OTHER_PIDFF_ENTRY("POS P", &cmsx_pidPosXY.P),
278 OTHER_PIDFF_ENTRY("POS I", &cmsx_pidPosXY.I),
279 OTHER_PIDFF_ENTRY("POS D", &cmsx_pidPosXY.D),
281 OTHER_PIDFF_ENTRY("VEL P", &cmsx_pidVelXY.P),
282 OTHER_PIDFF_ENTRY("VEL I", &cmsx_pidVelXY.I),
283 OTHER_PIDFF_ENTRY("VEL D", &cmsx_pidVelXY.D),
284 OTHER_PIDFF_ENTRY("VEL FF", &cmsx_pidVelXY.FF),
286 OSD_BACK_AND_END_ENTRY,
289 static const CMS_Menu cmsx_menuPidGpsnav = {
290 #ifdef CMS_MENU_DEBUG
291 .GUARD_text = "XGPSNAV",
292 .GUARD_type = OME_MENU,
293 #endif
294 .onEnter = cmsx_menuPidGpsnav_onEnter,
295 .onExit = cmsx_menuPidGpsnav_onExit,
296 .onGlobalExit = NULL,
297 .entries = cmsx_menuPidGpsnavEntries,
301 // MANUAL Rate & Expo
303 static const OSD_Entry cmsx_menuManualRateProfileEntries[] =
305 OSD_LABEL_DATA_ENTRY("-- MANUAL RATE --", profileIndexString),
307 OSD_SETTING_ENTRY("MANU ROLL RATE", SETTING_MANUAL_ROLL_RATE),
308 OSD_SETTING_ENTRY("MANU PITCH RATE", SETTING_MANUAL_PITCH_RATE),
309 OSD_SETTING_ENTRY("MANU YAW RATE", SETTING_MANUAL_YAW_RATE),
311 OSD_SETTING_ENTRY("MANU RC EXPO", SETTING_MANUAL_RC_EXPO),
312 OSD_SETTING_ENTRY("MANU RC YAW EXP", SETTING_MANUAL_RC_YAW_EXPO),
314 OSD_BACK_AND_END_ENTRY,
317 static const CMS_Menu cmsx_menuManualRateProfile = {
318 #ifdef CMS_MENU_DEBUG
319 .GUARD_text = "MENUMANURATE",
320 .GUARD_type = OME_MENU,
321 #endif
322 .onEnter = NULL,
323 .onExit = NULL,
324 .onGlobalExit = NULL,
325 .entries = cmsx_menuManualRateProfileEntries
329 // Rate & Expo
331 static const OSD_Entry cmsx_menuRateProfileEntries[] =
333 OSD_LABEL_DATA_ENTRY("-- RATE --", profileIndexString),
335 OSD_SETTING_ENTRY_TYPE("ROLL RATE", SETTING_ROLL_RATE, CMS_DATA_TYPE_ANGULAR_RATE),
336 OSD_SETTING_ENTRY_TYPE("PITCH RATE", SETTING_PITCH_RATE, CMS_DATA_TYPE_ANGULAR_RATE),
337 OSD_SETTING_ENTRY_TYPE("YAW RATE", SETTING_YAW_RATE, CMS_DATA_TYPE_ANGULAR_RATE),
339 OSD_SETTING_ENTRY("RC EXPO", SETTING_RC_EXPO),
340 OSD_SETTING_ENTRY("RC YAW EXP", SETTING_RC_YAW_EXPO),
342 OSD_SETTING_ENTRY("THR MID", SETTING_THR_MID),
343 OSD_SETTING_ENTRY("THR EXPO", SETTING_THR_EXPO),
345 OSD_SETTING_ENTRY("THRPID ATT", SETTING_TPA_RATE),
346 OSD_SETTING_ENTRY_STEP("TPA BRKPT", SETTING_TPA_BREAKPOINT, 10),
348 OSD_BACK_AND_END_ENTRY,
351 static const CMS_Menu cmsx_menuRateProfile = {
352 #ifdef CMS_MENU_DEBUG
353 .GUARD_text = "MENURATE",
354 .GUARD_type = OME_MENU,
355 #endif
356 .onEnter = NULL,
357 .onExit = NULL,
358 .onGlobalExit = NULL,
359 .entries = cmsx_menuRateProfileEntries
362 #ifdef NOT_YET
363 static uint8_t cmsx_dtermSetpointWeight;
364 static uint8_t cmsx_setpointRelaxRatio;
365 static uint8_t cmsx_angleStrength;
366 static uint8_t cmsx_horizonStrength;
367 static uint8_t cmsx_horizonTransition;
369 static long cmsx_profileOtherOnEnter(void)
371 profileIndexString[1] = '0' + tmpProfileIndex;
373 cmsx_dtermSetpointWeight = pidProfile()->dtermSetpointWeight;
374 cmsx_setpointRelaxRatio = pidProfile()->setpointRelaxRatio;
376 cmsx_angleStrength = pidProfile()[PIDLEVEL].P;
377 cmsx_horizonStrength = pidProfile()[PIDLEVEL].I;
378 cmsx_horizonTransition = pidProfile()[PIDLEVEL].D;
380 return 0;
383 static long cmsx_profileOtherOnExit(const OSD_Entry *self)
385 UNUSED(self);
387 pidProfileMutable()->dtermSetpointWeight = cmsx_dtermSetpointWeight;
388 pidProfileMutable()->setpointRelaxRatio = cmsx_setpointRelaxRatio;
390 pidProfileMutable()[PIDLEVEL].P = cmsx_angleStrength;
391 pidProfileMutable()[PIDLEVEL].I = cmsx_horizonStrength;
392 pidProfileMutable()[PIDLEVEL].D = cmsx_horizonTransition;
394 return 0;
397 static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
398 { "-- OTHER PP --", OME_Label, NULL, profileIndexString, 0 },
400 { "D SETPT WT", OME_FLOAT, NULL, &(OSD_FLOAT_t){ &cmsx_dtermSetpointWeight, 0, 255, 1, 10 }, 0 },
401 { "SETPT TRS", OME_FLOAT, NULL, &(OSD_FLOAT_t){ &cmsx_setpointRelaxRatio, 0, 100, 1, 10 }, 0 },
402 { "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_angleStrength, 0, 200, 1 } , 0 },
403 { "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_horizonStrength, 0, 200, 1 } , 0 },
404 { "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_horizonTransition, 0, 200, 1 } , 0 },
406 OSD_BACK_AND_END_ENTRY,
409 static const CMS_Menu cmsx_menuProfileOther = {
410 #ifdef CMS_MENU_DEBUG
411 .GUARD_text = "XPROFOTHER",
412 .GUARD_type = OME_MENU,
413 #endif
414 .onEnter = cmsx_profileOtherOnEnter,
415 .onExit = cmsx_profileOtherOnExit,
416 .onGlobalExit = NULL,
417 .entries = cmsx_menuProfileOtherEntries,
419 #endif // NOT_YET
422 // Per profile filters
424 static const OSD_Entry cmsx_menuFilterPerProfileEntries[] =
426 OSD_LABEL_DATA_ENTRY("-- FILTERING --", profileIndexString),
427 OSD_SETTING_ENTRY("GYRO MAIN", SETTING_GYRO_MAIN_LPF_HZ),
428 OSD_SETTING_ENTRY("DTERM LPF", SETTING_DTERM_LPF_HZ),
429 #ifdef USE_DYNAMIC_FILTERS
430 OSD_SETTING_ENTRY("MATRIX FILTER", SETTING_DYNAMIC_GYRO_NOTCH_ENABLED),
431 OSD_SETTING_ENTRY("MATRIX MIN HZ", SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ), //dynamic_gyro_notch_min_hz
432 OSD_SETTING_ENTRY("MATRIX Q", SETTING_DYNAMIC_GYRO_NOTCH_Q), //dynamic_gyro_notch_q
433 #endif
434 #ifdef USE_GYRO_KALMAN
435 OSD_SETTING_ENTRY("UNICORN FILTER", SETTING_SETPOINT_KALMAN_ENABLED), //setpoint_kalman_enabled
436 OSD_SETTING_ENTRY("UNICORN Q", SETTING_SETPOINT_KALMAN_Q), //setpoint_kalman_q
437 #endif
438 OSD_BACK_AND_END_ENTRY,
441 static const CMS_Menu cmsx_menuFilterPerProfile = {
442 #ifdef CMS_MENU_DEBUG
443 .GUARD_text = "XFLTPP",
444 .GUARD_type = OME_MENU,
445 #endif
446 .onEnter = NULL,
447 .onExit = NULL,
448 .onGlobalExit = NULL,
449 .entries = cmsx_menuFilterPerProfileEntries,
452 static const OSD_Entry cmsx_menuMechanicsEntries[] =
454 OSD_LABEL_DATA_ENTRY("-- MECHANICS --", profileIndexString),
455 #ifdef USE_D_BOOST
456 OSD_SETTING_ENTRY("DBOOST_MIN", SETTING_D_BOOST_MIN),
457 OSD_SETTING_ENTRY("DBOOST_MAX", SETTING_D_BOOST_MAX),
458 #endif
459 #ifdef USE_ANTIGRAVITY
460 OSD_SETTING_ENTRY("ANTIGRAV. GAIN", SETTING_ANTIGRAVITY_GAIN),
461 #endif
462 OSD_SETTING_ENTRY("ITERM RELAX", SETTING_MC_ITERM_RELAX),
463 OSD_SETTING_ENTRY("ITERM CUTOFF", SETTING_MC_ITERM_RELAX_CUTOFF),
464 OSD_SETTING_ENTRY("CD LPF", SETTING_MC_CD_LPF_HZ),
466 OSD_BACK_AND_END_ENTRY,
469 static const CMS_Menu cmsx_menuMechanics = {
470 #ifdef CMS_MENU_DEBUG
471 .GUARD_text = "XGYROGLB",
472 .GUARD_type = OME_MENU,
473 #endif
474 .onEnter = NULL,
475 .onExit = NULL,
476 .onGlobalExit = NULL,
477 .entries = cmsx_menuMechanicsEntries,
480 static const OSD_Entry cmsx_menuImuEntries[] =
482 OSD_LABEL_ENTRY("-- PID TUNING --"),
484 // Profile dependent
485 OSD_UINT8_CALLBACK_ENTRY("PID PROF", cmsx_profileIndexOnChange, (&(const OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1})),
486 OSD_SUBMENU_ENTRY("EZTUNE", &cmsx_menuEzTune),
487 OSD_SUBMENU_ENTRY("PID", &cmsx_menuPid),
488 OSD_SUBMENU_ENTRY("PID ALTMAG", &cmsx_menuPidAltMag),
489 OSD_SUBMENU_ENTRY("PID GPSNAV", &cmsx_menuPidGpsnav),
490 OSD_SUBMENU_ENTRY("FILTERING", &cmsx_menuFilterPerProfile),
491 OSD_SUBMENU_ENTRY("MECHANICS", &cmsx_menuMechanics),
493 // Rate profile dependent
494 OSD_UINT8_CALLBACK_ENTRY("RATE PROF", cmsx_profileIndexOnChange, (&(const OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_CONTROL_RATE_PROFILE_COUNT, 1})),
495 OSD_SUBMENU_ENTRY("RATE", &cmsx_menuRateProfile),
496 OSD_SUBMENU_ENTRY("MANU RATE", &cmsx_menuManualRateProfile),
498 // Global
500 #ifdef NOT_YET
501 {"OTHER PP", OME_Submenu, cmsMenuChange, &cmsx_menuProfileOther, 0},
502 // Profile independent
503 {"FILT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0},
504 #endif
506 OSD_BACK_AND_END_ENTRY,
509 const CMS_Menu cmsx_menuImu = {
510 #ifdef CMS_MENU_DEBUG
511 .GUARD_text = "XIMU",
512 .GUARD_type = OME_MENU,
513 #endif
514 .onEnter = cmsx_menuImu_onEnter,
515 .onExit = NULL,
516 .onGlobalExit = NULL,
517 .entries = cmsx_menuImuEntries,
519 #endif // CMS