Merge pull request #4982 from mikeller/added_rx_processing_function

Added rx data processing function and used it for FPort processing.

src/main/fc/fc_core.c

@@ -397,7 +397,9 @@ void processRx(timeUs_t currentTimeUs)
     static bool armedBeeperOn = false;
     static bool airmodeIsActivated;
 
-    calculateRxChannelsAndUpdateFailsafe(currentTimeUs);
+    if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {
+        return;
+    }
 
     // in 3D mode, we need to be able to disarm by switch at any time
     if (feature(FEATURE_3D)) {

src/main/rx/fport.c

@@ -133,6 +133,9 @@ static volatile bool clearToSend = false;
 
 static volatile uint8_t framePosition = 0;
 
+static smartPortPayload_t *mspPayload = NULL;
+static timeUs_t lastRcFrameReceivedMs = 0;
+
 static serialPort_t *fportPort;
 static bool telemetryEnabled = false;
 
@@ -239,9 +242,6 @@ static bool checkChecksum(uint8_t *data, uint8_t length)
 static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
     static smartPortPayload_t payloadBuffer;
-    static smartPortPayload_t *mspPayload = NULL;
-    static bool hasTelemetryRequest = false;
-    static timeUs_t lastRcFrameReceivedMs = 0;
 
     uint8_t result = RX_FRAME_PENDING;
 
@@ -261,7 +261,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
                     if (frameLength != FPORT_FRAME_PAYLOAD_LENGTH_CONTROL) {
                         reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE);
                     } else {
-                        result = sbusChannelsDecode(rxRuntimeConfig, &frame->data.controlData.channels);
+                        result |= sbusChannelsDecode(rxRuntimeConfig, &frame->data.controlData.channels);
 
                         setRssiUnfiltered(scaleRange(constrain(frame->data.controlData.rssi, 0, 100), 0, 100, 0, 1024), RSSI_SOURCE_RX_PROTOCOL);
 
@@ -282,13 +282,14 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
                         switch(frame->data.telemetryData.frameId) {
                         case FPORT_FRAME_ID_NULL:
                         case FPORT_FRAME_ID_DATA: // never used
-                            hasTelemetryRequest = true;
+                            result = result | RX_FRAME_PROCESSING_REQUIRED;
 
                             break;
                         case FPORT_FRAME_ID_READ:
                         case FPORT_FRAME_ID_WRITE: // never used
                             memcpy(&payloadBuffer, &frame->data.telemetryData, sizeof(smartPortPayload_t));
                             mspPayload = &payloadBuffer;
+                            result = result | RX_FRAME_PROCESSING_REQUIRED;
 
                             break;
                         default:
@@ -309,8 +310,23 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
         }
 
         rxBufferReadIndex = (rxBufferReadIndex + 1) % NUM_RX_BUFFERS;
+    }
+
+    if (lastRcFrameReceivedMs && ((millis() - lastRcFrameReceivedMs) > FPORT_MAX_TELEMETRY_AGE_MS)) {
+        setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
+        lastRcFrameReceivedMs = 0;
+    }
+
+    return result;
+}
+
+static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    UNUSED(rxRuntimeConfig);
+
+    bool processingDone = false;
+
 #if defined(USE_TELEMETRY_SMARTPORT)
-    } else {
     timeUs_t currentTimeUs = micros();
     if (telemetryEnabled && clearToSend && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
         if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) {
@@ -320,9 +336,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
         if (clearToSend) {
            DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs);
 
-                if (hasTelemetryRequest || mspPayload) {
             processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
-                }
 
             if (clearToSend) {
                 smartPortWriteFrameFport(&emptySmartPortFrame);
@@ -330,16 +344,14 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
                 clearToSend = false;
             }
         }
+
+        processingDone = true;
     }
+#else
+    processingDone = true;
 #endif
-    }
 
-    if (lastRcFrameReceivedMs && ((millis() - lastRcFrameReceivedMs) > FPORT_MAX_TELEMETRY_AGE_MS)) {
+    return processingDone;
-        setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
-        lastRcFrameReceivedMs = 0;
-    }
-
-    return result;
 }
 
 bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
@@ -352,6 +364,7 @@ bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
     rxRuntimeConfig->rxRefreshRate = 11000;
 
     rxRuntimeConfig->rcFrameStatusFn = fportFrameStatus;
+    rxRuntimeConfig->rcProcessFrameFn = fportProcessFrame;
 
     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
     if (!portConfig) {

src/main/rx/rx.c

@@ -76,6 +76,8 @@ static timeUs_t lastMspRssiUpdateUs = 0;
 rssiSource_t rssiSource;
 
 static bool rxDataReceived = false;
+static bool processingRequired = false;
+
 static bool rxSignalReceived = false;
 static bool rxSignalReceivedNotDataDriven = false;
 static bool rxFlightChannelsValid = false;
@@ -204,6 +206,13 @@ static uint8_t nullFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
     return RX_FRAME_PENDING;
 }
 
