Modified irctramp.c

patch/inav/src/main/io/vtx_tramp.c

@@ -56,7 +56,7 @@ typedef enum {
     VTX_STATE_RESET         = 0,
     VTX_STATE_OFFILE        = 1,    // Not detected
     VTX_STATE_DETECTING     = 2,    // 
-    VTX_STATE_IDLE          = 3,    // Idle, ready to sent commands
+    VTX_STATE_IDLE          = 3,    // Idle, ready to send commands
     VTX_STATE_QUERY_DELAY   = 4,
     VTX_STATE_QUERY_STATUS  = 5,
     VTX_STATE_WAIT_STATUS   = 6,    // Wait for VTX state
@@ -158,7 +158,7 @@ static bool vtxProtoRecv(void)
                     // Reset the receiver state - keep waiting
                     vtxState.recvPtr = 0;
                 }
-                // Make sure it's not the echo one (half-duplex serial might receive it's own data)
+                // Make sure it's not the echo one (half-duplex serial might receive its own data)
                 else if (memcmp(&vtxState.recvPkt, &vtxState.sendPkt, VTX_PKT_SIZE) == 0) {
                     vtxState.recvPtr = 0;
                 }
@@ -223,13 +223,13 @@ static vtxProtoResponseType_e vtxProtoProcessResponse(void)
             vtxState.capabilities.powerMax = vtxState.recvPkt[6] | (vtxState.recvPkt[7] << 8);
 
             if (vtxState.capabilities.freqMin != 0 && vtxState.capabilities.freqMin < vtxState.capabilities.freqMax) {
-                // Some TRAMP VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX)
-                // Make use of vtxSettingsConfig()->maxPowerOverride to override
+                // Some third-party Tramp VTXes report capped max power
+                // Override using CLI vtx_max_power_override if defined
                 if (vtxSettingsConfig()->maxPowerOverride != 0) {
                     vtxState.capabilities.powerMax = vtxSettingsConfig()->maxPowerOverride;
                 }
 
-                // Update max power metadata so OSD settings would match VTX capabilities
+                // Update max power metadata so OSD settings match VTX capabilities
                 vtxProtoUpdatePowerMetadata(vtxState.capabilities.powerMax);
 
                 return VTX_RESPONSE_TYPE_CAPABILITIES;
@@ -238,9 +238,8 @@ static vtxProtoResponseType_e vtxProtoProcessResponse(void)
 
         case 0x76:
             vtxState.state.freq = vtxState.recvPkt[2] | (vtxState.recvPkt[3] << 8);
-            vtxState.state.power = vtxState.recvPkt[4]|(vtxState.recvPkt[5] << 8);
+            vtxState.state.power = vtxState.recvPkt[4] | (vtxState.recvPkt[5] << 8);
             vtxState.state.pitMode = vtxState.recvPkt[7];
-            //vtxState.state.power = vtxState.recvPkt[8]|(vtxState.recvPkt[9] << 8);
             return VTX_RESPONSE_TYPE_STATUS;
     }
 
