From d524d8c8cce27426996cc053dab73f40b2e17b0a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sat, 19 Jan 2019 16:19:11 +0000 Subject: [PATCH] Refactor handling of RC input data - Remove globals rcData and rcRaw - Use accesor functions to retrieve RX input data - Refactor rx.c to use an array of structs instead of multiple arrays - Drop the RC Preview menu from CMS. Implementing this without globals requires significant flash and the usefulness of this menu is questionable. If we get complains, we can add it back later. Flash usage goes down ~250 bytes due to the removed menu. Final binary is mostly unaffected, since LTO is able to inline the new accessor functions in most cases. --- src/main/blackbox/blackbox.c | 2 +- src/main/cms/cms.c | 6 ++-- src/main/cms/cms_menu_misc.c | 45 ------------------------- src/main/common/logic_condition.c | 4 +-- src/main/fc/fc_core.c | 10 +++--- src/main/fc/fc_msp.c | 2 +- src/main/fc/rc_adjustments.c | 6 ++-- src/main/fc/rc_controls.c | 26 +++++++-------- src/main/fc/rc_modes.c | 2 +- src/main/flight/failsafe.c | 6 ++-- src/main/flight/mixer.c | 2 +- src/main/flight/pid.c | 2 +- src/main/flight/servos.c | 36 ++++++++++---------- src/main/io/ledstrip.c | 4 +-- src/main/io/osd.c | 2 +- src/main/io/rcdevice_cam.c | 6 ++-- src/main/rx/rx.c | 55 +++++++++++++++++++++---------- src/main/rx/rx.h | 13 ++++++-- src/main/telemetry/mavlink.c | 20 ++++++----- 19 files changed, 118 insertions(+), 131 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 57e98ba7e2..620cc23d12 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1371,7 +1371,7 @@ static void loadMainState(timeUs_t currentTimeUs) #endif for (int i = 0; i < 4; i++) { - blackboxCurrent->rcData[i] = rcData[i]; + blackboxCurrent->rcData[i] = rxGetChannelValue(i); blackboxCurrent->rcCommand[i] = rcCommand[i]; } diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index e34345aaae..fa3c4a200b 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -800,9 +800,9 @@ void cmsYieldDisplay(displayPort_t *pPort, timeMs_t duration) // Stick/key detection and key codes -#define IS_HI(X) (rcData[X] > 1750) -#define IS_LO(X) (rcData[X] < 1250) -#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) +#define IS_HI(X) (rxGetChannelValue(X) > 1750) +#define IS_LO(X) (rxGetChannelValue(X) < 1250) +#define IS_MID(X) (rxGetChannelValue(X) > 1250 && rxGetChannelValue(X) < 1750) #define KEY_NONE 0 #define KEY_UP 1 diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 48fb39cebf..2a3a7610d7 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -38,49 +38,6 @@ #include "sensors/battery.h" -// -// Misc -// - -static long cmsx_menuRcConfirmBack(const OSD_Entry *self) -{ - if (self && self->type == OME_Back) - return 0; - else - return -1; -} - -// -// RC preview -// -static const OSD_Entry cmsx_menuRcEntries[] = -{ - OSD_LABEL_ENTRY("-- RC PREV --"), - - OSD_INT16_RO_ENTRY("ROLL", &rcData[ROLL]), - OSD_INT16_RO_ENTRY("PITCH", &rcData[PITCH]), - OSD_INT16_RO_ENTRY("THR", &rcData[THROTTLE]), - OSD_INT16_RO_ENTRY("YAW", &rcData[YAW]), - - OSD_INT16_RO_ENTRY("AUX1", &rcData[AUX1]), - OSD_INT16_RO_ENTRY("AUX2", &rcData[AUX2]), - OSD_INT16_RO_ENTRY("AUX3", &rcData[AUX3]), - OSD_INT16_RO_ENTRY("AUX4", &rcData[AUX4]), - - OSD_BACK_AND_END_ENTRY, -}; - -static const CMS_Menu cmsx_menuRcPreview = { -#ifdef CMS_MENU_DEBUG - .GUARD_text = "XRCPREV", - .GUARD_type = OME_MENU, -#endif - .onEnter = NULL, - .onExit = cmsx_menuRcConfirmBack, - .onGlobalExit = NULL, - .entries = cmsx_menuRcEntries -}; - static const OSD_Entry menuMiscEntries[]= { OSD_LABEL_ENTRY("-- MISC --"), @@ -91,8 +48,6 @@ static const OSD_Entry menuMiscEntries[]= OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT), #endif - OSD_SUBMENU_ENTRY("RC PREV", &cmsx_menuRcPreview), - OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/common/logic_condition.c b/src/main/common/logic_condition.c index 83517f9625..d29a6c8a97 100644 --- a/src/main/common/logic_condition.c +++ b/src/main/common/logic_condition.c @@ -208,7 +208,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) { case LOGIC_CONDITION_OPERAND_TYPE_RC_CHANNEL: //Extract RC channel raw value if (operand >= 1 && operand <= 16) { - retVal = rcData[operand - 1]; + retVal = rxGetChannelValue(operand - 1); } break; @@ -240,4 +240,4 @@ void logicConditionUpdateTask(timeUs_t currentTimeUs) { for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) { logicConditionProcess(i); } -} \ No newline at end of file +} diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index a9ce120e2d..5d3bfcb376 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -304,9 +304,9 @@ void annexCode(void) } else { // Compute ROLL PITCH and YAW command - rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); - rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); - rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcYawExpo8 : currentControlRateProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband); + rcCommand[ROLL] = getAxisRcCommand(rxGetChannelValue(ROLL), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); + rcCommand[PITCH] = getAxisRcCommand(rxGetChannelValue(PITCH), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); + rcCommand[YAW] = -getAxisRcCommand(rxGetChannelValue(YAW), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcYawExpo8 : currentControlRateProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband); // Apply manual control rates if (FLIGHT_MODE(MANUAL_MODE)) { @@ -316,7 +316,7 @@ void annexCode(void) } //Compute THROTTLE command - throttleValue = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); + throttleValue = constrain(rxGetChannelValue(THROTTLE), rxConfig()->mincheck, PWM_RANGE_MAX); throttleValue = (uint32_t)(throttleValue - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); // [MINCHECK;2000] -> [0;1000] rcCommand[THROTTLE] = rcLookupThrottle(throttleValue); @@ -708,7 +708,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) // sticks, do not process yaw input from the rx. We do this so the // motors do not spin up while we are trying to arm or disarm. // Allow yaw control for tricopters if the user wants the servo to move even when unarmed. - if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck + if (isUsingSticksForArming() && rxGetChannelValue(THROTTLE) <= rxConfig()->mincheck && !((mixerConfig()->platformType == PLATFORM_TRICOPTER) && servoConfig()->tri_unarmed_servo) && mixerConfig()->platformType != PLATFORM_AIRPLANE ) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8e3b6858c2..d76b68d69b 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -507,7 +507,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_RC: for (int i = 0; i < rxRuntimeConfig.channelCount; i++) { - sbufWriteU16(dst, rcRaw[i]); + sbufWriteU16(dst, rxGetRawChannelValue(i)); } break; diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 3f18bfacc5..735cc426b8 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -601,9 +601,9 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, bool canUseRxD if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) { int delta; - if (rcData[channelIndex] > PWM_RANGE_MIDDLE + 200) { + if (rxGetChannelValue(channelIndex) > PWM_RANGE_MIDDLE + 200) { delta = adjustmentState->config->data.stepConfig.step; - } else if (rcData[channelIndex] < PWM_RANGE_MIDDLE - 200) { + } else if (rxGetChannelValue(channelIndex) < PWM_RANGE_MIDDLE - 200) { delta = 0 - adjustmentState->config->data.stepConfig.step; } else { // returning the switch to the middle immediately resets the ready state @@ -620,7 +620,7 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, bool canUseRxD #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) { const uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); - const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; + const uint8_t position = (constrain(rxGetChannelValue(channelIndex), 900, 2100 - 1) - 900) / rangeWidth; applySelectAdjustment(adjustmentFunction, position); #endif diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 2d17aeb57f..64b33504b7 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -101,9 +101,9 @@ bool areSticksDeflectedMoreThanPosHoldDeadband(void) throttleStatus_e calculateThrottleStatus(void) { const uint16_t deadband3d_throttle = rcControlsConfig()->deadband3d_throttle; - if (feature(FEATURE_3D) && (rcData[THROTTLE] > (PWM_RANGE_MIDDLE - deadband3d_throttle) && rcData[THROTTLE] < (PWM_RANGE_MIDDLE + deadband3d_throttle))) + if (feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) > (PWM_RANGE_MIDDLE - deadband3d_throttle) && rxGetChannelValue(THROTTLE) < (PWM_RANGE_MIDDLE + deadband3d_throttle))) return THROTTLE_LOW; - else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->mincheck)) + else if (!feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck)) return THROTTLE_LOW; return THROTTLE_HIGH; @@ -111,8 +111,8 @@ throttleStatus_e calculateThrottleStatus(void) rollPitchStatus_e calculateRollPitchCenterStatus(void) { - if (((rcData[PITCH] < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rcData[PITCH] > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND))) - && ((rcData[ROLL] < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rcData[ROLL] > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND)))) + if (((rxGetChannelValue(PITCH) < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rxGetChannelValue(PITCH) > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND))) + && ((rxGetChannelValue(ROLL) < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rxGetChannelValue(ROLL) > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND)))) return CENTERED; return NOT_CENTERED; @@ -139,17 +139,17 @@ static void updateRcStickPositions(void) { stickPositions_e tmp = 0; - tmp |= ((rcData[ROLL] > rxConfig()->mincheck) ? 0x02 : 0x00) << (ROLL * 2); - tmp |= ((rcData[ROLL] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (ROLL * 2); + tmp |= ((rxGetChannelValue(ROLL) > rxConfig()->mincheck) ? 0x02 : 0x00) << (ROLL * 2); + tmp |= ((rxGetChannelValue(ROLL) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (ROLL * 2); - tmp |= ((rcData[PITCH] > rxConfig()->mincheck) ? 0x02 : 0x00) << (PITCH * 2); - tmp |= ((rcData[PITCH] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (PITCH * 2); + tmp |= ((rxGetChannelValue(PITCH) > rxConfig()->mincheck) ? 0x02 : 0x00) << (PITCH * 2); + tmp |= ((rxGetChannelValue(PITCH) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (PITCH * 2); - tmp |= ((rcData[YAW] > rxConfig()->mincheck) ? 0x02 : 0x00) << (YAW * 2); - tmp |= ((rcData[YAW] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (YAW * 2); + tmp |= ((rxGetChannelValue(YAW) > rxConfig()->mincheck) ? 0x02 : 0x00) << (YAW * 2); + tmp |= ((rxGetChannelValue(YAW) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (YAW * 2); - tmp |= ((rcData[THROTTLE] > rxConfig()->mincheck) ? 0x02 : 0x00) << (THROTTLE * 2); - tmp |= ((rcData[THROTTLE] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (THROTTLE * 2); + tmp |= ((rxGetChannelValue(THROTTLE) > rxConfig()->mincheck) ? 0x02 : 0x00) << (THROTTLE * 2); + tmp |= ((rxGetChannelValue(THROTTLE) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (THROTTLE * 2); rcStickPositions = tmp; } @@ -350,5 +350,5 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } int32_t getRcStickDeflection(int32_t axis) { - return MIN(ABS(rcData[axis] - PWM_RANGE_MIDDLE), 500); + return MIN(ABS(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE), 500); } diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index dfe2e5ec17..de02ddfc2c 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -102,7 +102,7 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) // No need to constrain() here, since we're testing for a closed range defined // by the channelRange_t. If channelValue has an invalid value, the test will // be false anyway. - uint16_t channelValue = rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT]; + uint16_t channelValue = rxGetChannelValue(auxChannelIndex + NON_AUX_CHANNEL_COUNT); return (channelValue >= CHANNEL_RANGE_MIN + (range->startStep * CHANNEL_RANGE_STEP_WIDTH) && channelValue < CHANNEL_RANGE_MIN + (range->endStep * CHANNEL_RANGE_STEP_WIDTH)); } diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index fd45ddc6f8..c22186e743 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -332,9 +332,9 @@ static bool failsafeCheckStickMotion(void) if (failsafeConfig()->failsafe_stick_motion_threshold > 0) { uint32_t totalRcDelta = 0; - totalRcDelta += ABS(rcData[ROLL] - PWM_RANGE_MIDDLE); - totalRcDelta += ABS(rcData[PITCH] - PWM_RANGE_MIDDLE); - totalRcDelta += ABS(rcData[YAW] - PWM_RANGE_MIDDLE); + totalRcDelta += ABS(rxGetChannelValue(ROLL) - PWM_RANGE_MIDDLE); + totalRcDelta += ABS(rxGetChannelValue(PITCH) - PWM_RANGE_MIDDLE); + totalRcDelta += ABS(rxGetChannelValue(YAW) - PWM_RANGE_MIDDLE); return totalRcDelta >= failsafeConfig()->failsafe_stick_motion_threshold; } diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index ac12fa408b..f2c1351a07 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -403,7 +403,7 @@ motorStatus_e getMotorStatus(void) return MOTOR_STOPPED_AUTO; } - if (rcData[THROTTLE] < rxConfig()->mincheck) { + if (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck) { if ((STATE(FIXED_WING) || !isAirmodeActive()) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) { return MOTOR_STOPPED_USER; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 774e230955..3618e0ddd7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -371,7 +371,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(void) * Compute stick position in range of [-1.0f : 1.0f] without deadband and expo */ for (int axis = 0; axis < 3; axis++) { - pidState[axis].stickPosition = constrain(rcData[axis] - PWM_RANGE_MIDDLE, -500, 500) / 500.0f; + pidState[axis].stickPosition = constrain(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE, -500, 500) / 500.0f; } // If nothing changed - don't waste time recalculating coefficients diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 0efc0744f4..a98e956e69 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -237,7 +237,7 @@ void servoMixer(float dT) input[INPUT_STABILIZED_YAW] = axisPID[YAW]; // Reverse yaw servo when inverted in 3D mode only for multirotor and tricopter - if (feature(FEATURE_3D) && (rcData[THROTTLE] < PWM_RANGE_MIDDLE) && + if (feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) < PWM_RANGE_MIDDLE) && (mixerConfig()->platformType == PLATFORM_MULTIROTOR || mixerConfig()->platformType == PLATFORM_TRICOPTER)) { input[INPUT_STABILIZED_YAW] *= -1; } @@ -268,22 +268,24 @@ void servoMixer(float dT) // 2000 - 1500 = +500 // 1500 - 1500 = 0 // 1000 - 1500 = -500 - input[INPUT_RC_ROLL] = rcData[ROLL] - PWM_RANGE_MIDDLE; - input[INPUT_RC_PITCH] = rcData[PITCH] - PWM_RANGE_MIDDLE; - input[INPUT_RC_YAW] = rcData[YAW] - PWM_RANGE_MIDDLE; - input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH5] = rcData[AUX1] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH6] = rcData[AUX2] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH7] = rcData[AUX3] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH8] = rcData[AUX4] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH9] = rcData[AUX5] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH10] = rcData[AUX6] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH11] = rcData[AUX7] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH12] = rcData[AUX8] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH13] = rcData[AUX9] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH14] = rcData[AUX10] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH15] = rcData[AUX11] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH16] = rcData[AUX12] - PWM_RANGE_MIDDLE; +#define GET_RX_CHANNEL_INPUT(x) (rxGetChannelValue(x) - PWM_RANGE_MIDDLE) + input[INPUT_RC_ROLL] = GET_RX_CHANNEL_INPUT(ROLL); + input[INPUT_RC_PITCH] = GET_RX_CHANNEL_INPUT(PITCH); + input[INPUT_RC_YAW] = GET_RX_CHANNEL_INPUT(YAW); + input[INPUT_RC_THROTTLE] = GET_RX_CHANNEL_INPUT(THROTTLE); + input[INPUT_RC_CH5] = GET_RX_CHANNEL_INPUT(AUX1); + input[INPUT_RC_CH6] = GET_RX_CHANNEL_INPUT(AUX2); + input[INPUT_RC_CH7] = GET_RX_CHANNEL_INPUT(AUX3); + input[INPUT_RC_CH8] = GET_RX_CHANNEL_INPUT(AUX4); + input[INPUT_RC_CH9] = GET_RX_CHANNEL_INPUT(AUX5); + input[INPUT_RC_CH10] = GET_RX_CHANNEL_INPUT(AUX6); + input[INPUT_RC_CH11] = GET_RX_CHANNEL_INPUT(AUX7); + input[INPUT_RC_CH12] = GET_RX_CHANNEL_INPUT(AUX8); + input[INPUT_RC_CH13] = GET_RX_CHANNEL_INPUT(AUX9); + input[INPUT_RC_CH14] = GET_RX_CHANNEL_INPUT(AUX10); + input[INPUT_RC_CH15] = GET_RX_CHANNEL_INPUT(AUX11); + input[INPUT_RC_CH16] = GET_RX_CHANNEL_INPUT(AUX12); +#undef GET_RX_CHANNEL_INPUT for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servo[i] = 0; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 8a63f43fa5..d3f25acb4e 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -727,7 +727,7 @@ static void applyLedThrustRingLayer(bool updateNow, timeUs_t *timer) if (updateNow) { rotationPhase = rotationPhase > 0 ? rotationPhase - 1 : ledCounts.ringSeqLen - 1; - int scale = scaledThrottle; // ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; + int scale = scaledThrottle; // ARMING_FLAG(ARMED) ? scaleRange(rxGetChannelValue(THROTTLE), PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; *timer += LED_STRIP_HZ(5) * 10 / scale; // 5 - 50Hz update rate } @@ -949,7 +949,7 @@ void ledStripUpdate(timeUs_t currentTimeUs) // apply all layers; triggered timed functions has to update timers - scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; + scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rxGetChannelValue(THROTTLE), PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; applyLedFixedLayers(); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 95cb4b1e5e..947718661a 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -834,7 +834,7 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t { buff[0] = SYM_BLANK; buff[1] = SYM_THR; - int16_t thr = rcData[THROTTLE]; + int16_t thr = rxGetChannelValue(THROTTLE); if (autoThr && navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; buff[1] = SYM_AUTO_THR1; diff --git a/src/main/io/rcdevice_cam.c b/src/main/io/rcdevice_cam.c index aa890e2317..4478af13f1 100644 --- a/src/main/io/rcdevice_cam.c +++ b/src/main/io/rcdevice_cam.c @@ -34,9 +34,9 @@ #ifdef USE_RCDEVICE -#define IS_HI(X) (rcData[X] > 1750) -#define IS_LO(X) (rcData[X] < 1250) -#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) +#define IS_HI(X) (rxGetChannelValue(X) > 1750) +#define IS_LO(X) (rxGetChannelValue(X) < 1250) +#define IS_MID(X) (rxGetChannelValue(X) > 1250 && rxGetChannelValue(X) < 1750) static runcamDevice_t runcamDevice; runcamDevice_t *camDevice = &runcamDevice; rcdeviceSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index cc3f1fa387..3a8da7f533 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -67,6 +67,12 @@ //#define DEBUG_RX_SIGNAL_LOSS +typedef struct rcChannel_s { + int16_t raw; // Value received via RX - [1000;2000] + int16_t data; // Value after processing - [1000;2000] + timeMs_t expiresAt; // Time when this value becomes too old and it's discarded +} rcChannel_t; + const char rcChannelLetters[] = "AERT"; static uint16_t rssi = 0; // range: [0;1023] @@ -88,9 +94,7 @@ static timeUs_t needRxSignalBefore = 0; static timeUs_t suspendRxSignalUntil = 0; static uint8_t skipRxSamples = 0; -int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] -int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] -timeMs_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +static rcChannel_t rcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT]; #define MAX_INVALID_PULS_TIME 300 @@ -242,12 +246,16 @@ void rxInit(void) rxRuntimeConfig.requireFiltering = false; rcSampleIndex = 0; + timeMs_t nowMs = millis(); + for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { - rcData[i] = PWM_RANGE_MIDDLE; - rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME; + rcChannels[i].raw = PWM_RANGE_MIDDLE; + rcChannels[i].data = PWM_RANGE_MIDDLE; + rcChannels[i].expiresAt = nowMs + MAX_INVALID_PULS_TIME; } - rcData[THROTTLE] = (feature(FEATURE_3D)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec; + rcChannels[THROTTLE].raw = (feature(FEATURE_3D)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec; + rcChannels[THROTTLE].data = rcChannels[THROTTLE].raw; // Initialize ARM switch to OFF position when arming via switch is defined for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { @@ -260,7 +268,9 @@ void rxInit(void) value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationConditions(i)->range.endStep + 1)); } // Initialize ARM AUX channel to OFF value - rcData[modeActivationConditions(i)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = value; + rcChannel_t *armChannel = &rcChannels[modeActivationConditions(i)->auxChannelIndex + NON_AUX_CHANNEL_COUNT]; + armChannel->raw = value; + armChannel->data = value; } } @@ -507,32 +517,32 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } // Store as rxRaw - rcRaw[channel] = sample; + rcChannels[channel].raw = sample; // Apply invalid pulse value logic if (!isPulseValid(sample)) { - sample = rcData[channel]; // hold channel, replace with old value - if ((currentTimeMs > rcInvalidPulsPeriod[channel]) && (channel < NON_AUX_CHANNEL_COUNT)) { + sample = rcChannels[channel].data; // hold channel, replace with old value + if ((currentTimeMs > rcChannels[channel].expiresAt) && (channel < NON_AUX_CHANNEL_COUNT)) { rxFlightChannelsValid = false; } } else { - rcInvalidPulsPeriod[channel] = currentTimeMs + MAX_INVALID_PULS_TIME; + rcChannels[channel].expiresAt = currentTimeMs + MAX_INVALID_PULS_TIME; } // Save channel value rcStaging[channel] = sample; } - // Update rcData channel value if receiver is not in failsafe mode - // If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good rcData values are retained + // Update channel input value if receiver is not in failsafe mode + // If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good input values are retained if (rxFlightChannelsValid && rxSignalReceived) { if (rxRuntimeConfig.requireFiltering) { for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { - rcData[channel] = applyChannelFiltering(channel, rcStaging[channel]); + rcChannels[channel].data = applyChannelFiltering(channel, rcStaging[channel]); } } else { for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { - rcData[channel] = rcStaging[channel]; + rcChannels[channel].data = rcStaging[channel]; } } } @@ -621,8 +631,9 @@ static void updateRSSIPWM(void) { int16_t pwmRssi = 0; // Read value of AUX channel as rssi - if (rxConfig()->rssi_channel > 0) { - pwmRssi = rcRaw[rxConfig()->rssi_channel - 1]; + unsigned rssiChannel = rxConfig()->rssi_channel; + if (rssiChannel > 0) { + pwmRssi = rcChannels[rssiChannel - 1].raw; // Range of rawPwmRssi is [1000;2000]. rssi should be in [0;1023]; uint16_t rawRSSI = (uint16_t)((constrain(pwmRssi - 1000, 0, 1000) / 1000.0f) * (RSSI_MAX_VALUE * 1.0f)); @@ -677,3 +688,13 @@ uint16_t rxGetRefreshRate(void) { return rxRuntimeConfig.rxRefreshRate; } + +int16_t rxGetChannelValue(unsigned channelNumber) +{ + return rcChannels[channelNumber].data; +} + +int16_t rxGetRawChannelValue(unsigned channelNumber) +{ + return rcChannels[channelNumber].raw; +} diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 7ebb8fa412..ead08021a6 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -95,9 +95,6 @@ typedef enum { extern const char rcChannelLetters[]; -extern int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] -extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] - #define MAX_MAPPABLE_RX_INPUTS 4 #define RSSI_VISIBLE_VALUE_MIN 0 @@ -187,3 +184,13 @@ void suspendRxSignal(void); void resumeRxSignal(void); uint16_t rxGetRefreshRate(void); + +// Processed RC channel value. These values might include +// filtering and some extra processing like value holding +// during failsafe. Most callers should use this instead +// of rxGetRawChannelValue() +int16_t rxGetChannelValue(unsigned channelNumber); + +// Raw RC channel data as received by the RX. Should only +// be used by very low level subsystems, like blackbox. +int16_t rxGetRawChannelValue(unsigned channelNumber); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 17e23c8e0a..660e6f74c5 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -247,29 +247,31 @@ void mavlinkSendSystemStatus(void) void mavlinkSendRCChannelsAndRSSI(void) { +#define GET_CHANNEL_VALUE(x) ((rxRuntimeConfig.channelCount >= (x + 1)) ? rxGetChannelValue(x) : 0) mavlink_msg_rc_channels_raw_pack(mavSystemId, mavComponentId, &mavSendMsg, // time_boot_ms Timestamp (milliseconds since system boot) millis(), // port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. 0, // chan1_raw RC channel 1 value, in microseconds - (rxRuntimeConfig.channelCount >= 1) ? rcData[0] : 0, + GET_CHANNEL_VALUE(0), // chan2_raw RC channel 2 value, in microseconds - (rxRuntimeConfig.channelCount >= 2) ? rcData[1] : 0, + GET_CHANNEL_VALUE(1), // chan3_raw RC channel 3 value, in microseconds - (rxRuntimeConfig.channelCount >= 3) ? rcData[2] : 0, + GET_CHANNEL_VALUE(2), // chan4_raw RC channel 4 value, in microseconds - (rxRuntimeConfig.channelCount >= 4) ? rcData[3] : 0, + GET_CHANNEL_VALUE(3), // chan5_raw RC channel 5 value, in microseconds - (rxRuntimeConfig.channelCount >= 5) ? rcData[4] : 0, + GET_CHANNEL_VALUE(4), // chan6_raw RC channel 6 value, in microseconds - (rxRuntimeConfig.channelCount >= 6) ? rcData[5] : 0, + GET_CHANNEL_VALUE(5), // chan7_raw RC channel 7 value, in microseconds - (rxRuntimeConfig.channelCount >= 7) ? rcData[6] : 0, + GET_CHANNEL_VALUE(6), // chan8_raw RC channel 8 value, in microseconds - (rxRuntimeConfig.channelCount >= 8) ? rcData[7] : 0, + GET_CHANNEL_VALUE(7), // rssi Receive signal strength indicator, 0: 0%, 255: 100% scaleRange(getRSSI(), 0, 1023, 0, 255)); +#undef GET_CHANNEL_VALUE mavlinkSendMessage(); } @@ -413,7 +415,7 @@ void mavlinkSendHUDAndHeartbeat(void) // heading Current heading in degrees, in compass units (0..360, 0=north) DECIDEGREES_TO_DEGREES(attitude.values.yaw), // throttle Current throttle setting in integer percent, 0 to 100 - scaleRange(constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100), + scaleRange(constrain(rxGetChannelValue(THROTTLE), PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100), // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate) mavAltitude, // climb Current climb rate in meters/second