src/main/io/vtx_tramp.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 /* Created by jflyper */
  22
  23 #include <stdbool.h>
  24 #include <stdint.h>
  25 #include <ctype.h>
  26 #include <string.h>
  27
  28 #include "platform.h"
  29
  30 #if defined(USE_VTX_TRAMP) && defined(USE_VTX_CONTROL)
  31
  32 #include "build/debug.h"
  33 #include "drivers/vtx_common.h"
  34 #include "drivers/time.h"
  35
  36 #include "common/maths.h"
  37 #include "common/utils.h"
  38 #include "common/crc.h"
  39
  40 #include "io/serial.h"
  41 #include "io/vtx_tramp.h"
  42 #include "io/vtx_control.h"
  43 #include "io/vtx.h"
  44 #include "io/vtx_string.h"
  45
  46 #define VTX_PKT_SIZE                16
  47 #define VTX_PROTO_STATE_TIMEOUT_MS  1000
  48 #define VTX_STATUS_INTERVAL_MS      2000
  49
  50 #define VTX_UPDATE_REQ_NONE         0x00
  51 #define VTX_UPDATE_REQ_FREQUENCY    0x01
  52 #define VTX_UPDATE_REQ_POWER        0x02
  53 #define VTX_UPDATE_REQ_PITMODE      0x04
  54
  55 typedef enum {
  56     VTX_STATE_RESET         = 0,
  57     VTX_STATE_OFFILE        = 1,    // Not detected
  58     VTX_STATE_DETECTING     = 2,    //
  59     VTX_STATE_IDLE          = 3,    // Idle, ready to sent commands
  60     VTX_STATE_QUERY_DELAY   = 4,
  61     VTX_STATE_QUERY_STATUS  = 5,
  62     VTX_STATE_WAIT_STATUS   = 6,    // Wait for VTX state
  63 } vtxProtoState_e;
  64
  65 typedef enum {
  66     VTX_RESPONSE_TYPE_NONE,
  67     VTX_RESPONSE_TYPE_CAPABILITIES,
  68     VTX_RESPONSE_TYPE_STATUS,
  69 } vtxProtoResponseType_e;
  70
  71 typedef struct {
  72     vtxProtoState_e protoState;
  73     timeMs_t        lastStateChangeMs;
  74     timeMs_t        lastStatusQueryMs;
  75     int             protoTimeoutCount;
  76     unsigned        updateReqMask;
  77
  78     // VTX capabilities
  79     struct {
  80         unsigned freqMin;   // min freq
  81         unsigned freqMax;   // max freq
  82         unsigned powerMax;  //
  83     } capabilities;
  84
  85     // Requested VTX state
  86     struct {
  87         // Only tracking
  88         int      band;
  89         int      channel;
  90         unsigned powerIndex;
  91
  92         // Actual settings to send to the VTX
  93         unsigned freq;
  94         unsigned power;
  95     } request;
  96
  97     // Actual VTX state: updated from actual VTX
  98     struct {
  99         unsigned freq;      // Frequency in MHz
 100         unsigned power;
 101         unsigned temp;
 102         bool     pitMode;
 103     } state;
 104
 105     struct {
 106         int              powerTableCount;
 107         const uint16_t * powerTablePtr;
 108     } metadata;
 109
 110     // Comms flags and state
 111     uint8_t         sendPkt[VTX_PKT_SIZE];
 112     uint8_t         recvPkt[VTX_PKT_SIZE];
 113     unsigned        recvPtr;
 114     serialPort_t *  port;
 115 } vtxProtoState_t;
 116
 117 static vtxProtoState_t vtxState;
 118
 119 static void vtxProtoUpdatePowerMetadata(uint16_t maxPower);
 120
 121 static bool trampIsValidResponseCode(uint8_t code)
 122 {
 123     return (code == 'r' || code == 'v' || code == 's');
 124 }
 125
 126 static bool vtxProtoRecv(void)
 127 {
 128     uint8_t * bufPtr = (uint8_t*)&vtxState.recvPkt;
 129     while (serialRxBytesWaiting(vtxState.port)) {
 130         const uint8_t c = serialRead(vtxState.port);
 131
 132         if (vtxState.recvPtr == 0) {
 133             // Wait for sync byte
 134             if (c == 0x0F) {
 135                 bufPtr[vtxState.recvPtr++] = c;
 136             }
 137         }
 138         else if (vtxState.recvPtr == 1) {
 139             // Check if we received a valid response code
 140             if (trampIsValidResponseCode(c)) {
 141                 bufPtr[vtxState.recvPtr++] = c;
 142             }
 143             else {
 144                 vtxState.recvPtr = 0;
 145             }
 146         }
 147         else {
 148             // Consume character and check if we have got a full packet
 149             if (vtxState.recvPtr < VTX_PKT_SIZE) {
 150                 bufPtr[vtxState.recvPtr++] = c;
 151             }
 152
 153             if (vtxState.recvPtr == VTX_PKT_SIZE) {
 154                 // Full packet received - validate packet, make sure it's the one we expect
 155                 const bool pktValid = ((bufPtr[14] == crc8_sum_update(0, &bufPtr[1], 13)) && (bufPtr[15] == 0));
 156
 157                 if (!pktValid) {
 158                     // Reset the receiver state - keep waiting
 159                     vtxState.recvPtr = 0;
 160                 }
 161                 // Make sure it's not the echo one (half-duplex serial might receive it's own data)
 162                 else if (memcmp(&vtxState.recvPkt, &vtxState.sendPkt, VTX_PKT_SIZE) == 0) {
 163                     vtxState.recvPtr = 0;
 164                 }
 165                 // Valid receive packet
 166                 else {
 167                     return true;
 168                 }
 169             }
 170         }
 171     }
 172
 173     return false;
 174 }
 175
 176 static void vtxProtoSend(uint8_t cmd, uint16_t param)
 177 {
 178     // Craft the packet
 179     memset(vtxState.sendPkt, 0, ARRAYLEN(vtxState.sendPkt));
 180     vtxState.sendPkt[0] = 15;
 181     vtxState.sendPkt[1] = cmd;
 182     vtxState.sendPkt[2] = param & 0xff;
 183     vtxState.sendPkt[3] = (param >> 8) & 0xff;
 184     vtxState.sendPkt[14] = crc8_sum_update(0, &vtxState.sendPkt[1], 13);
 185
 186     // Send data
 187     serialWriteBuf(vtxState.port, (uint8_t *)&vtxState.sendPkt, sizeof(vtxState.sendPkt));
 188
 189     // Reset cmd response state
 190     vtxState.recvPtr = 0;
 191 }
 192
 193 static void vtxProtoSetState(vtxProtoState_e newState)
 194 {
 195     vtxState.lastStateChangeMs = millis();
 196     vtxState.protoState = newState;
 197 }
 198
 199 static bool vtxProtoTimeout(void)
 200 {
 201     return (millis() - vtxState.lastStateChangeMs) > VTX_PROTO_STATE_TIMEOUT_MS;
 202 }
 203
 204 static void vtxProtoQueryCapabilities(void)
 205 {
 206     vtxProtoSend(0x72, 0);
 207 }
 208
 209 static void vtxProtoQueryStatus(void)
 210 {
 211     vtxProtoSend(0x76, 0);
 212     vtxState.lastStatusQueryMs = millis();
 213 }
 214
 215 /*
 216 static void vtxProtoQueryTemperature(void)
 217 {
 218     vtxProtoSend('s', 0);
 219 }
 220 */
 221
 222 static vtxProtoResponseType_e vtxProtoProcessResponse(void)
 223 {
 224     const uint8_t respCode = vtxState.recvPkt[1];
 225
 226     switch (respCode) {
 227         case 0x72:
 228             vtxState.capabilities.freqMin = vtxState.recvPkt[2] | (vtxState.recvPkt[3] << 8);
 229             vtxState.capabilities.freqMax = vtxState.recvPkt[4] | (vtxState.recvPkt[5] << 8);
 230             vtxState.capabilities.powerMax = vtxState.recvPkt[6] | (vtxState.recvPkt[7] << 8);
 231
 232             if (vtxState.capabilities.freqMin != 0 && vtxState.capabilities.freqMin < vtxState.capabilities.freqMax) {
 233                 // Some TRAMP VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX)
 234                 // Make use of vtxSettingsConfig()->maxPowerOverride to override
 235                 if (vtxSettingsConfig()->maxPowerOverride != 0) {
 236                     vtxState.capabilities.powerMax = vtxSettingsConfig()->maxPowerOverride;
 237                 }
 238
 239                 // Update max power metadata so OSD settings would match VTX capabilities
 240                 vtxProtoUpdatePowerMetadata(vtxState.capabilities.powerMax);
 241
 242                 return VTX_RESPONSE_TYPE_CAPABILITIES;
 243             }
 244             break;
 245
 246         case 0x76:
 247             vtxState.state.freq = vtxState.recvPkt[2] | (vtxState.recvPkt[3] << 8);
 248             vtxState.state.power = vtxState.recvPkt[4]|(vtxState.recvPkt[5] << 8);
 249             vtxState.state.pitMode = vtxState.recvPkt[7];
 250             //vtxState.state.power = vtxState.recvPkt[8]|(vtxState.recvPkt[9] << 8);
 251             return VTX_RESPONSE_TYPE_STATUS;
 252     }
 253
 254     return VTX_RESPONSE_TYPE_NONE;
 255 }
 256
 257 static void vtxProtoSetPitMode(uint16_t mode)
 258 {
 259     vtxProtoSend(0x73, mode);
 260 }
 261
 262 static void vtxProtoSetPower(uint16_t power)
 263 {
 264     vtxProtoSend(0x50, power);
 265 }
 266
 267 static void vtxProtoSetFrequency(uint16_t freq)
 268 {
 269     vtxProtoSend(0x46, freq);
 270 }
 271
 272 static void impl_Process(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
 273 {
 274     // Glue function betwen VTX VTable and actual driver protothread
 275     UNUSED(vtxDevice);
 276     UNUSED(currentTimeUs);
 277
 278     if (!vtxState.port) {
 279         return;
 280     }
 281
 282     switch((int)vtxState.protoState) {
 283         case VTX_STATE_RESET:
 284             vtxState.protoTimeoutCount = 0;
 285             vtxState.updateReqMask = VTX_UPDATE_REQ_NONE;
 286             vtxProtoSetState(VTX_STATE_OFFILE);
 287             break;
 288
 289         // Send request for capabilities
 290         case VTX_STATE_OFFILE:
 291             vtxProtoQueryCapabilities();
 292             vtxProtoSetState(VTX_STATE_DETECTING);
 293             break;
 294
 295         // Detect VTX. We only accept VTX_RESPONSE_TYPE_CAPABILITIES here
 296         case VTX_STATE_DETECTING:
 297             if (vtxProtoRecv()) {
 298                 if (vtxProtoProcessResponse() == VTX_RESPONSE_TYPE_CAPABILITIES) {
 299                     // VTX sent capabilities. Query status now
 300                     vtxState.protoTimeoutCount = 0;
 301                     vtxProtoSetState(VTX_STATE_QUERY_STATUS);
 302                 }
 303                 else {
 304                     // Unexpected response. Re-initialize
 305                     vtxProtoSetState(VTX_STATE_RESET);
 306                 }
 307             }
 308             else if (vtxProtoTimeout()) {
 309                 // Time-out while waiting for capabilities. Reset the state
 310                 vtxProtoSetState(VTX_STATE_RESET);
 311             }
 312             break;
 313
 314         // Send requests to update freqnecy and power, periodically poll device for liveness
 315         case VTX_STATE_IDLE:
 316             if (vtxState.updateReqMask != VTX_UPDATE_REQ_NONE) {
 317                 // Updates pending. Send an appropriate command
 318                 if (vtxState.updateReqMask & VTX_UPDATE_REQ_PITMODE) {
 319                     // Only disabling PIT mode supported
 320                     vtxState.updateReqMask &= ~VTX_UPDATE_REQ_PITMODE;
 321                     vtxProtoSetPitMode(0);
 322                     vtxProtoSetState(VTX_STATE_QUERY_DELAY);
 323                 }
 324                 else if (vtxState.updateReqMask & VTX_UPDATE_REQ_FREQUENCY) {
 325                     vtxState.updateReqMask &= ~VTX_UPDATE_REQ_FREQUENCY;
 326                     vtxProtoSetFrequency(vtxState.request.freq);
 327                     vtxProtoSetState(VTX_STATE_QUERY_DELAY);
 328                 }
 329                 else if (vtxState.updateReqMask & VTX_UPDATE_REQ_POWER) {
 330                     vtxState.updateReqMask &= ~VTX_UPDATE_REQ_POWER;
 331                     vtxProtoSetPower(vtxState.request.power);
 332                     vtxProtoSetState(VTX_STATE_QUERY_DELAY);
 333                 }
 334             }
 335             else if ((millis() - vtxState.lastStatusQueryMs) > VTX_STATUS_INTERVAL_MS) {
 336                 // Poll VTX for status updates
 337                 vtxProtoSetState(VTX_STATE_QUERY_STATUS);
 338             }
 339             break;
 340
 341         case VTX_STATE_QUERY_DELAY:
 342             // We get here after sending the command. We give VTX some time to process the command
 343             // and switch to VTX_STATE_QUERY_STATUS
 344             if (vtxProtoTimeout()) {
 345                 // We gave VTX some time to process the command. Query status to confirm success
 346                 vtxProtoSetState(VTX_STATE_QUERY_STATUS);
 347             }
 348             break;
 349
 350         case VTX_STATE_QUERY_STATUS:
 351             // Just query status, nothing special
 352             vtxProtoQueryStatus();
 353             vtxProtoSetState(VTX_STATE_WAIT_STATUS);
 354             break;
 355
 356         case VTX_STATE_WAIT_STATUS:
 357             if (vtxProtoRecv()) {
 358                 vtxState.protoTimeoutCount = 0;
 359
 360                 if (vtxProtoProcessResponse() == VTX_RESPONSE_TYPE_STATUS) {
 361                     // Check if VTX state matches VTX request
 362                     if (!(vtxState.updateReqMask & VTX_UPDATE_REQ_FREQUENCY) && (vtxState.state.freq != vtxState.request.freq)) {
 363                         vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
 364                     }
 365
 366                     if (!(vtxState.updateReqMask & VTX_UPDATE_REQ_POWER) && (vtxState.state.power != vtxState.request.power)) {
 367                         vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
 368                     }
 369
 370                     // We got the status response - proceed to IDLE
 371                     vtxProtoSetState(VTX_STATE_IDLE);
 372                 }
 373                 else {
 374                     // Unexpected response. Query for STATUS again
 375                     vtxProtoSetState(VTX_STATE_QUERY_STATUS);
 376                 }
 377             }
 378             else if (vtxProtoTimeout()) {
 379                 vtxState.protoTimeoutCount++;
 380                 if (vtxState.protoTimeoutCount > 3) {
 381                     vtxProtoSetState(VTX_STATE_RESET);
 382                 }
 383                 else {
 384                     vtxProtoSetState(VTX_STATE_QUERY_STATUS);
 385                 }
 386             }
 387             break;
 388     }
 389 }
 390
 391 static vtxDevType_e impl_GetDeviceType(const vtxDevice_t *vtxDevice)
 392 {
 393     UNUSED(vtxDevice);
 394     return VTXDEV_TRAMP;
 395 }
 396
 397 static bool impl_IsReady(const vtxDevice_t *vtxDevice)
 398 {
 399     return vtxDevice != NULL && vtxState.port != NULL && vtxState.protoState >= VTX_STATE_IDLE;
 400 }
 401
 402 static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_t channel)
 403 {
 404     UNUSED(vtxDevice);
 405
 406     if (!impl_IsReady(vtxDevice)) {
 407         return;
 408     }
 409
 410     // TRAMP is 5.8 GHz only
 411     uint16_t newFreqMhz  = vtx58_Bandchan2Freq(band, channel);
 412
 413     if (newFreqMhz < vtxState.capabilities.freqMin || newFreqMhz > vtxState.capabilities.freqMax) {
 414         return;
 415     }
 416
 417     // Cache band and channel
 418     vtxState.request.band = band;
 419     vtxState.request.channel = channel;
 420     vtxState.request.freq = newFreqMhz;
 421     vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
 422 }
 423
 424 static void impl_SetPowerByIndex(vtxDevice_t * vtxDevice, uint8_t index)
 425 {
 426     UNUSED(vtxDevice);
 427
 428     if (!impl_IsReady(vtxDevice) || index < 1 || index > vtxState.metadata.powerTableCount) {
 429         return;
 430     }
 431
 432     unsigned reqPower = vtxState.metadata.powerTablePtr[index - 1];
 433
 434     // Cap the power to the max capability of the VTX
 435     vtxState.request.power = MIN(reqPower, vtxState.capabilities.powerMax);
 436     vtxState.request.powerIndex = index;
 437
 438     vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
 439 }
 440
 441 static void impl_SetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
 442 {
 443     UNUSED(vtxDevice);
 444
 445     if (onoff == 0) {
 446         vtxState.updateReqMask |= VTX_UPDATE_REQ_PITMODE;
 447     }
 448 }
 449
 450 static bool impl_GetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
 451 {
 452     if (!impl_IsReady(vtxDevice)) {
 453         return false;
 454     }
 455
 456     // if in user-freq mode then report band as zero
 457     *pBand = vtxState.request.band;
 458     *pChannel = vtxState.request.channel;
 459     return true;
 460 }
 461
 462 static bool impl_GetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
 463 {
 464     if (!impl_IsReady(vtxDevice)) {
 465         return false;
 466     }
 467
 468     *pIndex = vtxState.request.powerIndex;
 469
 470     return true;
 471 }
 472
 473 static bool impl_GetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
 474 {
 475     if (!impl_IsReady(vtxDevice)) {
 476         return false;
 477     }
 478
 479     *pOnOff = vtxState.state.pitMode ? 1 : 0;
 480     return true;
 481 }
 482
 483 static bool impl_GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
 484 {
 485     if (!impl_IsReady(vtxDevice)) {
 486         return false;
 487     }
 488
 489     *pFreq = vtxState.request.freq;
 490     return true;
 491 }
 492
 493 static bool impl_GetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
 494 {
 495     if (!impl_IsReady(vtxDevice)) {
 496         return false;
 497     }
 498
 499     *pIndex = vtxState.request.powerIndex;
 500     *pPowerMw = vtxState.request.power;
 501     return true;
 502 }
 503
 504 static bool impl_GetOsdInfo(const  vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
 505 {
 506     if (!impl_IsReady(vtxDevice)) {
 507         return false;
 508     }
 509
 510     pOsdInfo->band = vtxState.request.band;
 511     pOsdInfo->channel = vtxState.request.channel;
 512     pOsdInfo->frequency = vtxState.request.freq;
 513     pOsdInfo->powerIndex = vtxState.request.powerIndex;
 514     pOsdInfo->powerMilliwatt = vtxState.request.power;
 515     pOsdInfo->bandLetter = vtx58BandNames[vtxState.request.band][0];
 516     pOsdInfo->bandName = vtx58BandNames[vtxState.request.band];
 517     pOsdInfo->channelName = vtx58ChannelNames[vtxState.request.channel];
 518     pOsdInfo->powerIndexLetter = '0' + vtxState.request.powerIndex;
 519     return true;
 520 }
 521
 522 /*****************************************************************************/
 523 static const vtxVTable_t impl_vtxVTable = {
 524     .process = impl_Process,
 525     .getDeviceType = impl_GetDeviceType,
 526     .isReady = impl_IsReady,
 527     .setBandAndChannel = impl_SetBandAndChannel,
 528     .setPowerByIndex = impl_SetPowerByIndex,
 529     .setPitMode = impl_SetPitMode,
 530     .getBandAndChannel = impl_GetBandAndChannel,
 531     .getPowerIndex = impl_GetPowerIndex,
 532     .getPitMode = impl_GetPitMode,
 533     .getFrequency = impl_GetFreq,
 534     .getPower = impl_GetPower,
 535     .getOsdInfo = impl_GetOsdInfo,
 536 };
 537
 538 static vtxDevice_t impl_vtxDevice = {
 539     .vTable = &impl_vtxVTable,
 540     .capability.bandCount = VTX_TRAMP_5G8_BAND_COUNT,
 541     .capability.channelCount = VTX_TRAMP_5G8_CHANNEL_COUNT,
 542     .capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT,
 543     .capability.bandNames = (char **)vtx58BandNames,
 544     .capability.channelNames = (char **)vtx58ChannelNames,
 545     .capability.powerNames = NULL,
 546 };
 547
 548 const uint16_t trampPowerTable_200[VTX_TRAMP_MAX_POWER_COUNT]         = { 25, 100, 200, 200, 200 };
 549 const char * const trampPowerNames_200[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "200", "200" };
 550
 551 const uint16_t trampPowerTable_400[VTX_TRAMP_MAX_POWER_COUNT]         = { 25, 100, 200, 400, 400 };
 552 const char * const trampPowerNames_400[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "400" };
 553
 554 const uint16_t trampPowerTable_600[VTX_TRAMP_MAX_POWER_COUNT]         = { 25, 100, 200, 400, 600 };
 555 const char * const trampPowerNames_600[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "600" };
 556
 557 const uint16_t trampPowerTable_800[VTX_TRAMP_MAX_POWER_COUNT]         = { 25, 100, 200, 500, 800 };
 558 const char * const trampPowerNames_800[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "500", "800" };
 559
 560 static void vtxProtoUpdatePowerMetadata(uint16_t maxPower)
 561 {
 562     if (maxPower >= 800) {
 563         // Max power 800mW: Use 25, 100, 200, 500, 800 table
 564         vtxState.metadata.powerTablePtr  = trampPowerTable_800;
 565         vtxState.metadata.powerTableCount = VTX_TRAMP_MAX_POWER_COUNT;
 566
 567         impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_800;
 568         impl_vtxDevice.capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT;
 569     }
 570     else if (maxPower >= 600) {
 571         // Max power 600mW: Use 25, 100, 200, 400, 600 table
 572         vtxState.metadata.powerTablePtr  = trampPowerTable_600;
 573         vtxState.metadata.powerTableCount = VTX_TRAMP_MAX_POWER_COUNT;
 574
 575         impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_600;
 576         impl_vtxDevice.capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT;
 577     }
 578     else if (maxPower >= 400) {
 579         // Max power 400mW: Use 25, 100, 200, 400 table
 580         vtxState.metadata.powerTablePtr  = trampPowerTable_400;
 581         vtxState.metadata.powerTableCount = 4;
 582
 583         impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_400;
 584         impl_vtxDevice.capability.powerCount = 4;
 585     }
 586     else if (maxPower >= 200) {
 587         // Max power 200mW: Use 25, 100, 200 table
 588         vtxState.metadata.powerTablePtr  = trampPowerTable_200;
 589         vtxState.metadata.powerTableCount = 3;
 590
 591         impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_200;
 592         impl_vtxDevice.capability.powerCount = 3;
 593     }
 594     else {
 595         // Default to standard TRAMP 600mW VTX
 596         vtxState.metadata.powerTablePtr  = trampPowerTable_600;
 597         vtxState.metadata.powerTableCount = VTX_TRAMP_MAX_POWER_COUNT;
 598
 599         impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_600;
 600         impl_vtxDevice.capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT;
 601     }
 602
 603 }
 604
 605 bool vtxTrampInit(void)
 606 {
 607     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP);
 608
 609     if (portConfig) {
 610         portOptions_t portOptions = 0;
 611         portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR);
 612         vtxState.port = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions);
 613     }
 614
 615     if (!vtxState.port) {
 616         return false;
 617     }
 618
 619     vtxProtoUpdatePowerMetadata(600);
 620     vtxCommonSetDevice(&impl_vtxDevice);
 621
 622     vtxState.protoState = VTX_STATE_RESET;
 623
 624     return true;
 625 }
 626
 627 #endif