diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index b03b4f0a33..7c5ec33cc2 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -42,7 +42,7 @@ typedef struct profile_s { uint8_t acc_unarmedcal; // turn automatic acc compensation on/off - uint32_t activate[CHECKBOX_ITEM_COUNT]; // activate switches, bitmask, 3 bits per channel, lower 16 bits aux1-4, upper 16 bits aux 5-8 + modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; // Radio/ESC-related configuration uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index c916a0496b..6bb7f67ceb 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -140,7 +140,7 @@ void applyAltHold(void) void updateAltHoldState(void) { // Baro alt hold activate - if (!rcOptions[BOXBARO]) { + if (!IS_RC_MODE_ACTIVE(BOXBARO)) { DISABLE_FLIGHT_MODE(BARO_MODE); return; } @@ -157,7 +157,7 @@ void updateAltHoldState(void) void updateSonarAltHoldState(void) { // Sonar alt hold activate - if (!rcOptions[BOXSONAR]) { + if (!IS_RC_MODE_ACTIVE(BOXSONAR)) { DISABLE_FLIGHT_MODE(SONAR_MODE); return; } diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 1f82938879..e7c0f47a7d 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -532,7 +532,7 @@ void mixTable(void) servo[0] = determineServoMiddleOrForwardFromChannel(0); servo[1] = determineServoMiddleOrForwardFromChannel(1); - if (rcOptions[BOXCAMSTAB]) { + if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { if (gimbalConfig->gimbal_flags & GIMBAL_MIXTILT) { servo[0] -= (-(int32_t)servoConf[0].rate) * inclination.values.pitchDeciDegrees / 50 - (int32_t)servoConf[1].rate * inclination.values.rollDeciDegrees / 50; servo[1] += (-(int32_t)servoConf[0].rate) * inclination.values.pitchDeciDegrees / 50 + (int32_t)servoConf[1].rate * inclination.values.rollDeciDegrees / 50; diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 8c89dd14c7..1326dfcbef 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -652,7 +652,7 @@ void updateGpsWaypointsAndMode(void) if (STATE(GPS_FIX) && GPS_numSat >= 5) { // if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority - if (rcOptions[BOXGPSHOME]) { + if (IS_RC_MODE_ACTIVE(BOXGPSHOME)) { if (!FLIGHT_MODE(GPS_HOME_MODE)) { ENABLE_FLIGHT_MODE(GPS_HOME_MODE); DISABLE_FLIGHT_MODE(GPS_HOLD_MODE); @@ -663,7 +663,7 @@ void updateGpsWaypointsAndMode(void) } else { DISABLE_FLIGHT_MODE(GPS_HOME_MODE); - if (rcOptions[BOXGPSHOLD] && areSticksInApModePosition(gpsProfile->ap_mode)) { + if (IS_RC_MODE_ACTIVE(BOXGPSHOLD) && areSticksInApModePosition(gpsProfile->ap_mode)) { if (!FLIGHT_MODE(GPS_HOLD_MODE)) { ENABLE_FLIGHT_MODE(GPS_HOLD_MODE); GPSNavReset = 0; diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index e0d93c1e1c..2194de7868 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -68,7 +68,7 @@ void beepcodeUpdateState(bool warn_vbat) static failsafeBeeperWarnings_e warn_failsafe = FAILSAFE_IDLE; //===================== BeeperOn via rcOptions ===================== - if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch + if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) { // unconditional beeper on via AUXn switch beeperOnBox = 1; } else { beeperOnBox = 0; @@ -95,7 +95,7 @@ void beepcodeUpdateState(bool warn_vbat) #ifdef GPS //===================== GPS fix notification handling ===================== if (sensors(SENSOR_GPS)) { - if ((rcOptions[BOXGPSHOME] || rcOptions[BOXGPSHOLD]) && !STATE(GPS_FIX)) { // if no fix and gps funtion is activated: do warning beeps + if ((IS_RC_MODE_ACTIVE(BOXGPSHOME) || IS_RC_MODE_ACTIVE(BOXGPSHOLD)) && !STATE(GPS_FIX)) { // if no fix and gps funtion is activated: do warning beeps warn_noGPSfix = 1; } else { warn_noGPSfix = 0; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 91a2a95903..c5ac654fff 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -43,14 +43,7 @@ int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW -// each entry in the array is a bitmask, 3 bits per aux channel (only aux 1 to 4), aux1 is first, each bit corresponds to an rc channel reading -// bit 1 - stick LOW -// bit 2 - stick MIDDLE -// bit 3 - stick HIGH -// an option is enabled when ANY channel has an appropriate reading corresponding to the bit. -// an option is disabled when NO channel has an appropriate reading corresponding to the bit. -// example: 110000000001 - option is only enabled when AUX1 is LOW or AUX4 is MEDIUM or HIGH. -uint8_t rcOptions[CHECKBOX_ITEM_COUNT]; +uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e bool areSticksInApModePosition(uint16_t ap_mode) { @@ -67,7 +60,9 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband return THROTTLE_HIGH; } -void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, uint32_t *activate, bool retarded_arm, bool disarm_kill_switch) + + +void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, modeActivationCondition_t *modeActivationConditions, 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 @@ -90,10 +85,12 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat rcDelayCommand = 0; rcSticks = stTmp; - // perform actions - if (activate[BOXARM] > 0) { + bool isUsingSticksToArm = true; // FIXME calculate from modeActivationConditions // FIXME this calculation only needs to be done after loading a profile - if (rcOptions[BOXARM]) { + // perform actions + if (!isUsingSticksToArm) { + + if (IS_RC_MODE_ACTIVE(BOXARM)) { // Arming via ARM BOX if (throttleStatus == THROTTLE_LOW) { if (ARMING_FLAG(OK_TO_ARM)) { @@ -119,14 +116,15 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat if (ARMING_FLAG(ARMED)) { // actions during armed - // Disarm on throttle down + yaw - if (activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE)) - mwDisarm(); - - // Disarm on roll (only when retarded_arm is enabled) - if (retarded_arm && activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) - mwDisarm(); + if (isUsingSticksToArm) { + // Disarm on throttle down + yaw + if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) + mwDisarm(); + // Disarm on roll (only when retarded_arm is enabled) + if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) + mwDisarm(); + } return; } @@ -172,16 +170,19 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat return; } - if (activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE)) { - // Arm via YAW - mwArm(); - return; - } + if (isUsingSticksToArm) { - if (retarded_arm && activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI)) { - // Arm via ROLL - mwArm(); - return; + 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) { @@ -226,40 +227,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat #define MAX_AUX_STATE_CHANNELS 8 -void updateRcOptions(uint32_t *activate) +void updateRcOptions(modeActivationCondition_t *modeActivationConditions) { - // auxState is a bitmask, 3 bits per channel. - // lower 16 bits contain aux 4 to 1 (msb to lsb) - // upper 16 bits contain aux 8 to 5 (msb to lsb) - // - // the three bits are as follows: - // bit 1 is SET when the stick is less than 1300 - // bit 2 is SET when the stick is between 1300 and 1700 - // bit 3 is SET when the stick is above 1700 - - // if the value is 1300 or 1700 NONE of the three bits are set. - - int i; - uint32_t auxState = 0; - uint8_t shift = 0; - int8_t chunkOffset = 0; - - for (i = 0; i < rxRuntimeConfig.auxChannelCount && i < MAX_AUX_STATE_CHANNELS; i++) { - if (i > 0 && i % 4 == 0) { - chunkOffset -= 4; - shift += 16; - } - - uint8_t bitIndex = 3 * (i + chunkOffset); - - uint32_t temp = - (rcData[AUX1 + i] < 1300) << bitIndex | - (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (bitIndex + 1) | - (rcData[AUX1 + i] > 1700) << (bitIndex + 2); - - auxState |= temp << shift; - } - - for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) - rcOptions[i] = (auxState & activate[i]) > 0; + rcModeActivationMask = 0; // FIXME implement, use rcData & modeActivationConditions } diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 9fb6ee7cc5..68a83f3df3 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -44,7 +44,9 @@ typedef enum { CHECKBOX_ITEM_COUNT } boxId_e; -extern uint8_t rcOptions[CHECKBOX_ITEM_COUNT]; +extern uint32_t rcModeActivationMask; + +#define IS_RC_MODE_ACTIVE(modeId) ((1 << modeId) & rcModeActivationMask) typedef enum rc_alias { ROLL = 0, @@ -79,6 +81,33 @@ typedef enum { #define THR_CE (3 << (2 * THROTTLE)) #define THR_HI (2 << (2 * THROTTLE)) +#define MAX_MODE_ACTIVATION_CONDITION_COUNT 40 +// 40 is enough for 1 mode for each position of 11 * 3 position switches and a 6 pos switch. +// however, that is unlikely because you don't define the 'off' positions, so for a 3 position +// switch it's normal that only 2 values would be configured. +// this leaves plenty of 'slots' free for cases where you enable multiple modes for a switch +// position (like gps rth + horizon + baro + beeper) + + +#define MODE_STEP_TO_CHANNEL_VALUE(step) (900 + 25 * step) +#define CHANNEL_VALUE_TO_STEP(channelValue) (constrain(channelValue, 900, 2100) - 900 / 25) + +typedef struct modeActivationCondition_s { + boxId_e modeId; + uint8_t auxChannelIndex; + + // steps are 25 apart + // a value of 0 corresponds to a channel value of 900 or less + // a value of 47 corresponds to a channel value of 2100 or more + // 48 steps between 900 and 1200 + uint8_t rangeStartStep; + uint8_t rangeEndStep; +} modeActivationCondition_t; + +// sizeof(modeActivationCondition_t) * MAX_MODE_ACTIVATION_COUNT = 4 * 40 = 160 bytes +// sizeof(uint32_t) * CHECKBOX_ITEM_COUNT = 4 * 21 = 84 + + typedef struct controlRateConfig_s { uint8_t rcRate8; uint8_t rcExpo8; @@ -92,8 +121,8 @@ extern int16_t rcCommand[4]; bool areSticksInApModePosition(uint16_t ap_mode); throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); -void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, uint32_t *activate, bool retarded_arm, bool disarm_kill_switch); +void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, modeActivationCondition_t *modeActivationConditions, bool retarded_arm, bool disarm_kill_switch); -void updateRcOptions(uint32_t *activate); +void updateRcOptions(modeActivationCondition_t *modeActivationConditions); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 60c5975913..d1f1946db2 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -139,7 +139,7 @@ typedef struct { // should be sorted a..z for bsearch() const clicmd_t cmdTable[] = { - { "aux", "feature_name auxflag or blank for list", cliAux }, + { "aux", "show/set aux settings", cliAux }, { "cmix", "design custom mixer", cliCMix }, #ifdef LED_STRIP { "color", "configure colors", cliColor }, @@ -396,24 +396,31 @@ static int cliCompare(const void *a, const void *b) static void cliAux(char *cmdline) { - int i, val = 0; + int i = 0; uint8_t len; char *ptr; len = strlen(cmdline); if (len == 0) { // print out aux channel settings - for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) - printf("aux %u %u\r\n", i, currentProfile->activate[i]); + for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + modeActivationCondition_t *mac = ¤tProfile->modeActivationConditions[i]; + printf("aux %u %u\r\n", + i, + mac->modeId, + mac->auxChannelIndex, + MODE_STEP_TO_CHANNEL_VALUE(mac->rangeStartStep), + MODE_STEP_TO_CHANNEL_VALUE(mac->rangeEndStep) + ); + } } else { ptr = cmdline; i = atoi(ptr); - if (i < CHECKBOX_ITEM_COUNT) { + if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { ptr = strchr(cmdline, ' '); - val = atoi(ptr); - currentProfile->activate[i] = val; + // FIXME implement setting currentProfile->modeActivationConditions based on remaining string } else { - printf("Invalid Feature index: must be < %u\r\n", CHECKBOX_ITEM_COUNT); + printf("index: must be < %u\r\n", MAX_MODE_ACTIVATION_CONDITION_COUNT); } } } diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 1b4632b671..74c4e0f0bb 100755 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -146,6 +146,7 @@ typedef struct box_e { const uint8_t permanentId; // } box_t; +// FIXME remove ;'s static const box_t const boxes[] = { { BOXARM, "ARM;", 0 }, { BOXANGLE, "ANGLE;", 1 }, @@ -505,20 +506,20 @@ static bool processOutCommand(uint8_t cmdMSP) IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO | IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG | IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE | - rcOptions[BOXHEADADJ] << BOXHEADADJ | - rcOptions[BOXCAMSTAB] << BOXCAMSTAB | - rcOptions[BOXCAMTRIG] << BOXCAMTRIG | + IS_RC_MODE_ACTIVE(BOXHEADADJ) << BOXHEADADJ | + IS_RC_MODE_ACTIVE(BOXCAMSTAB) << BOXCAMSTAB | + IS_RC_MODE_ACTIVE(BOXCAMTRIG) << BOXCAMTRIG | IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE)) << BOXGPSHOME | IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD | IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU | - rcOptions[BOXBEEPERON] << BOXBEEPERON | - rcOptions[BOXLEDMAX] << BOXLEDMAX | - rcOptions[BOXLLIGHTS] << BOXLLIGHTS | - rcOptions[BOXCALIB] << BOXCALIB | - rcOptions[BOXGOV] << BOXGOV | - rcOptions[BOXOSD] << BOXOSD | - rcOptions[BOXTELEMETRY] << BOXTELEMETRY | - rcOptions[BOXAUTOTUNE] << BOXAUTOTUNE | + IS_RC_MODE_ACTIVE(BOXBEEPERON) << BOXBEEPERON | + IS_RC_MODE_ACTIVE(BOXLEDMAX) << BOXLEDMAX | + IS_RC_MODE_ACTIVE(BOXLLIGHTS) << BOXLLIGHTS | + IS_RC_MODE_ACTIVE(BOXCALIB) << BOXCALIB | + IS_RC_MODE_ACTIVE(BOXGOV) << BOXGOV | + IS_RC_MODE_ACTIVE(BOXOSD) << BOXOSD | + IS_RC_MODE_ACTIVE(BOXTELEMETRY) << BOXTELEMETRY | + IS_RC_MODE_ACTIVE(BOXAUTOTUNE) << BOXAUTOTUNE | IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM; for (i = 0; i < activeBoxIdCount; i++) { @@ -632,6 +633,8 @@ static bool processOutCommand(uint8_t cmdMSP) headSerialReply(sizeof(pidnames) - 1); serializeNames(pidnames); break; + // FIXME provide backward compatible solution? what what version of MSP? 6pos? would only work if all step values are on 6pos boundaries + /* case MSP_BOX: headSerialReply(4 * activeBoxIdCount); for (i = 0; i < activeBoxIdCount; i++) @@ -639,6 +642,7 @@ static bool processOutCommand(uint8_t cmdMSP) for (i = 0; i < activeBoxIdCount; i++) serialize16((currentProfile->activate[activeBoxIds[i]] >> 16) & ACTIVATE_MASK); break; + */ case MSP_BOXNAMES: // headSerialReply(sizeof(boxnames) - 1); serializeBoxNamesReply(); @@ -803,12 +807,15 @@ static bool processInCommand(void) } } break; + // FIXME provide backward compatible solution? what what version of MSP? 6pos? would only work if all step values are on 6pos boundaries + /* case MSP_SET_BOX: for (i = 0; i < activeBoxIdCount; i++) currentProfile->activate[activeBoxIds[i]] = read16() & ACTIVATE_MASK; for (i = 0; i < activeBoxIdCount; i++) currentProfile->activate[activeBoxIds[i]] |= (read16() & ACTIVATE_MASK) << 16; break; + */ case MSP_SET_RC_TUNING: currentProfile->controlRateConfig.rcRate8 = read8(); currentProfile->controlRateConfig.rcExpo8 = read8(); diff --git a/src/main/mw.c b/src/main/mw.c index f2f407d57f..c28d47139d 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -111,7 +111,7 @@ void updateAutotuneState(void) static bool landedAfterAutoTuning = false; static bool autoTuneWasUsed = false; - if (rcOptions[BOXAUTOTUNE]) { + if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { if (!FLIGHT_MODE(AUTOTUNE_MODE)) { if (ARMING_FLAG(ARMED)) { if (isAutotuneIdle() || landedAfterAutoTuning) { @@ -246,7 +246,7 @@ void annexCode(void) if (ARMING_FLAG(ARMED)) { LED0_ON; } else { - if (rcOptions[BOXARM] == 0) { + if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } @@ -259,7 +259,7 @@ void annexCode(void) DISABLE_ARMING_FLAG(OK_TO_ARM); } - if (rcOptions[BOXAUTOTUNE]) { + if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } @@ -342,11 +342,11 @@ void handleInflightCalibrationStickPosition(void) void updateInflightCalibrationState(void) { - if (AccInflightCalibrationArmed && ARMING_FLAG(ARMED) && rcData[THROTTLE] > masterConfig.rxConfig.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement + if (AccInflightCalibrationArmed && ARMING_FLAG(ARMED) && rcData[THROTTLE] > masterConfig.rxConfig.mincheck && !IS_RC_MODE_ACTIVE(BOXARM)) { // Copter is airborne and you are turning it off via boxarm : start measurement InflightcalibratingA = 50; AccInflightCalibrationArmed = false; } - if (rcOptions[BOXCALIB]) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored + if (IS_RC_MODE_ACTIVE(BOXCALIB)) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone) InflightcalibratingA = 50; AccInflightCalibrationActive = true; @@ -466,7 +466,7 @@ void processRx(void) // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { - if (!rcOptions[BOXARM]) + if (!IS_RC_MODE_ACTIVE(BOXARM)) mwDisarm(); } @@ -488,17 +488,17 @@ void processRx(void) resetErrorGyro(); } - processRcStickPositions(&masterConfig.rxConfig, throttleStatus, currentProfile->activate, masterConfig.retarded_arm, masterConfig.disarm_kill_switch); + processRcStickPositions(&masterConfig.rxConfig, throttleStatus, currentProfile->modeActivationConditions, masterConfig.retarded_arm, masterConfig.disarm_kill_switch); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } - updateRcOptions(currentProfile->activate); + updateRcOptions(currentProfile->modeActivationConditions); bool canUseHorizonMode = true; - if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) { + if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; @@ -510,7 +510,7 @@ void processRx(void) DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } - if (rcOptions[BOXHORIZON] && canUseHorizonMode) { + if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); @@ -530,7 +530,7 @@ void processRx(void) #ifdef MAG if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { - if (rcOptions[BOXMAG]) { + if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = heading; @@ -538,14 +538,14 @@ void processRx(void) } else { DISABLE_FLIGHT_MODE(MAG_MODE); } - if (rcOptions[BOXHEADFREE]) { + if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } - if (rcOptions[BOXHEADADJ]) { + if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { headFreeModeHold = heading; // acquire new heading } } @@ -557,7 +557,7 @@ void processRx(void) } #endif - if (rcOptions[BOXPASSTHRU]) { + if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 14463e2afd..9a129891f6 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -106,7 +106,7 @@ bool determineNewTelemetryEnabledState(void) if (telemetryPortIsShared) { if (telemetryConfig->telemetry_switch) - enabled = rcOptions[BOXTELEMETRY]; + enabled = IS_RC_MODE_ACTIVE(BOXTELEMETRY); else enabled = ARMING_FLAG(ARMED); }