diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 4dcfe79573..aaab1976ae 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -107,8 +107,8 @@ static timeUs_t needRxSignalBefore = 0; static timeUs_t suspendRxSignalUntil = 0; static uint8_t skipRxSamples = 0; -static float rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] -float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] +static float rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // last received raw value, as it comes +float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // scaled, modified, checked and constrained values uint32_t validRxSignalTimeout[MAX_SUPPORTED_RC_CHANNEL_COUNT]; #define MAX_INVALID_PULSE_TIME_MS 300 // hold time in milliseconds after bad channel or Rx link loss @@ -507,7 +507,7 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current { const uint8_t frameStatus = rxRuntimeState.rcFrameStatusFn(&rxRuntimeState); DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, (frameStatus & RX_FRAME_FAILSAFE)); - signalReceived = (frameStatus & RX_FRAME_COMPLETE) && !((frameStatus & RX_FRAME_FAILSAFE) || (frameStatus & RX_FRAME_DROPPED)); + signalReceived = (frameStatus & RX_FRAME_COMPLETE) && !(frameStatus & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED)); setLinkQuality(signalReceived, currentDeltaTimeUs); auxiliaryProcessingRequired |= (frameStatus & RX_FRAME_PROCESSING_REQUIRED); } @@ -519,21 +519,22 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current // true only when a new packet arrives needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; rxSignalReceived = true; // immediately process packet data + if (useDataDrivenProcessing) { + rxDataProcessingRequired = true; + // process the new Rx packet when it arrives + } } else { // watch for next packet if (cmpTimeUs(currentTimeUs, needRxSignalBefore) > 0) { // initial time to signalReceived failure is 100ms, then we check every 100ms rxSignalReceived = false; needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; - // review rcData values every 100ms in failsafe changed them + // review and process rcData values every 100ms in case failsafe changed them rxDataProcessingRequired = true; } } DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 0, rxSignalReceived); - // process the new Rx packet when it arrives - rxDataProcessingRequired |= (signalReceived && useDataDrivenProcessing); - } #if defined(USE_PWM) || defined(USE_PPM) @@ -602,8 +603,7 @@ STATIC_UNIT_TESTED float applyRxChannelRangeConfiguraton(float sample, const rxC } sample = scaleRangef(sample, range->min, range->max, PWM_RANGE_MIN, PWM_RANGE_MAX); - sample = constrainf(sample, PWM_PULSE_MIN, PWM_PULSE_MAX); - + // out of range channel values are now constrained after the validity check in detectAndApplySignalLossBehaviour() return sample; } @@ -645,7 +645,7 @@ void detectAndApplySignalLossBehaviour(void) for (int channel = 0; channel < rxChannelCount; channel++) { float sample = rcRaw[channel]; const bool thisChannelValid = rxFlightChannelsValid && isPulseValid(sample); - // if the packet is bad, we don't allow any channels to be good + // if the whole packet is bad, consider all channels bad if (thisChannelValid) { // reset the invalid pulse period timer for every good channel @@ -653,9 +653,10 @@ void detectAndApplySignalLossBehaviour(void) } if (ARMING_FLAG(ARMED) && failsafeIsActive()) { - // apply failsafe values, until failsafe ends, or disarmed, unless in GPS Return (where stick values should remain) + // while in failsafe Stage 2, pass incoming flight channel values unless they are bad + // this allows GPS Return to detect the 30% requirement for termination if (channel < NON_AUX_CHANNEL_COUNT) { - if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { + if (!thisChannelValid) { if (channel == THROTTLE ) { sample = failsafeConfig()->failsafe_throttle; } else { @@ -663,7 +664,7 @@ void detectAndApplySignalLossBehaviour(void) } } } else if (!failsafeAuxSwitch) { - // aux channels as Set in Configurator, unless failsafe initiated by switch + // set aux channels as per Stage 1 Configurator values, unless failsafe was initiated by switch sample = getRxfailValue(channel); } } else { @@ -674,7 +675,8 @@ void detectAndApplySignalLossBehaviour(void) } } else if (!thisChannelValid) { if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) { - // HOLD bad channel/s for MAX_INVALID_PULSE_TIME_MS (300ms) after Rx loss + sample = rcData[channel]; + // HOLD last valid value on bad channel/s for MAX_INVALID_PULSE_TIME_MS (300ms) } else { // then use STAGE 1 failsafe values if (channel < NON_AUX_CHANNEL_COUNT) { @@ -687,6 +689,8 @@ void detectAndApplySignalLossBehaviour(void) } } + sample = constrainf(sample, PWM_PULSE_MIN, PWM_PULSE_MAX); + #if defined(USE_PWM) || defined(USE_PPM) if (rxRuntimeState.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeState.rxProvider == RX_PROVIDER_PPM) { // smooth output for PWM and PPM using moving average @@ -695,8 +699,9 @@ void detectAndApplySignalLossBehaviour(void) #endif { - // set rcData to either clean raw incoming values, or failsafe-modified values + // set rcData to either validated incoming values, or failsafe-modified values rcData[channel] = sample; + } } diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index 0a96d8db55..27220b1091 100644 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -66,8 +66,8 @@ TEST(RxChannelRangeTest, TestRxChannelRanges) EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(1000, RANGE_CONFIGURATION(1000, 2000))); EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(1000, 2000))); EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(2000, RANGE_CONFIGURATION(1000, 2000))); - EXPECT_EQ(750, applyRxChannelRangeConfiguraton(700, RANGE_CONFIGURATION(1000, 2000))); - EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(1000, 2000))); + EXPECT_EQ(700, applyRxChannelRangeConfiguraton(700, RANGE_CONFIGURATION(1000, 2000))); + EXPECT_EQ(2500, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(1000, 2000))); // Reversed channel EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(1000, RANGE_CONFIGURATION(2000, 1000))); @@ -78,31 +78,31 @@ TEST(RxChannelRangeTest, TestRxChannelRanges) EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(900, RANGE_CONFIGURATION(900, 1900))); EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1400, RANGE_CONFIGURATION(900, 1900))); EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(1900, RANGE_CONFIGURATION(900, 1900))); - EXPECT_EQ(750, applyRxChannelRangeConfiguraton(600, RANGE_CONFIGURATION(900, 1900))); - EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(900, 1900))); + EXPECT_EQ(700, applyRxChannelRangeConfiguraton(600, RANGE_CONFIGURATION(900, 1900))); + EXPECT_EQ(2600, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(900, 1900))); // Narrower range than expected EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(1300, RANGE_CONFIGURATION(1300, 1700))); EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(1300, 1700))); EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(1700, RANGE_CONFIGURATION(1300, 1700))); - EXPECT_EQ(750, applyRxChannelRangeConfiguraton(700, RANGE_CONFIGURATION(1300, 1700))); - EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(1300, 1700))); + EXPECT_EQ(-500, applyRxChannelRangeConfiguraton(700, RANGE_CONFIGURATION(1300, 1700))); + EXPECT_EQ(4000, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(1300, 1700))); // Wider range than expected EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(900, RANGE_CONFIGURATION(900, 2100))); EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(900, 2100))); EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(2100, RANGE_CONFIGURATION(900, 2100))); EXPECT_EQ(750, applyRxChannelRangeConfiguraton(600, RANGE_CONFIGURATION(900, 2100))); - EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(2700, RANGE_CONFIGURATION(900, 2100))); + EXPECT_EQ(2500, applyRxChannelRangeConfiguraton(2700, RANGE_CONFIGURATION(900, 2100))); // extreme out of range - EXPECT_EQ(750, applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(1000, 2000))); - EXPECT_EQ(750, applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(1300, 1700))); - EXPECT_EQ(750, applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(900, 2100))); + EXPECT_EQ(1, applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(1000, 2000))); + EXPECT_EQ(-2245, applyRxChannelRangeConfiguraton(2, RANGE_CONFIGURATION(1300, 1700))); + EXPECT_EQ(252.5, applyRxChannelRangeConfiguraton(3, RANGE_CONFIGURATION(900, 2100))); - EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(1000, 2000))); - EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(1300, 1700))); - EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(900, 2100))); + EXPECT_EQ(10000, applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(1000, 2000))); + EXPECT_EQ(22750, applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(1300, 1700))); + EXPECT_EQ(25250, applyRxChannelRangeConfiguraton(30000, RANGE_CONFIGURATION(900, 2100))); }