MSP2 via CSFR/SmartPort telemetry

src/main/rx/crsf.c

@@ -41,7 +41,7 @@
 #include "rx/crsf.h"
 
 #include "telemetry/crsf.h"
-#define CRSF_TIME_NEEDED_PER_FRAME_US   1100 // 700 ms + 400 ms for potential ad-hoc request
+#define CRSF_TIME_NEEDED_PER_FRAME_US   1750 // 700 ms + 400 ms for potential ad-hoc request
 #define CRSF_TIME_BETWEEN_FRAMES_US     6667 // At fastest, frames are sent by the transmitter every 6.667 milliseconds, 150 Hz
 
 #define CRSF_DIGITAL_CHANNEL_MIN 172
@@ -55,7 +55,7 @@ STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
 STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
 
 static serialPort_t *serialPort;
-static timeUs_t crsfFrameStartAt = 0;
+static timeUs_t crsfFrameStartAtUs = 0;
 static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
 static uint8_t telemetryBufLen = 0;
 
@@ -141,20 +141,20 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData)
     UNUSED(rxCallbackData);
 
     static uint8_t crsfFramePosition = 0;
-    const timeUs_t now = micros();
+    const timeUs_t currentTimeUs = microsISR();
 
 #ifdef DEBUG_CRSF_PACKETS
     debug[2] = now - crsfFrameStartAt;
 #endif
 
-    if (now > crsfFrameStartAt + CRSF_TIME_NEEDED_PER_FRAME_US) {
+    if (cmpTimeUs(currentTimeUs, crsfFrameStartAtUs) > CRSF_TIME_NEEDED_PER_FRAME_US) {
         // We've received a character after max time needed to complete a frame,
         // so this must be the start of a new frame.
         crsfFramePosition = 0;
     }
 
     if (crsfFramePosition == 0) {
-        crsfFrameStartAt = now;
+        crsfFrameStartAtUs = currentTimeUs;
     }
     // assume frame is 5 bytes long until we have received the frame length
     // full frame length includes the length of the address and framelength fields
@@ -174,8 +174,8 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData)
                         case CRSF_FRAMETYPE_MSP_REQ:
                         case CRSF_FRAMETYPE_MSP_WRITE: {
                             uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
-                            if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
-                                crsfScheduleMspResponse();
+                            if (bufferCrsfMspFrame(frameStart, crsfFrame.frame.frameLength - 4)) {
+                                crsfScheduleMspResponse(crsfFrame.frame.payload[1]);
                             }
                             break;
                         }
@@ -289,7 +289,7 @@ void crsfRxSendTelemetryData(void)
         // check that we are not in bi dir mode or that we are not currently receiving data (ie in the middle of an RX frame)
         // and that there is time to send the telemetry frame before the next RX frame arrives
         if (CRSF_PORT_OPTIONS & SERIAL_BIDIR) {
-            const timeDelta_t timeSinceStartOfFrame = cmpTimeUs(micros(), crsfFrameStartAt);
+            const timeDelta_t timeSinceStartOfFrame = cmpTimeUs(micros(), crsfFrameStartAtUs);
             if ((timeSinceStartOfFrame < CRSF_TIME_NEEDED_PER_FRAME_US) ||
                 (timeSinceStartOfFrame > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) {
                 return;
@@ -300,6 +300,11 @@ void crsfRxSendTelemetryData(void)
     }
 }
 
+bool crsfRxIsTelemetryBufEmpty(void)
+{
+    return telemetryBufLen == 0;
+}
+
 bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 {
     for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {

src/main/rx/crsf.h

@@ -130,6 +130,7 @@ typedef union crsfFrame_u {
 
 void crsfRxWriteTelemetryData(const void *data, int len);
 void crsfRxSendTelemetryData(void);
+bool crsfRxIsTelemetryBufEmpty(void);
 
 struct rxConfig_s;
 struct rxRuntimeConfig_s;

src/main/telemetry/crsf.c

@@ -107,25 +107,36 @@ bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength)
 
 bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
 {
-    bool requestHandled = false;
+    static bool replyPending = false;
+    if (replyPending) {
+        if (crsfRxIsTelemetryBufEmpty()) {
+            replyPending = sendMspReply(payloadSize, responseFn);
+        }
+        return replyPending;
+    }
+
     if (!mspRxBuffer.len) {
         return false;
     }
     int pos = 0;
     while (true) {
         const int mspFrameLength = mspRxBuffer.bytes[pos];
         if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength)) {
-            requestHandled |= sendMspReply(payloadSize, responseFn);
+            if (crsfRxIsTelemetryBufEmpty()) {
+                replyPending = sendMspReply(payloadSize, responseFn);
+            } else {
+                replyPending = true;
+            }
         }
         pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
         ATOMIC_BLOCK(NVIC_PRIO_SERIALUART) {
             if (pos >= mspRxBuffer.len) {
                 mspRxBuffer.len = 0;
-                return requestHandled;
+                return replyPending ;
             }
         }
     }
-    return requestHandled;
+    return replyPending;
 }
 #endif
 
@@ -453,28 +464,36 @@ static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
 
 static bool mspReplyPending;
 
-void crsfScheduleMspResponse(void)
+//Id of the last receiver MSP frame over CRSF. Needed to send response with correct frame ID
+static uint8_t mspRequestOriginID = 0;
+
+void crsfScheduleMspResponse(uint8_t requestOriginID)
 {
     mspReplyPending = true;
+    mspRequestOriginID = requestOriginID;
 }
 
-void crsfSendMspResponse(uint8_t *payload)
+void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize)
 {
     sbuf_t crsfPayloadBuf;
     sbuf_t *dst = &crsfPayloadBuf;
 
     crsfInitializeFrame(dst);
-    sbufWriteU8(dst, CRSF_FRAME_TX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
+    sbufWriteU8(dst, payloadSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
     crsfSerialize8(dst, CRSF_FRAMETYPE_MSP_RESP);
-    crsfSerialize8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
+    crsfSerialize8(dst, mspRequestOriginID);
     crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
-    crsfSerializeData(dst, (const uint8_t*)payload, CRSF_FRAME_TX_MSP_FRAME_SIZE);
+    crsfSerializeData(dst, (const uint8_t*)payload, payloadSize);
     crsfFinalize(dst);
 }
 #endif
 
 static void processCrsf(void)
 {
+    if (!crsfRxIsTelemetryBufEmpty()) {
+        return; // do nothing if telemetry ouptut buffer is not empty yet.
+    }
+
     static uint8_t crsfScheduleIndex = 0;
     const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex];
 

src/main/telemetry/crsf.h

@@ -27,7 +27,7 @@ void initCrsfTelemetry(void);
 bool checkCrsfTelemetryState(void);
 void handleCrsfTelemetry(timeUs_t currentTimeUs);
 void crsfScheduleDeviceInfoResponse(void);
-void crsfScheduleMspResponse(void);
+void crsfScheduleMspResponse(uint8_t requestOriginID);
 int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
 #if defined(USE_MSP_OVER_TELEMETRY)
 void initCrsfMspBuffer(void);