Merge pull request #6056 from mikeller/move_slowdown_to_fport

Moving sensor frame throttling from SmartPort to FPort only.

src/main/rx/fport.c

@@ -53,6 +53,8 @@
 #define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
 #define FPORT_MAX_TELEMETRY_AGE_MS 500
 
+#define FPORT_TELEMETRY_MAX_CONSECUTIVE_SENSORS 2
+
 
 #define FPORT_FRAME_MARKER 0x7E
 
@@ -335,6 +337,8 @@ static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
     UNUSED(rxRuntimeConfig);
 
 #if defined(USE_TELEMETRY_SMARTPORT)
+    static uint8_t consecutiveSensorCount = 0;
+
     timeUs_t currentTimeUs = micros();
     if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) {
        clearToSend = false;
@@ -343,7 +347,16 @@ static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
     if (clearToSend) {
         DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs);
 
-        processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
+        if (consecutiveSensorCount >= FPORT_TELEMETRY_MAX_CONSECUTIVE_SENSORS && !smartPortPayloadContainsMSP(mspPayload)) {
+// Stop one cycle to avoid saturating the buffer in the RX, since the
+// downstream bandwidth doesn't allow sensor sensors on every cycle.
+// We allow MSP frames to run over the resting period, expecting that
+// the caller won't flood us with requests.
+            consecutiveSensorCount = 0;
+        } else {
+            consecutiveSensorCount++;
+            processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
+        }
 
         if (clearToSend) {
             smartPortWriteFrameFport(&emptySmartPortFrame);

src/main/telemetry/smartport.c

@@ -149,13 +149,8 @@ enum
 
 static uint16_t frSkyDataIdTable[MAX_DATAIDS];
 
-// number of sensors to send before taking a rest
-// seems to improve throughput and prevents "sensor lost" issue (oversaturation of send queue???)
-#define SENSOR_REST_PERIOD 3
-
 #ifdef USE_ESC_SENSOR
 // number of sensors to send between sending the ESC sensors
-// must be greater than and not a multiple of SENSOR_REST_PERIOD
 #define ESC_SENSOR_PERIOD 7
 
 static uint16_t frSkyEscDataIdTable[] = {
@@ -291,6 +286,12 @@ void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port)
     }
 }
 
+bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload)
+{
+    return payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT;
+}
+
+
 void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum)
 {
     uint8_t *data = (uint8_t *)payload;
@@ -468,7 +469,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
         // unless we start receiving other sensors' packets
 
 #if defined(USE_MSP_OVER_TELEMETRY)
-        if (payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT) {
+        if (smartPortPayloadContainsMSP(payload)) {
             // Pass only the payload: skip frameId
             uint8_t *frameStart = (uint8_t *)&payload->valueId;
             smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE);
@@ -501,11 +502,6 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
         // we can send back any data we want, our tables keep track of the order and frequency of each data type we send
         frSkyTableInfo_t * tableInfo = &frSkyDataIdTableInfo;
 
-        if (smartPortIdCycleCnt % SENSOR_REST_PERIOD == 0) {
-            smartPortIdCycleCnt++;
-            return;
-        }
-
 #ifdef USE_ESC_SENSOR
         if (smartPortIdCycleCnt >= ESC_SENSOR_PERIOD) {
             // send ESC sensors

src/main/telemetry/smartport.h

@@ -75,3 +75,4 @@ smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPor
 struct serialPort_s;
 void smartPortWriteFrameSerial(const smartPortPayload_t *payload, struct serialPort_s *port, uint16_t checksum);
 void smartPortSendByte(uint8_t c, uint16_t *checksum, struct serialPort_s *port);
+bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload);