src/main/cms/cms_menu_imu.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   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.
   8  *
   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.
  13  *
  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/>.
  16  */
  17
  18 // Menu contents for PID, RATES, RC preview, misc
  19 // Should be part of the relevant .c file.
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_CMS
  28
  29 #include "common/utils.h"
  30
  31 #include "cms/cms.h"
  32 #include "cms/cms_types.h"
  33 #include "cms/cms_menu_imu.h"
  34
  35 #include "common/axis.h"
  36
  37 #include "flight/pid.h"
  38
  39 #include "fc/config.h"
  40 #include "fc/controlrate_profile.h"
  41 #include "fc/rc_controls.h"
  42 #include "fc/settings.h"
  43
  44 #include "navigation/navigation.h"
  45
  46 #include "sensors/gyro.h"
  47
  48
  49 //
  50 // PIDFF
  51 //
  52 #define PIDFF_MIN 0
  53 #define PIDFF_STEP 1
  54
  55 #define RPY_PIDFF_MAX 200
  56 #define OTHER_PIDDF_MAX 255
  57
  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)
  61
  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;
  70
  71 static uint8_t tmpProfileIndex;
  72 static uint8_t profileIndex;
  73 static char profileIndexString[] = " p";
  74
  75 static void cmsx_ReadPidToArray(pid8_t *dst, int pidIndex)
  76 {
  77     memcpy(dst, &pidBank()->pid[pidIndex], sizeof(*dst));
  78 }
  79
  80 static void cmsx_WritebackPidFromArray(const pid8_t *src, int pidIndex)
  81 {
  82     memcpy(&pidBankMutable()->pid[pidIndex], src, sizeof(*src));
  83 }
  84
  85 static long cmsx_menuImu_onEnter(const OSD_Entry *from)
  86 {
  87     UNUSED(from);
  88
  89     profileIndex = getConfigProfile();
  90     tmpProfileIndex = profileIndex + 1;
  91     profileIndexString[1] = '0' + tmpProfileIndex;
  92
  93     return 0;
  94 }
  95
  96 static long cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *ptr)
  97 {
  98     UNUSED(displayPort);
  99     UNUSED(ptr);
 100
 101     profileIndex = tmpProfileIndex - 1;
 102     profileIndexString[1] = '0' + tmpProfileIndex;
 103     setConfigProfile(profileIndex);
 104
 105     return 0;
 106 }
 107
 108 static long cmsx_PidRead(void)
 109 {
 110     cmsx_ReadPidToArray(&cmsx_pidRoll, PID_ROLL);
 111     cmsx_ReadPidToArray(&cmsx_pidPitch, PID_PITCH);
 112     cmsx_ReadPidToArray(&cmsx_pidYaw, PID_YAW);
 113
 114     return 0;
 115 }
 116
 117 static long cmsx_PidOnEnter(const OSD_Entry *from)
 118 {
 119     UNUSED(from);
 120
 121     profileIndexString[1] = '0' + tmpProfileIndex;
 122     cmsx_PidRead();
 123
 124     return 0;
 125 }
 126
 127 static long cmsx_PidWriteback(const OSD_Entry *self)
 128 {
 129     UNUSED(self);
 130
 131     cmsx_WritebackPidFromArray(&cmsx_pidRoll, PID_ROLL);
 132     cmsx_WritebackPidFromArray(&cmsx_pidPitch, PID_PITCH);
 133     cmsx_WritebackPidFromArray(&cmsx_pidYaw, PID_YAW);
 134
 135     schedulePidGainsUpdate();
 136
 137     return 0;
 138 }
 139
 140 static const OSD_Entry cmsx_menuPidEntries[] =
 141 {
 142     OSD_LABEL_DATA_ENTRY("-- PID --", profileIndexString),
 143
 144     RPY_PIDFF_ENTRY("ROLL  P", &cmsx_pidRoll.P),
 145     RPY_PIDFF_ENTRY("ROLL  I", &cmsx_pidRoll.I),
 146     RPY_PIDFF_ENTRY("ROLL  D", &cmsx_pidRoll.D),
 147     RPY_PIDFF_ENTRY("ROLL  FF", &cmsx_pidRoll.FF),
 148
 149     RPY_PIDFF_ENTRY("PITCH P", &cmsx_pidPitch.P),
 150     RPY_PIDFF_ENTRY("PITCH I", &cmsx_pidPitch.I),
 151     RPY_PIDFF_ENTRY("PITCH D", &cmsx_pidPitch.D),
 152     RPY_PIDFF_ENTRY("PITCH FF", &cmsx_pidPitch.FF),
 153
 154     RPY_PIDFF_ENTRY("YAW   P", &cmsx_pidYaw.P),
 155     RPY_PIDFF_ENTRY("YAW   I", &cmsx_pidYaw.I),
 156     RPY_PIDFF_ENTRY("YAW   D", &cmsx_pidYaw.D),
 157     RPY_PIDFF_ENTRY("YAW   FF", &cmsx_pidYaw.FF),
 158
 159     OSD_BACK_AND_END_ENTRY,
 160 };
 161
 162 static const CMS_Menu cmsx_menuPid = {
 163 #ifdef CMS_MENU_DEBUG
 164     .GUARD_text = "XPID",
 165     .GUARD_type = OME_MENU,
 166 #endif
 167     .onEnter = cmsx_PidOnEnter,
 168     .onExit = cmsx_PidWriteback,
 169     .onGlobalExit = NULL,
 170     .entries = cmsx_menuPidEntries
 171 };
 172
 173 static long cmsx_menuPidAltMag_onEnter(const OSD_Entry *from)
 174 {
 175     UNUSED(from);
 176
 177     cmsx_ReadPidToArray(&cmsx_pidPosZ, PID_POS_Z);
 178     cmsx_ReadPidToArray(&cmsx_pidVelZ, PID_VEL_Z);
 179     cmsx_ReadPidToArray(&cmsx_pidHead, PID_HEADING);
 180
 181     return 0;
 182 }
 183
 184 static long cmsx_menuPidAltMag_onExit(const OSD_Entry *self)
 185 {
 186     UNUSED(self);
 187
 188     cmsx_WritebackPidFromArray(&cmsx_pidPosZ, PID_POS_Z);
 189     cmsx_WritebackPidFromArray(&cmsx_pidVelZ, PID_VEL_Z);
 190     cmsx_WritebackPidFromArray(&cmsx_pidHead, PID_HEADING);
 191
 192     navigationUsePIDs();
 193
 194     return 0;
 195 }
 196
 197 static const OSD_Entry cmsx_menuPidAltMagEntries[] =
 198 {
 199     OSD_LABEL_DATA_ENTRY("-- ALT&MAG --", profileIndexString),
 200
 201     OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P),
 202     OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I),
 203     OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D),
 204
 205     OTHER_PIDFF_ENTRY("VEL P", &cmsx_pidVelZ.P),
 206     OTHER_PIDFF_ENTRY("VEL I", &cmsx_pidVelZ.I),
 207     OTHER_PIDFF_ENTRY("VEL D", &cmsx_pidVelZ.D),
 208
 209     OTHER_PIDFF_ENTRY("MAG P", &cmsx_pidHead.P),
 210
 211     OSD_BACK_AND_END_ENTRY,
 212 };
 213
 214 static const CMS_Menu cmsx_menuPidAltMag = {
 215 #ifdef CMS_MENU_DEBUG
 216     .GUARD_text = "XALTMAG",
 217     .GUARD_type = OME_MENU,
 218 #endif
 219     .onEnter = cmsx_menuPidAltMag_onEnter,
 220     .onExit = cmsx_menuPidAltMag_onExit,
 221     .onGlobalExit = NULL,
 222     .entries = cmsx_menuPidAltMagEntries,
 223 };
 224
 225 static long cmsx_menuPidGpsnav_onEnter(const OSD_Entry *from)
 226 {
 227     UNUSED(from);
 228
 229     cmsx_ReadPidToArray(&cmsx_pidPosXY, PID_POS_XY);
 230     cmsx_ReadPidToArray(&cmsx_pidVelXY, PID_VEL_XY);
 231
 232     return 0;
 233 }
 234
 235 static long cmsx_menuPidGpsnav_onExit(const OSD_Entry *self)
 236 {
 237     UNUSED(self);
 238
 239     cmsx_WritebackPidFromArray(&cmsx_pidPosXY, PID_POS_XY);
 240     cmsx_WritebackPidFromArray(&cmsx_pidVelXY, PID_VEL_XY);
 241
 242     navigationUsePIDs();
 243
 244     return 0;
 245 }
 246
 247 static const OSD_Entry cmsx_menuPidGpsnavEntries[] =
 248 {
 249     OSD_LABEL_DATA_ENTRY("-- GPSNAV --", profileIndexString),
 250
 251     OTHER_PIDFF_ENTRY("POS P", &cmsx_pidPosXY.P),
 252     OTHER_PIDFF_ENTRY("POS I", &cmsx_pidPosXY.I),
 253     OTHER_PIDFF_ENTRY("POS D", &cmsx_pidPosXY.D),
 254
 255     OTHER_PIDFF_ENTRY("VEL P", &cmsx_pidVelXY.P),
 256     OTHER_PIDFF_ENTRY("VEL I", &cmsx_pidVelXY.I),
 257     OTHER_PIDFF_ENTRY("VEL D", &cmsx_pidVelXY.D),
 258     OTHER_PIDFF_ENTRY("VEL FF", &cmsx_pidVelXY.FF),
 259
 260     OSD_BACK_AND_END_ENTRY,
 261 };
 262
 263 static const CMS_Menu cmsx_menuPidGpsnav = {
 264 #ifdef CMS_MENU_DEBUG
 265     .GUARD_text = "XGPSNAV",
 266     .GUARD_type = OME_MENU,
 267 #endif
 268     .onEnter = cmsx_menuPidGpsnav_onEnter,
 269     .onExit = cmsx_menuPidGpsnav_onExit,
 270     .onGlobalExit = NULL,
 271     .entries = cmsx_menuPidGpsnavEntries,
 272 };
 273
 274 //
 275 // MANUAL Rate & Expo
 276 //
 277 static const OSD_Entry cmsx_menuManualRateProfileEntries[] =
 278 {
 279     OSD_LABEL_DATA_ENTRY("-- MANUAL RATE --", profileIndexString),
 280
 281     OSD_SETTING_ENTRY("MANU ROLL RATE", SETTING_MANUAL_ROLL_RATE),
 282     OSD_SETTING_ENTRY("MANU PITCH RATE", SETTING_MANUAL_PITCH_RATE),
 283     OSD_SETTING_ENTRY("MANU YAW RATE", SETTING_MANUAL_YAW_RATE),
 284
 285     OSD_SETTING_ENTRY("MANU RC EXPO", SETTING_MANUAL_RC_EXPO),
 286     OSD_SETTING_ENTRY("MANU RC YAW EXP", SETTING_MANUAL_RC_YAW_EXPO),
 287
 288     OSD_BACK_AND_END_ENTRY,
 289 };
 290
 291 static const CMS_Menu cmsx_menuManualRateProfile = {
 292 #ifdef CMS_MENU_DEBUG
 293     .GUARD_text = "MENUMANURATE",
 294     .GUARD_type = OME_MENU,
 295 #endif
 296     .onEnter = NULL,
 297     .onExit = NULL,
 298     .onGlobalExit = NULL,
 299     .entries = cmsx_menuManualRateProfileEntries
 300 };
 301
 302 //
 303 // Rate & Expo
 304 //
 305 static const OSD_Entry cmsx_menuRateProfileEntries[] =
 306 {
 307     OSD_LABEL_DATA_ENTRY("-- RATE --", profileIndexString),
 308
 309     OSD_SETTING_ENTRY_TYPE("ROLL RATE", SETTING_ROLL_RATE, CMS_DATA_TYPE_ANGULAR_RATE),
 310     OSD_SETTING_ENTRY_TYPE("PITCH RATE", SETTING_PITCH_RATE, CMS_DATA_TYPE_ANGULAR_RATE),
 311     OSD_SETTING_ENTRY_TYPE("YAW RATE", SETTING_YAW_RATE, CMS_DATA_TYPE_ANGULAR_RATE),
 312
 313     OSD_SETTING_ENTRY("RC EXPO", SETTING_RC_EXPO),
 314     OSD_SETTING_ENTRY("RC YAW EXP", SETTING_RC_YAW_EXPO),
 315
 316     OSD_SETTING_ENTRY("THR MID", SETTING_THR_MID),
 317     OSD_SETTING_ENTRY("THR EXPO", SETTING_THR_EXPO),
 318
 319     OSD_SETTING_ENTRY("THRPID ATT", SETTING_TPA_RATE),
 320     OSD_SETTING_ENTRY_STEP("TPA BRKPT", SETTING_TPA_BREAKPOINT, 10),
 321
 322     OSD_BACK_AND_END_ENTRY,
 323 };
 324
 325 static const CMS_Menu cmsx_menuRateProfile = {
 326 #ifdef CMS_MENU_DEBUG
 327     .GUARD_text = "MENURATE",
 328     .GUARD_type = OME_MENU,
 329 #endif
 330     .onEnter = NULL,
 331     .onExit = NULL,
 332     .onGlobalExit = NULL,
 333     .entries = cmsx_menuRateProfileEntries
 334 };
 335
 336 #ifdef NOT_YET
 337 static uint8_t cmsx_dtermSetpointWeight;
 338 static uint8_t cmsx_setpointRelaxRatio;
 339 static uint8_t cmsx_angleStrength;
 340 static uint8_t cmsx_horizonStrength;
 341 static uint8_t cmsx_horizonTransition;
 342
 343 static long cmsx_profileOtherOnEnter(void)
 344 {
 345     profileIndexString[1] = '0' + tmpProfileIndex;
 346
 347     cmsx_dtermSetpointWeight = pidProfile()->dtermSetpointWeight;
 348     cmsx_setpointRelaxRatio  = pidProfile()->setpointRelaxRatio;
 349
 350     cmsx_angleStrength       = pidProfile()[PIDLEVEL].P;
 351     cmsx_horizonStrength     = pidProfile()[PIDLEVEL].I;
 352     cmsx_horizonTransition   = pidProfile()[PIDLEVEL].D;
 353
 354     return 0;
 355 }
 356
 357 static long cmsx_profileOtherOnExit(const OSD_Entry *self)
 358 {
 359     UNUSED(self);
 360
 361     pidProfileMutable()->dtermSetpointWeight = cmsx_dtermSetpointWeight;
 362     pidProfileMutable()->setpointRelaxRatio  = cmsx_setpointRelaxRatio;
 363
 364     pidProfileMutable()[PIDLEVEL].P        = cmsx_angleStrength;
 365     pidProfileMutable()[PIDLEVEL].I        = cmsx_horizonStrength;
 366     pidProfileMutable()[PIDLEVEL].D        = cmsx_horizonTransition;
 367
 368     return 0;
 369 }
 370
 371 static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
 372     { "-- OTHER PP --", OME_Label, NULL, profileIndexString, 0 },
 373
 374     { "D SETPT WT",  OME_FLOAT, NULL, &(OSD_FLOAT_t){ &cmsx_dtermSetpointWeight, 0, 255, 1, 10 }, 0 },
 375     { "SETPT TRS",   OME_FLOAT, NULL, &(OSD_FLOAT_t){ &cmsx_setpointRelaxRatio,  0, 100, 1, 10 }, 0 },
 376     { "ANGLE STR",   OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_angleStrength,       0, 200, 1 }    , 0 },
 377     { "HORZN STR",   OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_horizonStrength,     0, 200, 1 }    , 0 },
 378     { "HORZN TRS",   OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_horizonTransition,   0, 200, 1 }    , 0 },
 379
 380     OSD_BACK_AND_END_ENTRY,
 381 };
 382
 383 static const CMS_Menu cmsx_menuProfileOther = {
 384 #ifdef CMS_MENU_DEBUG
 385     .GUARD_text = "XPROFOTHER",
 386     .GUARD_type = OME_MENU,
 387 #endif
 388     .onEnter = cmsx_profileOtherOnEnter,
 389     .onExit = cmsx_profileOtherOnExit,
 390     .onGlobalExit = NULL,
 391     .entries = cmsx_menuProfileOtherEntries,
 392 };
 393 #endif // NOT_YET
 394
 395 //
 396 // Per profile filters
 397 //
 398 static const OSD_Entry cmsx_menuFilterPerProfileEntries[] =
 399 {
 400     OSD_LABEL_DATA_ENTRY("-- FILTERING  --", profileIndexString),
 401     OSD_SETTING_ENTRY("HARDWARE LPF", SETTING_GYRO_HARDWARE_LPF),
 402     OSD_SETTING_ENTRY("GYRO MAIN", SETTING_GYRO_MAIN_LPF_HZ),
 403     OSD_SETTING_ENTRY("DTERM LPF", SETTING_DTERM_LPF_HZ),
 404     OSD_SETTING_ENTRY("DTERM LPF2", SETTING_DTERM_LPF2_HZ),
 405 #ifdef USE_DYNAMIC_FILTERS
 406     OSD_SETTING_ENTRY("MATRIX FILTER", SETTING_DYNAMIC_GYRO_NOTCH_ENABLED),
 407     OSD_SETTING_ENTRY("MATRIX MIN HZ", SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ),  //dynamic_gyro_notch_min_hz
 408     OSD_SETTING_ENTRY("MATRIX Q", SETTING_DYNAMIC_GYRO_NOTCH_Q),            //dynamic_gyro_notch_q
 409 #endif
 410 #ifdef USE_GYRO_KALMAN
 411     OSD_SETTING_ENTRY("UNICORN FILTER", SETTING_SETPOINT_KALMAN_ENABLED),   //setpoint_kalman_enabled
 412     OSD_SETTING_ENTRY("UNICORN Q", SETTING_SETPOINT_KALMAN_Q),              //setpoint_kalman_q
 413 #endif
 414     OSD_BACK_AND_END_ENTRY,
 415 };
 416
 417 static const CMS_Menu cmsx_menuFilterPerProfile = {
 418 #ifdef CMS_MENU_DEBUG
 419     .GUARD_text = "XFLTPP",
 420     .GUARD_type = OME_MENU,
 421 #endif
 422     .onEnter = NULL,
 423     .onExit = NULL,
 424     .onGlobalExit = NULL,
 425     .entries = cmsx_menuFilterPerProfileEntries,
 426 };
 427
 428 static const OSD_Entry cmsx_menuMechanicsEntries[] =
 429 {
 430     OSD_LABEL_DATA_ENTRY("-- MECHANICS --", profileIndexString),
 431 #ifdef USE_D_BOOST
 432     OSD_SETTING_ENTRY("DBOOST_MIN", SETTING_D_BOOST_MIN),
 433     OSD_SETTING_ENTRY("DBOOST_MAX", SETTING_D_BOOST_MAX),
 434 #endif
 435 #ifdef USE_ANTIGRAVITY
 436     OSD_SETTING_ENTRY("ANTIGRAV. GAIN", SETTING_ANTIGRAVITY_GAIN),
 437 #endif
 438     OSD_SETTING_ENTRY("ITERM RELAX", SETTING_MC_ITERM_RELAX),
 439     OSD_SETTING_ENTRY("ITERM CUTOFF", SETTING_MC_ITERM_RELAX_CUTOFF),
 440     OSD_SETTING_ENTRY("CD LPF", SETTING_MC_CD_LPF_HZ),
 441
 442     OSD_BACK_AND_END_ENTRY,
 443 };
 444
 445 static const CMS_Menu cmsx_menuMechanics = {
 446 #ifdef CMS_MENU_DEBUG
 447     .GUARD_text = "XGYROGLB",
 448     .GUARD_type = OME_MENU,
 449 #endif
 450     .onEnter = NULL,
 451     .onExit = NULL,
 452     .onGlobalExit = NULL,
 453     .entries = cmsx_menuMechanicsEntries,
 454 };
 455
 456 static const OSD_Entry cmsx_menuImuEntries[] =
 457 {
 458     OSD_LABEL_ENTRY("-- PID TUNING --"),
 459
 460     // Profile dependent
 461     OSD_UINT8_CALLBACK_ENTRY("PID PROF", cmsx_profileIndexOnChange, (&(const OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_PROFILE_COUNT, 1})),
 462     OSD_SUBMENU_ENTRY("PID", &cmsx_menuPid),
 463     OSD_SUBMENU_ENTRY("PID ALTMAG", &cmsx_menuPidAltMag),
 464     OSD_SUBMENU_ENTRY("PID GPSNAV", &cmsx_menuPidGpsnav),
 465     OSD_SUBMENU_ENTRY("FILTERING", &cmsx_menuFilterPerProfile),
 466     OSD_SUBMENU_ENTRY("MECHANICS",  &cmsx_menuMechanics),
 467
 468     // Rate profile dependent
 469     OSD_UINT8_CALLBACK_ENTRY("RATE PROF", cmsx_profileIndexOnChange, (&(const OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_CONTROL_RATE_PROFILE_COUNT, 1})),
 470     OSD_SUBMENU_ENTRY("RATE", &cmsx_menuRateProfile),
 471     OSD_SUBMENU_ENTRY("MANU RATE", &cmsx_menuManualRateProfile),
 472
 473     // Global
 474
 475 #ifdef NOT_YET
 476     {"OTHER PP",  OME_Submenu, cmsMenuChange,                 &cmsx_menuProfileOther,                                      0},
 477     // Profile independent
 478     {"FILT GLB",  OME_Submenu, cmsMenuChange,                 &cmsx_menuFilterGlobal,                                      0},
 479 #endif
 480
 481     OSD_BACK_AND_END_ENTRY,
 482 };
 483
 484 const CMS_Menu cmsx_menuImu = {
 485 #ifdef CMS_MENU_DEBUG
 486     .GUARD_text = "XIMU",
 487     .GUARD_type = OME_MENU,
 488 #endif
 489     .onEnter = cmsx_menuImu_onEnter,
 490     .onExit = NULL,
 491     .onGlobalExit = NULL,
 492     .entries = cmsx_menuImuEntries,
 493 };
 494 #endif // CMS