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     // Default to 5.8 GHz
 411     uint16_t newFreqMhz = vtx58_Bandchan2Freq(band, channel);
 412
 413     if (vtxSettingsConfig()->frequencyGroup == FREQUENCYGROUP_1G3) {
 414         newFreqMhz = vtx1G3_Bandchan2Freq(band, channel);
 415     }
 416
 417     if (newFreqMhz < vtxState.capabilities.freqMin || newFreqMhz > vtxState.capabilities.freqMax) {
 418         return;
 419     }
 420
 421     // Cache band and channel
 422     vtxState.request.band = band;
 423     vtxState.request.channel = channel;
 424     vtxState.request.freq = newFreqMhz;
 425     vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
 426 }
 427
 428 static void impl_SetPowerByIndex(vtxDevice_t * vtxDevice, uint8_t index)
 429 {
 430     UNUSED(vtxDevice);
 431
 432     if (!impl_IsReady(vtxDevice) || index < 1 || index > vtxState.metadata.powerTableCount) {
 433         return;
 434     }
 435
 436     unsigned reqPower = vtxState.metadata.powerTablePtr[index - 1];
 437
 438     // Cap the power to the max capability of the VTX
 439     vtxState.request.power = MIN(reqPower, vtxState.capabilities.powerMax);
 440     vtxState.request.powerIndex = index;
 441
 442     vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
 443 }
 444
 445 static void impl_SetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
 446 {
 447     UNUSED(vtxDevice);
 448
 449     if (onoff == 0) {
 450         vtxState.updateReqMask |= VTX_UPDATE_REQ_PITMODE;
 451     }
 452 }
 453
 454 static bool impl_GetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
 455 {
 456     if (!impl_IsReady(vtxDevice)) {
 457         return false;
 458     }
 459
 460     // if in user-freq mode then report band as zero
 461     *pBand = vtxState.request.band;
 462     *pChannel = vtxState.request.channel;
 463     return true;
 464 }
 465
 466 static bool impl_GetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
 467 {
 468     if (!impl_IsReady(vtxDevice)) {
 469         return false;
 470     }
 471
 472     *pIndex = vtxState.request.powerIndex;
 473
 474     return true;
 475 }
 476
 477 static bool impl_GetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
 478 {
 479     if (!impl_IsReady(vtxDevice)) {
 480         return false;
 481     }
 482
 483     *pOnOff = vtxState.state.pitMode ? 1 : 0;
 484     return true;
 485 }
 486
 487 static bool impl_GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
 488 {
 489     if (!impl_IsReady(vtxDevice)) {
 490         return false;
 491     }
 492
 493     *pFreq = vtxState.request.freq;
 494     return true;
 495 }
 496
 497 static bool impl_GetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
 498 {
 499     if (!impl_IsReady(vtxDevice)) {
 500         return false;
 501     }
 502
 503     *pIndex = vtxState.request.powerIndex;
 504     *pPowerMw = vtxState.request.power;
 505     return true;
 506 }
 507
 508 static bool impl_GetOsdInfo(const  vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
 509 {
 510     if (!impl_IsReady(vtxDevice)) {
 511         return false;
 512     }
 513
 514     pOsdInfo->band = vtxState.request.band;
 515     pOsdInfo->channel = vtxState.request.channel;
 516     pOsdInfo->frequency = vtxState.request.freq;
 517     pOsdInfo->powerIndex = vtxState.request.powerIndex;
 518     pOsdInfo->powerMilliwatt = vtxState.request.power;
 519
 520     switch (vtxSettingsConfig()->frequencyGroup) {
 521         case FREQUENCYGROUP_1G3:
 522             pOsdInfo->bandLetter = vtx1G3BandNames[vtxState.request.band][0];
 523             pOsdInfo->bandName = vtx1G3BandNames[vtxState.request.band];
 524             pOsdInfo->channelName = vtx1G3ChannelNames[vtxState.request.channel];
 525             break;
 526         default: // Currently all except 1.3GHz
 527             pOsdInfo->bandLetter = vtx58BandNames[vtxState.request.band][0];
 528             pOsdInfo->bandName = vtx58BandNames[vtxState.request.band];
 529             pOsdInfo->channelName = vtx58ChannelNames[vtxState.request.channel];
 530             break;
 531     }
 532
 533     pOsdInfo->powerIndexLetter = '0' + vtxState.request.powerIndex;
 534     return true;
 535 }
 536
 537 /*****************************************************************************/
 538 static const vtxVTable_t impl_vtxVTable = {
 539     .process = impl_Process,
 540     .getDeviceType = impl_GetDeviceType,
 541     .isReady = impl_IsReady,
 542     .setBandAndChannel = impl_SetBandAndChannel,
 543     .setPowerByIndex = impl_SetPowerByIndex,
 544     .setPitMode = impl_SetPitMode,
 545     .getBandAndChannel = impl_GetBandAndChannel,
 546     .getPowerIndex = impl_GetPowerIndex,
 547     .getPitMode = impl_GetPitMode,
 548     .getFrequency = impl_GetFreq,
 549     .getPower = impl_GetPower,
 550     .getOsdInfo = impl_GetOsdInfo,
 551 };
 552
 553 static vtxDevice_t impl_vtxDevice = {
 554     .vTable = &impl_vtxVTable,
 555     .capability.bandCount = VTX_TRAMP_5G8_BAND_COUNT,
 556     .capability.channelCount = VTX_TRAMP_5G8_CHANNEL_COUNT,
 557     .capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT,
 558     .capability.bandNames = (char **)vtx58BandNames,
 559     .capability.channelNames = (char **)vtx58ChannelNames,
 560     .capability.powerNames = NULL,
 561 };
 562
 563 const uint16_t trampPowerTable_5G8_200[VTX_TRAMP_5G8_MAX_POWER_COUNT]         = { 25, 100, 200, 200, 200 };
 564 const char * const trampPowerNames_5G8_200[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "200", "200" };
 565
 566 const uint16_t trampPowerTable_5G8_400[VTX_TRAMP_5G8_MAX_POWER_COUNT]         = { 25, 100, 200, 400, 400 };
 567 const char * const trampPowerNames_5G8_400[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "400" };
 568
 569 const uint16_t trampPowerTable_5G8_600[VTX_TRAMP_5G8_MAX_POWER_COUNT]         = { 25, 100, 200, 400, 600 };
 570 const char * const trampPowerNames_5G8_600[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "600" };
 571
 572 const uint16_t trampPowerTable_5G8_800[VTX_TRAMP_5G8_MAX_POWER_COUNT]         = { 25, 100, 200, 500, 800 };
 573 const char * const trampPowerNames_5G8_800[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "500", "800" };
 574
 575 const uint16_t trampPowerTable_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT]         = { 25, 200, 800 };
 576 const char * const trampPowerNames_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "800" };
 577
 578 const uint16_t trampPowerTable_1G3_2000[VTX_TRAMP_1G3_MAX_POWER_COUNT]         = { 25, 200, 2000 };
 579 const char * const trampPowerNames_1G3_2000[VTX_TRAMP_1G3_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "2000" };
 580
 581 static void vtxProtoUpdatePowerMetadata(uint16_t maxPower)
 582 {
 583     switch (vtxSettingsConfig()->frequencyGroup) {
 584         case FREQUENCYGROUP_1G3:
 585             if (maxPower >= 2000) {
 586                vtxState.metadata.powerTablePtr  = trampPowerTable_1G3_2000;
 587                vtxState.metadata.powerTableCount = VTX_TRAMP_1G3_MAX_POWER_COUNT;
 588
 589                impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_1G3_2000;
 590                impl_vtxDevice.capability.powerCount = VTX_TRAMP_1G3_MAX_POWER_COUNT;
 591             }
 592             else {
 593                vtxState.metadata.powerTablePtr  = trampPowerTable_1G3_800;
 594                vtxState.metadata.powerTableCount = VTX_TRAMP_1G3_MAX_POWER_COUNT;
 595
 596                impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_1G3_800;
 597                impl_vtxDevice.capability.powerCount = VTX_TRAMP_1G3_MAX_POWER_COUNT;
 598             }
 599             impl_vtxDevice.capability.bandCount = VTX_TRAMP_1G3_BAND_COUNT;
 600             impl_vtxDevice.capability.channelCount = VTX_TRAMP_1G3_CHANNEL_COUNT;
 601             impl_vtxDevice.capability.bandNames = (char **)vtx1G3BandNames;
 602             impl_vtxDevice.capability.channelNames = (char **)vtx1G3ChannelNames;
 603             break;
 604         default:
 605             if (maxPower >= 800) {
 606                 // Max power 800mW: Use 25, 100, 200, 500, 800 table
 607                 vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_800;
 608                 vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
 609
 610                 impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_800;
 611                 impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
 612             }
 613             else if (maxPower >= 600) {
 614                 // Max power 600mW: Use 25, 100, 200, 400, 600 table
 615                 vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_600;
 616                 vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
 617
 618                 impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_600;
 619                 impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
 620             }
 621             else if (maxPower >= 400) {
 622                 // Max power 400mW: Use 25, 100, 200, 400 table
 623                 vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_400;
 624                 vtxState.metadata.powerTableCount = 4;
 625
 626                 impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_400;
 627                 impl_vtxDevice.capability.powerCount = 4;
 628             }
 629             else if (maxPower >= 200) {
 630                 // Max power 200mW: Use 25, 100, 200 table
 631                 vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_200;
 632                 vtxState.metadata.powerTableCount = 3;
 633
 634                 impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_200;
 635                 impl_vtxDevice.capability.powerCount = 3;
 636             }
 637             else {
 638                 // Default to standard TRAMP 600mW VTX
 639                 vtxState.metadata.powerTablePtr  = trampPowerTable_5G8_600;
 640                 vtxState.metadata.powerTableCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
 641
 642                 impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_5G8_600;
 643                 impl_vtxDevice.capability.powerCount = VTX_TRAMP_5G8_MAX_POWER_COUNT;
 644             }
 645             break;
 646     }
 647 }
 648
 649 bool vtxTrampInit(void)
 650 {
 651     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP);
 652
 653     if (portConfig) {
 654         portOptions_t portOptions = 0;
 655         portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR);
 656         portOptions = portOptions | (vtxConfig()->softSerialShortStop ? SERIAL_SHORTSTOP : SERIAL_LONGSTOP);
 657         vtxState.port = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions);
 658     }
 659
 660     if (!vtxState.port) {
 661         return false;
 662     }
 663
 664     vtxProtoUpdatePowerMetadata(600);
 665     vtxCommonSetDevice(&impl_vtxDevice);
 666
 667     vtxState.protoState = VTX_STATE_RESET;
 668
 669     return true;
 670 }
 671
 672 #endif