MSP 1.54 support - add vtx_low_power_disarm

emuflight · Apr 5, 2024 · ad448e6 · ad448e6
1 parent 6244dbc
commit ad448e6
Showing 1 changed file with 34 additions and 26 deletions.
diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
@@ -1364,6 +1364,9 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) {
     sbufWriteU8(dst, vtxSettingsConfig()->power);
     sbufWriteU8(dst, pitmode);
     sbufWriteU16(dst, vtxSettingsConfig()->freq);
+    //MSP 1.54
+    sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
+    //END MSP 1.54
     // future extensions here...
     } else {
     sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected
@@ -2027,32 +2030,37 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) {
 #endif
 #ifdef USE_VTX_COMMON
         case MSP_SET_VTX_CONFIG: {
-    vtxDevice_t *vtxDevice = vtxCommonDevice();
-    if (vtxDevice) {
-    if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) {
-    uint16_t newFrequency = sbufReadU16(src);
-    if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) {  //value is band and channel
-    const uint8_t newBand = (newFrequency / 8) + 1;
-    const uint8_t newChannel = (newFrequency % 8) + 1;
-    vtxSettingsConfigMutable()->band = newBand;
-    vtxSettingsConfigMutable()->channel = newChannel;
-    vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(newBand, newChannel);
-    } else {  //value is frequency in MHz
-    vtxSettingsConfigMutable()->band = 0;
-    vtxSettingsConfigMutable()->freq = newFrequency;
-    }
-    if (sbufBytesRemaining(src) > 1) {
-    vtxSettingsConfigMutable()->power = sbufReadU8(src);
-    // Delegate pitmode to vtx directly
-    const uint8_t newPitmode = sbufReadU8(src);
-    uint8_t currentPitmode = 0;
-    vtxCommonGetPitMode(vtxDevice, &currentPitmode);
-    if (currentPitmode != newPitmode) {
-    vtxCommonSetPitMode(vtxDevice, newPitmode);
-    }
-    }
-    }
-    }
+            vtxDevice_t *vtxDevice = vtxCommonDevice();
+            if (vtxDevice) {
+                if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) {
+                    uint16_t newFrequency = sbufReadU16(src);
+                    if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) {  //value is band and channel
+                    const uint8_t newBand = (newFrequency / 8) + 1;
+                    const uint8_t newChannel = (newFrequency % 8) + 1;
+                    vtxSettingsConfigMutable()->band = newBand;
+                    vtxSettingsConfigMutable()->channel = newChannel;
+                    vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(newBand, newChannel);
+                } else {  //value is frequency in MHz
+                    vtxSettingsConfigMutable()->band = 0;
+                    vtxSettingsConfigMutable()->freq = newFrequency;
+                }
+                if (sbufBytesRemaining(src) > 1) {
+                    vtxSettingsConfigMutable()->power = sbufReadU8(src);
+                    // Delegate pitmode to vtx directly
+                    const uint8_t newPitmode = sbufReadU8(src);
+                    uint8_t currentPitmode = 0;
+                    vtxCommonGetPitMode(vtxDevice, &currentPitmode);
+                    if (currentPitmode != newPitmode) {
+                        vtxCommonSetPitMode(vtxDevice, newPitmode);
+                    }
+                }
+                // MSP 1.54
+                if (sbufBytesRemaining(src) >= 1) {
+                    vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src);
+                }
+                // END MSP 1.54
+            }
+        }
     }
     break;
 #endif