Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 17:55:30 +03:00
Code Issues Wiki Activity

Merge pull request #11541 from ctzsnooze/Fix-GPS-Return-failsafe-with-bad-channel-data

Browse source 
Improve failsafe handling for Rx's that send bad channel information.
This commit is contained in:
haslinghuis 2022-04-21 01:00:04 +02:00 • committed by GitHub
commit d08ab1c6e8
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 33 additions and 28 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -107,8 +107,8 @@ static timeUs_t needRxSignalBefore = 0;
static timeUs_t suspendRxSignalUntil = 0; static timeUs_t suspendRxSignalUntil = 0;
static uint8_t skipRxSamples = 0; static uint8_t skipRxSamples = 0;
static float rcRaw[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]; // interval [1000;2000] float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // scaled, modified, checked and constrained values
uint32_t validRxSignalTimeout[MAX_SUPPORTED_RC_CHANNEL_COUNT]; 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 #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); const uint8_t frameStatus = rxRuntimeState.rcFrameStatusFn(&rxRuntimeState);
DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, (frameStatus & RX_FRAME_FAILSAFE)); 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); setLinkQuality(signalReceived, currentDeltaTimeUs);
auxiliaryProcessingRequired |= (frameStatus & RX_FRAME_PROCESSING_REQUIRED); 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 // true only when a new packet arrives
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
rxSignalReceived = true; // immediately process packet data rxSignalReceived = true; // immediately process packet data
if (useDataDrivenProcessing) {
rxDataProcessingRequired = true;
// process the new Rx packet when it arrives
}
} else { } else {
// watch for next packet // watch for next packet
if (cmpTimeUs(currentTimeUs, needRxSignalBefore) > 0) { if (cmpTimeUs(currentTimeUs, needRxSignalBefore) > 0) {
// initial time to signalReceived failure is 100ms, then we check every 100ms // initial time to signalReceived failure is 100ms, then we check every 100ms
rxSignalReceived = false; rxSignalReceived = false;
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; 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; rxDataProcessingRequired = true;
} }
} }
DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 0, rxSignalReceived); 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) #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 = 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; return sample;
} }
@ -645,7 +645,7 @@ void detectAndApplySignalLossBehaviour(void)
for (int channel = 0; channel < rxChannelCount; channel++) { for (int channel = 0; channel < rxChannelCount; channel++) {
float sample = rcRaw[channel]; float sample = rcRaw[channel];
const bool thisChannelValid = rxFlightChannelsValid && isPulseValid(sample); 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) { if (thisChannelValid) {
// reset the invalid pulse period timer for every good channel // reset the invalid pulse period timer for every good channel
@ -653,9 +653,10 @@ void detectAndApplySignalLossBehaviour(void)
} }
if (ARMING_FLAG(ARMED) && failsafeIsActive()) { 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 (channel < NON_AUX_CHANNEL_COUNT) {
if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { if (!thisChannelValid) {
if (channel == THROTTLE ) { if (channel == THROTTLE ) {
sample = failsafeConfig()->failsafe_throttle; sample = failsafeConfig()->failsafe_throttle;
} else { } else {
@ -663,7 +664,7 @@ void detectAndApplySignalLossBehaviour(void)
} }
} }
} else if (!failsafeAuxSwitch) { } 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); sample = getRxfailValue(channel);
} }
} else { } else {
@ -674,7 +675,8 @@ void detectAndApplySignalLossBehaviour(void)
} }
} else if (!thisChannelValid) { } else if (!thisChannelValid) {
if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) { 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 { } else {
// then use STAGE 1 failsafe values // then use STAGE 1 failsafe values
if (channel < NON_AUX_CHANNEL_COUNT) { 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 defined(USE_PWM) || defined(USE_PPM)
if (rxRuntimeState.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeState.rxProvider == RX_PROVIDER_PPM) { if (rxRuntimeState.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeState.rxProvider == RX_PROVIDER_PPM) {
// smooth output for PWM and PPM using moving average // smooth output for PWM and PPM using moving average
@ -695,8 +699,9 @@ void detectAndApplySignalLossBehaviour(void)
#endif #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; rcData[channel] = sample;
} }
} }

26
src/test/unit/rx_ranges_unittest.cc
View file

@ -66,8 +66,8 @@ TEST(RxChannelRangeTest, TestRxChannelRanges)
EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(1000, RANGE_CONFIGURATION(1000, 2000))); EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(1000, RANGE_CONFIGURATION(1000, 2000)));
EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(1000, 2000))); EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(1000, 2000)));
EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(2000, RANGE_CONFIGURATION(1000, 2000))); EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(2000, RANGE_CONFIGURATION(1000, 2000)));
EXPECT_EQ(750, applyRxChannelRangeConfiguraton(700, RANGE_CONFIGURATION(1000, 2000))); EXPECT_EQ(700, applyRxChannelRangeConfiguraton(700, RANGE_CONFIGURATION(1000, 2000)));
EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(1000, 2000))); EXPECT_EQ(2500, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(1000, 2000)));
// Reversed channel // Reversed channel
EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(1000, RANGE_CONFIGURATION(2000, 1000))); 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(1000, applyRxChannelRangeConfiguraton(900, RANGE_CONFIGURATION(900, 1900)));
EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1400, RANGE_CONFIGURATION(900, 1900))); EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1400, RANGE_CONFIGURATION(900, 1900)));
EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(1900, RANGE_CONFIGURATION(900, 1900))); EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(1900, RANGE_CONFIGURATION(900, 1900)));
EXPECT_EQ(750, applyRxChannelRangeConfiguraton(600, RANGE_CONFIGURATION(900, 1900))); EXPECT_EQ(700, applyRxChannelRangeConfiguraton(600, RANGE_CONFIGURATION(900, 1900)));
EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(900, 1900))); EXPECT_EQ(2600, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(900, 1900)));
// Narrower range than expected // Narrower range than expected
EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(1300, RANGE_CONFIGURATION(1300, 1700))); EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(1300, RANGE_CONFIGURATION(1300, 1700)));
EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(1300, 1700))); EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(1300, 1700)));
EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(1700, RANGE_CONFIGURATION(1300, 1700))); EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(1700, RANGE_CONFIGURATION(1300, 1700)));
EXPECT_EQ(750, applyRxChannelRangeConfiguraton(700, RANGE_CONFIGURATION(1300, 1700))); EXPECT_EQ(-500, applyRxChannelRangeConfiguraton(700, RANGE_CONFIGURATION(1300, 1700)));
EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(1300, 1700))); EXPECT_EQ(4000, applyRxChannelRangeConfiguraton(2500, RANGE_CONFIGURATION(1300, 1700)));
// Wider range than expected // Wider range than expected
EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(900, RANGE_CONFIGURATION(900, 2100))); EXPECT_EQ(1000, applyRxChannelRangeConfiguraton(900, RANGE_CONFIGURATION(900, 2100)));
EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(900, 2100))); EXPECT_EQ(1500, applyRxChannelRangeConfiguraton(1500, RANGE_CONFIGURATION(900, 2100)));
EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(2100, RANGE_CONFIGURATION(900, 2100))); EXPECT_EQ(2000, applyRxChannelRangeConfiguraton(2100, RANGE_CONFIGURATION(900, 2100)));
EXPECT_EQ(750, applyRxChannelRangeConfiguraton(600, 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 // extreme out of range
EXPECT_EQ(750, applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(1000, 2000))); EXPECT_EQ(1, applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(1000, 2000)));
EXPECT_EQ(750, applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(1300, 1700))); EXPECT_EQ(-2245, applyRxChannelRangeConfiguraton(2, RANGE_CONFIGURATION(1300, 1700)));
EXPECT_EQ(750, applyRxChannelRangeConfiguraton(1, RANGE_CONFIGURATION(900, 2100))); EXPECT_EQ(252.5, applyRxChannelRangeConfiguraton(3, RANGE_CONFIGURATION(900, 2100)));
EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(1000, 2000))); EXPECT_EQ(10000, applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(1000, 2000)));
EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(1300, 1700))); EXPECT_EQ(22750, applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(1300, 1700)));
EXPECT_EQ(2250, applyRxChannelRangeConfiguraton(10000, RANGE_CONFIGURATION(900, 2100))); EXPECT_EQ(25250, applyRxChannelRangeConfiguraton(30000, RANGE_CONFIGURATION(900, 2100)));
} }