@@ -264,7 +263,6 @@ static void vtxProtoSetFrequency(uint16_t freq)
 
 static void impl_Process(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
 {
-    // Glue function betwen VTX VTable and actual driver protothread
     UNUSED(vtxDevice);
     UNUSED(currentTimeUs);
 
@@ -289,27 +287,22 @@ static void impl_Process(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
         case VTX_STATE_DETECTING:
             if (vtxProtoRecv()) {
                 if (vtxProtoProcessResponse() == VTX_RESPONSE_TYPE_CAPABILITIES) {
-                    // VTX sent capabilities. Query status now
                     vtxState.protoTimeoutCount = 0;
                     vtxProtoSetState(VTX_STATE_QUERY_STATUS);
                 }
                 else {
-                    // Unexpected response. Re-initialize
                     vtxProtoSetState(VTX_STATE_RESET);
                 }
             }
             else if (vtxProtoTimeout()) {
-                // Time-out while waiting for capabilities. Reset the state
                 vtxProtoSetState(VTX_STATE_RESET);
             }
             break;
 
-        // Send requests to update freqnecy and power, periodically poll device for liveness
+        // Send requests to update frequency and power, periodically poll device for liveness
         case VTX_STATE_IDLE:
             if (vtxState.updateReqMask != VTX_UPDATE_REQ_NONE) {
-                // Updates pending. Send an appropriate command
                 if (vtxState.updateReqMask & VTX_UPDATE_REQ_PITMODE) {
-                    // Only disabling PIT mode supported
                     vtxState.updateReqMask &= ~VTX_UPDATE_REQ_PITMODE;
                     vtxProtoSetPitMode(0);
                     vtxProtoSetState(VTX_STATE_QUERY_DELAY);
@@ -326,22 +319,17 @@ static void impl_Process(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
                 }
             }
             else if ((millis() - vtxState.lastStatusQueryMs) > VTX_STATUS_INTERVAL_MS) {
-                // Poll VTX for status updates
                 vtxProtoSetState(VTX_STATE_QUERY_STATUS);
             }
             break;
 
         case VTX_STATE_QUERY_DELAY:
-            // We get here after sending the command. We give VTX some time to process the command
-            // and switch to VTX_STATE_QUERY_STATUS
             if (vtxProtoTimeout()) {
-                // We gave VTX some time to process the command. Query status to confirm success
                 vtxProtoSetState(VTX_STATE_QUERY_STATUS);
             }
             break;
 
         case VTX_STATE_QUERY_STATUS:
-            // Just query status, nothing special
             vtxProtoQueryStatus();
             vtxProtoSetState(VTX_STATE_WAIT_STATUS);
             break;
@@ -351,7 +339,6 @@ static void impl_Process(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
                 vtxState.protoTimeoutCount = 0;
 
                 if (vtxProtoProcessResponse() == VTX_RESPONSE_TYPE_STATUS) {
-                    // Check if VTX state matches VTX request
                     if (!(vtxState.updateReqMask & VTX_UPDATE_REQ_FREQUENCY) && (vtxState.state.freq != vtxState.request.freq)) {
                         vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
                     }
@@ -360,11 +347,9 @@ static void impl_Process(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
                         vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
                     }
 
-                    // We got the status response - proceed to IDLE
                     vtxProtoSetState(VTX_STATE_IDLE);
                 }
                 else {
-                    // Unexpected response. Query for STATUS again
                     vtxProtoSetState(VTX_STATE_QUERY_STATUS);
                 }
             }
@@ -400,7 +385,6 @@ static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_
         return;
     }
 
-    // Default to 5.8 GHz
     uint16_t newFreqMhz = vtx58_Bandchan2Freq(band, channel);
 
     if (vtxSettingsConfig()->frequencyGroup == FREQUENCYGROUP_1G3) {
@@ -411,7 +395,6 @@ static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_
         return;
     }
 
-    // Cache band and channel
     vtxState.request.band = band;
     vtxState.request.channel = channel;
     vtxState.request.freq = newFreqMhz;
@@ -428,8 +411,7 @@ static void impl_SetPowerByIndex(vtxDevice_t * vtxDevice, uint8_t index)
 
     unsigned reqPower = vtxState.metadata.powerTablePtr[index - 1];
 
-    // Cap the power to the max capability of the VTX
-    vtxState.request.power = MIN(reqPower, vtxState.capabilities.powerMax);
+    vtxState.request.power = reqPower;
     vtxState.request.powerIndex = index;
 
     vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
@@ -450,7 +432,6 @@ static bool impl_GetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand,
         return false;
     }
 
-    // if in user-freq mode then report band as zero
     *pBand = vtxState.request.band;
     *pChannel = vtxState.request.channel;
     return true;
@@ -498,7 +479,7 @@ static bool impl_GetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_
     return true;
 }
 
-static bool impl_GetOsdInfo(const  vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
+static bool impl_GetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
 {
     if (!impl_IsReady(vtxDevice)) {
         return false;
@@ -516,7 +497,7 @@ static bool impl_GetOsdInfo(const  vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t *
             pOsdInfo->bandName = vtx1G3BandNames[vtxState.request.band];
             pOsdInfo->channelName = vtx1G3ChannelNames[vtxState.request.channel];
             break;
-        default: // Currently all except 1.3GHz
+        default:
             pOsdInfo->bandLetter = vtx58BandNames[vtxState.request.band][0];
             pOsdInfo->bandName = vtx58BandNames[vtxState.request.band];
             pOsdInfo->channelName = vtx58ChannelNames[vtxState.request.channel];
@@ -562,19 +543,18 @@ const char * const trampPowerNames_5G8_400[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] =
 const uint16_t trampPowerTable_5G8_600[VTX_TRAMP_5G8_MAX_POWER_COUNT]         = { 25, 100, 200, 400, 600 };
 const char * const trampPowerNames_5G8_600[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "600" };
 
-const uint16_t trampPowerTable_5G8_800[VTX_TRAMP_5G8_MAX_POWER_COUNT]         = { 25, 200, 400, 600, 600 };
-const char * const trampPowerNames_5G8_800[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "400", "800", "800" };
+const uint16_t trampPowerTable_5G8_800[VTX_TRAMP_5G8_MAX_POWER_COUNT]         = { 25, 100, 200, 400, 800 };
+const char * const trampPowerNames_5G8_800[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "800" };
 
+// SpeedyBee TX Ultra (4 levels: send 25, 100, 200, 500 to trigger 25mW, 200mW, 800mW, 1.6W MAX)
 const uint16_t trampPowerTable_5G8_1600[VTX_TRAMP_5G8_MAX_POWER_COUNT]         = { 25, 100, 200, 500, 500 };
-const char * const trampPowerNames_5G8_1600[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "800", "1600", "1600" };
+const char * const trampPowerNames_5G8_1600[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "800", "1.6W", "1.6W" };
 
 const uint16_t trampPowerTable_5G8_2500[VTX_TRAMP_5G8_MAX_POWER_COUNT]         = { 25, 200, 600, 1600, 2500 };
 const char * const trampPowerNames_5G8_2500[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "600", "1600", "2500" };
 
-const uint16_t trampPowerTable_5G8_3000[VTX_TRAMP_5G8_MAX_POWER_COUNT]         = { 25, 1000, 2000, 3000, 3000 };
-const char * const trampPowerNames_5G8_3000[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "1000", "2000", "3000", "3000" };
-
-
+const uint16_t trampPowerTable_5G8_3000[VTX_TRAMP_5G8_MAX_POWER_COUNT]         = { 25, 500, 1000, 2000, 3000 };
+const char * const trampPowerNames_5G8_3000[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "500", "1000", "2000", "3000" };
 
 const uint16_t trampPowerTable_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT]         = { 25, 200, 800 };
 const char * const trampPowerNames_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "800" };
@@ -607,31 +587,28 @@ static void vtxProtoUpdatePowerMetadata(uint16_t maxPower)
             break;
         default:
             if (maxPower >= 3000) {
-            // Max power 3000mW: Use 25, 1000, 2000, 3000 table
-            vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_3000;
-            vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
+                vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_3000;
+                vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
             
-            impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_3000;
-            impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
+                impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_3000;
+                impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
             }
             else if (maxPower >= 2500) {
-            // Max power 2500mW
-            vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_2500;
-            vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
+                vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_2500;
+                vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
                     
-            impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_2500;
-            impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
+                impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_2500;
+                impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
             }
             else if (maxPower >= 1600) {
-            // Max power 1600mW
-            vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_1600;
-            vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
+                // SpeedyBee TX Ultra table (4 levels: sends 25, 100, 200, 500)
+                vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_1600;
+                vtxState.metadata.powerTableCount = 4;
                 
-            impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_1600;
-            impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
+                impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_1600;
+                impl_vtxDevice.capability.powerCount = 4;
             }
             else if (maxPower >= 800) {
-                // Max power 800mW: Use 25, 100, 200, 500, 800 table
                 vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_800;
                 vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
                 
@@ -639,7 +616,6 @@ static void vtxProtoUpdatePowerMetadata(uint16_t maxPower)
                 impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
             }
             else if (maxPower >= 600) {
-                // Max power 600mW: Use 25, 100, 200, 400, 600 table
                 vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_600;
                 vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
 
@@ -647,7 +623,6 @@ static void vtxProtoUpdatePowerMetadata(uint16_t maxPower)
                 impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
             }
             else if (maxPower >= 400) {
-                // Max power 400mW: Use 25, 100, 200, 400 table
                 vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_400;
                 vtxState.metadata.powerTableCount = 4;
 
@@ -655,7 +630,6 @@ static void vtxProtoUpdatePowerMetadata(uint16_t maxPower)
                 impl_vtxDevice.capability.powerCount = 4;
             }
             else if (maxPower >= 200) {
-                // Max power 200mW: Use 25, 100, 200 table
                 vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_200;
                 vtxState.metadata.powerTableCount = 3;
 
@@ -663,7 +637,6 @@ static void vtxProtoUpdatePowerMetadata(uint16_t maxPower)
                 impl_vtxDevice.capability.powerCount = 3;
             }
             else {
-                // Default to standard TRAMP 600mW VTX
                 vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_600;
                 vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;