Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 08:15:26 +03:00
Code Issues Wiki Activity

Refactor handling of RC input data

Browse source 
- 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.
This commit is contained in:
Alberto García Hierro 2019-01-19 16:19:11 +00:00
parent fb62330722
commit d524d8c8cc
19 changed files with 118 additions and 131 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/blackbox/blackbox.c
View file

@ -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];
}

6
src/main/cms/cms.c
View file

@ -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

45
src/main/cms/cms_menu_misc.c
View file

@ -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,
};

4
src/main/common/logic_condition.c
View file

@ -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);
}
}
}

10
src/main/fc/fc_core.c
View file

@ -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
) {

2
src/main/fc/fc_msp.c
View file

@ -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;

6
src/main/fc/rc_adjustments.c
View file

@ -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

26
src/main/fc/rc_controls.c
View file

@ -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);
}

2
src/main/fc/rc_modes.c
View file

@ -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));
}

6
src/main/flight/failsafe.c
View file

@ -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;
}

2
src/main/flight/mixer.c
View file

@ -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;
}

2
src/main/flight/pid.c
View file

@ -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

36
src/main/flight/servos.c
View file

@ -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;

4
src/main/io/ledstrip.c
View file

@ -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();

2
src/main/io/osd.c
View file

@ -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;

6
src/main/io/rcdevice_cam.c
View file

@ -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];

55
src/main/rx/rx.c
View file

@ -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;
}

13
src/main/rx/rx.h
View file

@ -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);

20
src/main/telemetry/mavlink.c
View file

@ -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