diff --git a/src/msp_displayport.c b/src/msp_displayport.c index 23911f3..27f5a52 100644 --- a/src/msp_displayport.c +++ b/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