Skip to content

Commit d524d8c

Browse files
committed
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.
1 parent fb62330 commit d524d8c

File tree

19 files changed

+118
-131
lines changed

19 files changed

+118
-131
lines changed

src/main/blackbox/blackbox.c

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1371,7 +1371,7 @@ static void loadMainState(timeUs_t currentTimeUs)
13711371
#endif
13721372

13731373
for (int i = 0; i < 4; i++) {
1374-
blackboxCurrent->rcData[i] = rcData[i];
1374+
blackboxCurrent->rcData[i] = rxGetChannelValue(i);
13751375
blackboxCurrent->rcCommand[i] = rcCommand[i];
13761376
}
13771377

src/main/cms/cms.c

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -800,9 +800,9 @@ void cmsYieldDisplay(displayPort_t *pPort, timeMs_t duration)
800800

801801
// Stick/key detection and key codes
802802

803-
#define IS_HI(X) (rcData[X] > 1750)
804-
#define IS_LO(X) (rcData[X] < 1250)
805-
#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
803+
#define IS_HI(X) (rxGetChannelValue(X) > 1750)
804+
#define IS_LO(X) (rxGetChannelValue(X) < 1250)
805+
#define IS_MID(X) (rxGetChannelValue(X) > 1250 && rxGetChannelValue(X) < 1750)
806806

807807
#define KEY_NONE 0
808808
#define KEY_UP 1

src/main/cms/cms_menu_misc.c

Lines changed: 0 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -38,49 +38,6 @@
3838

3939
#include "sensors/battery.h"
4040

41-
//
42-
// Misc
43-
//
44-
45-
static long cmsx_menuRcConfirmBack(const OSD_Entry *self)
46-
{
47-
if (self && self->type == OME_Back)
48-
return 0;
49-
else
50-
return -1;
51-
}
52-
53-
//
54-
// RC preview
55-
//
56-
static const OSD_Entry cmsx_menuRcEntries[] =
57-
{
58-
OSD_LABEL_ENTRY("-- RC PREV --"),
59-
60-
OSD_INT16_RO_ENTRY("ROLL", &rcData[ROLL]),
61-
OSD_INT16_RO_ENTRY("PITCH", &rcData[PITCH]),
62-
OSD_INT16_RO_ENTRY("THR", &rcData[THROTTLE]),
63-
OSD_INT16_RO_ENTRY("YAW", &rcData[YAW]),
64-
65-
OSD_INT16_RO_ENTRY("AUX1", &rcData[AUX1]),
66-
OSD_INT16_RO_ENTRY("AUX2", &rcData[AUX2]),
67-
OSD_INT16_RO_ENTRY("AUX3", &rcData[AUX3]),
68-
OSD_INT16_RO_ENTRY("AUX4", &rcData[AUX4]),
69-
70-
OSD_BACK_AND_END_ENTRY,
71-
};
72-
73-
static const CMS_Menu cmsx_menuRcPreview = {
74-
#ifdef CMS_MENU_DEBUG
75-
.GUARD_text = "XRCPREV",
76-
.GUARD_type = OME_MENU,
77-
#endif
78-
.onEnter = NULL,
79-
.onExit = cmsx_menuRcConfirmBack,
80-
.onGlobalExit = NULL,
81-
.entries = cmsx_menuRcEntries
82-
};
83-
8441
static const OSD_Entry menuMiscEntries[]=
8542
{
8643
OSD_LABEL_ENTRY("-- MISC --"),
@@ -91,8 +48,6 @@ static const OSD_Entry menuMiscEntries[]=
9148
OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT),
9249
#endif
9350

94-
OSD_SUBMENU_ENTRY("RC PREV", &cmsx_menuRcPreview),
95-
9651
OSD_BACK_AND_END_ENTRY,
9752
};
9853

src/main/common/logic_condition.c

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -208,7 +208,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
208208
case LOGIC_CONDITION_OPERAND_TYPE_RC_CHANNEL:
209209
//Extract RC channel raw value
210210
if (operand >= 1 && operand <= 16) {
211-
retVal = rcData[operand - 1];
211+
retVal = rxGetChannelValue(operand - 1);
212212
}
213213
break;
214214

@@ -240,4 +240,4 @@ void logicConditionUpdateTask(timeUs_t currentTimeUs) {
240240
for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
241241
logicConditionProcess(i);
242242
}
243-
}
243+
}

