diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index a264a9497f..c5b5bb083e 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -54,6 +54,20 @@ #define SUMD_BUFFSIZE (SUMD_MAX_CHANNEL * 2 + 5) // 6 channels + 5 = 17 bytes for 6 channels #define SUMD_BAUDRATE 115200 +#define SUMD_TIME_NEEDED_PER_FRAME 4000 + +#define SUMD_OFFSET_CHANNEL_1_HIGH 3 +#define SUMD_OFFSET_CHANNEL_1_LOW 4 +#define SUMD_BYTES_PER_CHANNEL 2 +#define SUMD_SYNC_BYTE_INDEX 0 +#define SUMD_CHANNEL_COUNT_INDEX 2 + +#define SUMD_HEADER_LENGTH 3 +#define SUMD_CRC_LENGTH 2 + +#define SUMDV1_FRAME_STATE_OK 0x01 +#define SUMDV3_FRAME_STATE_OK 0x03 +#define SUMD_FRAME_STATE_FAILSAFE 0x81 static bool sumdFrameDone = false; static uint16_t sumdChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT]; @@ -61,53 +75,49 @@ static uint16_t crc; static uint8_t sumd[SUMD_BUFFSIZE] = { 0, }; static uint8_t sumdChannelCount; +static timeDelta_t lastFrameDelta = 0; // Receive ISR callback static void sumdDataReceive(uint16_t c, void *data) { UNUSED(data); - uint32_t sumdTime; - static uint32_t sumdTimeLast; + static timeUs_t sumdTimeLast; static uint8_t sumdIndex; + static timeUs_t lastFrameCompleteTimeUs = 0; - sumdTime = micros(); - if ((sumdTime - sumdTimeLast) > 4000) + const timeUs_t sumdTime = microsISR(); + if (cmpTimeUs(sumdTime, sumdTimeLast) > SUMD_TIME_NEEDED_PER_FRAME) { sumdIndex = 0; + } sumdTimeLast = sumdTime; - if (sumdIndex == 0) { - if (c != SUMD_SYNCBYTE) + if (sumdIndex == SUMD_SYNC_BYTE_INDEX) { + if (c != SUMD_SYNCBYTE) { return; - else - { + } else { sumdFrameDone = false; // lazy main loop didnt fetch the stuff crc = 0; } - } - if (sumdIndex == 2) + } else if (sumdIndex == SUMD_CHANNEL_COUNT_INDEX) { sumdChannelCount = (uint8_t)c; - if (sumdIndex < SUMD_BUFFSIZE) + } + + if (sumdIndex < SUMD_BUFFSIZE) { sumd[sumdIndex] = (uint8_t)c; + } + sumdIndex++; - if (sumdIndex < sumdChannelCount * 2 + 4) + if (sumdIndex <= sumdChannelCount * SUMD_BYTES_PER_CHANNEL + SUMD_HEADER_LENGTH) { crc = crc16_ccitt(crc, (uint8_t)c); - else - if (sumdIndex == sumdChannelCount * 2 + 5) { - sumdIndex = 0; - sumdFrameDone = true; - } + } else if (sumdIndex == sumdChannelCount * SUMD_BYTES_PER_CHANNEL + SUMD_HEADER_LENGTH + SUMD_CRC_LENGTH) { + lastFrameDelta = cmpTimeUs(sumdTime, lastFrameCompleteTimeUs); + lastFrameCompleteTimeUs = sumdTime; + sumdIndex = 0; + sumdFrameDone = true; + } } -#define SUMD_OFFSET_CHANNEL_1_HIGH 3 -#define SUMD_OFFSET_CHANNEL_1_LOW 4 -#define SUMD_BYTES_PER_CHANNEL 2 - - -#define SUMDV1_FRAME_STATE_OK 0x01 -#define SUMDV3_FRAME_STATE_OK 0x03 -#define SUMD_FRAME_STATE_FAILSAFE 0x81 - static uint8_t sumdFrameStatus(rxRuntimeState_t *rxRuntimeState) { UNUSED(rxRuntimeState); @@ -122,22 +132,23 @@ static uint8_t sumdFrameStatus(rxRuntimeState_t *rxRuntimeState) // verify CRC if (crc != ((sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_HIGH] << 8) | - (sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_LOW]))) + (sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_LOW]))) { return frameStatus; - - switch (sumd[1]) { - case SUMD_FRAME_STATE_FAILSAFE: - frameStatus = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE; - break; - case SUMDV1_FRAME_STATE_OK: - case SUMDV3_FRAME_STATE_OK: - frameStatus = RX_FRAME_COMPLETE; - break; - default: - return frameStatus; } - unsigned channelsToProcess = MIN(sumdChannelCount, MAX_SUPPORTED_RC_CHANNEL_COUNT); + switch (sumd[1]) { + case SUMD_FRAME_STATE_FAILSAFE: + frameStatus = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE; + break; + case SUMDV1_FRAME_STATE_OK: + case SUMDV3_FRAME_STATE_OK: + frameStatus = RX_FRAME_COMPLETE; + break; + default: + return frameStatus; + } + + const unsigned channelsToProcess = MIN(sumdChannelCount, MAX_SUPPORTED_RC_CHANNEL_COUNT); for (unsigned channelIndex = 0; channelIndex < channelsToProcess; channelIndex++) { sumdChannels[channelIndex] = ( @@ -154,6 +165,11 @@ static uint16_t sumdReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t ch return sumdChannels[chan] / 8; } +static timeDelta_t sumdFrameDelta(void) +{ + return lastFrameDelta; +} + bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) { UNUSED(rxConfig); @@ -163,6 +179,7 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) rxRuntimeState->rcReadRawFn = sumdReadRawRC; rxRuntimeState->rcFrameStatusFn = sumdFrameStatus; + rxRuntimeState->rcFrameDeltaFn = sumdFrameDelta; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { diff --git a/src/test/unit/rx_sumd_unittest.cc b/src/test/unit/rx_sumd_unittest.cc index 82f0d93a9c..05d6aedab9 100644 --- a/src/test/unit/rx_sumd_unittest.cc +++ b/src/test/unit/rx_sumd_unittest.cc @@ -69,6 +69,11 @@ uint32_t micros(void) return microseconds_stub_value; } +uint32_t microsISR(void) +{ + return micros(); +} + #define SERIAL_BUFFER_SIZE 256 #define SERIAL_PORT_DUMMY_IDENTIFIER (serialPortIdentifier_e)0x1234