void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{
currentProfile->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
currentProfile->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
saveConfigAndNotify();
}
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{
accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
saveConfigAndNotify();
}
void vtxInit(void)
{
bool settingsUpdated = false;
// sync frequency in parameter group when band/channel are specified
const uint16_t freq = vtx58_Bandchan2Freq(vtxSettingsConfig()->band, vtxSettingsConfig()->channel);
if (vtxSettingsConfig()->band && freq != vtxSettingsConfig()->freq) {
vtxSettingsConfigMutable()->freq = freq;
settingsUpdated = true;
}
#if defined(VTX_SETTINGS_FREQCMD)
// constrain pit mode frequency
if (vtxSettingsConfig()->pitModeFreq) {
const uint16_t constrainedPitModeFreq = MAX(vtxSettingsConfig()->pitModeFreq, VTX_SETTINGS_MIN_USER_FREQ);
if (constrainedPitModeFreq != vtxSettingsConfig()->pitModeFreq) {
vtxSettingsConfigMutable()->pitModeFreq = constrainedPitModeFreq;
settingsUpdated = true;
}
}
#endif
if (settingsUpdated) {
saveConfigAndNotify();
}
}
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{
masterConfig.accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
masterConfig.accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
saveConfigAndNotify();
}
void updateAutotuneState(void)
{
static bool landedAfterAutoTuning = false;
static bool autoTuneWasUsed = false;
if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
if (!FLIGHT_MODE(AUTOTUNE_MODE)) {
if (ARMING_FLAG(ARMED)) {
if (isAutotuneIdle() || landedAfterAutoTuning) {
autotuneReset();
landedAfterAutoTuning = false;
}
autotuneBeginNextPhase(¤tProfile->pidProfile);
ENABLE_FLIGHT_MODE(AUTOTUNE_MODE);
autoTuneWasUsed = true;
} else {
if (havePidsBeenUpdatedByAutotune()) {
saveConfigAndNotify();
autotuneReset();
}
}
}
return;
}
if (FLIGHT_MODE(AUTOTUNE_MODE)) {
autotuneEndPhase();
DISABLE_FLIGHT_MODE(AUTOTUNE_MODE);
}
if (!ARMING_FLAG(ARMED) && autoTuneWasUsed) {
landedAfterAutoTuning = true;
}
}
long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
{
if (ptr) {
displayClear(pDisplay);
displayWrite(pDisplay, 5, 3, "REBOOTING...");
displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing?
stopMotors();
stopPwmAllMotors();
delay(200);
cmsTraverseGlobalExit(&menuMain);
if (currentMenu->onExit)
currentMenu->onExit((OSD_Entry *)NULL); // Forced exit
saveConfigAndNotify();
}
cmsInMenu = false;
displayRelease(pDisplay);
currentMenu = NULL;
if (ptr)
systemReset();
ENABLE_ARMING_FLAG(OK_TO_ARM);
return 0;
}
void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
{
static int32_t a[3];
uint8_t axis;
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (isOnFirstAccelerationCalibrationCycle())
a[axis] = 0;
// Sum up CALIBRATING_ACC_CYCLES readings
a[axis] += accADC[axis];
// Reset global variables to prevent other code from using un-calibrated data
accADC[axis] = 0;
accelerationTrims->raw[axis] = 0;
}
if (isOnFinalAccelerationCalibrationCycle()) {
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
accelerationTrims->raw[X] = (a[X] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
accelerationTrims->raw[Y] = (a[Y] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
accelerationTrims->raw[Z] = (a[Z] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
resetRollAndPitchTrims(rollAndPitchTrims);
saveConfigAndNotify();
}
calibratingA--;
}
static void cmsx_Vtx_ConfigWriteback(void)
{
// update vtx_ settings
vtxSettingsConfigMutable()->band = cmsx_vtxBand + 1;
vtxSettingsConfigMutable()->channel = cmsx_vtxChannel;
vtxSettingsConfigMutable()->power = cmsx_vtxPower + VTX_RTC6705_MIN_POWER;
vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(cmsx_vtxBand + 1, cmsx_vtxChannel);
saveConfigAndNotify();
}
static long saCmsCommence(displayPort_t *pDisp, const void *self)
{
UNUSED(pDisp);
UNUSED(self);
const vtxSettingsConfig_t prevSettings = {
.band = vtxSettingsConfig()->band,
.channel = vtxSettingsConfig()->channel,
.freq = vtxSettingsConfig()->freq,
.power = vtxSettingsConfig()->power,
.lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm,
};
vtxSettingsConfig_t newSettings = prevSettings;
if (saCmsOpmodel == SACMS_OPMODEL_RACE) {
// Race model
// Setup band, freq and power.
newSettings.band = saCmsBand;
newSettings.channel = saCmsChan;
newSettings.freq = vtx58_Bandchan2Freq(saCmsBand, saCmsChan);
// If in pit mode, cancel it.
if (saCmsPitFMode == 0)
saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE);
else
saSetMode(SA_MODE_CLR_PITMODE|SA_MODE_SET_OUT_RANGE_PITMODE);
} else {
// Freestyle model
// Setup band and freq / user freq
if (saCmsFselModeNew == 0) {
newSettings.band = saCmsBand;
newSettings.channel = saCmsChan;
newSettings.freq = vtx58_Bandchan2Freq(saCmsBand, saCmsChan);
} else {
saSetMode(0); //make sure FREE mode is setup
newSettings.band = 0;
newSettings.freq = saCmsUserFreq;
}
}
newSettings.power = saCmsPower;
if (memcmp(&prevSettings, &newSettings, sizeof(vtxSettingsConfig_t))) {
vtxSettingsConfigMutable()->band = newSettings.band;
vtxSettingsConfigMutable()->channel = newSettings.channel;
vtxSettingsConfigMutable()->power = newSettings.power;
vtxSettingsConfigMutable()->freq = newSettings.freq;
saveConfigAndNotify();
}
return MENU_CHAIN_BACK;
}
void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
{
uint8_t axis;
static int32_t b[3];
static int16_t accZero_saved[3] = { 0, 0, 0 };
static rollAndPitchTrims_t angleTrim_saved = { { 0, 0 } };
// Saving old zeropoints before measurement
if (InflightcalibratingA == 50) {
accZero_saved[X] = accelerationTrims->raw[X];
accZero_saved[Y] = accelerationTrims->raw[Y];
accZero_saved[Z] = accelerationTrims->raw[Z];
angleTrim_saved.values.roll = rollAndPitchTrims->values.roll;
angleTrim_saved.values.pitch = rollAndPitchTrims->values.pitch;
}
if (InflightcalibratingA > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (InflightcalibratingA == 50)
b[axis] = 0;
// Sum up 50 readings
b[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
accelerationTrims->raw[axis] = 0;
}
// all values are measured
if (InflightcalibratingA == 1) {
AccInflightCalibrationActive = false;
AccInflightCalibrationMeasurementDone = true;
queueConfirmationBeep(5); // beeper to indicating the end of calibration
// recover saved values to maintain current flight behaviour until new values are transferred
accelerationTrims->raw[X] = accZero_saved[X];
accelerationTrims->raw[Y] = accZero_saved[Y];
accelerationTrims->raw[Z] = accZero_saved[Z];
rollAndPitchTrims->values.roll = angleTrim_saved.values.roll;
rollAndPitchTrims->values.pitch = angleTrim_saved.values.pitch;
}
InflightcalibratingA--;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (AccInflightCalibrationSavetoEEProm) { // the aircraft is landed, disarmed and the combo has been done again
AccInflightCalibrationSavetoEEProm = false;
accelerationTrims->raw[X] = b[X] / 50;
accelerationTrims->raw[Y] = b[Y] / 50;
accelerationTrims->raw[Z] = b[Z] / 50 - acc_1G; // for nunchuck 200=1G
resetRollAndPitchTrims(rollAndPitchTrims);
saveConfigAndNotify();
}
}
void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
{
static uint32_t tCal = 0;
static flightDynamicsTrims_t magZeroTempMin;
static flightDynamicsTrims_t magZeroTempMax;
magDev.read(&magDev, magADCRaw);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
mag.magADC[axis] = magADCRaw[axis];
}
alignSensors(mag.magADC, magDev.magAlign);
if (STATE(CALIBRATE_MAG)) {
tCal = currentTime;
for (int axis = 0; axis < 3; axis++) {
magZero->raw[axis] = 0;
magZeroTempMin.raw[axis] = mag.magADC[axis];
magZeroTempMax.raw[axis] = mag.magADC[axis];
}
DISABLE_STATE(CALIBRATE_MAG);
}
if (magInit) { // we apply offset only once mag calibration is done
mag.magADC[X] -= magZero->raw[X];
mag.magADC[Y] -= magZero->raw[Y];
mag.magADC[Z] -= magZero->raw[Z];
}
if (tCal != 0) {
if ((currentTime - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
LED0_TOGGLE;
for (int axis = 0; axis < 3; axis++) {
if (mag.magADC[axis] < magZeroTempMin.raw[axis])
magZeroTempMin.raw[axis] = mag.magADC[axis];
if (mag.magADC[axis] > magZeroTempMax.raw[axis])
magZeroTempMax.raw[axis] = mag.magADC[axis];
}
} else {
tCal = 0;
for (int axis = 0; axis < 3; axis++) {
magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets
}
saveConfigAndNotify();
}
}
}
void updateGtuneState(void)
{
static bool GTuneWasUsed = false;
if (IS_RC_MODE_ACTIVE(BOXGTUNE)) {
if (!FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
ENABLE_FLIGHT_MODE(GTUNE_MODE);
init_Gtune(¤tProfile->pidProfile);
GTuneWasUsed = true;
}
if (!FLIGHT_MODE(GTUNE_MODE) && !ARMING_FLAG(ARMED) && GTuneWasUsed) {
saveConfigAndNotify();
GTuneWasUsed = false;
}
} else {
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
DISABLE_FLIGHT_MODE(GTUNE_MODE);
}
}
}
void updateGtuneState(void)
{
static bool GTuneWasUsed = false;
if (rcModeIsActive(BOXGTUNE)) {
if (!FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
ENABLE_FLIGHT_MODE(GTUNE_MODE);
init_Gtune();
GTuneWasUsed = true;
}
if (!FLIGHT_MODE(GTUNE_MODE) && !ARMING_FLAG(ARMED) && GTuneWasUsed) {
saveConfigAndNotify();
GTuneWasUsed = false;
}
} else {
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
DISABLE_FLIGHT_MODE(GTUNE_MODE);
}
}
}
void osdExitMenu(void *ptr)
{
max7456ClearScreen();
max7456Write(5, 3, "RESTARTING IMU...");
max7456RefreshAll();
stopMotors();
stopPwmAllMotors();
delay(200);
if (ptr) {
// save local variables to configuration
if (featureBlackbox)
featureSet(FEATURE_BLACKBOX);
else
featureClear(FEATURE_BLACKBOX);
if (featureLedstrip)
featureSet(FEATURE_LED_STRIP);
else
featureClear(FEATURE_LED_STRIP);
#if defined(VTX) || defined(USE_RTC6705)
if (featureVtx)
featureSet(FEATURE_VTX);
else
featureClear(FEATURE_VTX);
#endif // VTX || USE_RTC6705
#ifdef VTX
masterConfig.vtxBand = vtxBand;
masterConfig.vtx_channel = vtxChannel - 1;
#endif // VTX
#ifdef USE_RTC6705
masterConfig.vtx_channel = vtxBand * 8 + vtxChannel - 1;
#endif // USE_RTC6705
saveConfigAndNotify();
}
systemReset();
}
void applyAndSaveBoardAlignmentDelta(int16_t roll, int16_t pitch)
{
updateBoardAlignment(&masterConfig.boardAlignment, roll, pitch);
saveConfigAndNotify();
}
void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm)
{
static timeMs_t lastTickTimeMs = 0;
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 uint32_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
const timeMs_t currentTimeMs = millis();
updateRcStickPositions();
uint32_t stTmp = getRcStickPositions();
if (stTmp == rcSticks) {
if (rcDelayCommand < 250) {
if ((currentTimeMs - lastTickTimeMs) >= MIN_RC_TICK_INTERVAL_MS) {
lastTickTimeMs = currentTimeMs;
rcDelayCommand++;
}
}
} else
rcDelayCommand = 0;
rcSticks = stTmp;
// perform actions
if (!isUsingSticksToArm) {
if (IS_RC_MODE_ACTIVE(BOXARM)) {
rcDisarmTicks = 0;
// Arming via ARM BOX
if (throttleStatus == THROTTLE_LOW) {
if (ARMING_FLAG(OK_TO_ARM)) {
mwArm();
}
}
} else {
// Disarming via ARM BOX
// Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver
// and can't afford to risk disarming in the air
if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) {
rcDisarmTicks++;
if (rcDisarmTicks > 3) { // Wait for at least 3 RX ticks (60ms @ 50Hz RX)
if (disarm_kill_switch) {
mwDisarm(DISARM_SWITCH);
} else if (throttleStatus == THROTTLE_LOW) {
mwDisarm(DISARM_SWITCH);
}
}
}
else {
rcDisarmTicks = 0;
}
}
}
// KILLSWITCH disarms instantly
if (IS_RC_MODE_ACTIVE(BOXKILLSWITCH)) {
mwDisarm(DISARM_KILLSWITCH);
}
if (rcDelayCommand != 20) {
return;
}
if (isUsingSticksToArm) {
// Disarm on throttle down + yaw
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
// Dont disarm if fixedwing and motorstop
if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && fixed_wing_auto_arm) {
return;
}
else if (ARMING_FLAG(ARMED)) {
mwDisarm(DISARM_STICKS);
}
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
int i = 0;
// GYRO calibration
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
return;
}
#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
// Save waypoint list
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
const bool success = saveNonVolatileWaypointList();
beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
}
// Load waypoint list
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
const bool success = loadNonVolatileWaypointList();
beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
}
#endif
// 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) {
setConfigProfileAndWriteEEPROM(i - 1);
return;
}
// Save config
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
saveConfigAndNotify();
}
// Arming by sticks
if (isUsingSticksToArm) {
if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && fixed_wing_auto_arm) {
// Auto arm on throttle when using fixedwing and motorstop
if (throttleStatus != THROTTLE_LOW) {
mwArm();
return;
}
}
else {
if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
// Arm via YAW
mwArm();
return;
}
}
}
// Calibrating Acc
if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
return;
}
// Calibrating Mag
if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
ENABLE_STATE(CALIBRATE_MAG);
return;
}
// Accelerometer Trim
if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
applyAndSaveBoardAlignmentDelta(0, -2);
rcDelayCommand = 10;
return;
} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
applyAndSaveBoardAlignmentDelta(0, 2);
rcDelayCommand = 10;
return;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
applyAndSaveBoardAlignmentDelta(-2, 0);
rcDelayCommand = 10;
return;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
applyAndSaveBoardAlignmentDelta(2, 0);
rcDelayCommand = 10;
return;
}
}
void performAcclerationCalibration(void)
{
int axisIndex = getPrimaryAxisIndex(accADC);
// Check if sample is usable
if (axisIndex < 0) {
return;
}
// Top-up and first calibration cycle, reset everything
if (axisIndex == 0 && isOnFirstAccelerationCalibrationCycle()) {
for (int axis = 0; axis < 6; axis++) {
calibratedAxis[axis] = false;
accSamples[axis][X] = 0;
accSamples[axis][Y] = 0;
accSamples[axis][Z] = 0;
}
calibratedAxisCount = 0;
sensorCalibrationResetState(&calState);
}
if (!calibratedAxis[axisIndex]) {
sensorCalibrationPushSampleForOffsetCalculation(&calState, accADC);
accSamples[axisIndex][X] += accADC[X];
accSamples[axisIndex][Y] += accADC[Y];
accSamples[axisIndex][Z] += accADC[Z];
if (isOnFinalAccelerationCalibrationCycle()) {
calibratedAxis[axisIndex] = true;
calibratedAxisCount++;
beeperConfirmationBeeps(2);
}
}
if (calibratedAxisCount == 6) {
float accTmp[3];
int32_t accSample[3];
/* Calculate offset */
sensorCalibrationSolveForOffset(&calState, accTmp);
for (int axis = 0; axis < 3; axis++) {
accZero->raw[axis] = lrintf(accTmp[axis]);
}
/* Not we can offset our accumulated averages samples and calculate scale factors and calculate gains */
sensorCalibrationResetState(&calState);
for (int axis = 0; axis < 6; axis++) {
accSample[X] = accSamples[axis][X] / CALIBRATING_ACC_CYCLES - accZero->raw[X];
accSample[Y] = accSamples[axis][Y] / CALIBRATING_ACC_CYCLES - accZero->raw[Y];
accSample[Z] = accSamples[axis][Z] / CALIBRATING_ACC_CYCLES - accZero->raw[Z];
sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample, acc.acc_1G);
}
sensorCalibrationSolveForScale(&calState, accTmp);
for (int axis = 0; axis < 3; axis++) {
accGain->raw[axis] = lrintf(accTmp[axis] * 4096);
}
saveConfigAndNotify();
}
calibratingA--;
}
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch)
{
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
if (!isUsingSticksToArm) {
if (rcModeIsActive(BOXARM)) {
// Arming via ARM BOX
if (throttleStatus == THROTTLE_LOW) {
if (ARMING_FLAG(OK_TO_ARM)) {
mwArm();
}
}
} else {
// Disarming via ARM BOX
if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) {
if (disarm_kill_switch) {
mwDisarm();
} else if (throttleStatus == THROTTLE_LOW) {
mwDisarm();
}
}
}
}
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))
mwDisarm();
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
rcDelayCommand = 0; // reset so disarm tone will repeat
}
}
// Disarm on roll (only when retarded_arm is enabled)
if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) {
if (ARMING_FLAG(ARMED))
mwDisarm();
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
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
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);
beeperConfirmationBeeps(i);
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
mwArm();
return;
}
if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI)) {
// Arm via ROLL
mwArm();
return;
}
}
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 DISPLAY
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
displayDisablePageCycling();
}
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
displayEnablePageCycling();
}
#endif
}
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
}
