AP_RCProtocol: handle 0x10 and 0x00 frames, check scheduler delay and fix incoming crc

Respond to 0x10 when the rx is controlling bandwidth, else respond only to 0x00
but in this case never exceed a max number of consecutive frames.
Always respond to polling and if necessary send a null frame.
moved delay check after get_telem_data() to account for scheduler time
align incoming to outgoing crc

libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp

@@ -42,6 +42,12 @@ extern const AP_HAL::HAL& hal;
 
 #define FPORT_TYPE_CONTROL 0
 #define FPORT_TYPE_DOWNLINK 1
+#define FPORT_PRIM_NULL 0x00
+#define FPORT_PRIM_DATA 0x10
+#define FPORT_PRIM_READ 0x30
+#define FPORT_PRIM_WRITE 0x31
+
+#define MAX_FPORT_CONSECUTIVE_FRAMES 2
 
 struct PACKED FPort_Frame {
     uint8_t header; // 0x7E
@@ -80,6 +86,18 @@ struct PACKED FPort_Frame {
     };
 };
 
+struct {
+    bool available = false;
+    uint32_t data;
+    uint16_t appid;
+    uint8_t frame;
+} telem_data;
+
+// receiver sends 0x10 when ready to receive telemetry frames (R-XSR)
+bool rx_driven_frame_rate = false;
+// if the receiver is not controlling frame rate apply a constraint on consecutive frames
+uint8_t consecutive_telemetry_frame_count;
+
 static_assert(sizeof(FPort_Frame) == FPORT_CONTROL_FRAME_SIZE, "FPort_Frame incorrect size");
 
 // constructor
@@ -130,10 +148,30 @@ void AP_RCProtocol_FPort::decode_control(const FPort_Frame &frame)
 void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
 {
 #if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
-    if (frame.downlink.prim != 0x10 && frame.downlink.prim != 0x00) {
-        // only respond to frame types 0x00 or 0x10
-        return;
+    switch (frame.downlink.prim) {
+        case FPORT_PRIM_DATA:
+            // we've seen at least one 0x10 frame
+            rx_driven_frame_rate = true;
+            break;
+        case FPORT_PRIM_NULL:
+            if (rx_driven_frame_rate) {
+                return;
+            }
+            // with 0x00 and no rx control we have a constraint
+            // on max consecutive frames
+            if (consecutive_telemetry_frame_count >= MAX_FPORT_CONSECUTIVE_FRAMES) {
+                consecutive_telemetry_frame_count = 0;
+                return;
+            } else {
+                consecutive_telemetry_frame_count++;
+            }
+            break;
+        case FPORT_PRIM_READ:
+        case FPORT_PRIM_WRITE:
+            // do not respond to 0x30 and 0x31
+            return;
     }
+
     /*
       if we are getting FPORT over a UART then we can ask the FrSky
       telem library for some passthrough data to send back, enabling
@@ -146,6 +184,21 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
         return;
     }
 
+    /*
+      get SPort data from FRSky_Telem or send a null frame.
+      We save the data to a variable so in case we're late we'll
+      send it in the next call, this prevents corruption of
+      status text messages
+     */
+    if (!telem_data.available) {
+        if (!AP_Frsky_Telem::get_telem_data(telem_data.frame, telem_data.appid, telem_data.data)) {
+            // nothing to send, send a null frame
+            telem_data.frame = 0x00; 
+            telem_data.appid = 0x00; 
+            telem_data.data = 0x00;
+        }
+        telem_data.available = true;
+    }
     /*
       check that we haven't been too slow in responding to the new
       UART data. If we respond too late then we will corrupt the next
@@ -158,54 +211,45 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
         // we've been too slow in responding
         return;
     }
+    uint8_t buf[10];
 
-    /*
-      get the SPort data from FRSky_Telem and send it as an uplink
-      packet
-     */
-    uint8_t frametype;
-    uint16_t appid;
-    uint32_t data;
-    if (AP_Frsky_Telem::get_telem_data(frametype, appid, data)) {
-        uint8_t buf[10];
-
-        buf[0] = 0x08;
-        buf[1] = 0x81;
-        buf[2] = frametype;
-        buf[3] = appid & 0xFF;
-        buf[4] = appid >> 8;
-        memcpy(&buf[5], &data, 4);
-
-        uint16_t sum = 0;
-        for (uint8_t i=0; i<sizeof(buf)-1; i++) {
-            sum += buf[i];
-            sum += sum >> 8;
-            sum &= 0xFF;              
-        }
-        sum = 0xff - ((sum & 0xff) + (sum >> 8));
-        buf[9] = sum;
+    buf[0] = 0x08;
+    buf[1] = 0x81;
+    buf[2] = telem_data.frame;
+    buf[3] = telem_data.appid & 0xFF;
+    buf[4] = telem_data.appid >> 8;
+    memcpy(&buf[5], &telem_data.data, 4);
 
-        // perform byte stuffing per FPort spec
-        uint8_t len = 0;
-        uint8_t buf2[sizeof(buf)*2+1];
+    uint16_t sum = 0;
+    for (uint8_t i=0; i<sizeof(buf)-1; i++) {
+        sum += buf[i];
+        sum += sum >> 8;
+        sum &= 0xFF;              
+    }
+    sum = 0xff - ((sum & 0xff) + (sum >> 8));
+    buf[9] = (uint8_t)sum;
 
-        if (rc().fport_pad()) {
-            // this padding helps on some uarts that have hw pullups
-            buf2[len++] = 0xff;
-        }
+    // perform byte stuffing per FPort spec
+    uint8_t len = 0;
+    uint8_t buf2[sizeof(buf)*2+1];
 
-        for (uint8_t i=0; i<sizeof(buf); i++) {
-            uint8_t c = buf[i];
-            if (c == FRAME_DLE || buf[i] == FRAME_HEAD) {
-                buf2[len++] = FRAME_DLE;
-                buf2[len++] = c ^ FRAME_XOR;
-            } else {
-                buf2[len++] = c;
-            }
-        }
+    if (rc().fport_pad()) {
+        // this padding helps on some uarts that have hw pullups
+        buf2[len++] = 0xff;
+    }
 
-        uart->write(buf2, len);
+    for (uint8_t i=0; i<sizeof(buf); i++) {
+        uint8_t c = buf[i];
+        if (c == FRAME_DLE || buf[i] == FRAME_HEAD) {
+            buf2[len++] = FRAME_DLE;
+            buf2[len++] = c ^ FRAME_XOR;
+        } else {
+            buf2[len++] = c;
+        }
     }
+    uart->write(buf2, len);
+    // get fresh telem_data in the next call
+    telem_data.available = false;
 #endif
 }
 
@@ -306,6 +350,8 @@ bool AP_RCProtocol_FPort::check_checksum(void)
     uint16_t sum = 0;
     for (uint8_t i=0; i<len; i++) {
         sum += b[i];
+        sum += sum >> 8;
+        sum &= 0xFF;            
     }
     sum = (sum & 0xff) + (sum >> 8);
     return sum == 0xff;