+static bool nullProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    UNUSED(rxRuntimeConfig);
+
+    return true;
+}
+
 #define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels
 
 static uint8_t validFlightChannelMask;
@@ -302,6 +311,7 @@ void rxInit(void)
 {
     rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
     rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
+    rxRuntimeConfig.rcProcessFrameFn = nullProcessFrame;
     rcSampleIndex = 0;
     needRxSignalMaxDelayUs = DELAY_10_HZ;
 
@@ -433,7 +443,6 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
     } else
 #endif
     {
-        rxDataReceived = false;
         const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
         if (frameStatus & RX_FRAME_COMPLETE) {
             rxDataReceived = true;
@@ -441,8 +450,12 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
             rxSignalReceived = !rxIsInFailsafeMode;
             needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
         }
+
+        if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) {
+            processingRequired = true;
         }
-    return rxDataReceived || (currentTimeUs >= rxUpdateAt); // data driven or 50Hz
+    }
+    return rxDataReceived || processingRequired || (currentTimeUs >= rxUpdateAt); // data driven or 50Hz
 }
 
 static uint16_t calculateChannelMovingAverage(uint8_t chan, uint16_t sample)
@@ -597,8 +610,18 @@ static void detectAndApplySignalLossBehaviour(timeUs_t currentTimeUs)
 #endif
 }
 
-void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
+bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
 {
+    if (processingRequired) {
+        processingRequired = !rxRuntimeConfig.rcProcessFrameFn(&rxRuntimeConfig);
+    }
+
+    if (!rxDataReceived) {
+        return false;
+    }
+
+    rxDataReceived = false;
+
     rxUpdateAt = currentTimeUs + DELAY_50_HZ;
 
     // only proceed when no more samples to skip and suspend period is over
@@ -606,13 +629,16 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
         if (currentTimeUs > suspendRxSignalUntil) {
             skipRxSamples--;
         }
-        return;
+
+        return true;
     }
 
     readRxChannelsApplyRanges();
     detectAndApplySignalLossBehaviour(currentTimeUs);
 
     rcSampleIndex++;
+
+    return true;
 }
 
 void parseRcChannels(const char *input, rxConfig_t *rxConfig)

src/main/rx/rx.h

@@ -43,7 +43,8 @@
 typedef enum {
     RX_FRAME_PENDING = 0,
     RX_FRAME_COMPLETE = (1 << 0),
-    RX_FRAME_FAILSAFE = (1 << 1)
+    RX_FRAME_FAILSAFE = (1 << 1),
+    RX_FRAME_PROCESSING_REQUIRED = (1 << 2),
 } rxFrameState_e;
 
 typedef enum {
@@ -150,12 +151,14 @@ PG_DECLARE(rxConfig_t, rxConfig);
 struct rxRuntimeConfig_s;
 typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeConfig_s *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
 typedef uint8_t (*rcFrameStatusFnPtr)(struct rxRuntimeConfig_s *rxRuntimeConfig);
+typedef bool (*rcProcessFrameFnPtr)(const struct rxRuntimeConfig_s *rxRuntimeConfig);
 
 typedef struct rxRuntimeConfig_s {
     uint8_t             channelCount; // number of RC channels as reported by current input driver
     uint16_t            rxRefreshRate;
     rcReadRawDataFnPtr  rcReadRawFn;
     rcFrameStatusFnPtr  rcFrameStatusFn;
+    rcProcessFrameFnPtr rcProcessFrameFn;
     uint16_t            *channelData;
     void                *frameData;
 } rxRuntimeConfig_t;
@@ -176,7 +179,7 @@ void rxInit(void);
 bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
 bool rxIsReceivingSignal(void);
 bool rxAreFlightChannelsValid(void);
-void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);
+bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);
 
 void parseRcChannels(const char *input, rxConfig_t *rxConfig);
 

src/test/unit/arming_prevention_unittest.cc

@@ -64,6 +64,7 @@ extern "C" {
     gpsSolutionData_t gpsSol;
     uint32_t targetPidLooptime;
     bool cmsInMenu = false;
+    rxRuntimeConfig_t rxRuntimeConfig = {};
 }
 
 uint32_t simulationFeatureFlags = 0;
@@ -634,7 +635,7 @@ extern "C" {
     void mixTable(timeUs_t , uint8_t) {};
     void writeMotors(void) {};
     void writeServos(void) {};
-    void calculateRxChannelsAndUpdateFailsafe(timeUs_t) {}
+    bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; }
     bool isMixerUsingServos(void) { return false; }
     void gyroUpdate(timeUs_t) {}
     timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; }