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.
29 #include "common/utils.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"
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
)
89 profileIndex
= getConfigProfile();
90 tmpProfileIndex
= profileIndex
+ 1;
91 profileIndexString
[1] = '0' + tmpProfileIndex
;
96 static long cmsx_profileIndexOnChange(displayPort_t
*displayPort
, const void *ptr
)
101 profileIndex
= tmpProfileIndex
- 1;
102 profileIndexString
[1] = '0' + tmpProfileIndex
;
103 setConfigProfile(profileIndex
);
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
);
117 static long cmsx_PidOnEnter(const OSD_Entry
*from
)
121 profileIndexString
[1] = '0' + tmpProfileIndex
;
127 static long cmsx_PidWriteback(const OSD_Entry
*self
)
131 cmsx_WritebackPidFromArray(&cmsx_pidRoll
, PID_ROLL
);
132 cmsx_WritebackPidFromArray(&cmsx_pidPitch
, PID_PITCH
);
133 cmsx_WritebackPidFromArray(&cmsx_pidYaw
, PID_YAW
);
135 schedulePidGainsUpdate();
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
= {
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
,
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
)
201 cmsx_ReadPidToArray(&cmsx_pidPosZ
, PID_POS_Z
);
202 cmsx_ReadPidToArray(&cmsx_pidVelZ
, PID_VEL_Z
);
203 cmsx_ReadPidToArray(&cmsx_pidHead
, PID_HEADING
);
208 static long cmsx_menuPidAltMag_onExit(const OSD_Entry
*self
)
212 cmsx_WritebackPidFromArray(&cmsx_pidPosZ
, PID_POS_Z
);
213 cmsx_WritebackPidFromArray(&cmsx_pidVelZ
, PID_VEL_Z
);
214 cmsx_WritebackPidFromArray(&cmsx_pidHead
, PID_HEADING
);
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
,
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
)
255 cmsx_ReadPidToArray(&cmsx_pidPosXY
, PID_POS_XY
);
256 cmsx_ReadPidToArray(&cmsx_pidVelXY
, PID_VEL_XY
);
261 static long cmsx_menuPidGpsnav_onExit(const OSD_Entry
*self
)
265 cmsx_WritebackPidFromArray(&cmsx_pidPosXY
, PID_POS_XY
);
266 cmsx_WritebackPidFromArray(&cmsx_pidVelXY
, PID_VEL_XY
);
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
,
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
,
324 .onGlobalExit
= NULL
,
325 .entries
= cmsx_menuManualRateProfileEntries
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
,
358 .onGlobalExit
= NULL
,
359 .entries
= cmsx_menuRateProfileEntries
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
;
383 static long cmsx_profileOtherOnExit(const OSD_Entry
*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
;
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
,
414 .onEnter
= cmsx_profileOtherOnEnter
,
415 .onExit
= cmsx_profileOtherOnExit
,
416 .onGlobalExit
= NULL
,
417 .entries
= cmsx_menuProfileOtherEntries
,
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
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
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
,
448 .onGlobalExit
= NULL
,
449 .entries
= cmsx_menuFilterPerProfileEntries
,
452 static const OSD_Entry cmsx_menuMechanicsEntries
[] =
454 OSD_LABEL_DATA_ENTRY("-- MECHANICS --", profileIndexString
),
456 OSD_SETTING_ENTRY("DBOOST_MIN", SETTING_D_BOOST_MIN
),
457 OSD_SETTING_ENTRY("DBOOST_MAX", SETTING_D_BOOST_MAX
),
459 #ifdef USE_ANTIGRAVITY
460 OSD_SETTING_ENTRY("ANTIGRAV. GAIN", SETTING_ANTIGRAVITY_GAIN
),
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
,
476 .onGlobalExit
= NULL
,
477 .entries
= cmsx_menuMechanicsEntries
,
480 static const OSD_Entry cmsx_menuImuEntries
[] =
482 OSD_LABEL_ENTRY("-- PID TUNING --"),
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
),
501 {"OTHER PP", OME_Submenu
, cmsMenuChange
, &cmsx_menuProfileOther
, 0},
502 // Profile independent
503 {"FILT GLB", OME_Submenu
, cmsMenuChange
, &cmsx_menuFilterGlobal
, 0},
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
,
514 .onEnter
= cmsx_menuImu_onEnter
,
516 .onGlobalExit
= NULL
,
517 .entries
= cmsx_menuImuEntries
,