src/main/fc/rc_adjustments.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 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include "platform.h"
  23
  24 #include "blackbox/blackbox.h"
  25 #include "blackbox/blackbox_fielddefs.h"
  26
  27 #include "common/axis.h"
  28 #include "common/maths.h"
  29 #include "common/utils.h"
  30
  31 #include "config/feature.h"
  32 #include "config/parameter_group.h"
  33 #include "config/parameter_group_ids.h"
  34
  35 #include "drivers/time.h"
  36 #include "drivers/vtx_common.h"
  37
  38 #include "fc/config.h"
  39 #include "fc/controlrate_profile.h"
  40 #include "fc/rc_adjustments.h"
  41 #include "fc/rc_curves.h"
  42 #include "fc/settings.h"
  43
  44 #include "navigation/navigation.h"
  45 #include "navigation/navigation_private.h"
  46
  47 #include "flight/mixer.h"
  48 #include "flight/pid.h"
  49
  50 #include "io/beeper.h"
  51 #include "io/vtx.h"
  52
  53 #include "sensors/battery.h"
  54 #include "sensors/boardalignment.h"
  55
  56 #include "rx/rx.h"
  57
  58 PG_REGISTER_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges, PG_ADJUSTMENT_RANGE_CONFIG, 0);
  59
  60 static uint8_t adjustmentStateMask = 0;
  61
  62 #define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex)
  63 #define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex)
  64
  65 #define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex))
  66
  67 // sync with adjustmentFunction_e
  68 static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = {
  69     {
  70         .adjustmentFunction = ADJUSTMENT_RC_RATE,
  71         .mode = ADJUSTMENT_MODE_STEP,
  72         .data = { .stepConfig = { .step = 1 }}
  73     }, {
  74         .adjustmentFunction = ADJUSTMENT_RC_EXPO,
  75         .mode = ADJUSTMENT_MODE_STEP,
  76         .data = { .stepConfig = { .step = 1 }}
  77     }, {
  78         .adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO,
  79         .mode = ADJUSTMENT_MODE_STEP,
  80         .data = { .stepConfig = { .step = 1 }}
  81     }, {
  82         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE,
  83         .mode = ADJUSTMENT_MODE_STEP,
  84         .data = { .stepConfig = { .step = 1 }}
  85     }, {
  86         .adjustmentFunction = ADJUSTMENT_YAW_RATE,
  87         .mode = ADJUSTMENT_MODE_STEP,
  88         .data = { .stepConfig = { .step = 1 }}
  89     }, {
  90         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
  91         .mode = ADJUSTMENT_MODE_STEP,
  92         .data = { .stepConfig = { .step = 1 }}
  93     }, {
  94         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
  95         .mode = ADJUSTMENT_MODE_STEP,
  96         .data = { .stepConfig = { .step = 1 }}
  97     }, {
  98         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
  99         .mode = ADJUSTMENT_MODE_STEP,
 100         .data = { .stepConfig = { .step = 1 }}
 101     }, {
 102         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_FF,
 103         .mode = ADJUSTMENT_MODE_STEP,
 104         .data = { .stepConfig = { .step = 1 }}
 105     }, {
 106         .adjustmentFunction = ADJUSTMENT_PITCH_P,
 107         .mode = ADJUSTMENT_MODE_STEP,
 108         .data = { .stepConfig = { .step = 1 }}
 109     }, {
 110         .adjustmentFunction = ADJUSTMENT_PITCH_I,
 111         .mode = ADJUSTMENT_MODE_STEP,
 112         .data = { .stepConfig = { .step = 1 }}
 113     }, {
 114         .adjustmentFunction = ADJUSTMENT_PITCH_D,
 115         .mode = ADJUSTMENT_MODE_STEP,
 116         .data = { .stepConfig = { .step = 1 }}
 117     }, {
 118         .adjustmentFunction = ADJUSTMENT_PITCH_FF,
 119         .mode = ADJUSTMENT_MODE_STEP,
 120         .data = { .stepConfig = { .step = 1 }}
 121     }, {
 122         .adjustmentFunction = ADJUSTMENT_ROLL_P,
 123         .mode = ADJUSTMENT_MODE_STEP,
 124         .data = { .stepConfig = { .step = 1 }}
 125     }, {
 126         .adjustmentFunction = ADJUSTMENT_ROLL_I,
 127         .mode = ADJUSTMENT_MODE_STEP,
 128         .data = { .stepConfig = { .step = 1 }}
 129     }, {
 130         .adjustmentFunction = ADJUSTMENT_ROLL_D,
 131         .mode = ADJUSTMENT_MODE_STEP,
 132         .data = { .stepConfig = { .step = 1 }}
 133     }, {
 134         .adjustmentFunction = ADJUSTMENT_ROLL_FF,
 135         .mode = ADJUSTMENT_MODE_STEP,
 136         .data = { .stepConfig = { .step = 1 }}
 137     }, {
 138         .adjustmentFunction = ADJUSTMENT_YAW_P,
 139         .mode = ADJUSTMENT_MODE_STEP,
 140         .data = { .stepConfig = { .step = 1 }}
 141     }, {
 142         .adjustmentFunction = ADJUSTMENT_YAW_I,
 143         .mode = ADJUSTMENT_MODE_STEP,
 144         .data = { .stepConfig = { .step = 1 }}
 145     }, {
 146         .adjustmentFunction = ADJUSTMENT_YAW_D,
 147         .mode = ADJUSTMENT_MODE_STEP,
 148         .data = { .stepConfig = { .step = 1 }}
 149     }, {
 150         .adjustmentFunction = ADJUSTMENT_YAW_FF,
 151         .mode = ADJUSTMENT_MODE_STEP,
 152         .data = { .stepConfig = { .step = 1 }}
 153     }, {
 154         .adjustmentFunction = ADJUSTMENT_RATE_PROFILE,
 155         .mode = ADJUSTMENT_MODE_STEP,
 156         .data = { .stepConfig = { .step = 1 }}
 157     }, {
 158         .adjustmentFunction = ADJUSTMENT_PITCH_RATE,
 159         .mode = ADJUSTMENT_MODE_STEP,
 160         .data = { .stepConfig = { .step = 1 }}
 161     }, {
 162         .adjustmentFunction = ADJUSTMENT_ROLL_RATE,
 163         .mode = ADJUSTMENT_MODE_STEP,
 164         .data = { .stepConfig = { .step = 1 }}
 165     }, {
 166         .adjustmentFunction = ADJUSTMENT_RC_YAW_EXPO,
 167         .mode = ADJUSTMENT_MODE_STEP,
 168         .data = { .stepConfig = { .step = 1 }}
 169     }, {
 170         .adjustmentFunction = ADJUSTMENT_MANUAL_RC_EXPO,
 171         .mode = ADJUSTMENT_MODE_STEP,
 172         .data = { .stepConfig = { .step = 1 }}
 173     }, {
 174         .adjustmentFunction = ADJUSTMENT_MANUAL_RC_YAW_EXPO,
 175         .mode = ADJUSTMENT_MODE_STEP,
 176         .data = { .stepConfig = { .step = 1 }}
 177     }, {
 178         .adjustmentFunction = ADJUSTMENT_MANUAL_PITCH_ROLL_RATE,
 179         .mode = ADJUSTMENT_MODE_STEP,
 180         .data = { .stepConfig = { .step = 1 }}
 181     }, {
 182         .adjustmentFunction = ADJUSTMENT_MANUAL_ROLL_RATE,
 183         .mode = ADJUSTMENT_MODE_STEP,
 184         .data = { .stepConfig = { .step = 1 }}
 185     }, {
 186         .adjustmentFunction = ADJUSTMENT_MANUAL_PITCH_RATE,
 187         .mode = ADJUSTMENT_MODE_STEP,
 188         .data = { .stepConfig = { .step = 1 }}
 189     }, {
 190         .adjustmentFunction = ADJUSTMENT_MANUAL_YAW_RATE,
 191         .mode = ADJUSTMENT_MODE_STEP,
 192         .data = { .stepConfig = { .step = 1 }}
 193     }, {
 194         .adjustmentFunction = ADJUSTMENT_NAV_FW_CRUISE_THR,
 195         .mode = ADJUSTMENT_MODE_STEP,
 196         .data = { .stepConfig = { .step = 10 }}
 197     }, {
 198         .adjustmentFunction = ADJUSTMENT_NAV_FW_PITCH2THR,
 199         .mode = ADJUSTMENT_MODE_STEP,
 200         .data = { .stepConfig = { .step = 1 }}
 201     }, {
 202         .adjustmentFunction = ADJUSTMENT_ROLL_BOARD_ALIGNMENT,
 203         .mode = ADJUSTMENT_MODE_STEP,
 204         .data = { .stepConfig = { .step = 5 }}
 205     }, {
 206         .adjustmentFunction = ADJUSTMENT_PITCH_BOARD_ALIGNMENT,
 207         .mode = ADJUSTMENT_MODE_STEP,
 208         .data = { .stepConfig = { .step = 5 }}
 209     }, {
 210         .adjustmentFunction = ADJUSTMENT_LEVEL_P,
 211         .mode = ADJUSTMENT_MODE_STEP,
 212         .data = { .stepConfig = { .step = 1 }}
 213     }, {
 214         .adjustmentFunction = ADJUSTMENT_LEVEL_I,
 215         .mode = ADJUSTMENT_MODE_STEP,
 216         .data = { .stepConfig = { .step = 1 }}
 217     }, {
 218         .adjustmentFunction = ADJUSTMENT_LEVEL_D,
 219         .mode = ADJUSTMENT_MODE_STEP,
 220         .data = { .stepConfig = { .step = 1 }}
 221     }, {
 222         .adjustmentFunction = ADJUSTMENT_POS_XY_P,
 223         .mode = ADJUSTMENT_MODE_STEP,
 224         .data = { .stepConfig = { .step = 1 }}
 225     }, {
 226         .adjustmentFunction = ADJUSTMENT_POS_XY_I,
 227         .mode = ADJUSTMENT_MODE_STEP,
 228         .data = { .stepConfig = { .step = 1 }}
 229     }, {
 230         .adjustmentFunction = ADJUSTMENT_POS_XY_D,
 231         .mode = ADJUSTMENT_MODE_STEP,
 232         .data = { .stepConfig = { .step = 1 }}
 233     }, {
 234         .adjustmentFunction = ADJUSTMENT_POS_Z_P,
 235         .mode = ADJUSTMENT_MODE_STEP,
 236         .data = { .stepConfig = { .step = 1 }}
 237     }, {
 238         .adjustmentFunction = ADJUSTMENT_POS_Z_I,
 239         .mode = ADJUSTMENT_MODE_STEP,
 240         .data = { .stepConfig = { .step = 1 }}
 241     }, {
 242         .adjustmentFunction = ADJUSTMENT_POS_Z_D,
 243         .mode = ADJUSTMENT_MODE_STEP,
 244         .data = { .stepConfig = { .step = 1 }}
 245     }, {
 246         .adjustmentFunction = ADJUSTMENT_HEADING_P,
 247         .mode = ADJUSTMENT_MODE_STEP,
 248         .data = { .stepConfig = { .step = 1 }}
 249     }, {
 250         .adjustmentFunction = ADJUSTMENT_VEL_XY_P,
 251         .mode = ADJUSTMENT_MODE_STEP,
 252         .data = { .stepConfig = { .step = 1 }}
 253     }, {
 254         .adjustmentFunction = ADJUSTMENT_VEL_XY_I,
 255         .mode = ADJUSTMENT_MODE_STEP,
 256         .data = { .stepConfig = { .step = 1 }}
 257     }, {
 258         .adjustmentFunction = ADJUSTMENT_VEL_XY_D,
 259         .mode = ADJUSTMENT_MODE_STEP,
 260         .data = { .stepConfig = { .step = 1 }}
 261     }, {
 262         .adjustmentFunction = ADJUSTMENT_VEL_Z_P,
 263         .mode = ADJUSTMENT_MODE_STEP,
 264         .data = { .stepConfig = { .step = 1 }}
 265     }, {
 266         .adjustmentFunction = ADJUSTMENT_VEL_Z_I,
 267         .mode = ADJUSTMENT_MODE_STEP,
 268         .data = { .stepConfig = { .step = 1 }}
 269     }, {
 270         .adjustmentFunction = ADJUSTMENT_VEL_Z_D,
 271         .mode = ADJUSTMENT_MODE_STEP,
 272         .data = { .stepConfig = { .step = 1 }}
 273     }, {
 274         .adjustmentFunction = ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE,
 275         .mode = ADJUSTMENT_MODE_STEP,
 276         .data = { .stepConfig = { .step = 5 }}
 277     }, {
 278         .adjustmentFunction = ADJUSTMENT_VTX_POWER_LEVEL,
 279         .mode = ADJUSTMENT_MODE_STEP,
 280         .data = { .stepConfig = { .step = 1 }}
 281     }, {
 282         .adjustmentFunction = ADJUSTMENT_TPA,
 283         .mode = ADJUSTMENT_MODE_STEP,
 284         .data = { .stepConfig = { .step = 1 }}
 285     }, {
 286         .adjustmentFunction = ADJUSTMENT_TPA_BREAKPOINT,
 287         .mode = ADJUSTMENT_MODE_STEP,
 288         .data = { .stepConfig = { .step = 5 }}
 289     }, {
 290         .adjustmentFunction = ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS,
 291         .mode = ADJUSTMENT_MODE_STEP,
 292         .data = { .stepConfig = { .step = 1 }}
 293     }, {
 294         .adjustmentFunction = ADJUSTMENT_FW_TPA_TIME_CONSTANT,
 295         .mode = ADJUSTMENT_MODE_STEP,
 296         .data = { .stepConfig = { .step = 5 }}
 297     }, {
 298         .adjustmentFunction = ADJUSTMENT_FW_LEVEL_TRIM,
 299         .mode = ADJUSTMENT_MODE_STEP,
 300         .data = { .stepConfig = { .step = 1 }}
 301     }, {
 302         .adjustmentFunction = ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX,
 303         .mode = ADJUSTMENT_MODE_STEP,
 304         .data = { .stepConfig = { .step = 1 }}
 305     }
 306 };
 307
 308 #define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1
 309
 310 static adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
 311
 312 static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig)
 313 {
 314     adjustmentState_t * const adjustmentState = &adjustmentStates[index];
 315
 316     if (adjustmentState->config == adjustmentConfig) {
 317         // already configured
 318         return;
 319     }
 320     adjustmentState->auxChannelIndex = auxSwitchChannelIndex;
 321     adjustmentState->config = adjustmentConfig;
 322     adjustmentState->timeoutAt = 0;
 323
 324     MARK_ADJUSTMENT_FUNCTION_AS_READY(index);
 325 }
 326
 327 static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue)
 328 {
 329 #ifndef USE_BLACKBOX
 330     UNUSED(adjustmentFunction);
 331     UNUSED(newValue);
 332 #else
 333     if (feature(FEATURE_BLACKBOX)) {
 334         flightLogEvent_inflightAdjustment_t eventData;
 335         eventData.adjustmentFunction = adjustmentFunction;
 336         eventData.newValue = newValue;
 337         eventData.floatFlag = false;
 338         blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
 339     }
 340 #endif
 341 }
 342
 343 static void applyAdjustmentU8(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta, int low, int high)
 344 {
 345     int newValue = constrain((int)(*val) + delta, low, high);
 346     *val = newValue;
 347     blackboxLogInflightAdjustmentEvent(adjustmentFunction, newValue);
 348 }
 349
 350 static void applyAdjustmentU16(adjustmentFunction_e adjustmentFunction, uint16_t *val, int delta, int low, int high)
 351 {
 352     int newValue = constrain((int)(*val) + delta, low, high);
 353     *val = newValue;
 354     blackboxLogInflightAdjustmentEvent(adjustmentFunction, newValue);
 355 }
 356
 357 static void applyAdjustmentExpo(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta)
 358 {
 359     applyAdjustmentU8(adjustmentFunction, val, delta, SETTING_RC_EXPO_MIN, SETTING_RC_EXPO_MAX);
 360 }
 361
 362 static void applyAdjustmentManualRate(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta)
 363 {
 364     return applyAdjustmentU8(adjustmentFunction, val, delta, SETTING_CONSTANT_MANUAL_RATE_MIN, SETTING_CONSTANT_MANUAL_RATE_MAX);
 365 }
 366
 367 static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint16_t *val, int delta)
 368 {
 369     applyAdjustmentU16(adjustmentFunction, val, delta, SETTING_CONSTANT_RPYL_PID_MIN, SETTING_CONSTANT_RPYL_PID_MAX);
 370 }
 371
 372 static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
 373 {
 374     if (delta > 0) {
 375         beeperConfirmationBeeps(2);
 376     } else {
 377         beeperConfirmationBeeps(1);
 378     }
 379     switch (adjustmentFunction) {
 380         case ADJUSTMENT_RC_EXPO:
 381             applyAdjustmentExpo(ADJUSTMENT_RC_EXPO, &controlRateConfig->stabilized.rcExpo8, delta);
 382             break;
 383         case ADJUSTMENT_RC_YAW_EXPO:
 384             applyAdjustmentExpo(ADJUSTMENT_RC_YAW_EXPO, &controlRateConfig->stabilized.rcYawExpo8, delta);
 385             break;
 386         case ADJUSTMENT_MANUAL_RC_EXPO:
 387             applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_EXPO, &controlRateConfig->manual.rcExpo8, delta);
 388             break;
 389         case ADJUSTMENT_MANUAL_RC_YAW_EXPO:
 390             applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_YAW_EXPO, &controlRateConfig->manual.rcYawExpo8, delta);
 391             break;
 392         case ADJUSTMENT_THROTTLE_EXPO:
 393             applyAdjustmentExpo(ADJUSTMENT_THROTTLE_EXPO, &controlRateConfig->throttle.rcExpo8, delta);
 394             break;
 395         case ADJUSTMENT_PITCH_ROLL_RATE:
 396         case ADJUSTMENT_PITCH_RATE:
 397             applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlRateConfig->stabilized.rates[FD_PITCH], delta, SETTING_PITCH_RATE_MIN, SETTING_PITCH_RATE_MAX);
 398             if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
 399                 schedulePidGainsUpdate();
 400                 break;
 401             }
 402             // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
 403             FALLTHROUGH;
 404
 405         case ADJUSTMENT_ROLL_RATE:
 406             applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlRateConfig->stabilized.rates[FD_ROLL], delta, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
 407             schedulePidGainsUpdate();
 408             break;
 409         case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE:
 410         case ADJUSTMENT_MANUAL_ROLL_RATE:
 411             applyAdjustmentManualRate(ADJUSTMENT_MANUAL_ROLL_RATE, &controlRateConfig->manual.rates[FD_ROLL], delta);
 412             if (adjustmentFunction == ADJUSTMENT_MANUAL_ROLL_RATE)
 413                 break;
 414             // follow though for combined ADJUSTMENT_MANUAL_PITCH_ROLL_RATE
 415             FALLTHROUGH;
 416         case ADJUSTMENT_MANUAL_PITCH_RATE:
 417             applyAdjustmentManualRate(ADJUSTMENT_MANUAL_PITCH_RATE, &controlRateConfig->manual.rates[FD_PITCH], delta);
 418             break;
 419         case ADJUSTMENT_YAW_RATE:
 420             applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlRateConfig->stabilized.rates[FD_YAW], delta, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
 421             schedulePidGainsUpdate();
 422             break;
 423         case ADJUSTMENT_MANUAL_YAW_RATE:
 424             applyAdjustmentManualRate(ADJUSTMENT_MANUAL_YAW_RATE, &controlRateConfig->manual.rates[FD_YAW], delta);
 425             break;
 426         case ADJUSTMENT_PITCH_ROLL_P:
 427         case ADJUSTMENT_PITCH_P:
 428             applyAdjustmentPID(ADJUSTMENT_PITCH_P, &pidBankMutable()->pid[PID_PITCH].P, delta);
 429             if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
 430                 schedulePidGainsUpdate();
 431                 break;
 432             }
 433             // follow though for combined ADJUSTMENT_PITCH_ROLL_P
 434             FALLTHROUGH;
 435
 436         case ADJUSTMENT_ROLL_P:
 437             applyAdjustmentPID(ADJUSTMENT_ROLL_P, &pidBankMutable()->pid[PID_ROLL].P, delta);
 438             schedulePidGainsUpdate();
 439             break;
 440         case ADJUSTMENT_PITCH_ROLL_I:
 441         case ADJUSTMENT_PITCH_I:
 442             applyAdjustmentPID(ADJUSTMENT_PITCH_I, &pidBankMutable()->pid[PID_PITCH].I, delta);
 443             if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
 444                 schedulePidGainsUpdate();
 445                 break;
 446             }
 447             // follow though for combined ADJUSTMENT_PITCH_ROLL_I
 448             FALLTHROUGH;
 449
 450         case ADJUSTMENT_ROLL_I:
 451             applyAdjustmentPID(ADJUSTMENT_ROLL_I, &pidBankMutable()->pid[PID_ROLL].I, delta);
 452             schedulePidGainsUpdate();
 453             break;
 454         case ADJUSTMENT_PITCH_ROLL_D:
 455         case ADJUSTMENT_PITCH_D:
 456             applyAdjustmentPID(ADJUSTMENT_PITCH_D, &pidBankMutable()->pid[PID_PITCH].D, delta);
 457             if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
 458                 schedulePidGainsUpdate();
 459                 break;
 460             }
 461             // follow though for combined ADJUSTMENT_PITCH_ROLL_D
 462             FALLTHROUGH;
 463
 464         case ADJUSTMENT_ROLL_D:
 465             applyAdjustmentPID(ADJUSTMENT_ROLL_D, &pidBankMutable()->pid[PID_ROLL].D, delta);
 466             schedulePidGainsUpdate();
 467             break;
 468         case ADJUSTMENT_PITCH_ROLL_FF:
 469         case ADJUSTMENT_PITCH_FF:
 470             applyAdjustmentPID(ADJUSTMENT_PITCH_FF, &pidBankMutable()->pid[PID_PITCH].FF, delta);
 471             if (adjustmentFunction == ADJUSTMENT_PITCH_FF) {
 472                 schedulePidGainsUpdate();
 473                 break;
 474             }
 475             // follow though for combined ADJUSTMENT_PITCH_ROLL_FF
 476             FALLTHROUGH;
 477
 478         case ADJUSTMENT_ROLL_FF:
 479             applyAdjustmentPID(ADJUSTMENT_ROLL_FF, &pidBankMutable()->pid[PID_ROLL].FF, delta);
 480             schedulePidGainsUpdate();
 481             break;
 482         case ADJUSTMENT_YAW_P:
 483             applyAdjustmentPID(ADJUSTMENT_YAW_P, &pidBankMutable()->pid[PID_YAW].P, delta);
 484             schedulePidGainsUpdate();
 485             break;
 486         case ADJUSTMENT_YAW_I:
 487             applyAdjustmentPID(ADJUSTMENT_YAW_I, &pidBankMutable()->pid[PID_YAW].I, delta);
 488             schedulePidGainsUpdate();
 489             break;
 490         case ADJUSTMENT_YAW_D:
 491             applyAdjustmentPID(ADJUSTMENT_YAW_D, &pidBankMutable()->pid[PID_YAW].D, delta);
 492             schedulePidGainsUpdate();
 493             break;
 494         case ADJUSTMENT_YAW_FF:
 495             applyAdjustmentPID(ADJUSTMENT_YAW_FF, &pidBankMutable()->pid[PID_YAW].FF, delta);
 496             schedulePidGainsUpdate();
 497             break;
 498         case ADJUSTMENT_NAV_FW_CRUISE_THR:
 499             applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &currentBatteryProfileMutable->nav.fw.cruise_throttle, delta, SETTING_NAV_FW_CRUISE_THR_MIN, SETTING_NAV_FW_CRUISE_THR_MAX);
 500             break;
 501         case ADJUSTMENT_NAV_FW_PITCH2THR:
 502             applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &currentBatteryProfileMutable->nav.fw.pitch_to_throttle, delta, SETTING_NAV_FW_PITCH2THR_MIN, SETTING_NAV_FW_PITCH2THR_MAX);
 503             break;
 504         case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
 505             updateBoardAlignment(delta, 0);
 506             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_BOARD_ALIGNMENT, boardAlignment()->rollDeciDegrees);
 507             break;
 508         case ADJUSTMENT_PITCH_BOARD_ALIGNMENT:
 509             updateBoardAlignment(0, delta);
 510             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_BOARD_ALIGNMENT, boardAlignment()->pitchDeciDegrees);
 511             break;
 512         case ADJUSTMENT_LEVEL_P:
 513             applyAdjustmentPID(ADJUSTMENT_LEVEL_P, &pidBankMutable()->pid[PID_LEVEL].P, delta);
 514             // TODO: Need to call something to take it into account?
 515             break;
 516         case ADJUSTMENT_LEVEL_I:
 517             applyAdjustmentPID(ADJUSTMENT_LEVEL_I, &pidBankMutable()->pid[PID_LEVEL].I, delta);
 518             // TODO: Need to call something to take it into account?
 519             break;
 520         case ADJUSTMENT_LEVEL_D:
 521             applyAdjustmentPID(ADJUSTMENT_LEVEL_D, &pidBankMutable()->pid[PID_LEVEL].D, delta);
 522             // TODO: Need to call something to take it into account?
 523             break;
 524         case ADJUSTMENT_POS_XY_P:
 525             applyAdjustmentPID(ADJUSTMENT_POS_XY_P, &pidBankMutable()->pid[PID_POS_XY].P, delta);
 526             navigationUsePIDs();
 527             break;
 528         case ADJUSTMENT_POS_XY_I:
 529             applyAdjustmentPID(ADJUSTMENT_POS_XY_I, &pidBankMutable()->pid[PID_POS_XY].I, delta);
 530             navigationUsePIDs();
 531             break;
 532         case ADJUSTMENT_POS_XY_D:
 533             applyAdjustmentPID(ADJUSTMENT_POS_XY_D, &pidBankMutable()->pid[PID_POS_XY].D, delta);
 534             navigationUsePIDs();
 535             break;
 536         case ADJUSTMENT_POS_Z_P:
 537             applyAdjustmentPID(ADJUSTMENT_POS_Z_P, &pidBankMutable()->pid[PID_POS_Z].P, delta);
 538             navigationUsePIDs();
 539             break;
 540         case ADJUSTMENT_POS_Z_I:
 541             applyAdjustmentPID(ADJUSTMENT_POS_Z_I, &pidBankMutable()->pid[PID_POS_Z].I, delta);
 542             navigationUsePIDs();
 543             break;
 544         case ADJUSTMENT_POS_Z_D:
 545             applyAdjustmentPID(ADJUSTMENT_POS_Z_D, &pidBankMutable()->pid[PID_POS_Z].D, delta);
 546             navigationUsePIDs();
 547             break;
 548         case ADJUSTMENT_HEADING_P:
 549             applyAdjustmentPID(ADJUSTMENT_HEADING_P, &pidBankMutable()->pid[PID_HEADING].P, delta);
 550             // TODO: navigationUsePIDs()?
 551             break;
 552         case ADJUSTMENT_VEL_XY_P:
 553             applyAdjustmentPID(ADJUSTMENT_VEL_XY_P, &pidBankMutable()->pid[PID_VEL_XY].P, delta);
 554             navigationUsePIDs();
 555             break;
 556         case ADJUSTMENT_VEL_XY_I:
 557             applyAdjustmentPID(ADJUSTMENT_VEL_XY_I, &pidBankMutable()->pid[PID_VEL_XY].I, delta);
 558             navigationUsePIDs();
 559             break;
 560         case ADJUSTMENT_VEL_XY_D:
 561             applyAdjustmentPID(ADJUSTMENT_VEL_XY_D, &pidBankMutable()->pid[PID_VEL_XY].D, delta);
 562             navigationUsePIDs();
 563             break;
 564         case ADJUSTMENT_VEL_Z_P:
 565             applyAdjustmentPID(ADJUSTMENT_VEL_Z_P, &pidBankMutable()->pid[PID_VEL_Z].P, delta);
 566             navigationUsePIDs();
 567             break;
 568         case ADJUSTMENT_VEL_Z_I:
 569             applyAdjustmentPID(ADJUSTMENT_VEL_Z_I, &pidBankMutable()->pid[PID_VEL_Z].I, delta);
 570             navigationUsePIDs();
 571             break;
 572         case ADJUSTMENT_VEL_Z_D:
 573             applyAdjustmentPID(ADJUSTMENT_VEL_Z_D, &pidBankMutable()->pid[PID_VEL_Z].D, delta);
 574             navigationUsePIDs();
 575             break;
 576         case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
 577             applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &navConfigMutable()->fw.minThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX);
 578             break;
 579 #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
 580         case ADJUSTMENT_VTX_POWER_LEVEL:
 581             {
 582                 vtxDeviceCapability_t vtxDeviceCapability;
 583                 if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
 584                     applyAdjustmentU8(ADJUSTMENT_VTX_POWER_LEVEL, &vtxSettingsConfigMutable()->power, delta, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
 585                 }
 586             }
 587             break;
 588 #endif
 589         case ADJUSTMENT_TPA:
 590             applyAdjustmentU8(ADJUSTMENT_TPA, &controlRateConfig->throttle.dynPID, delta, 0, SETTING_TPA_RATE_MAX);
 591             break;
 592         case ADJUSTMENT_TPA_BREAKPOINT:
 593             applyAdjustmentU16(ADJUSTMENT_TPA_BREAKPOINT, &controlRateConfig->throttle.pa_breakpoint, delta, PWM_RANGE_MIN, PWM_RANGE_MAX);
 594             break;
 595         case ADJUSTMENT_FW_TPA_TIME_CONSTANT:
 596             applyAdjustmentU16(ADJUSTMENT_FW_TPA_TIME_CONSTANT, &controlRateConfig->throttle.fixedWingTauMs, delta, SETTING_FW_TPA_TIME_CONSTANT_MIN, SETTING_FW_TPA_TIME_CONSTANT_MAX);
 597             break;
 598         case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS:
 599             applyAdjustmentU8(ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, &navConfigMutable()->fw.control_smoothness, delta, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MIN, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MAX);
 600             break;
 601         case ADJUSTMENT_FW_LEVEL_TRIM:
 602         {
 603             float newValue = pidProfileMutable()->fixedWingLevelTrim + (delta / 10.0f);
 604             if (newValue > SETTING_FW_LEVEL_PITCH_TRIM_MAX) {newValue = (float)SETTING_FW_LEVEL_PITCH_TRIM_MAX;}
 605             else if (newValue < SETTING_FW_LEVEL_PITCH_TRIM_MIN) {newValue = (float)SETTING_FW_LEVEL_PITCH_TRIM_MIN;}
 606             pidProfileMutable()->fixedWingLevelTrim = newValue;
 607             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FW_LEVEL_TRIM, (int)(newValue * 10.0f));
 608             break;
 609         }
 610 #ifdef USE_MULTI_MISSION
 611         case ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX:
 612             if (posControl.multiMissionCount && !FLIGHT_MODE(NAV_WP_MODE)) {
 613                 applyAdjustmentU8(ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX, &navConfigMutable()->general.waypoint_multi_mission_index, delta, SETTING_NAV_WP_MULTI_MISSION_INDEX_MIN, posControl.multiMissionCount);
 614             }
 615             break;
 616 #endif
 617         default:
 618             break;
 619     };
 620 }
 621
 622 #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT
 623 static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
 624 {
 625     bool applied = false;
 626
 627     switch (adjustmentFunction) {
 628         case ADJUSTMENT_RATE_PROFILE:
 629             if (getCurrentControlRateProfile() != position) {
 630                 changeControlRateProfile(position);
 631                 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position);
 632                 applied = true;
 633             }
 634             break;
 635     }
 636
 637     if (applied) {
 638         beeperConfirmationBeeps(position + 1);
 639     }
 640 }
 641 #endif
 642
 643 #define RESET_FREQUENCY_2HZ (1000 / 2)
 644
 645 void processRcAdjustments(controlRateConfig_t *controlRateConfig, bool canUseRxData)
 646 {
 647     const uint32_t now = millis();
 648
 649     for (int adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
 650         adjustmentState_t * const adjustmentState = &adjustmentStates[adjustmentIndex];
 651
 652         if (!adjustmentState->config) {
 653             continue;
 654         }
 655         const uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction;
 656         if (adjustmentFunction == ADJUSTMENT_NONE) {
 657             continue;
 658         }
 659
 660         const int32_t signedDiff = now - adjustmentState->timeoutAt;
 661         const bool canResetReadyStates = signedDiff >= 0L;
 662
 663         if (canResetReadyStates) {
 664             adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
 665             MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
 666         }
 667
 668         if (!canUseRxData) {
 669             continue;
 670         }
 671
 672         const uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex;
 673
 674         if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
 675             int delta;
 676             if (rxGetChannelValue(channelIndex) > PWM_RANGE_MIDDLE + 200) {
 677                 delta = adjustmentState->config->data.stepConfig.step;
 678             } else if (rxGetChannelValue(channelIndex) < PWM_RANGE_MIDDLE - 200) {
 679                 delta = 0 - adjustmentState->config->data.stepConfig.step;
 680             } else {
 681                 // returning the switch to the middle immediately resets the ready state
 682                 MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
 683                 adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
 684                 continue;
 685             }
 686             if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) {
 687                 continue;
 688             }
 689
 690             // it is legitimate to adjust an otherwise const item here
 691             applyStepAdjustment(controlRateConfig, adjustmentFunction, delta);
 692 #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT
 693         } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
 694             const uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
 695             const uint8_t position = (constrain(rxGetChannelValue(channelIndex), 900, 2100 - 1) - 900) / rangeWidth;
 696
 697             applySelectAdjustment(adjustmentFunction, position);
 698 #endif
 699         }
 700         MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex);
 701     }
 702 }
 703
 704 void resetAdjustmentStates(void)
 705 {
 706     memset(adjustmentStates, 0, sizeof(adjustmentStates));
 707 }
 708
 709 void updateAdjustmentStates(bool canUseRxData)
 710 {
 711     for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
 712         const adjustmentRange_t * const adjustmentRange = adjustmentRanges(index);
 713         if (adjustmentRange->adjustmentFunction == ADJUSTMENT_NONE) {
 714             // Range not set up
 715             continue;
 716         }
 717         const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
 718         adjustmentState_t * const adjustmentState = &adjustmentStates[adjustmentRange->adjustmentIndex];
 719
 720         if (canUseRxData && isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) {
 721             if (!adjustmentState->config) {
 722                 configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig);
 723             }
 724         } else {
 725             if (adjustmentState->config == adjustmentConfig) {
 726                 adjustmentState->config = NULL;
 727             }
 728         }
 729     }
 730 }
 731
 732 bool isAdjustmentFunctionSelected(uint8_t adjustmentFunction) {
 733     for (uint8_t index = 0; index < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; ++index) {
 734         if (adjustmentStates[index].config && adjustmentStates[index].config->adjustmentFunction == adjustmentFunction) {
 735             return true;
 736         }
 737     }
 738     return false;
 739 }
 740
 741 uint8_t getActiveAdjustmentFunctions(uint8_t *adjustmentFunctions) {
 742     uint8_t adjustmentCount = 0;
 743     for (uint8_t i = 0; i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; i++) {
 744         if (adjustmentStates[i].config) {
 745             adjustmentCount++;
 746             adjustmentFunctions[i] = adjustmentStates[i].config->adjustmentFunction;
 747         }
 748     }
 749     return adjustmentCount;
 750 }