src/main/fc/rc_adjustments.c

   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 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24
  25 #include <math.h>
  26
  27 #include "platform.h"
  28
  29 #include "blackbox/blackbox.h"
  30 #include "blackbox/blackbox_fielddefs.h"
  31
  32 #include "build/build_config.h"
  33
  34 #include "common/axis.h"
  35 #include "common/maths.h"
  36 #include "common/utils.h"
  37
  38 #include "config/feature.h"
  39
  40 #include "drivers/time.h"
  41
  42 #include "config/config.h"
  43 #include "fc/controlrate_profile.h"
  44 #include "fc/rc_controls.h"
  45 #include "fc/rc.h"
  46
  47 #include "flight/pid.h"
  48 #include "flight/pid_init.h"
  49
  50 #include "io/beeper.h"
  51 #include "io/ledstrip.h"
  52 #include "io/pidaudio.h"
  53
  54 #include "osd/osd.h"
  55
  56 #include "pg/pg.h"
  57 #include "pg/pg_ids.h"
  58 #include "pg/rx.h"
  59
  60 #include "rx/rx.h"
  61
  62 #include "rc_adjustments.h"
  63
  64 #define ADJUSTMENT_RANGE_COUNT_INVALID -1
  65
  66 PG_REGISTER_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges, PG_ADJUSTMENT_RANGE_CONFIG, 2);
  67
  68 uint8_t pidAudioPositionToModeMap[7] = {
  69     // on a pot with a center detent, it's easy to have center area for off/default, then three positions to the left and three to the right.
  70     // current implementation yields RC values as below.
  71
  72     PID_AUDIO_PIDSUM_X,     //   900 - ~1071 - Min
  73     PID_AUDIO_PIDSUM_Y,     // ~1071 - ~1242
  74     PID_AUDIO_PIDSUM_XY,    // ~1242 - ~1414
  75     PID_AUDIO_OFF,          // ~1414 - ~1585 - Center
  76     PID_AUDIO_OFF,          // ~1585 - ~1757
  77     PID_AUDIO_OFF,          // ~1757 - ~1928
  78     PID_AUDIO_OFF,          // ~1928 -  2100 - Max
  79
  80     // Note: Last 3 positions are currently pending implementations and use PID_AUDIO_OFF for now.
  81 };
  82
  83 STATIC_UNIT_TESTED int stepwiseAdjustmentCount = ADJUSTMENT_RANGE_COUNT_INVALID;
  84 STATIC_UNIT_TESTED timedAdjustmentState_t stepwiseAdjustments[MAX_ADJUSTMENT_RANGE_COUNT];
  85
  86 STATIC_UNIT_TESTED int continuosAdjustmentCount;
  87 STATIC_UNIT_TESTED continuosAdjustmentState_t continuosAdjustments[MAX_ADJUSTMENT_RANGE_COUNT];
  88
  89 static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue)
  90 {
  91 #ifndef USE_BLACKBOX
  92     UNUSED(adjustmentFunction);
  93     UNUSED(newValue);
  94 #else
  95     if (blackboxConfig()->device) {
  96         flightLogEvent_inflightAdjustment_t eventData;
  97         eventData.adjustmentFunction = adjustmentFunction;
  98         eventData.newValue = newValue;
  99         eventData.floatFlag = false;
 100         blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
 101     }
 102 #endif
 103 }
 104
 105 // sync with adjustmentFunction_e
 106 static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = {
 107     {
 108         .adjustmentFunction = ADJUSTMENT_RC_RATE,
 109         .mode = ADJUSTMENT_MODE_STEP,
 110         .data = { .step = 1 }
 111     }, {
 112         .adjustmentFunction = ADJUSTMENT_RC_EXPO,
 113         .mode = ADJUSTMENT_MODE_STEP,
 114         .data = { .step = 1 }
 115     }, {
 116         .adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO,
 117         .mode = ADJUSTMENT_MODE_STEP,
 118         .data = { .step = 1 }
 119     }, {
 120         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE,
 121         .mode = ADJUSTMENT_MODE_STEP,
 122         .data = { .step = 1 }
 123     }, {
 124         .adjustmentFunction = ADJUSTMENT_YAW_RATE,
 125         .mode = ADJUSTMENT_MODE_STEP,
 126         .data = { .step = 1 }
 127     }, {
 128         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
 129         .mode = ADJUSTMENT_MODE_STEP,
 130         .data = { .step = 1 }
 131     }, {
 132         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
 133         .mode = ADJUSTMENT_MODE_STEP,
 134         .data = { .step = 1 }
 135     }, {
 136         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
 137         .mode = ADJUSTMENT_MODE_STEP,
 138         .data = { .step = 1 }
 139     }, {
 140         .adjustmentFunction = ADJUSTMENT_YAW_P,
 141         .mode = ADJUSTMENT_MODE_STEP,
 142         .data = { .step = 1 }
 143     }, {
 144         .adjustmentFunction = ADJUSTMENT_YAW_I,
 145         .mode = ADJUSTMENT_MODE_STEP,
 146         .data = { .step = 1 }
 147     }, {
 148         .adjustmentFunction = ADJUSTMENT_YAW_D,
 149         .mode = ADJUSTMENT_MODE_STEP,
 150         .data = { .step = 1 }
 151     }, {
 152         .adjustmentFunction = ADJUSTMENT_RATE_PROFILE,
 153         .mode = ADJUSTMENT_MODE_SELECT,
 154         .data = { .switchPositions = 3 }
 155     }, {
 156         .adjustmentFunction = ADJUSTMENT_PITCH_RATE,
 157         .mode = ADJUSTMENT_MODE_STEP,
 158         .data = { .step = 1 }
 159     }, {
 160         .adjustmentFunction = ADJUSTMENT_ROLL_RATE,
 161         .mode = ADJUSTMENT_MODE_STEP,
 162         .data = { .step = 1 }
 163     }, {
 164         .adjustmentFunction = ADJUSTMENT_PITCH_P,
 165         .mode = ADJUSTMENT_MODE_STEP,
 166         .data = { .step = 1 }
 167     }, {
 168         .adjustmentFunction = ADJUSTMENT_PITCH_I,
 169         .mode = ADJUSTMENT_MODE_STEP,
 170         .data = { .step = 1 }
 171     }, {
 172         .adjustmentFunction = ADJUSTMENT_PITCH_D,
 173         .mode = ADJUSTMENT_MODE_STEP,
 174         .data = { .step = 1 }
 175     }, {
 176         .adjustmentFunction = ADJUSTMENT_ROLL_P,
 177         .mode = ADJUSTMENT_MODE_STEP,
 178         .data = { .step = 1 }
 179     }, {
 180         .adjustmentFunction = ADJUSTMENT_ROLL_I,
 181         .mode = ADJUSTMENT_MODE_STEP,
 182         .data = { .step = 1 }
 183     }, {
 184         .adjustmentFunction = ADJUSTMENT_ROLL_D,
 185         .mode = ADJUSTMENT_MODE_STEP,
 186         .data = { .step = 1 }
 187     }, {
 188         .adjustmentFunction = ADJUSTMENT_RC_RATE_YAW,
 189         .mode = ADJUSTMENT_MODE_STEP,
 190         .data = { .step = 1 }
 191     }, {
 192         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_F,
 193         .mode = ADJUSTMENT_MODE_STEP,
 194         .data = { .step = 1 }
 195     }, {
 196         .adjustmentFunction = ADJUSTMENT_FEEDFORWARD_TRANSITION,
 197         .mode = ADJUSTMENT_MODE_STEP,
 198         .data = { .step = 1 }
 199     }, {
 200         .adjustmentFunction = ADJUSTMENT_HORIZON_STRENGTH,
 201         .mode = ADJUSTMENT_MODE_SELECT,
 202         .data = { .switchPositions = 255 }
 203     }, {
 204         .adjustmentFunction = ADJUSTMENT_PID_AUDIO,
 205         .mode = ADJUSTMENT_MODE_SELECT,
 206         .data = { .switchPositions = ARRAYLEN(pidAudioPositionToModeMap) }
 207     }, {
 208         .adjustmentFunction = ADJUSTMENT_PITCH_F,
 209         .mode = ADJUSTMENT_MODE_STEP,
 210         .data = { .step = 1 }
 211     }, {
 212         .adjustmentFunction = ADJUSTMENT_ROLL_F,
 213         .mode = ADJUSTMENT_MODE_STEP,
 214         .data = { .step = 1 }
 215     }, {
 216         .adjustmentFunction = ADJUSTMENT_YAW_F,
 217         .mode = ADJUSTMENT_MODE_STEP,
 218         .data = { .step = 1 }
 219     }, {
 220         .adjustmentFunction = ADJUSTMENT_OSD_PROFILE,
 221         .mode = ADJUSTMENT_MODE_SELECT,
 222         .data = { .switchPositions = 3 }
 223     }, {
 224         .adjustmentFunction = ADJUSTMENT_LED_PROFILE,
 225         .mode = ADJUSTMENT_MODE_SELECT,
 226         .data = { .switchPositions = 3 }
 227     }
 228 };
 229
 230 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
 231 static const char * const adjustmentLabels[] = {
 232     "RC RATE",
 233     "RC EXPO",
 234     "THROTTLE EXPO",
 235     "ROLL RATE",
 236     "YAW RATE",
 237     "PITCH/ROLL P",
 238     "PITCH/ROLL I",
 239     "PITCH/ROLL D",
 240     "YAW P",
 241     "YAW I",
 242     "YAW D",
 243     "RATE PROFILE",
 244     "PITCH RATE",
 245     "ROLL RATE",
 246     "PITCH P",
 247     "PITCH I",
 248     "PITCH D",
 249     "ROLL P",
 250     "ROLL I",
 251     "ROLL D",
 252     "RC RATE YAW",
 253     "PITCH/ROLL F",
 254     "FF TRANSITION",
 255     "HORIZON STRENGTH",
 256     "ROLL RC RATE",
 257     "PITCH RC RATE",
 258     "ROLL RC EXPO",
 259     "PITCH RC EXPO",
 260     "PID AUDIO",
 261     "PITCH F",
 262     "ROLL F",
 263     "YAW F",
 264     "OSD PROFILE",
 265 };
 266
 267 static int adjustmentRangeNameIndex = 0;
 268 static int adjustmentRangeValue = -1;
 269 #endif
 270
 271 static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
 272 {
 273     beeperConfirmationBeeps(delta > 0 ? 2 : 1);
 274     int newValue;
 275     switch (adjustmentFunction) {
 276     case ADJUSTMENT_RC_RATE:
 277     case ADJUSTMENT_ROLL_RC_RATE:
 278         newValue = constrain((int)controlRateConfig->rcRates[FD_ROLL] + delta, 1, CONTROL_RATE_CONFIG_RC_RATES_MAX);
 279         controlRateConfig->rcRates[FD_ROLL] = newValue;
 280         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RC_RATE, newValue);
 281         if (adjustmentFunction == ADJUSTMENT_ROLL_RC_RATE) {
 282             break;
 283         }
 284         // fall through for combined ADJUSTMENT_RC_EXPO
 285         FALLTHROUGH;
 286     case ADJUSTMENT_PITCH_RC_RATE:
 287         newValue = constrain((int)controlRateConfig->rcRates[FD_PITCH] + delta, 1, CONTROL_RATE_CONFIG_RC_RATES_MAX);
 288         controlRateConfig->rcRates[FD_PITCH] = newValue;
 289         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RC_RATE, newValue);
 290         break;
 291     case ADJUSTMENT_RC_EXPO:
 292     case ADJUSTMENT_ROLL_RC_EXPO:
 293         newValue = constrain((int)controlRateConfig->rcExpo[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX);
 294         controlRateConfig->rcExpo[FD_ROLL] = newValue;
 295         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RC_EXPO, newValue);
 296         if (adjustmentFunction == ADJUSTMENT_ROLL_RC_EXPO) {
 297             break;
 298         }
 299         // fall through for combined ADJUSTMENT_RC_EXPO
 300         FALLTHROUGH;
 301     case ADJUSTMENT_PITCH_RC_EXPO:
 302         newValue = constrain((int)controlRateConfig->rcExpo[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX);
 303         controlRateConfig->rcExpo[FD_PITCH] = newValue;
 304         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RC_EXPO, newValue);
 305         break;
 306     case ADJUSTMENT_THROTTLE_EXPO:
 307         newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c
 308         controlRateConfig->thrExpo8 = newValue;
 309         initRcProcessing();
 310         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
 311         break;
 312     case ADJUSTMENT_PITCH_ROLL_RATE:
 313     case ADJUSTMENT_PITCH_RATE:
 314         newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_RATE_MAX);
 315         controlRateConfig->rates[FD_PITCH] = newValue;
 316         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
 317         if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
 318             break;
 319         }
 320         // fall through for combined ADJUSTMENT_PITCH_ROLL_RATE
 321         FALLTHROUGH;
 322     case ADJUSTMENT_ROLL_RATE:
 323         newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_RATE_MAX);
 324         controlRateConfig->rates[FD_ROLL] = newValue;
 325         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
 326         break;
 327     case ADJUSTMENT_YAW_RATE:
 328         newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_RATE_MAX);
 329         controlRateConfig->rates[FD_YAW] = newValue;
 330         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
 331         break;
 332     case ADJUSTMENT_PITCH_ROLL_P:
 333     case ADJUSTMENT_PITCH_P:
 334         newValue = constrain((int)currentPidProfile->pid[PID_PITCH].P + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 335         currentPidProfile->pid[PID_PITCH].P = newValue;
 336         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
 337
 338         if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
 339             break;
 340         }
 341         // fall through for combined ADJUSTMENT_PITCH_ROLL_P
 342         FALLTHROUGH;
 343     case ADJUSTMENT_ROLL_P:
 344         newValue = constrain((int)currentPidProfile->pid[PID_ROLL].P + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 345         currentPidProfile->pid[PID_ROLL].P = newValue;
 346         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
 347         break;
 348     case ADJUSTMENT_PITCH_ROLL_I:
 349     case ADJUSTMENT_PITCH_I:
 350         newValue = constrain((int)currentPidProfile->pid[PID_PITCH].I + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 351         currentPidProfile->pid[PID_PITCH].I = newValue;
 352         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
 353         if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
 354             break;
 355         }
 356         // fall through for combined ADJUSTMENT_PITCH_ROLL_I
 357         FALLTHROUGH;
 358     case ADJUSTMENT_ROLL_I:
 359         newValue = constrain((int)currentPidProfile->pid[PID_ROLL].I + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 360         currentPidProfile->pid[PID_ROLL].I = newValue;
 361         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
 362         break;
 363     case ADJUSTMENT_PITCH_ROLL_D:
 364     case ADJUSTMENT_PITCH_D:
 365         newValue = constrain((int)currentPidProfile->pid[PID_PITCH].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 366         currentPidProfile->pid[PID_PITCH].D = newValue;
 367         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
 368         if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
 369             break;
 370         }
 371         // fall through for combined ADJUSTMENT_PITCH_ROLL_D
 372         FALLTHROUGH;
 373     case ADJUSTMENT_ROLL_D:
 374         newValue = constrain((int)currentPidProfile->pid[PID_ROLL].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 375         currentPidProfile->pid[PID_ROLL].D = newValue;
 376         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
 377         break;
 378     case ADJUSTMENT_YAW_P:
 379         newValue = constrain((int)currentPidProfile->pid[PID_YAW].P + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 380         currentPidProfile->pid[PID_YAW].P = newValue;
 381         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
 382         break;
 383     case ADJUSTMENT_YAW_I:
 384         newValue = constrain((int)currentPidProfile->pid[PID_YAW].I + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 385         currentPidProfile->pid[PID_YAW].I = newValue;
 386         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
 387         break;
 388     case ADJUSTMENT_YAW_D:
 389         newValue = constrain((int)currentPidProfile->pid[PID_YAW].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 390         currentPidProfile->pid[PID_YAW].D = newValue;
 391         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
 392         break;
 393     case ADJUSTMENT_RC_RATE_YAW:
 394         newValue = constrain((int)controlRateConfig->rcRates[FD_YAW] + delta, 1, CONTROL_RATE_CONFIG_RC_RATES_MAX);
 395         controlRateConfig->rcRates[FD_YAW] = newValue;
 396         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
 397         break;
 398     case ADJUSTMENT_PITCH_ROLL_F:
 399     case ADJUSTMENT_PITCH_F:
 400         newValue = constrain(currentPidProfile->pid[PID_PITCH].F + delta, 0, 2000);
 401         currentPidProfile->pid[PID_PITCH].F = newValue;
 402         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_F, newValue);
 403
 404         if (adjustmentFunction == ADJUSTMENT_PITCH_F) {
 405             break;
 406         }
 407         // fall through for combined ADJUSTMENT_PITCH_ROLL_F
 408         FALLTHROUGH;
 409     case ADJUSTMENT_ROLL_F:
 410         newValue = constrain(currentPidProfile->pid[PID_ROLL].F + delta, 0, 2000);
 411         currentPidProfile->pid[PID_ROLL].F = newValue;
 412         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_F, newValue);
 413         break;
 414     case ADJUSTMENT_YAW_F:
 415         newValue = constrain(currentPidProfile->pid[PID_YAW].F + delta, 0, 2000);
 416         currentPidProfile->pid[PID_YAW].F = newValue;
 417         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F, newValue);
 418         break;
 419     case ADJUSTMENT_FEEDFORWARD_TRANSITION:
 420         newValue = constrain(currentPidProfile->feedforwardTransition + delta, 1, 100); // FIXME magic numbers repeated in cli.c
 421         currentPidProfile->feedforwardTransition = newValue;
 422         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
 423         break;
 424     default:
 425         newValue = -1;
 426         break;
 427     };
 428
 429     return newValue;
 430 }
 431
 432 static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjustmentFunction_e adjustmentFunction, int value)
 433 {
 434     int newValue;
 435
 436     switch (adjustmentFunction) {
 437     case ADJUSTMENT_RC_RATE:
 438     case ADJUSTMENT_ROLL_RC_RATE:
 439         newValue = constrain(value, 1, CONTROL_RATE_CONFIG_RC_RATES_MAX);
 440         controlRateConfig->rcRates[FD_ROLL] = newValue;
 441         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RC_RATE, newValue);
 442         if (adjustmentFunction == ADJUSTMENT_ROLL_RC_RATE) {
 443             break;
 444         }
 445         // fall through for combined ADJUSTMENT_RC_EXPO
 446         FALLTHROUGH;
 447     case ADJUSTMENT_PITCH_RC_RATE:
 448         newValue = constrain(value, 1, CONTROL_RATE_CONFIG_RC_RATES_MAX);
 449         controlRateConfig->rcRates[FD_PITCH] = newValue;
 450         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RC_RATE, newValue);
 451         break;
 452     case ADJUSTMENT_RC_EXPO:
 453     case ADJUSTMENT_ROLL_RC_EXPO:
 454         newValue = constrain(value, 1, CONTROL_RATE_CONFIG_RC_EXPO_MAX);
 455         controlRateConfig->rcExpo[FD_ROLL] = newValue;
 456         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RC_EXPO, newValue);
 457         if (adjustmentFunction == ADJUSTMENT_ROLL_RC_EXPO) {
 458             break;
 459         }
 460         // fall through for combined ADJUSTMENT_RC_EXPO
 461         FALLTHROUGH;
 462     case ADJUSTMENT_PITCH_RC_EXPO:
 463         newValue = constrain(value, 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX);
 464         controlRateConfig->rcExpo[FD_PITCH] = newValue;
 465         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RC_EXPO, newValue);
 466         break;
 467     case ADJUSTMENT_THROTTLE_EXPO:
 468         newValue = constrain(value, 0, 100); // FIXME magic numbers repeated in cli.c
 469         controlRateConfig->thrExpo8 = newValue;
 470         initRcProcessing();
 471         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
 472         break;
 473     case ADJUSTMENT_PITCH_ROLL_RATE:
 474     case ADJUSTMENT_PITCH_RATE:
 475         newValue = constrain(value, 0, CONTROL_RATE_CONFIG_RATE_MAX);
 476         controlRateConfig->rates[FD_PITCH] = newValue;
 477         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
 478         if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
 479             break;
 480         }
 481         // fall through for combined ADJUSTMENT_PITCH_ROLL_RATE
 482         FALLTHROUGH;
 483     case ADJUSTMENT_ROLL_RATE:
 484         newValue = constrain(value, 0, CONTROL_RATE_CONFIG_RATE_MAX);
 485         controlRateConfig->rates[FD_ROLL] = newValue;
 486         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
 487         break;
 488     case ADJUSTMENT_YAW_RATE:
 489         newValue = constrain(value, 0, CONTROL_RATE_CONFIG_RATE_MAX);
 490         controlRateConfig->rates[FD_YAW] = newValue;
 491         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
 492         break;
 493     case ADJUSTMENT_PITCH_ROLL_P:
 494     case ADJUSTMENT_PITCH_P:
 495         newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
 496         currentPidProfile->pid[PID_PITCH].P = newValue;
 497         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
 498
 499         if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
 500             break;
 501         }
 502         // fall through for combined ADJUSTMENT_PITCH_ROLL_P
 503         FALLTHROUGH;
 504     case ADJUSTMENT_ROLL_P:
 505         newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
 506         currentPidProfile->pid[PID_ROLL].P = newValue;
 507         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
 508         break;
 509     case ADJUSTMENT_PITCH_ROLL_I:
 510     case ADJUSTMENT_PITCH_I:
 511         newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
 512         currentPidProfile->pid[PID_PITCH].I = newValue;
 513         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
 514         if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
 515             break;
 516         }
 517         // fall through for combined ADJUSTMENT_PITCH_ROLL_I
 518         FALLTHROUGH;
 519     case ADJUSTMENT_ROLL_I:
 520         newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
 521         currentPidProfile->pid[PID_ROLL].I = newValue;
 522         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
 523         break;
 524     case ADJUSTMENT_PITCH_ROLL_D:
 525     case ADJUSTMENT_PITCH_D:
 526         newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
 527         currentPidProfile->pid[PID_PITCH].D = newValue;
 528         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
 529         if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
 530             break;
 531         }
 532         // fall through for combined ADJUSTMENT_PITCH_ROLL_D
 533         FALLTHROUGH;
 534     case ADJUSTMENT_ROLL_D:
 535         newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
 536         currentPidProfile->pid[PID_ROLL].D = newValue;
 537         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
 538         break;
 539     case ADJUSTMENT_YAW_P:
 540         newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
 541         currentPidProfile->pid[PID_YAW].P = newValue;
 542         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
 543         break;
 544     case ADJUSTMENT_YAW_I:
 545         newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
 546         currentPidProfile->pid[PID_YAW].I = newValue;
 547         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
 548         break;
 549     case ADJUSTMENT_YAW_D:
 550         newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
 551         currentPidProfile->pid[PID_YAW].D = newValue;
 552         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
 553         break;
 554     case ADJUSTMENT_RC_RATE_YAW:
 555         newValue = constrain(value, 1, CONTROL_RATE_CONFIG_RC_RATES_MAX);
 556         controlRateConfig->rcRates[FD_YAW] = newValue;
 557         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
 558         break;
 559     case ADJUSTMENT_PITCH_ROLL_F:
 560     case ADJUSTMENT_PITCH_F:
 561         newValue = constrain(value, 0, 2000);
 562         currentPidProfile->pid[PID_PITCH].F = newValue;
 563         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_F, newValue);
 564
 565         if (adjustmentFunction == ADJUSTMENT_PITCH_F) {
 566             break;
 567         }
 568         // fall through for combined ADJUSTMENT_PITCH_ROLL_F
 569         FALLTHROUGH;
 570     case ADJUSTMENT_ROLL_F:
 571         newValue = constrain(value, 0, 2000);
 572         currentPidProfile->pid[PID_ROLL].F = newValue;
 573         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_F, newValue);
 574         break;
 575     case ADJUSTMENT_YAW_F:
 576         newValue = constrain(value, 0, 2000);
 577         currentPidProfile->pid[PID_YAW].F = newValue;
 578         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F, newValue);
 579         break;
 580     case ADJUSTMENT_FEEDFORWARD_TRANSITION:
 581         newValue = constrain(value, 1, 100); // FIXME magic numbers repeated in cli.c
 582         currentPidProfile->feedforwardTransition = newValue;
 583         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
 584         break;
 585     default:
 586         newValue = -1;
 587         break;
 588     };
 589
 590     return newValue;
 591 }
 592
 593 static uint8_t applySelectAdjustment(adjustmentFunction_e adjustmentFunction, uint8_t position)
 594 {
 595     uint8_t beeps = 0;
 596
 597     switch (adjustmentFunction) {
 598     case ADJUSTMENT_RATE_PROFILE:
 599         if (getCurrentControlRateProfileIndex() != position) {
 600             changeControlRateProfile(position);
 601             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position);
 602
 603             beeps = position + 1;
 604         }
 605         break;
 606     case ADJUSTMENT_HORIZON_STRENGTH:
 607         {
 608             uint8_t newValue = constrain(position, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 609             if (currentPidProfile->pid[PID_LEVEL].D != newValue) {
 610                 beeps = ((newValue - currentPidProfile->pid[PID_LEVEL].D) / 8) + 1;
 611                 currentPidProfile->pid[PID_LEVEL].D = newValue;
 612                 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_HORIZON_STRENGTH, position);
 613             }
 614         }
 615         break;
 616     case ADJUSTMENT_PID_AUDIO:
 617 #ifdef USE_PID_AUDIO
 618         {
 619             pidAudioModes_e newMode = pidAudioPositionToModeMap[position];
 620             if (newMode != pidAudioGetMode()) {
 621                 pidAudioSetMode(newMode);
 622             }
 623         }
 624 #endif
 625         break;
 626     case ADJUSTMENT_OSD_PROFILE:
 627 #ifdef USE_OSD_PROFILES
 628         if (getCurrentOsdProfileIndex() != (position + 1)) {
 629             changeOsdProfileIndex(position+1);
 630         }
 631 #endif
 632         break;
 633     case ADJUSTMENT_LED_PROFILE:
 634 #ifdef USE_LED_STRIP
 635         if (getLedProfile() != position) {
 636             setLedProfile(position);
 637         }
 638 #endif
 639         break;
 640
 641     default:
 642         break;
 643     }
 644
 645     if (beeps) {
 646         beeperConfirmationBeeps(beeps);
 647     }
 648
 649     return position;
 650 }
 651
 652 #define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1
 653
 654 static void calcActiveAdjustmentRanges(void)
 655 {
 656     adjustmentRange_t defaultAdjustmentRange;
 657     memset(&defaultAdjustmentRange, 0, sizeof(defaultAdjustmentRange));
 658
 659     stepwiseAdjustmentCount = 0;
 660     continuosAdjustmentCount = 0;
 661     for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
 662         const adjustmentRange_t * const adjustmentRange = adjustmentRanges(i);
 663         if (memcmp(adjustmentRange, &defaultAdjustmentRange, sizeof(defaultAdjustmentRange)) != 0) {
 664             const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentConfig - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
 665             if (adjustmentRange->adjustmentCenter == 0 && adjustmentConfig->mode != ADJUSTMENT_MODE_SELECT) {
 666                 timedAdjustmentState_t *adjustmentState = &stepwiseAdjustments[stepwiseAdjustmentCount++];
 667                 adjustmentState->adjustmentRangeIndex = i;
 668                 adjustmentState->timeoutAt = 0;
 669                 adjustmentState->ready = true;
 670             } else {
 671                 continuosAdjustmentState_t *adjustmentState = &continuosAdjustments[continuosAdjustmentCount++];
 672                 adjustmentState->adjustmentRangeIndex = i;
 673                 adjustmentState->lastRcData = 0;
 674             }
 675         }
 676     }
 677 }
 678
 679 #define VALUE_DISPLAY_LATENCY_MS 2000
 680
 681 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
 682 static void updateOsdAdjustmentData(int newValue, adjustmentFunction_e adjustmentFunction)
 683 {
 684     static timeMs_t lastValueChangeMs;
 685
 686     timeMs_t currentTimeMs = millis();
 687     if (newValue != -1
 688         && adjustmentFunction != ADJUSTMENT_RATE_PROFILE  // Rate profile already has an OSD element
 689 #ifdef USE_OSD_PROFILES
 690         && adjustmentFunction != ADJUSTMENT_OSD_PROFILE
 691 #endif
 692         ) {
 693         adjustmentRangeNameIndex = adjustmentFunction;
 694         adjustmentRangeValue = newValue;
 695
 696         lastValueChangeMs = currentTimeMs;
 697     }
 698
 699     if (cmp32(currentTimeMs, lastValueChangeMs + VALUE_DISPLAY_LATENCY_MS) >= 0) {
 700         adjustmentRangeNameIndex = 0;
 701     }
 702 }
 703 #endif
 704
 705 #define RESET_FREQUENCY_2HZ (1000 / 2)
 706
 707 static void processStepwiseAdjustments(controlRateConfig_t *controlRateConfig, const bool canUseRxData)
 708 {
 709     const timeMs_t now = millis();
 710
 711     for (int index = 0; index < stepwiseAdjustmentCount; index++) {
 712         timedAdjustmentState_t *adjustmentState = &stepwiseAdjustments[index];
 713         const adjustmentRange_t *const adjustmentRange = adjustmentRanges(adjustmentState->adjustmentRangeIndex);
 714         const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentConfig - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
 715         const adjustmentFunction_e adjustmentFunction = adjustmentConfig->adjustmentFunction;
 716
 717         if (!isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range) ||
 718             adjustmentFunction == ADJUSTMENT_NONE) {
 719             adjustmentState->timeoutAt = 0;
 720
 721             continue;
 722         }
 723
 724         if (cmp32(now, adjustmentState->timeoutAt) >= 0) {
 725             adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
 726             adjustmentState->ready = true;
 727         }
 728
 729         if (!canUseRxData) {
 730             continue;
 731         }
 732
 733         const uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentRange->auxSwitchChannelIndex;
 734
 735         if (adjustmentConfig->mode == ADJUSTMENT_MODE_STEP) {
 736             int delta;
 737             if (rcData[channelIndex] > rxConfig()->midrc + 200) {
 738                 delta = adjustmentConfig->data.step;
 739             } else if (rcData[channelIndex] < rxConfig()->midrc - 200) {
 740                 delta = -adjustmentConfig->data.step;
 741             } else {
 742                 // returning the switch to the middle immediately resets the ready state
 743                 adjustmentState->ready = true;
 744                 adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
 745                 continue;
 746             }
 747             if (!adjustmentState->ready) {
 748                 continue;
 749             }
 750
 751             int newValue = applyStepAdjustment(controlRateConfig, adjustmentFunction, delta);
 752
 753             setConfigDirty();
 754
 755             pidInitConfig(currentPidProfile);
 756
 757             adjustmentState->ready = false;
 758
 759 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
 760             updateOsdAdjustmentData(newValue, adjustmentConfig->adjustmentFunction);
 761 #else
 762             UNUSED(newValue);
 763 #endif
 764         }
 765     }
 766 }
 767
 768 static void setConfigDirtyIfNotPermanent(const channelRange_t *range)
 769 {
 770     if (!(range->startStep == MIN_MODE_RANGE_STEP && range->endStep == MAX_MODE_RANGE_STEP)) {
 771         // Only set the configuration dirty if this range is NOT permanently enabled (and the config thus never used).
 772         setConfigDirty();
 773     }
 774 }
 775
 776 static void processContinuosAdjustments(controlRateConfig_t *controlRateConfig)
 777 {
 778     for (int i = 0; i < continuosAdjustmentCount; i++) {
 779         continuosAdjustmentState_t *adjustmentState = &continuosAdjustments[i];
 780         const adjustmentRange_t * const adjustmentRange = adjustmentRanges(adjustmentState->adjustmentRangeIndex);
 781         const uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentRange->auxSwitchChannelIndex;
 782         const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentConfig - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
 783         const adjustmentFunction_e adjustmentFunction = adjustmentConfig->adjustmentFunction;
 784
 785         if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range) &&
 786             adjustmentFunction != ADJUSTMENT_NONE) {
 787
 788             if (rcData[channelIndex] != adjustmentState->lastRcData) {
 789                 int newValue = -1;
 790
 791                 if (adjustmentConfig->mode == ADJUSTMENT_MODE_SELECT) {
 792                     int switchPositions = adjustmentConfig->data.switchPositions;
 793                     if (adjustmentFunction == ADJUSTMENT_RATE_PROFILE && systemConfig()->rateProfile6PosSwitch) {
 794                         switchPositions =  6;
 795                     }
 796                     const uint16_t rangeWidth = (2100 - 900) / switchPositions;
 797                     const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
 798                     newValue = applySelectAdjustment(adjustmentFunction, position);
 799
 800                     setConfigDirtyIfNotPermanent(&adjustmentRange->range);
 801                 } else {
 802                     // If setting is defined for step adjustment and center value has been specified, apply values directly (scaled) from aux channel
 803                     if (adjustmentRange->adjustmentCenter &&
 804                         (adjustmentConfig->mode == ADJUSTMENT_MODE_STEP)) {
 805                         int value = (((rcData[channelIndex] - PWM_RANGE_MIDDLE) * adjustmentRange->adjustmentScale) / (PWM_RANGE_MIDDLE - PWM_RANGE_MIN)) + adjustmentRange->adjustmentCenter;
 806
 807                         newValue = applyAbsoluteAdjustment(controlRateConfig, adjustmentFunction, value);
 808
 809                         setConfigDirtyIfNotPermanent(&adjustmentRange->range);
 810
 811                         pidInitConfig(currentPidProfile);
 812                     }
 813                 }
 814 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
 815                 updateOsdAdjustmentData(newValue, adjustmentConfig->adjustmentFunction);
 816 #else
 817                 UNUSED(newValue);
 818 #endif
 819                 adjustmentState->lastRcData = rcData[channelIndex];
 820             }
 821         } else {
 822             adjustmentState->lastRcData = 0;
 823         }
 824     }
 825 }
 826
 827 void processRcAdjustments(controlRateConfig_t *controlRateConfig)
 828 {
 829     const bool canUseRxData = rxIsReceivingSignal();
 830
 831     // Recalculate the new active adjustments if required
 832     if (stepwiseAdjustmentCount == ADJUSTMENT_RANGE_COUNT_INVALID) {
 833         calcActiveAdjustmentRanges();
 834     }
 835
 836     processStepwiseAdjustments(controlRateConfig, canUseRxData);
 837
 838     if (canUseRxData) {
 839         processContinuosAdjustments(controlRateConfig);
 840     }
 841
 842 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
 843     // Hide the element if there is no change
 844     updateOsdAdjustmentData(-1, 0);
 845 #endif
 846 }
 847
 848 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
 849 const char *getAdjustmentsRangeName(void)
 850 {
 851     if (adjustmentRangeNameIndex > 0) {
 852         return &adjustmentLabels[adjustmentRangeNameIndex - 1][0];
 853     } else {
 854         return NULL;
 855     }
 856 }
 857
 858 int getAdjustmentsRangeValue(void)
 859 {
 860     return adjustmentRangeValue;
 861 }
 862 #endif
 863
 864 void activeAdjustmentRangeReset(void)
 865 {
 866     stepwiseAdjustmentCount = ADJUSTMENT_RANGE_COUNT_INVALID;
 867 }