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

Merge pull request #9511 from mikeller/add_rc_frame_interval_measurement 

Added protocol level RX frame rate measurement for FrSky FPort.
This commit is contained in:
Michael Keller 2020-03-08 12:42:22 +13:00 committed by GitHub
parent c002d04356 ec5816eae0
commit 7e4d8c10ee
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
16 changed files with 81 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/build/debug.c
View File

@ -95,4 +95,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"FF_INTERPOLATED",
"BLACKBOX_OUTPUT",
"GYRO_SAMPLE",
"RX_TIMING",
};

1
src/main/build/debug.h
View File

@ -111,6 +111,7 @@ typedef enum {
DEBUG_FF_INTERPOLATED,
DEBUG_BLACKBOX_OUTPUT,
DEBUG_GYRO_SAMPLE,
DEBUG_RX_TIMING,
DEBUG_COUNT
} debugType_e;

6
src/main/fc/core.c
View File

@ -742,6 +742,12 @@ bool processRx(timeUs_t currentTimeUs)
static bool sharedPortTelemetryEnabled = false;
#endif
timeDelta_t frameAgeUs;
timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs);
DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX));
DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX));
if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {
return false;
}

11
src/main/fc/tasks.c
View File

@ -166,12 +166,15 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
return;
}
timeDelta_t rxFrameDeltaUs;
if (!rxGetFrameDelta(&rxFrameDeltaUs)) {
rxFrameDeltaUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
timeDelta_t frameAgeUs;
timeDelta_t refreshRateUs = rxGetFrameDelta(&frameAgeUs);
if (!refreshRateUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) {
if (!rxTryGetFrameDeltaOrZero(&refreshRateUs)) {
refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
}
}
lastRxTimeUs = currentTimeUs;
currentRxRefreshRate = constrain(rxFrameDeltaUs, 1000, 30000);
currentRxRefreshRate = constrain(refreshRateUs, 1000, 30000);
isRXDataNew = true;
#ifdef USE_USB_CDC_HID

4
src/main/rx/crsf.c
View File

@ -369,7 +369,7 @@ void crsfRxSendTelemetryData(void)
}
}
static timeUs_t crsfFrameTimeUs(void)
static timeUs_t crsfFrameTimeUsOrZeroFn(void)
{
const timeUs_t result = lastRcFrameTimeUs;
lastRcFrameTimeUs = 0;
@ -387,7 +387,7 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->rcReadRawFn = crsfReadRawRC;
rxRuntimeState->rcFrameStatusFn = crsfFrameStatus;
rxRuntimeState->rcFrameTimeUsFn = crsfFrameTimeUs;
rxRuntimeState->rcFrameTimeUsOrZeroFn = crsfFrameTimeUsOrZeroFn;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {

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

@ -130,6 +130,7 @@ static const smartPortPayload_t emptySmartPortFrame = { .frameId = 0, .valueId =
typedef struct fportBuffer_s {
uint8_t data[BUFFER_SIZE];
uint8_t length;
timeUs_t frameStartTimeUs;
} fportBuffer_t;
static fportBuffer_t rxBuffer[NUM_RX_BUFFERS];
@ -150,6 +151,8 @@ static serialPort_t *fportPort;
static bool telemetryEnabled = false;
#endif
static timeUs_t lastRcFrameTimeUs = 0;
static void reportFrameError(uint8_t errorReason) {
static volatile uint16_t frameErrors = 0;
@ -169,7 +172,7 @@ static void fportDataReceive(uint16_t c, void *data)
static timeUs_t lastFrameReceivedUs = 0;
static bool telemetryFrame = false;
const timeUs_t currentTimeUs = micros();
const timeUs_t currentTimeUs = microsISR();
clearToSend = false;
@ -203,6 +206,8 @@ static void fportDataReceive(uint16_t c, void *data)
frameStartAt = currentTimeUs;
framePosition = 1;
rxBuffer[rxBufferWriteIndex].frameStartTimeUs = currentTimeUs;
} else if (framePosition > 0) {
if (framePosition >= BUFFER_SIZE + 1) {
framePosition = 0;
@ -286,6 +291,10 @@ static uint8_t fportFrameStatus(rxRuntimeState_t *rxRuntimeState)
setRssi(scaleRange(frame->data.controlData.rssi, 0, 100, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_PROTOCOL);
lastRcFrameReceivedMs = millis();
if (!(result & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED))) {
lastRcFrameTimeUs = rxBuffer[rxBufferReadIndex].frameStartTimeUs;
}
}
break;
@ -390,6 +399,11 @@ static bool fportProcessFrame(const rxRuntimeState_t *rxRuntimeState)
return true;
}
static timeUs_t fportFrameTimeUs(void)
{
return lastRcFrameTimeUs;
}
bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
{
static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
@ -401,6 +415,7 @@ bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->rcFrameStatusFn = fportFrameStatus;
rxRuntimeState->rcProcessFrameFn = fportProcessFrame;
rxRuntimeState->rcFrameTimeUsFn = fportFrameTimeUs;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {

4
src/main/rx/ibus.c
View File

@ -206,7 +206,7 @@ static uint16_t ibusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t ch
return ibusChannelData[chan];
}
static timeUs_t ibusFrameTimeUs(void)
static timeUs_t ibusFrameTimeUsOrZeroFn(void)
{
const timeUs_t result = lastRcFrameTimeUs;
lastRcFrameTimeUs = 0;
@ -223,7 +223,7 @@ bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->rcReadRawFn = ibusReadRawRC;
rxRuntimeState->rcFrameStatusFn = ibusFrameStatus;
rxRuntimeState->rcFrameTimeUsFn = ibusFrameTimeUs;
rxRuntimeState->rcFrameTimeUsOrZeroFn = ibusFrameTimeUsOrZeroFn;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {

4
src/main/rx/jetiexbus.c
View File

@ -247,7 +247,7 @@ static uint16_t jetiExBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8
return (jetiExBusChannelData[chan]);
}
static timeUs_t jetiExBusFrameTimeUs(void)
static timeUs_t jetiExBusFrameTimeUsOrZeroFn(void)
{
const timeUs_t result = lastRcFrameTimeUs;
lastRcFrameTimeUs = 0;
@ -263,7 +263,7 @@ bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->rcReadRawFn = jetiExBusReadRawRC;
rxRuntimeState->rcFrameStatusFn = jetiExBusFrameStatus;
rxRuntimeState->rcFrameTimeUsFn = jetiExBusFrameTimeUs;
rxRuntimeState->rcFrameTimeUsOrZeroFn = jetiExBusFrameTimeUsOrZeroFn;
jetiExBusFrameReset();

36
src/main/rx/rx.c
View File

@ -873,21 +873,41 @@ bool isRssiConfigured(void)
return rssiSource != RSSI_SOURCE_NONE;
}
bool rxGetFrameDelta(timeDelta_t *deltaUs)
bool rxTryGetFrameDeltaOrZero(timeDelta_t *deltaUs)
{
static timeUs_t previousFrameTimeUs = 0;
static timeUs_t previousFrameTimeUsOrZero = 0;
bool result = false;
*deltaUs = 0;
if (rxRuntimeState.rcFrameTimeUsFn) {
const timeUs_t frameTimeUs = rxRuntimeState.rcFrameTimeUsFn();
if (frameTimeUs) {
if (previousFrameTimeUs) {
*deltaUs = cmpTimeUs(frameTimeUs, previousFrameTimeUs);
if (rxRuntimeState.rcFrameTimeUsOrZeroFn) {
const timeUs_t frameTimeUsOrZero = rxRuntimeState.rcFrameTimeUsOrZeroFn();
if (frameTimeUsOrZero) {
if (previousFrameTimeUsOrZero) {
*deltaUs = cmpTimeUs(frameTimeUsOrZero, previousFrameTimeUsOrZero);
result = true;
}
previousFrameTimeUs = frameTimeUs;
previousFrameTimeUsOrZero = frameTimeUsOrZero;
}
}
return result; // No frame delta function available for protocol type or frames have stopped
}
timeDelta_t rxGetFrameDelta(timeDelta_t *frameAgeUs)
{
static timeUs_t previousFrameTimeUs = 0;
static timeDelta_t frameTimeDeltaUs = 0;
if (rxRuntimeState.rcFrameTimeUsFn) {
const timeUs_t frameTimeUs = rxRuntimeState.rcFrameTimeUsFn();
*frameAgeUs = cmpTimeUs(micros(), frameTimeUs);
const timeDelta_t deltaUs = cmpTimeUs(frameTimeUs, previousFrameTimeUs);
if (deltaUs) {
frameTimeDeltaUs = deltaUs;
previousFrameTimeUs = frameTimeUs;
}
}
return frameTimeDeltaUs;
}

9
src/main/rx/rx.h
View File

@ -125,7 +125,8 @@ struct rxRuntimeState_s;
typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeState_s *rxRuntimeState, uint8_t chan); // used by receiver driver to return channel data
typedef uint8_t (*rcFrameStatusFnPtr)(struct rxRuntimeState_s *rxRuntimeState);
typedef bool (*rcProcessFrameFnPtr)(const struct rxRuntimeState_s *rxRuntimeState);
typedef timeUs_t (*rcGetFrameTimeUsFnPtr)(void); // used to retrieve the timestamp in microseconds for the last channel data frame
typedef timeUs_t (*rcGetFrameTimeUsOrZeroFnPtr)(void); // used to retrieve the timestamp in microseconds for the last channel data frame, or 0, depending on suitablilty of the value for RC smoothing
typedef timeUs_t rcGetFrameTimeUsFn(void); // used to retrieve the timestamp in microseconds for the last channel data frame
typedef enum {
RX_PROVIDER_NONE = 0,
@ -144,7 +145,8 @@ typedef struct rxRuntimeState_s {
rcReadRawDataFnPtr rcReadRawFn;
rcFrameStatusFnPtr rcFrameStatusFn;
rcProcessFrameFnPtr rcProcessFrameFn;
rcGetFrameTimeUsFnPtr rcFrameTimeUsFn;
rcGetFrameTimeUsOrZeroFnPtr rcFrameTimeUsOrZeroFn;
rcGetFrameTimeUsFn *rcFrameTimeUsFn;
uint16_t *channelData;
void *frameData;
} rxRuntimeState_t;
@ -210,4 +212,5 @@ void resumeRxPwmPpmSignal(void);
uint16_t rxGetRefreshRate(void);
bool rxGetFrameDelta(timeDelta_t *deltaUs);
bool rxTryGetFrameDeltaOrZero(timeDelta_t *deltaUs);
timeDelta_t rxGetFrameDelta(timeDelta_t *frameAgeUs);

4
src/main/rx/sbus.c
View File

@ -160,7 +160,7 @@ static uint8_t sbusFrameStatus(rxRuntimeState_t *rxRuntimeState)
return frameStatus;
}
static timeUs_t sbusFrameTimeUs(void)
static timeUs_t sbusFrameTimeUsOrZeroFn(void)
{
const timeUs_t result = lastRcFrameTimeUs;
lastRcFrameTimeUs = 0;
@ -188,7 +188,7 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
}
rxRuntimeState->rcFrameStatusFn = sbusFrameStatus;
rxRuntimeState->rcFrameTimeUsFn = sbusFrameTimeUs;
rxRuntimeState->rcFrameTimeUsOrZeroFn = sbusFrameTimeUsOrZeroFn;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {

4
src/main/rx/spektrum.c
View File

@ -343,7 +343,7 @@ void srxlRxWriteTelemetryData(const void *data, int len)
}
#endif
static timeUs_t spektrumFrameTimeUs(void)
static timeUs_t spektrumFrameTimeUsOrZeroFn(void)
{
const timeUs_t result = lastRcFrameTimeUs;
lastRcFrameTimeUs = 0;
@ -397,7 +397,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->rcReadRawFn = spektrumReadRawRC;
rxRuntimeState->rcFrameStatusFn = spektrumFrameStatus;
rxRuntimeState->rcFrameTimeUsFn = spektrumFrameTimeUs;
rxRuntimeState->rcFrameTimeUsOrZeroFn = spektrumFrameTimeUsOrZeroFn;
#if defined(USE_TELEMETRY_SRXL)
rxRuntimeState->rcProcessFrameFn = spektrumProcessFrame;
#endif

4
src/main/rx/srxl2.c
View File

@ -480,7 +480,7 @@ void srxl2RxWriteData(const void *data, int len)
writeBufferIdx = len;
}
static timeUs_t srxl2FrameTimeUs(void)
static timeUs_t srxl2FrameTimeUsOrZeroFn(void)
{
const timeUs_t result = lastRcFrameTimeUs;
lastRcFrameTimeUs = 0;
@ -503,7 +503,7 @@ bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->rcReadRawFn = srxl2ReadRawRC;
rxRuntimeState->rcFrameStatusFn = srxl2FrameStatus;
rxRuntimeState->rcFrameTimeUsFn = srxl2FrameTimeUs;
rxRuntimeState->rcFrameTimeUsOrZeroFn = srxl2FrameTimeUsOrZeroFn;
rxRuntimeState->rcProcessFrameFn = srxl2ProcessFrame;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);

4
src/main/rx/sumd.c
View File

@ -167,7 +167,7 @@ static uint16_t sumdReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t ch
return sumdChannels[chan] / 8;
}
static timeUs_t sumdFrameTimeUs(void)
static timeUs_t sumdFrameTimeUsOrZeroFn(void)
{
const timeUs_t result = lastRcFrameTimeUs;
lastRcFrameTimeUs = 0;
@ -183,7 +183,7 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->rcReadRawFn = sumdReadRawRC;
rxRuntimeState->rcFrameStatusFn = sumdFrameStatus;
rxRuntimeState->rcFrameTimeUsFn = sumdFrameTimeUs;
rxRuntimeState->rcFrameTimeUsOrZeroFn = sumdFrameTimeUsOrZeroFn;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {

1
src/test/unit/arming_prevention_unittest.cc
View File

@ -1101,4 +1101,5 @@ extern "C" {
bool isUpright(void) { return mockIsUpright; }
void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {};
void gyroFiltering(timeUs_t) {};
timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; }
}

1
src/test/unit/vtx_unittest.cc
View File

@ -186,4 +186,5 @@ extern "C" {
bool isUpright(void) { return true; }
void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {};
void gyroFiltering(timeUs_t) {};
timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; }
}