rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #4670 from mikeller/fix_fport_rx 

Fixed FPort receiving state machine.
This commit is contained in:
Michael Keller 2017-11-28 19:00:08 +13:00 committed by GitHub
parent d3d5b107cc 6da0a21292
commit d3a0e18a0c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 26 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
src/main/rx/fport.c
View File

@ -43,7 +43,7 @@
#include "rx/fport.h" #include "rx/fport.h"
#define FPORT_TIME_NEEDED_PER_FRAME_US 5200 // Empirically established #define FPORT_TIME_NEEDED_PER_FRAME_US 3000
#define FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US 2000 #define FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US 2000
#define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500 #define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
#define FPORT_MAX_TELEMETRY_AGE_MS 500 #define FPORT_MAX_TELEMETRY_AGE_MS 500
@ -129,7 +129,7 @@ static volatile uint8_t rxBufferReadIndex = 0;
static volatile timeUs_t lastTelemetryFrameReceivedUs; static volatile timeUs_t lastTelemetryFrameReceivedUs;
static volatile bool clearToSend = false; static volatile bool clearToSend = false;
static volatile bool skipUntilStart = false; static volatile uint8_t framePosition = 0;
static serialPort_t *fportPort; static serialPort_t *fportPort;
static bool telemetryEnabled = false; static bool telemetryEnabled = false;
@ -147,57 +147,50 @@ static void fportDataReceive(uint16_t c, void *data)
UNUSED(data); UNUSED(data);
static timeUs_t frameStartAt = 0; static timeUs_t frameStartAt = 0;
static bool frameStarting = false;
static bool escapedCharacter = false; static bool escapedCharacter = false;
static uint8_t bufferPosition = 0;
static timeUs_t lastFrameReceivedUs = 0; static timeUs_t lastFrameReceivedUs = 0;
static bool telemetryFrame = false; static bool telemetryFrame = false;
const timeUs_t currentTimeUs = micros(); const timeUs_t currentTimeUs = micros();
if (!skipUntilStart && (frameStarting || bufferPosition > 0) && cmpTimeUs(currentTimeUs, frameStartAt) > FPORT_TIME_NEEDED_PER_FRAME_US + 500) { clearToSend = false;
if (framePosition > 1 && cmpTimeUs(currentTimeUs, frameStartAt) > FPORT_TIME_NEEDED_PER_FRAME_US + 500) {
reportFrameError(DEBUG_FPORT_ERROR_TIMEOUT); reportFrameError(DEBUG_FPORT_ERROR_TIMEOUT);
bufferPosition = 0; framePosition = 0;
clearToSend = false;
} }
uint8_t val = (uint8_t)c; uint8_t val = (uint8_t)c;
if (val == FPORT_FRAME_MARKER) { if (val == FPORT_FRAME_MARKER) {
skipUntilStart = false; if (framePosition > 1) {
frameStarting = true; const uint8_t nextWriteIndex = (rxBufferWriteIndex + 1) % NUM_RX_BUFFERS;
escapedCharacter = false;
frameStartAt = currentTimeUs;
if (bufferPosition > 0) {
uint8_t nextWriteIndex = (rxBufferWriteIndex + 1) % NUM_RX_BUFFERS;
if (nextWriteIndex != rxBufferReadIndex) { if (nextWriteIndex != rxBufferReadIndex) {
rxBuffer[rxBufferWriteIndex].length = bufferPosition; rxBuffer[rxBufferWriteIndex].length = framePosition - 1;
rxBufferWriteIndex = nextWriteIndex; rxBufferWriteIndex = nextWriteIndex;
} }
bufferPosition = 0;
if (telemetryFrame) { if (telemetryFrame) {
clearToSend = true; clearToSend = true;
lastTelemetryFrameReceivedUs = currentTimeUs; lastTelemetryFrameReceivedUs = currentTimeUs;
}
if (lastFrameReceivedUs) {
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_INTERVAL, currentTimeUs - lastFrameReceivedUs);
}
lastFrameReceivedUs = currentTimeUs;
} else {
clearToSend = false;
}
telemetryFrame = false; telemetryFrame = false;
} else if (skipUntilStart) { }
frameStarting = false;
bufferPosition = 0; DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_INTERVAL, currentTimeUs - lastFrameReceivedUs);
lastFrameReceivedUs = currentTimeUs;
escapedCharacter = false;
}
frameStartAt = currentTimeUs;
framePosition = 1;
} else if (framePosition > 0) {
if (framePosition >= BUFFER_SIZE + 1) {
framePosition = 0;
reportFrameError(DEBUG_FPORT_ERROR_OVERSIZE);
} else { } else {
if ((frameStarting || bufferPosition > 0) && bufferPosition < BUFFER_SIZE) {
if (escapedCharacter) { if (escapedCharacter) {
val = val ^ FPORT_ESCAPE_MASK; val = val ^ FPORT_ESCAPE_MASK;
escapedCharacter = false; escapedCharacter = false;
@ -207,25 +200,20 @@ static void fportDataReceive(uint16_t c, void *data)
return; return;
} }
if (bufferPosition == 1 && val == FPORT_FRAME_TYPE_TELEMETRY_REQUEST) { if (framePosition == 2 && val == FPORT_FRAME_TYPE_TELEMETRY_REQUEST) {
telemetryFrame = true; telemetryFrame = true;
} }
frameStarting = false; rxBuffer[rxBufferWriteIndex].data[framePosition - 1] = val;
rxBuffer[rxBufferWriteIndex].data[bufferPosition++] = val; framePosition = framePosition + 1;
} else if (bufferPosition >= BUFFER_SIZE) {
bufferPosition = 0;
reportFrameError(DEBUG_FPORT_ERROR_OVERSIZE);
} }
} }
} }
#if defined(USE_TELEMETRY_SMARTPORT) #if defined(USE_TELEMETRY_SMARTPORT)
static void smartPortWriteFrameFport(const smartPortPayload_t *payload) static void smartPortWriteFrameFport(const smartPortPayload_t *payload)
{ {
skipUntilStart = true; framePosition = 0;
uint16_t checksum = 0; uint16_t checksum = 0;
smartPortSendByte(FPORT_RESPONSE_FRAME_LENGTH, &checksum, fportPort); smartPortSendByte(FPORT_RESPONSE_FRAME_LENGTH, &checksum, fportPort);