diff --git a/src/main/main.c b/src/main/main.c index a858a8cda8..36c9d7c090 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -112,7 +112,7 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers, servoMixe void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); #endif void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration); -void rxInit(rxConfig_t *rxConfig); +void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions); void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); void imuInit(void); @@ -406,7 +406,7 @@ void init(void) failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); - rxInit(&masterConfig.rxConfig); + rxInit(&masterConfig.rxConfig, currentProfile->modeActivationConditions); #ifdef GPS if (feature(FEATURE_GPS)) { diff --git a/src/main/mw.c b/src/main/mw.c index 3a331be273..c1a936c49f 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -682,8 +682,8 @@ void filterRc(void){ if (isRXDataNew) { for (int channel=0; channel < 4; channel++) { - deltaRC[channel] = rcData[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); - lastCommand[channel] = rcData[channel]; + deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); + lastCommand[channel] = rcCommand[channel]; } isRXDataNew = false; @@ -692,10 +692,10 @@ void filterRc(void){ factor--; } - // Interpolate steps of rcData + // Interpolate steps of rcCommand if (factor > 0) { for (int channel=0; channel < 4; channel++) { - rcData[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; + rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; } } else { factor = 0; @@ -768,11 +768,12 @@ void loop(void) } } + annexCode(); + if (masterConfig.rxConfig.rcSmoothing) { filterRc(); } - annexCode(); #if defined(BARO) || defined(SONAR) haveProcessedAnnexCodeOnce = true; #endif diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 7a46d60a03..94bd328555 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -69,7 +69,10 @@ uint16_t rssi = 0; // range: [0;1023] static bool rxDataReceived = false; static bool rxSignalReceived = false; +static bool rxSignalReceivedNotDataDriven = false; static bool rxFlightChannelsValid = false; +static bool rxIsInFailsafeMode = true; +static bool rxIsInFailsafeModeNotDataDriven = true; static uint32_t rxUpdateAt = 0; static uint32_t needRxSignalBefore = 0; @@ -78,7 +81,9 @@ 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] +uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +#define MAX_INVALID_PULS_TIME 300 #define PPM_AND_PWM_SAMPLE_COUNT 3 #define DELAY_50_HZ (1000000 / 50) @@ -145,19 +150,36 @@ void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChann } } -void rxInit(rxConfig_t *rxConfig) +void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions) { uint8_t i; + uint16_t value; useRxConfig(rxConfig); rcSampleIndex = 0; for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { rcData[i] = rxConfig->midrc; + rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME; } rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig->midrc : rxConfig->rx_min_usec; + // Initialize ARM switch to OFF position when arming via switch is defined + for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[i]; + if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) { + // ARM switch is defined, determine an OFF value + if (modeActivationCondition->range.startStep > 0) { + value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationCondition->range.startStep - 1)); + } else { + value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationCondition->range.endStep + 1)); + } + // Initialize ARM AUX channel to OFF value + rcData[modeActivationCondition->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = value; + } + } + #ifdef SERIAL_RX if (feature(FEATURE_RX_SERIAL)) { serialRxInit(rxConfig); @@ -272,9 +294,7 @@ static void resetRxSignalReceivedFlagIfNeeded(uint32_t currentTime) if (((int32_t)(currentTime - needRxSignalBefore) >= 0)) { rxSignalReceived = false; -#ifdef DEBUG_RX_SIGNAL_LOSS - debug[0]++; -#endif + rxSignalReceivedNotDataDriven = false; } } @@ -307,7 +327,8 @@ void updateRx(uint32_t currentTime) if (frameStatus & SERIAL_RX_FRAME_COMPLETE) { rxDataReceived = true; - rxSignalReceived = (frameStatus & SERIAL_RX_FRAME_FAILSAFE) == 0; + rxIsInFailsafeMode = (frameStatus & SERIAL_RX_FRAME_FAILSAFE) != 0; + rxSignalReceived = !rxIsInFailsafeMode; needRxSignalBefore = currentTime + DELAY_10_HZ; } } @@ -318,13 +339,15 @@ void updateRx(uint32_t currentTime) if (rxDataReceived) { rxSignalReceived = true; + rxIsInFailsafeMode = false; needRxSignalBefore = currentTime + DELAY_5_HZ; } } if (feature(FEATURE_RX_PPM)) { if (isPPMDataBeingReceived()) { - rxSignalReceived = true; + rxSignalReceivedNotDataDriven = true; + rxIsInFailsafeModeNotDataDriven = false; needRxSignalBefore = currentTime + DELAY_10_HZ; resetPPMDataReceivedState(); } @@ -332,7 +355,8 @@ void updateRx(uint32_t currentTime) if (feature(FEATURE_RX_PARALLEL_PWM)) { if (isPWMDataBeingReceived()) { - rxSignalReceived = true; + rxSignalReceivedNotDataDriven = true; + rxIsInFailsafeModeNotDataDriven = false; needRxSignalBefore = currentTime + DELAY_10_HZ; } } @@ -441,15 +465,23 @@ static void detectAndApplySignalLossBehaviour(void) uint16_t sample; bool useValueFromRx = true; bool rxIsDataDriven = isRxDataDriven(); + uint32_t currentMilliTime = millis(); - if (!rxSignalReceived) { - if (rxIsDataDriven && rxDataReceived) { - // use the values from the RX - } else { - useValueFromRx = false; - } + if (!rxIsDataDriven) { + rxSignalReceived = rxSignalReceivedNotDataDriven; + rxIsInFailsafeMode = rxIsInFailsafeModeNotDataDriven; } + if (!rxSignalReceived || rxIsInFailsafeMode) { + useValueFromRx = false; + } + +#ifdef DEBUG_RX_SIGNAL_LOSS + debug[0] = rxSignalReceived; + debug[1] = rxIsInFailsafeMode; + debug[2] = rcReadRawFunc(&rxRuntimeConfig, 0); +#endif + rxResetFlightChannelStatus(); for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { @@ -459,11 +491,16 @@ static void detectAndApplySignalLossBehaviour(void) bool validPulse = isPulseValid(sample); if (!validPulse) { - sample = getRxfailValue(channel); + if (currentMilliTime < rcInvalidPulsPeriod[channel]) { + sample = rcData[channel]; // hold channel for MAX_INVALID_PULS_TIME + } else { + sample = getRxfailValue(channel); // after that apply rxfail value + rxUpdateFlightChannelStatus(channel, validPulse); + } + } else { + rcInvalidPulsPeriod[channel] = currentMilliTime + MAX_INVALID_PULS_TIME; } - rxUpdateFlightChannelStatus(channel, validPulse); - if (rxIsDataDriven) { rcData[channel] = sample; } else { @@ -476,7 +513,7 @@ static void detectAndApplySignalLossBehaviour(void) if ((rxFlightChannelsValid) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE)) { failsafeOnValidDataReceived(); } else { - rxSignalReceived = false; + rxIsInFailsafeMode = rxIsInFailsafeModeNotDataDriven = true; failsafeOnValidDataFailed(); for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { @@ -484,6 +521,9 @@ static void detectAndApplySignalLossBehaviour(void) } } +#ifdef DEBUG_RX_SIGNAL_LOSS + debug[3] = rcData[THROTTLE]; +#endif } void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime) diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc index c654594ea8..6b1d263ef7 100644 --- a/src/test/unit/rx_rx_unittest.cc +++ b/src/test/unit/rx_rx_unittest.cc @@ -24,10 +24,12 @@ extern "C" { #include "platform.h" #include "rx/rx.h" + #include "io/rc_controls.h" + #include "common/maths.h" uint32_t rcModeActivationMask; - void rxInit(rxConfig_t *rxConfig); + void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions); void rxResetFlightChannelStatus(void); bool rxHaveValidFlightChannels(void); bool isPulseValid(uint16_t pulseDuration); @@ -54,16 +56,28 @@ TEST(RxTest, TestValidFlightChannels) // and rxConfig_t rxConfig; + modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; memset(&rxConfig, 0, sizeof(rxConfig)); rxConfig.rx_min_usec = 1000; rxConfig.rx_max_usec = 2000; - // when - rxInit(&rxConfig); + memset(&modeActivationConditions, 0, sizeof(modeActivationConditions)); + modeActivationConditions[0].auxChannelIndex = 0; + modeActivationConditions[0].modeId = BOXARM; + modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN); + modeActivationConditions[0].range.endStep = CHANNEL_VALUE_TO_STEP(1600); + // when + rxInit(&rxConfig, modeActivationConditions); + + // then (ARM channel should be positioned just 1 step above active range to init to OFF) + EXPECT_EQ(1625, rcData[modeActivationConditions[0].auxChannelIndex + NON_AUX_CHANNEL_COUNT]); + + // given rxResetFlightChannelStatus(); + // and for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) { bool validPulse = isPulseValid(1500); rxUpdateFlightChannelStatus(channelIndex, validPulse); @@ -80,20 +94,29 @@ TEST(RxTest, TestInvalidFlightChannels) // and rxConfig_t rxConfig; + modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; memset(&rxConfig, 0, sizeof(rxConfig)); rxConfig.rx_min_usec = 1000; rxConfig.rx_max_usec = 2000; + memset(&modeActivationConditions, 0, sizeof(modeActivationConditions)); + modeActivationConditions[0].auxChannelIndex = 0; + modeActivationConditions[0].modeId = BOXARM; + modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(1400); + modeActivationConditions[0].range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX); + // and uint16_t channelPulses[MAX_SUPPORTED_RC_CHANNEL_COUNT]; memset(&channelPulses, 1500, sizeof(channelPulses)); // and - rxInit(&rxConfig); + rxInit(&rxConfig, modeActivationConditions); + + // then (ARM channel should be positioned just 1 step below active range to init to OFF) + EXPECT_EQ(1375, rcData[modeActivationConditions[0].auxChannelIndex + NON_AUX_CHANNEL_COUNT]); // and - for (uint8_t stickChannelIndex = 0; stickChannelIndex < STICK_CHANNEL_COUNT; stickChannelIndex++) { // given