void processRcStickPositions(throttleStatus_e throttleStatus)
{
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
static uint8_t rcDisarmTicks; // this is an extra guard for disarming through switch to prevent that one frame can disarm it
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
if (!isUsingSticksToArm) {
if (IS_RC_MODE_ACTIVE(BOXARM)) {
rcDisarmTicks = 0;
// Arming via ARM BOX
tryArm();
} else {
// Disarming via ARM BOX
resetArmingDisabled();
if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) {
rcDisarmTicks++;
if (rcDisarmTicks > 3) {
if (armingConfig()->disarm_kill_switch) {
disarm();
} else if (throttleStatus == THROTTLE_LOW) {
disarm();
}
}
}
}
}
if (rcDelayCommand != 20) {
return;
}
if (isUsingSticksToArm) {
// Disarm on throttle down + yaw
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (ARMING_FLAG(ARMED))
disarm();
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
rcDelayCommand = 0; // reset so disarm tone will repeat
}
}
}
if (ARMING_FLAG(ARMED)) {
// actions during armed
return;
}
// actions during not armed
i = 0;
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
// GYRO calibration
gyroStartCalibration(false);
#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
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) {
changePidProfile(i - 1);
return;
}
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
saveConfigAndNotify();
}
if (isUsingSticksToArm) {
if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
// Arm via YAW
tryArm();
return;
} else {
resetArmingDisabled();
}
}
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;
}
#ifdef USE_DASHBOARD
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
dashboardDisablePageCycling();
}
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
dashboardEnablePageCycling();
}
#endif
#ifdef VTX_CONTROL
if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI) {
vtxIncrementBand();
}
if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO) {
vtxDecrementBand();
}
if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_HI) {
vtxIncrementChannel();
}
if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_LO) {
vtxDecrementChannel();
}
#endif
}
void processRcStickPositions(throttleStatus_e throttleStatus)
{
// time the sticks are maintained
static int16_t rcDelayMs;
// hold sticks position for command combos
static uint8_t rcSticks;
// an extra guard for disarming through switch to prevent that one frame can disarm it
static uint8_t rcDisarmTicks;
static bool doNotRepeat;
#ifdef USE_CMS
if (cmsInMenu) {
return;
}
#endif
// checking sticks positions
uint8_t stTmp = 0;
for (int 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 (rcDelayMs <= INT16_MAX - (getTaskDeltaTime(TASK_SELF) / 1000)) {
rcDelayMs += getTaskDeltaTime(TASK_SELF) / 1000;
}
} else {
rcDelayMs = 0;
doNotRepeat = false;
}
rcSticks = stTmp;
// perform actions
if (!isUsingSticksToArm) {
if (IS_RC_MODE_ACTIVE(BOXARM)) {
rcDisarmTicks = 0;
// Arming via ARM BOX
tryArm();
} else {
// Disarming via ARM BOX
resetArmingDisabled();
if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) {
rcDisarmTicks++;
if (rcDisarmTicks > 3) {
if (armingConfig()->disarm_kill_switch) {
disarm();
} else if (throttleStatus == THROTTLE_LOW) {
disarm();
}
}
}
}
} else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
doNotRepeat = true;
// Disarm on throttle down + yaw
if (ARMING_FLAG(ARMED))
disarm();
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
repeatAfter(STICK_AUTOREPEAT_MS); // disarm tone will repeat
}
}
return;
} else if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
doNotRepeat = true;
if (!ARMING_FLAG(ARMED)) {
// Arm via YAW
tryArm();
} else {
resetArmingDisabled();
}
}
return;
}
if (ARMING_FLAG(ARMED) || doNotRepeat || rcDelayMs <= STICK_DELAY_MS) {
return;
}
doNotRepeat = true;
// actions during not armed
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
// GYRO calibration
gyroStartCalibration(false);
#ifdef USE_GPS
if (feature(FEATURE_GPS)) {
GPS_reset_home_position();
}
#endif
#ifdef USE_BARO
if (sensors(SENSOR_BARO))
baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
#endif
return;
}
if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
// Inflight ACC Calibration
handleInflightCalibrationStickPosition();
return;
}
// Change PID profile
int newPidProfile = 0;
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) { // ROLL left -> PID profile 1
newPidProfile = 1;
} else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) { // PITCH up -> PID profile 2
newPidProfile = 2;
} else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) { // ROLL right -> PID profile 3
newPidProfile = 3;
}
if (newPidProfile) {
changePidProfile(newPidProfile - 1);
return;
}
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
saveConfigAndNotify();
}
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;
}
if (FLIGHT_MODE(ANGLE_MODE|HORIZON_MODE)) {
// in ANGLE or HORIZON mode, so use sticks to apply accelerometer trims
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);
repeatAfter(STICK_AUTOREPEAT_MS);
return;
}
} else {
// in ACRO mode, so use sticks to change RATE profile
switch (rcSticks) {
case THR_HI + YAW_CE + PIT_HI + ROL_CE:
changeControlRateProfile(0);
return;
case THR_HI + YAW_CE + PIT_LO + ROL_CE:
changeControlRateProfile(1);
return;
case THR_HI + YAW_CE + PIT_CE + ROL_HI:
changeControlRateProfile(2);
return;
case THR_HI + YAW_CE + PIT_CE + ROL_LO:
changeControlRateProfile(3);
return;
}
}
#ifdef USE_DASHBOARD
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
dashboardDisablePageCycling();
}
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
dashboardEnablePageCycling();
}
#endif
#ifdef USE_VTX_CONTROL
if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI) {
vtxIncrementBand();
}
if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO) {
vtxDecrementBand();
}
if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_HI) {
vtxIncrementChannel();
}
if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_LO) {
vtxDecrementChannel();
}
#endif
#ifdef USE_CAMERA_CONTROL
if (rcSticks == THR_CE + YAW_HI + PIT_CE + ROL_CE) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_ENTER, 0);
repeatAfter(3 * STICK_DELAY_MS);
} else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_LO) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_LEFT, 0);
repeatAfter(3 * STICK_DELAY_MS);
} else if (rcSticks == THR_CE + YAW_CE + PIT_HI + ROL_CE) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 0);
repeatAfter(3 * STICK_DELAY_MS);
} else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_HI) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_RIGHT, 0);
repeatAfter(3 * STICK_DELAY_MS);
} else if (rcSticks == THR_CE + YAW_CE + PIT_LO + ROL_CE) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_DOWN, 0);
repeatAfter(3 * STICK_DELAY_MS);
} else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_CE) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 2000);
}
#endif
}