fix hdzero channel settings over displayport

iNavFlight · Nov 15, 2023 · a2a0562 · a2a0562
1 parent df543ff
commit a2a0562
Showing 1 changed file with 23 additions and 0 deletions.
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
@@ -2581,6 +2581,29 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
                         if (sbufBytesRemaining(src) > 0) {
                             vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src);
                         }
+
+                        // //////////////////////////////////////////////////////////
+                        // this code is taken from BF, it's hack for HDZERO VTX MSP frame
+                        // API version 1.42 - this parameter kept separate since clients may already be supplying
+                        if (sbufBytesRemaining(src) >= 2) {
+                            sbufReadU16(src); //skip pitModeFreq
+                        }
+
+                        // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency
+                        if (sbufBytesRemaining(src) >= 4) {
+                            uint8_t newBand = sbufReadU8(src);
+                            const uint8_t newChannel = sbufReadU8(src);
+                            vtxSettingsConfigMutable()->band = newBand;
+                            vtxSettingsConfigMutable()->channel = newChannel;
+                        }
+
+                       /* if (sbufBytesRemaining(src) >= 4) {
+                            sbufRead8(src); // freq_l
+                            sbufRead8(src); // freq_h
+                            sbufRead8(src); // band count
+                            sbufRead8(src); // channel count
+                        }*/
+                        // //////////////////////////////////////////////////////////
                     }
                 }
             }