diff --git a/src/main/config/runtime_config.c b/src/main/config/runtime_config.c index e0f3a0ed96..803486f512 100644 --- a/src/main/config/runtime_config.c +++ b/src/main/config/runtime_config.c @@ -24,15 +24,6 @@ uint8_t armingFlags = 0; uint8_t stateFlags = 0; uint16_t flightModeFlags = 0; -// 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]; - static uint32_t enabledSensors = 0; bool sensors(uint32_t mask) diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h index 2a96c5d5c4..676e924a26 100644 --- a/src/main/config/runtime_config.h +++ b/src/main/config/runtime_config.h @@ -17,33 +17,6 @@ #pragma once -enum { - BOXARM = 0, - BOXANGLE, - BOXHORIZON, - BOXBARO, - BOXMAG, - BOXHEADFREE, - BOXHEADADJ, - BOXCAMSTAB, - BOXCAMTRIG, - BOXGPSHOME, - BOXGPSHOLD, - BOXPASSTHRU, - BOXBEEPERON, - BOXLEDMAX, - BOXLEDLOW, - BOXLLIGHTS, - BOXCALIB, - BOXGOV, - BOXOSD, - BOXTELEMETRY, - BOXAUTOTUNE, - CHECKBOX_ITEM_COUNT -}; - -extern uint8_t rcOptions[CHECKBOX_ITEM_COUNT]; - // FIXME some of these are flight modes, some of these are general status indicators typedef enum { OK_TO_ARM = (1 << 0), diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index eb038c014e..95cdb079aa 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -23,6 +23,10 @@ #include "drivers/system.h" #include "flight/failsafe.h" #include "sensors/sensors.h" + +#include "rx/rx.h" +#include "io/rc_controls.h" + #include "config/runtime_config.h" #include "config/config.h" diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index c79067aa80..c3d070f4c0 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -43,6 +43,15 @@ 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]; + bool areSticksInApModePosition(uint16_t ap_mode) { return abs(rcCommand[ROLL]) < ap_mode && abs(rcCommand[PITCH]) < ap_mode; @@ -210,3 +219,35 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat return; } } + +#define MAX_AUX_STATE_CHANNELS 8 + +void updateRcOptions(uint32_t *activate) +{ + // Check AUX switches + + // auxState is a bitmask, 3 bits per channel. aux1 is first. + // lower 16 bits contain aux 1 to 4, upper 16 bits contain aux 5 to 8 + // + // 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; + + for (i = 0; i < rxRuntimeConfig.auxChannelCount && i < MAX_AUX_STATE_CHANNELS; i++) { + uint32_t temp = (rcData[AUX1 + i] < 1300) << (3 * i) | + (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | + (rcData[AUX1 + i] > 1700) << (3 * i + 2); + + if (i >= 4 && i < 8) { + temp <<= 16; + } + auxState |= temp; + } + for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) + rcOptions[i] = (auxState & activate[i]) > 0; +} diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 01726be6a7..77cdf2b4c3 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -17,6 +17,33 @@ #pragma once +enum { + BOXARM = 0, + BOXANGLE, + BOXHORIZON, + BOXBARO, + BOXMAG, + BOXHEADFREE, + BOXHEADADJ, + BOXCAMSTAB, + BOXCAMTRIG, + BOXGPSHOME, + BOXGPSHOLD, + BOXPASSTHRU, + BOXBEEPERON, + BOXLEDMAX, + BOXLEDLOW, + BOXLLIGHTS, + BOXCALIB, + BOXGOV, + BOXOSD, + BOXTELEMETRY, + BOXAUTOTUNE, + CHECKBOX_ITEM_COUNT +}; + +extern uint8_t rcOptions[CHECKBOX_ITEM_COUNT]; + typedef enum rc_alias { ROLL = 0, PITCH, @@ -66,3 +93,5 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, uint32_t *activate, bool retarded_arm); +void updateRcOptions(uint32_t *activate); + diff --git a/src/main/mw.c b/src/main/mw.c index e79d3e6dd6..cdd2303501 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -439,9 +439,6 @@ void executePeriodicTasks(void) void processRx(void) { - int i; - uint32_t auxState = 0; - calculateRxChannelsAndUpdateFailsafe(currentTime); // in 3D mode, we need to be able to disarm by switch at any time @@ -474,30 +471,14 @@ void processRx(void) updateInflightCalibrationState(); } - // Check AUX switches + updateRcOptions(currentProfile->activate); - // auxState is a bitmask, 3 bits per channel. aux1 is first. - // lower 16 bits contain aux 1 to 4, upper 16 bits contain aux 5 to 8 - // - // 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. - - for (i = 0; i < 4; i++) { - auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | - (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | - (rcData[AUX1 + i] > 1700) << (3 * i + 2); - auxState |= ((rcData[AUX5 + i] < 1300) << (3 * i) | - (1300 < rcData[AUX5 + i] && rcData[AUX5 + i] < 1700) << (3 * i + 1) | - (rcData[AUX5 + i] > 1700) << (3 * i + 2)) << 16; - } - for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) - rcOptions[i] = (auxState & currentProfile->activate[i]) > 0; + bool canUseHorizonMode = true; if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode + canUseHorizonMode = false; + if (!FLIGHT_MODE(ANGLE_MODE)) { resetErrorAngle(); ENABLE_FLIGHT_MODE(ANGLE_MODE); @@ -506,15 +487,17 @@ void processRx(void) DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } - if (rcOptions[BOXHORIZON]) { - DISABLE_FLIGHT_MODE(ANGLE_MODE); - if (!FLIGHT_MODE(HORIZON_MODE)) { - resetErrorAngle(); - ENABLE_FLIGHT_MODE(HORIZON_MODE); - } - } else { - DISABLE_FLIGHT_MODE(HORIZON_MODE); - } + if (rcOptions[BOXHORIZON] && canUseHorizonMode) { + + DISABLE_FLIGHT_MODE(ANGLE_MODE); + + if (!FLIGHT_MODE(HORIZON_MODE)) { + resetErrorAngle(); + ENABLE_FLIGHT_MODE(HORIZON_MODE); + } + } else { + DISABLE_FLIGHT_MODE(HORIZON_MODE); + } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 607174dafb..fc52985bbf 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -99,6 +99,8 @@ void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintTo } #endif +#define STICK_CHANNEL_COUNT 4 + void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe) { uint8_t i; @@ -123,6 +125,8 @@ void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe) if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) { rxPwmInit(&rxRuntimeConfig, &rcReadRawFunc); } + + rxRuntimeConfig.auxChannelCount = rxRuntimeConfig.channelCount - STICK_CHANNEL_COUNT; } #ifdef SERIAL_RX diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index b2b6ea0255..3629c6c0d3 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -62,6 +62,7 @@ typedef struct rxConfig_s { typedef struct rxRuntimeConfig_s { uint8_t channelCount; // number of rc channels as reported by current input driver + uint8_t auxChannelCount; } rxRuntimeConfig_t; extern rxRuntimeConfig_t rxRuntimeConfig; diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index ae4963348e..4442d73e6e 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -29,6 +29,9 @@ #include "drivers/serial_softserial.h" #include "io/serial.h" +#include "rx/rx.h" +#include "io/rc_controls.h" + #include "config/runtime_config.h" #include "config/config.h"