Merge pull request #221 from geoffsim/VTX-Config-Init

Process Initial MSP messages in the correct order.

src/msp_displayport.c

@@ -768,19 +768,30 @@ uint8_t msp_send_header_v2(uint16_t len, uint16_t msg) {
     return crc;
 }
 
-void msp_cmd_tx() // send 3 commands to FC
+ // Send commands to the FC.
+ // Ensure VARIANT is processed first, followed by CONFIG, then the others.
+void msp_cmd_tx()
 {
-    uint8_t i;
-    uint8_t const count = (fc_lock & FC_VTX_CONFIG_LOCK) ? 4 : 5;
+    uint8_t i, start = 0, count = 1;
+
+    // Wait for variant, then config, then continue with status/rx/osd_canvas
+    if (fc_lock & FC_VARIANT_LOCK) {
+        start = 1;
+        if (fc_lock & FC_VTX_CONFIG_LOCK) {
+            start = 2;
+            count = (msp_cmp_fc_variant("BTFL")) ? 3 : 2;
+        }
+    }
+
     uint8_t const msp_cmd[5] = {
         MSP_FC_VARIANT,
+        MSP_GET_VTX_CONFIG,
         MSP_STATUS,
         MSP_RC,
         MSP_GET_OSD_CANVAS,
-        MSP_GET_VTX_CONFIG,
     };
 
-    for (i = 0; i < count; i++) {
+    for (i = start; count--; i++) {
         msp_send_command(0, MSP_HEADER_V1);
         msp_tx(0x00);       // len
         msp_tx(msp_cmd[i]); // function