diff --git a/docs/Failsafe.md b/docs/Failsafe.md index 915d504cb..a5fafa4b6 100644 --- a/docs/Failsafe.md +++ b/docs/Failsafe.md @@ -22,9 +22,15 @@ The failsafe system attempts to detect when your receiver looses signal. It the The failsafe is activated when: +Either: a) no valid channel data from the RX via Serial RX. b) the first 4 Parallel PWM/PPM channels do not have valid signals. +And: +c) the failsafe guard time specified by `failsafe_delay` has elapsed. + +## Configuration + There are a few settings for it, as below. Failsafe delays are configured in 0.1 second steps. diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 37949dc94..5c6b3880e 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -573,7 +573,7 @@ void applyLedWarningLayer(uint8_t updateNow) if (feature(FEATURE_VBAT) && calculateBatteryState() != BATTERY_OK) { warningFlags |= WARNING_FLAG_LOW_BATTERY; } - if (failsafe->vTable->hasTimerElapsed()) { + if (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed()) { warningFlags |= WARNING_FLAG_FAILSAFE; } if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) { diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index d6e9247f6..71c543c4a 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -35,9 +35,14 @@ #include "rx/rx.h" #include "rx/sbus.h" +#define DEBUG_SBUS_PACKETS + + #define SBUS_MAX_CHANNEL 16 #define SBUS_FRAME_SIZE 25 -#define SBUS_SYNCBYTE 0x0F + +#define SBUS_FRAME_BEGIN_BYTE 0x0F +#define SBUS_FRAME_END_BYTE 0x00 #define SBUS_BAUDRATE 100000 @@ -48,6 +53,7 @@ static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); static uint32_t sbusChannelData[SBUS_MAX_CHANNEL]; static serialPort_t *sBusPort; +static uint32_t sbusSignalLostEventCount = 0; void sbusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint) { @@ -71,7 +77,14 @@ bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa return sBusPort != NULL; } -struct sbus_dat { +#define SBUS_FLAG_RESERVED_1 (1 << 0) +#define SBUS_FLAG_RESERVED_2 (1 << 1) +#define SBUS_FLAG_SIGNAL_LOSS (1 << 2) +#define SBUS_FLAG_FAILSAFE_ACTIVE (1 << 3) + +struct sbusFrame_s { + uint8_t syncByte; + // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes. unsigned int chan0 : 11; unsigned int chan1 : 11; unsigned int chan2 : 11; @@ -88,39 +101,53 @@ struct sbus_dat { unsigned int chan13 : 11; unsigned int chan14 : 11; unsigned int chan15 : 11; + uint8_t flags; + uint8_t endByte; } __attribute__ ((__packed__)); typedef union { - uint8_t in[SBUS_FRAME_SIZE]; - struct sbus_dat msg; -} sbus_msg; + uint8_t bytes[SBUS_FRAME_SIZE]; + struct sbusFrame_s frame; +} sbusFrame_t; -static sbus_msg sbus; +static sbusFrame_t sbusFrame; // Receive ISR callback static void sbusDataReceive(uint16_t c) { - uint32_t sbusTime; - static uint32_t sbusTimeLast; - static uint8_t sbusFramePosition; +#ifdef DEBUG_SBUS_PACKETS + static uint8_t sbusUnusedFrameCount = 0; +#endif - sbusTime = micros(); - if ((sbusTime - sbusTimeLast) > 2500) // sbus2 fast timing + static uint8_t sbusFramePosition = 0; + static uint32_t sbusTimeoutAt = 0; + uint32_t now = millis(); + + if ((int32_t)(sbusTimeoutAt - now) < 0) { sbusFramePosition = 0; - sbusTimeLast = sbusTime; + } + sbusTimeoutAt = now + 2500; - if (sbusFramePosition == 0 && c != SBUS_SYNCBYTE) + sbusFrame.bytes[sbusFramePosition] = (uint8_t)c; + + if (sbusFramePosition == 0 && c != SBUS_FRAME_BEGIN_BYTE) { return; + } - sbusFrameDone = false; // lazy main loop didnt fetch the stuff - if (sbusFramePosition != 0) - sbus.in[sbusFramePosition - 1] = (uint8_t)c; + sbusFramePosition++; - if (sbusFramePosition == SBUS_FRAME_SIZE - 1) { - sbusFrameDone = true; + if (sbusFramePosition == SBUS_FRAME_SIZE) { + if (sbusFrame.frame.endByte == SBUS_FRAME_END_BYTE) { + sbusFrameDone = true; + } sbusFramePosition = 0; } else { - sbusFramePosition++; +#ifdef DEBUG_SBUS_PACKETS + if (sbusFrameDone) { + sbusUnusedFrameCount++; + } +#endif + sbusFrameDone = false; } } @@ -130,26 +157,32 @@ bool sbusFrameComplete(void) return false; } sbusFrameDone = false; - if ((sbus.in[SBUS_FRAME_SIZE - 3] >> 3) & 0x0001) { + + if (sbusFrame.frame.flags & SBUS_FLAG_SIGNAL_LOSS) { + // internal failsafe enabled and rx failsafe flag set + sbusSignalLostEventCount++; + } + if (sbusFrame.frame.flags & SBUS_FLAG_FAILSAFE_ACTIVE) { // internal failsafe enabled and rx failsafe flag set return false; } - sbusChannelData[0] = sbus.msg.chan0; - sbusChannelData[1] = sbus.msg.chan1; - sbusChannelData[2] = sbus.msg.chan2; - sbusChannelData[3] = sbus.msg.chan3; - sbusChannelData[4] = sbus.msg.chan4; - sbusChannelData[5] = sbus.msg.chan5; - sbusChannelData[6] = sbus.msg.chan6; - sbusChannelData[7] = sbus.msg.chan7; - sbusChannelData[8] = sbus.msg.chan8; - sbusChannelData[9] = sbus.msg.chan9; - sbusChannelData[10] = sbus.msg.chan10; - sbusChannelData[11] = sbus.msg.chan11; - sbusChannelData[12] = sbus.msg.chan12; - sbusChannelData[13] = sbus.msg.chan13; - sbusChannelData[14] = sbus.msg.chan14; - sbusChannelData[15] = sbus.msg.chan15; + + sbusChannelData[0] = sbusFrame.frame.chan0; + sbusChannelData[1] = sbusFrame.frame.chan1; + sbusChannelData[2] = sbusFrame.frame.chan2; + sbusChannelData[3] = sbusFrame.frame.chan3; + sbusChannelData[4] = sbusFrame.frame.chan4; + sbusChannelData[5] = sbusFrame.frame.chan5; + sbusChannelData[6] = sbusFrame.frame.chan6; + sbusChannelData[7] = sbusFrame.frame.chan7; + sbusChannelData[8] = sbusFrame.frame.chan8; + sbusChannelData[9] = sbusFrame.frame.chan9; + sbusChannelData[10] = sbusFrame.frame.chan10; + sbusChannelData[11] = sbusFrame.frame.chan11; + sbusChannelData[12] = sbusFrame.frame.chan12; + sbusChannelData[13] = sbusFrame.frame.chan13; + sbusChannelData[14] = sbusFrame.frame.chan14; + sbusChannelData[15] = sbusFrame.frame.chan15; return true; }