src/main/fc/fc_core.c

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -304,9 +304,9 @@ void annexCode(void)
304304
}
305305
else {
306306
// Compute ROLL PITCH and YAW command
307-
rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband);
308-
rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband);
309-
rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcYawExpo8 : currentControlRateProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband);
307+
rcCommand[ROLL] = getAxisRcCommand(rxGetChannelValue(ROLL), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband);
308+
rcCommand[PITCH] = getAxisRcCommand(rxGetChannelValue(PITCH), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband);
309+
rcCommand[YAW] = -getAxisRcCommand(rxGetChannelValue(YAW), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcYawExpo8 : currentControlRateProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband);
310310

311311
// Apply manual control rates
312312
if (FLIGHT_MODE(MANUAL_MODE)) {
@@ -316,7 +316,7 @@ void annexCode(void)
316316
}
317317

318318
//Compute THROTTLE command
319-
throttleValue = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
319+
throttleValue = constrain(rxGetChannelValue(THROTTLE), rxConfig()->mincheck, PWM_RANGE_MAX);
320320
throttleValue = (uint32_t)(throttleValue - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); // [MINCHECK;2000] -> [0;1000]
321321
rcCommand[THROTTLE] = rcLookupThrottle(throttleValue);
322322

@@ -708,7 +708,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
708708
// sticks, do not process yaw input from the rx. We do this so the
709709
// motors do not spin up while we are trying to arm or disarm.
710710
// Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
711-
if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
711+
if (isUsingSticksForArming() && rxGetChannelValue(THROTTLE) <= rxConfig()->mincheck
712712
&& !((mixerConfig()->platformType == PLATFORM_TRICOPTER) && servoConfig()->tri_unarmed_servo)
713713
&& mixerConfig()->platformType != PLATFORM_AIRPLANE
714714
) {

src/main/fc/fc_msp.c

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -507,7 +507,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
507507

508508
case MSP_RC:
509509
for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
510-
sbufWriteU16(dst, rcRaw[i]);
510+
sbufWriteU16(dst, rxGetRawChannelValue(i));
511511
}
512512
break;
513513

src/main/fc/rc_adjustments.c

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -601,9 +601,9 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, bool canUseRxD
601601

602602
if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
603603
int delta;
604-
if (rcData[channelIndex] > PWM_RANGE_MIDDLE + 200) {
604+
if (rxGetChannelValue(channelIndex) > PWM_RANGE_MIDDLE + 200) {
605605
delta = adjustmentState->config->data.stepConfig.step;
606-
} else if (rcData[channelIndex] < PWM_RANGE_MIDDLE - 200) {
606+
} else if (rxGetChannelValue(channelIndex) < PWM_RANGE_MIDDLE - 200) {
607607
delta = 0 - adjustmentState->config->data.stepConfig.step;
608608
} else {
609609
// returning the switch to the middle immediately resets the ready state
@@ -620,7 +620,7 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, bool canUseRxD
620620
#ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT
621621
} else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
622622
const uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
623-
const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
623+
const uint8_t position = (constrain(rxGetChannelValue(channelIndex), 900, 2100 - 1) - 900) / rangeWidth;
624624

625625
applySelectAdjustment(adjustmentFunction, position);
626626
#endif

src/main/fc/rc_controls.c

Lines changed: 13 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -101,18 +101,18 @@ bool areSticksDeflectedMoreThanPosHoldDeadband(void)
101101
throttleStatus_e calculateThrottleStatus(void)
102102
{
103103
const uint16_t deadband3d_throttle = rcControlsConfig()->deadband3d_throttle;
104-
if (feature(FEATURE_3D) && (rcData[THROTTLE] > (PWM_RANGE_MIDDLE - deadband3d_throttle) && rcData[THROTTLE] < (PWM_RANGE_MIDDLE + deadband3d_throttle)))
104+
if (feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) > (PWM_RANGE_MIDDLE - deadband3d_throttle) && rxGetChannelValue(THROTTLE) < (PWM_RANGE_MIDDLE + deadband3d_throttle)))
105105
return THROTTLE_LOW;
106-
else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->mincheck))
106+
else if (!feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck))
107107
return THROTTLE_LOW;
108108

109109
return THROTTLE_HIGH;
110110
}
111111

112112
rollPitchStatus_e calculateRollPitchCenterStatus(void)
113113
{
114-
if (((rcData[PITCH] < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rcData[PITCH] > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND)))
115-
&& ((rcData[ROLL] < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rcData[ROLL] > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND))))
114+
if (((rxGetChannelValue(PITCH) < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rxGetChannelValue(PITCH) > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND)))
115+
&& ((rxGetChannelValue(ROLL) < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rxGetChannelValue(ROLL) > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND))))
116116
return CENTERED;
117117

