rc_controls.c 24.6 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
/*
 * This file is part of Cleanflight.
 *
 * Cleanflight is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Cleanflight is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdbool.h>
#include <stdint.h>
#include <string.h>

#include <math.h>

24
25
#include "platform.h"

26
27
28
29
30
31
#include "common/axis.h"
#include "common/maths.h"

#include "config/config.h"
#include "config/runtime_config.h"

32
#include "drivers/system.h"
33
#include "drivers/sensor.h"
34
35
#include "drivers/accgyro.h"

36
#include "sensors/barometer.h"
37
#include "sensors/battery.h"
38
39
40
41
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"

42
43
#include "rx/rx.h"

44
#include "io/gps.h"
45
#include "io/beeper.h"
46
#include "io/escservo.h"
47
#include "io/rc_controls.h"
48
#include "io/rc_curves.h"
49

50
51
#include "io/display.h"

52
#include "flight/pid.h"
53
#include "flight/navigation.h"
54
#include "flight/failsafe.h"
55
56
57
58

#include "mw.h"


59
static escAndServoConfig_t *escAndServoConfig;
60
static pidProfile_t *pidProfile;
61

62
// true if arming is done via the sticks (as opposed to a switch)
63
64
static bool isUsingSticksToArm = true;

65
66
int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW

67
uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
68

69
70

bool isUsingSticksForArming(void)
71
72
73
74
{
    return isUsingSticksToArm;
}

75

76
77
bool areSticksInApModePosition(uint16_t ap_mode)
{
78
    return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
79
80
81
82
83
84
85
86
87
88
89
90
}

throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
    if (feature(FEATURE_3D) && (rcData[THROTTLE] > (rxConfig->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig->midrc + deadband3d_throttle)))
        return THROTTLE_LOW;
    else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->mincheck))
        return THROTTLE_LOW;

    return THROTTLE_HIGH;
}

91
92


93
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch)
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
{
    static uint8_t rcDelayCommand;      // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
    static uint8_t rcSticks;            // this hold sticks position for command combos
    uint8_t stTmp = 0;
    int i;

    // ------------------ STICKS COMMAND HANDLER --------------------
    // checking sticks positions
    for (i = 0; i < 4; i++) {
        stTmp >>= 2;
        if (rcData[i] > rxConfig->mincheck)
            stTmp |= 0x80;  // check for MIN
        if (rcData[i] < rxConfig->maxcheck)
            stTmp |= 0x40;  // check for MAX
    }
    if (stTmp == rcSticks) {
        if (rcDelayCommand < 250)
            rcDelayCommand++;
    } else
        rcDelayCommand = 0;
    rcSticks = stTmp;

    // perform actions
117
    if (!isUsingSticksToArm) {
118

119
        if (IS_RC_MODE_ACTIVE(BOXARM)) {
120
121
122
123
124
125
126
127
            // Arming via ARM BOX
            if (throttleStatus == THROTTLE_LOW) {
                if (ARMING_FLAG(OK_TO_ARM)) {
                    mwArm();
                }
            }
        } else {
            // Disarming via ARM BOX
128
129

            if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive()  ) {
130
131
132
133
134
135
136
137
138
139
140
141
142
                if (disarm_kill_switch) {
                    mwDisarm();
                } else if (throttleStatus == THROTTLE_LOW) {
                    mwDisarm();
                }
            }
        }
    }

    if (rcDelayCommand != 20) {
        return;
    }

143
144
    if (isUsingSticksToArm) {
        // Disarm on throttle down + yaw
145
        if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
146
            if (ARMING_FLAG(ARMED))
147
                mwDisarm();
148
149
150
            else {
                beeper(BEEPER_DISARM_REPEAT);    // sound tone while stick held
                rcDelayCommand = 0;              // reset so disarm tone will repeat
151
152
            }
        }
153
            // Disarm on roll (only when retarded_arm is enabled)
154
        if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) {
155
            if (ARMING_FLAG(ARMED))
156
                mwDisarm();
157
158
159
            else {
                beeper(BEEPER_DISARM_REPEAT);    // sound tone while stick held
                rcDelayCommand = 0;              // reset so disarm tone will repeat
160
            }
161
        }
162
163
164
165
    }

    if (ARMING_FLAG(ARMED)) {
        // actions during armed
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
        return;
    }

    // actions during not armed
    i = 0;

    if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
        // GYRO calibration
        gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);

#ifdef GPS
        if (feature(FEATURE_GPS)) {
            GPS_reset_home_position();
        }
#endif

#ifdef BARO
        if (sensors(SENSOR_BARO))
            baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
#endif

        if (!sensors(SENSOR_MAG))
            heading = 0; // reset heading to zero after gyro calibration

        return;
    }

    if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
        // Inflight ACC Calibration
        handleInflightCalibrationStickPosition();
        return;
    }

    // Multiple configuration profiles
    if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO)          // ROLL left  -> Profile 1
        i = 1;
    else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE)     // PITCH up   -> Profile 2
        i = 2;
    else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI)     // ROLL right -> Profile 3
        i = 3;
    if (i) {
        changeProfile(i - 1);
        return;
    }

211
212
213
214
    if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
        saveConfigAndNotify();
    }

215
    if (isUsingSticksToArm) {
216

217
218
219
220
221
222
223
224
225
226
227
        if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
            // Arm via YAW
            mwArm();
            return;
        }

        if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI)) {
            // Arm via ROLL
            mwArm();
            return;
        }
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
    }

    if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
        // Calibrating Acc
        accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
        return;
    }


    if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
        // Calibrating Mag
        ENABLE_STATE(CALIBRATE_MAG);
        return;
    }


    // Accelerometer Trim

    rollAndPitchTrims_t accelerometerTrimsDelta;
    memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta));

    bool shouldApplyRollAndPitchTrimDelta = false;
    if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
        accelerometerTrimsDelta.values.pitch = 2;
        shouldApplyRollAndPitchTrimDelta = true;
    } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
        accelerometerTrimsDelta.values.pitch = -2;
        shouldApplyRollAndPitchTrimDelta = true;
    } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
        accelerometerTrimsDelta.values.roll = 2;
        shouldApplyRollAndPitchTrimDelta = true;
    } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
        accelerometerTrimsDelta.values.roll = -2;
        shouldApplyRollAndPitchTrimDelta = true;
    }
    if (shouldApplyRollAndPitchTrimDelta) {
        applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta);
        rcDelayCommand = 0; // allow autorepetition
        return;
    }
268

269
#ifdef DISPLAY
270
271
272
273
274
275
276
    if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
        displayDisablePageCycling();
    }

    if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
        displayEnablePageCycling();
    }
277
#endif
278

279
280
}

281
282
283
284
285
286
287
288
289
290
bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range) {
    if (!IS_RANGE_USABLE(range)) {
        return false;
    }

    uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
    return (channelValue >= 900 + (range->startStep * 25) &&
            channelValue < 900 + (range->endStep * 25));
}

291
void updateActivatedModes(modeActivationCondition_t *modeActivationConditions)
292
{
293
    rcModeActivationMask = 0;
294
295
296
297
298
299

    uint8_t index;

    for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
        modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];

300
        if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) {
301
302
303
            ACTIVATE_RC_MODE(modeActivationCondition->modeId);
        }
    }
304
}
305

306
uint8_t adjustmentStateMask = 0;
307

308
309
#define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex)
#define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex)
310

311
#define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex))
312

313
314
// sync with adjustmentFunction_e
static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = {
315
316
    {
        .adjustmentFunction = ADJUSTMENT_RC_RATE,
317
        .mode = ADJUSTMENT_MODE_STEP,
318
        .data = { .stepConfig = { .step = 1 }}
319
    },
320
321
    {
        .adjustmentFunction = ADJUSTMENT_RC_EXPO,
322
        .mode = ADJUSTMENT_MODE_STEP,
323
        .data = { .stepConfig = { .step = 1 }}
324
    },
325
326
    {
        .adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO,
327
        .mode = ADJUSTMENT_MODE_STEP,
328
        .data = { .stepConfig = { .step = 1 }}
329
    },
330
331
    {
        .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE,
332
        .mode = ADJUSTMENT_MODE_STEP,
333
        .data = { .stepConfig = { .step = 1 }}
334
335
336
    },
    {
        .adjustmentFunction = ADJUSTMENT_YAW_RATE,
337
        .mode = ADJUSTMENT_MODE_STEP,
338
        .data = { .stepConfig = { .step = 1 }}
339
340
341
    },
    {
        .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
342
        .mode = ADJUSTMENT_MODE_STEP,
343
        .data = { .stepConfig = { .step = 1 }}
344
345
346
    },
    {
        .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
347
        .mode = ADJUSTMENT_MODE_STEP,
348
        .data = { .stepConfig = { .step = 1 }}
349
350
351
    },
    {
        .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
352
        .mode = ADJUSTMENT_MODE_STEP,
353
        .data = { .stepConfig = { .step = 1 }}
354
355
356
    },
    {
        .adjustmentFunction = ADJUSTMENT_YAW_P,
357
        .mode = ADJUSTMENT_MODE_STEP,
358
        .data = { .stepConfig = { .step = 1 }}
359
360
361
    },
    {
        .adjustmentFunction = ADJUSTMENT_YAW_I,
362
        .mode = ADJUSTMENT_MODE_STEP,
363
        .data = { .stepConfig = { .step = 1 }}
364
365
366
    },
    {
        .adjustmentFunction = ADJUSTMENT_YAW_D,
367
        .mode = ADJUSTMENT_MODE_STEP,
368
        .data = { .stepConfig = { .step = 1 }}
369
370
371
372
    },
    {
        .adjustmentFunction = ADJUSTMENT_RATE_PROFILE,
        .mode = ADJUSTMENT_MODE_SELECT,
373
        .data = { .selectConfig = { .switchPositions = 3 }}
374
    },
375
376
377
    {
        .adjustmentFunction = ADJUSTMENT_PITCH_RATE,
        .mode = ADJUSTMENT_MODE_STEP,
378
        .data = { .stepConfig = { .step = 1 }}
379
380
381
382
    },
    {
        .adjustmentFunction = ADJUSTMENT_ROLL_RATE,
        .mode = ADJUSTMENT_MODE_STEP,
383
        .data = { .stepConfig = { .step = 1 }}
384
    },
385
386
387
    {
        .adjustmentFunction = ADJUSTMENT_PITCH_P,
        .mode = ADJUSTMENT_MODE_STEP,
388
        .data = { .stepConfig = { .step = 1 }}
389
390
391
392
    },
    {
        .adjustmentFunction = ADJUSTMENT_PITCH_I,
        .mode = ADJUSTMENT_MODE_STEP,
393
        .data = { .stepConfig = { .step = 1 }}
394
395
396
397
    },
    {
        .adjustmentFunction = ADJUSTMENT_PITCH_D,
        .mode = ADJUSTMENT_MODE_STEP,
398
        .data = { .stepConfig = { .step = 1 }}
399
400
401
402
    },
    {
        .adjustmentFunction = ADJUSTMENT_ROLL_P,
        .mode = ADJUSTMENT_MODE_STEP,
403
        .data = { .stepConfig = { .step = 1 }}
404
405
406
407
    },
    {
        .adjustmentFunction = ADJUSTMENT_ROLL_I,
        .mode = ADJUSTMENT_MODE_STEP,
408
        .data = { .stepConfig = { .step = 1 }}
409
410
411
412
    },
    {
        .adjustmentFunction = ADJUSTMENT_ROLL_D,
        .mode = ADJUSTMENT_MODE_STEP,
413
        .data = { .stepConfig = { .step = 1 }}
414
415
416
    }
};

417
418
#define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1

419
adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
420

421
void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) {
422
    adjustmentState_t *adjustmentState = &adjustmentStates[index];
423

424
    if (adjustmentState->config == adjustmentConfig) {
425
426
427
428
        // already configured
        return;
    }
    adjustmentState->auxChannelIndex = auxSwitchChannelIndex;
429
    adjustmentState->config = adjustmentConfig;
430
431
432
433
434
    adjustmentState->timeoutAt = 0;

    MARK_ADJUSTMENT_FUNCTION_AS_READY(index);
}

435
void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
436
    int newValue;
437
    float newFloatValue;
438

439
    if (delta > 0) {
440
        beeperConfirmationBeeps(2);
441
    } else {
442
        beeperConfirmationBeeps(1);
443
    }
444
445
    switch(adjustmentFunction) {
        case ADJUSTMENT_RC_RATE:
446
            newValue = (int)controlRateConfig->rcRate8 + delta;
447
448
449
            controlRateConfig->rcRate8 = constrain(newValue, 0, 250); // FIXME magic numbers repeated in serial_cli.c
            generatePitchRollCurve(controlRateConfig);
        break;
450
451
452
453
454
        case ADJUSTMENT_RC_EXPO:
            newValue = (int)controlRateConfig->rcExpo8 + delta;
            controlRateConfig->rcExpo8 = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
            generatePitchRollCurve(controlRateConfig);
            break;
455
456
457
458
459
        case ADJUSTMENT_THROTTLE_EXPO:
            newValue = (int)controlRateConfig->thrExpo8 + delta;
            controlRateConfig->thrExpo8 = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
            generateThrottleCurve(controlRateConfig, escAndServoConfig);
            break;
460
        case ADJUSTMENT_PITCH_ROLL_RATE:
461
        case ADJUSTMENT_PITCH_RATE:
462
            newValue = (int)controlRateConfig->rates[FD_PITCH] + delta;
463
            controlRateConfig->rates[FD_PITCH] = constrain(newValue, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
464
465
466
467
468
            if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
                break;
            }
            // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
        case ADJUSTMENT_ROLL_RATE:
469
            newValue = (int)controlRateConfig->rates[FD_ROLL] + delta;
470
            controlRateConfig->rates[FD_ROLL] = constrain(newValue, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
471
            break;
472
        case ADJUSTMENT_YAW_RATE:
473
            newValue = (int)controlRateConfig->rates[FD_YAW] + delta;
474
            controlRateConfig->rates[FD_YAW] = constrain(newValue, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
475
            break;
476
        case ADJUSTMENT_PITCH_ROLL_P:
477
        case ADJUSTMENT_PITCH_P:
478
            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
479
480
                newFloatValue = pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f);
                pidProfile->P_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
481
482
483
            } else {
                newValue = (int)pidProfile->P8[PIDPITCH] + delta;
                pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
484
485
486
487
488
489
490
491
492
493
            }
            if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
                break;
            }
            // follow though for combined ADJUSTMENT_PITCH_ROLL_P
        case ADJUSTMENT_ROLL_P:
            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
                newFloatValue = pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f);
                pidProfile->P_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
            } else {
494
495
496
                newValue = (int)pidProfile->P8[PIDROLL] + delta;
                pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
            }
497
498
            break;
        case ADJUSTMENT_PITCH_ROLL_I:
499
        case ADJUSTMENT_PITCH_I:
500
            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
501
                newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f);
502
                pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
503
504
505
            } else {
                newValue = (int)pidProfile->I8[PIDPITCH] + delta;
                pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
506
507
508
509
510
511
512
513
514
515
            }
            if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
                break;
            }
            // follow though for combined ADJUSTMENT_PITCH_ROLL_I
        case ADJUSTMENT_ROLL_I:
            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
                newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f);
                pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
            } else {
516
517
518
                newValue = (int)pidProfile->I8[PIDROLL] + delta;
                pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
            }
519
520
            break;
        case ADJUSTMENT_PITCH_ROLL_D:
521
        case ADJUSTMENT_PITCH_D:
522
            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
523
                newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f);
524
                pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
525
526
527
            } else {
                newValue = (int)pidProfile->D8[PIDPITCH] + delta;
                pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
528
529
530
531
532
533
534
535
536
537
            }
            if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
                break;
            }
            // follow though for combined ADJUSTMENT_PITCH_ROLL_D
        case ADJUSTMENT_ROLL_D:
            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
                newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f);
                pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
            } else {
538
539
540
                newValue = (int)pidProfile->D8[PIDROLL] + delta;
                pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
            }
541
542
            break;
        case ADJUSTMENT_YAW_P:
543
            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
544
545
                newFloatValue = pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f);
                pidProfile->P_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
546
547
548
549
            } else {
                newValue = (int)pidProfile->P8[PIDYAW] + delta;
                pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
            }
550
551
            break;
        case ADJUSTMENT_YAW_I:
552
            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
553
                newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f);
554
                pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
555
556
557
558
            } else {
                newValue = (int)pidProfile->I8[PIDYAW] + delta;
                pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
            }
559
560
            break;
        case ADJUSTMENT_YAW_D:
561
            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
562
                newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f);
563
                pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
564
565
566
567
            } else {
                newValue = (int)pidProfile->D8[PIDYAW] + delta;
                pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
            }
568
            break;
569
570
571
572
573
        default:
            break;
    };
}

574
575
void changeControlRateProfile(uint8_t profileIndex);

576
577
578
void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
{
    bool applied = false;
579
580
581
582
583

    switch(adjustmentFunction) {
        case ADJUSTMENT_RATE_PROFILE:
            if (getCurrentControlRateProfile() != position) {
                changeControlRateProfile(position);
584
                applied = true;
585
586
587
            }
            break;
    }
588
589

    if (applied) {
590
        beeperConfirmationBeeps(position + 1);
591
    }
592
593
}

594
595
596
597
598
599
600
#define RESET_FREQUENCY_2HZ (1000 / 2)

void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig)
{
    uint8_t adjustmentIndex;
    uint32_t now = millis();

601
602
603
    bool canUseRxData = rxIsReceivingSignal();


604
    for (adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
605
        adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex];
606

607
608
609
610
        if (!adjustmentState->config) {
            continue;
        }
        uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction;
611
612
613
614
        if (adjustmentFunction == ADJUSTMENT_NONE) {
            continue;
        }

615
        int32_t signedDiff = now - adjustmentState->timeoutAt;
616
617
        bool canResetReadyStates = signedDiff >= 0L;

618
        if (canResetReadyStates) {
619
            adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
620
            MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
621
622
        }

623
624
625
        if (!canUseRxData) {
            continue;
        }
626

627
        uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex;
628

629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
        if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
            int delta;
            if (rcData[channelIndex] > rxConfig->midrc + 200) {
                delta = adjustmentState->config->data.stepConfig.step;
            } else if (rcData[channelIndex] < rxConfig->midrc - 200) {
                delta = 0 - adjustmentState->config->data.stepConfig.step;
            } else {
                // returning the switch to the middle immediately resets the ready state
                MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
                adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
                continue;
            }
            if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) {
                continue;
            }
644

645
            applyStepAdjustment(controlRateConfig, adjustmentFunction, delta);
646
647
648
649
650
        } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
            uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
            uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;

            applySelectAdjustment(adjustmentFunction, position);
651
        }
652
        MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex);
653
654
655
    }
}

656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges)
{
    uint8_t index;

    for (index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
        adjustmentRange_t *adjustmentRange = &adjustmentRanges[index];

        if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) {

            const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];

            configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig);
        }
    }
}

672
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
673
    return MIN(ABS(rcData[axis] - midrc), 500);
674
675
}

676
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse)
677
678
679
{
    uint8_t index;

680
    escAndServoConfig = escAndServoConfigToUse;
681
    pidProfile = pidProfileToUse;
682

683
684
    isUsingSticksToArm = true;

685
686
    for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
        modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
687
        if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
688
689
690
691
692
            isUsingSticksToArm = false;
            break;
        }
    }
}
693
694
695
696
697
698

void resetAdjustmentStates(void)
{
    memset(adjustmentStates, 0, sizeof(adjustmentStates));
}