118118
return NOT_CENTERED;
@@ -139,17 +139,17 @@ static void updateRcStickPositions(void)
139139
{
140140
stickPositions_e tmp = 0;
141141

142-
tmp |= ((rcData[ROLL] > rxConfig()->mincheck) ? 0x02 : 0x00) << (ROLL * 2);
143-
tmp |= ((rcData[ROLL] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (ROLL * 2);
142+
tmp |= ((rxGetChannelValue(ROLL) > rxConfig()->mincheck) ? 0x02 : 0x00) << (ROLL * 2);
143+
tmp |= ((rxGetChannelValue(ROLL) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (ROLL * 2);
144144

145-
tmp |= ((rcData[PITCH] > rxConfig()->mincheck) ? 0x02 : 0x00) << (PITCH * 2);
146-
tmp |= ((rcData[PITCH] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (PITCH * 2);
145+
tmp |= ((rxGetChannelValue(PITCH) > rxConfig()->mincheck) ? 0x02 : 0x00) << (PITCH * 2);
146+
tmp |= ((rxGetChannelValue(PITCH) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (PITCH * 2);
147147

148-
tmp |= ((rcData[YAW] > rxConfig()->mincheck) ? 0x02 : 0x00) << (YAW * 2);
149-
tmp |= ((rcData[YAW] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (YAW * 2);
148+
tmp |= ((rxGetChannelValue(YAW) > rxConfig()->mincheck) ? 0x02 : 0x00) << (YAW * 2);
149+
tmp |= ((rxGetChannelValue(YAW) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (YAW * 2);
150150

151-
tmp |= ((rcData[THROTTLE] > rxConfig()->mincheck) ? 0x02 : 0x00) << (THROTTLE * 2);
152-
tmp |= ((rcData[THROTTLE] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (THROTTLE * 2);
151+
tmp |= ((rxGetChannelValue(THROTTLE) > rxConfig()->mincheck) ? 0x02 : 0x00) << (THROTTLE * 2);
152+
tmp |= ((rxGetChannelValue(THROTTLE) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (THROTTLE * 2);
153153

154154
rcStickPositions = tmp;
155155
}
@@ -350,5 +350,5 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
350350
}
351351

352352
int32_t getRcStickDeflection(int32_t axis) {
353-
return MIN(ABS(rcData[axis] - PWM_RANGE_MIDDLE), 500);
353+
return MIN(ABS(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE), 500);
354354
}

src/main/fc/rc_modes.c

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -102,7 +102,7 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range)
102102
// No need to constrain() here, since we're testing for a closed range defined
103103
// by the channelRange_t. If channelValue has an invalid value, the test will
104104
// be false anyway.
105-
uint16_t channelValue = rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT];
105+
uint16_t channelValue = rxGetChannelValue(auxChannelIndex + NON_AUX_CHANNEL_COUNT);
106106
return (channelValue >= CHANNEL_RANGE_MIN + (range->startStep * CHANNEL_RANGE_STEP_WIDTH) &&
107107
channelValue < CHANNEL_RANGE_MIN + (range->endStep * CHANNEL_RANGE_STEP_WIDTH));
108108
}

src/main/flight/failsafe.c

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -332,9 +332,9 @@ static bool failsafeCheckStickMotion(void)
332332
if (failsafeConfig()->failsafe_stick_motion_threshold > 0) {
333333
uint32_t totalRcDelta = 0;
334334

335-
totalRcDelta += ABS(rcData[ROLL] - PWM_RANGE_MIDDLE);
336-
totalRcDelta += ABS(rcData[PITCH] - PWM_RANGE_MIDDLE);
337-
totalRcDelta += ABS(rcData[YAW] - PWM_RANGE_MIDDLE);
335+
totalRcDelta += ABS(rxGetChannelValue(ROLL) - PWM_RANGE_MIDDLE);
336+
totalRcDelta += ABS(rxGetChannelValue(PITCH) - PWM_RANGE_MIDDLE);
337+
totalRcDelta += ABS(rxGetChannelValue(YAW) - PWM_RANGE_MIDDLE);
338338

339339
return totalRcDelta >= failsafeConfig()->failsafe_stick_motion_threshold;
340340
}

0 commit comments

Comments
 (0)