src/main/io/vtx_msp.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 phobos- */
  22
  23 #include <stdbool.h>
  24 #include <stdint.h>
  25 #include <stdlib.h>
  26 #include <math.h>
  27 #include <ctype.h>
  28 #include <string.h>
  29
  30 #include "platform.h"
  31
  32 #if defined(USE_VTX_MSP) && defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON)
  33
  34 #include "build/debug.h"
  35
  36 //#include "cms/cms_menu_vtx_msp.h"
  37 #include "common/crc.h"
  38 #include "common/log.h"
  39 #include "config/feature.h"
  40
  41 #include "drivers/vtx_common.h"
  42 //#include "drivers/vtx_table.h"
  43
  44 #include "fc/runtime_config.h"
  45 #include "flight/failsafe.h"
  46
  47 #include "io/serial.h"
  48 #include "io/vtx_msp.h"
  49 #include "io/vtx_control.h"
  50 #include "io/vtx_string.h"
  51 #include "io/vtx_smartaudio.h"
  52 #include "io/vtx.h"
  53 #include "io/displayport_msp_osd.h"
  54
  55 #include "msp/msp_protocol.h"
  56 #include "msp/msp_serial.h"
  57 #include "msp/msp.h"
  58
  59 //#include "pg/vtx_table.h"
  60 #include "fc/settings.h"
  61
  62 #include "rx/crsf.h"
  63 //#include "rx/crsf_protocol.h"
  64 #include "rx/rx.h"
  65
  66 #include "telemetry/msp_shared.h"
  67
  68 //static uint16_t mspConfFreq = 0;
  69 static uint8_t mspConfBand = SETTING_VTX_BAND_DEFAULT;
  70 static uint8_t mspConfChannel = SETTING_VTX_CHANNEL_DEFAULT;
  71 //static uint16_t mspConfPower = 0;
  72 static uint16_t mspConfPowerIndex = SETTING_VTX_POWER_DEFAULT;
  73 static uint8_t mspConfPitMode = 0;
  74 static bool mspVtxConfigChanged = false;
  75 static timeUs_t mspVtxLastTimeUs = 0;
  76 static bool prevLowPowerDisarmedState = false;
  77
  78 static const vtxVTable_t mspVTable; // forward
  79 static vtxDevice_t vtxMsp = {
  80     .vTable = &mspVTable,
  81     .capability.bandCount = VTX_MSP_TABLE_MAX_BANDS,
  82     .capability.channelCount = VTX_MSP_TABLE_MAX_CHANNELS,
  83     .capability.powerCount = VTX_MSP_TABLE_MAX_POWER_LEVELS,
  84     .capability.bandNames = (char **)vtx58BandNames,
  85     .capability.channelNames = (char **)vtx58ChannelNames,
  86     .capability.powerNames = (char**)saPowerNames
  87
  88 };
  89
  90 STATIC_UNIT_TESTED mspVtxStatus_e mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
  91 static uint8_t mspVtxPortIdentifier = 255;
  92
  93 #define MSP_VTX_REQUEST_PERIOD_US (200 * 1000) // 200ms
  94
  95 static bool isCrsfPortConfig(const serialPortConfig_t *portConfig)
  96 {
  97     return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & FUNCTION_VTX_MSP && rxConfig()->serialrx_provider == SERIALRX_CRSF;
  98 }
  99
 100 static bool isLowPowerDisarmed(void)
 101 {
 102     return (!ARMING_FLAG(ARMED) && !failsafeIsActive() &&
 103         (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS ||
 104         (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED))));
 105 }
 106
 107 bool isVtxConfigValid(const vtxConfig_t *cfg)
 108 {
 109     LOG_DEBUG(VTX, "msp isVtxConfigValid\r\n");
 110     for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; ++i) {
 111
 112         if (cfg->vtxChannelActivationConditions[i].band ||
 113             (cfg->vtxChannelActivationConditions[i].range.startStep && cfg->vtxChannelActivationConditions[i].range.endStep) ||
 114             cfg->vtxChannelActivationConditions[i].auxChannelIndex ||
 115             cfg->vtxChannelActivationConditions[i].channel) {
 116                 return true;
 117         }
 118     }
 119
 120     LOG_DEBUG(VTX, "msp Invalid Config!\r\n");
 121     return false;
 122 }
 123
 124
 125 void setMspVtxDeviceStatusReady(const int descriptor)
 126 {
 127     LOG_DEBUG(VTX, "msp setMspVtxDeviceStatusReady\r\n");
 128     UNUSED(descriptor);
 129
 130     mspVtxStatus = MSP_VTX_STATUS_READY;
 131     mspVtxConfigChanged = true;
 132 }
 133
 134
 135 void prepareMspFrame(uint8_t *mspFrame)
 136 {
 137     LOG_DEBUG(VTX, "msp PrepareMspFrame\r\n");
 138 /*
 139 HDZERO parsing
 140     fc_band_rx = msp_rx_buf[1];
 141     fc_channel_rx = msp_rx_buf[2];
 142     fc_pwr_rx = msp_rx_buf[3];
 143     fc_pit_rx = msp_rx_buf[4];
 144     fc_lp_rx = msp_rx_buf[8];
 145 */
 146
 147     uint8_t pitmode = 0;
 148     vtxCommonGetPitMode(&vtxMsp, &pitmode);
 149
 150     mspFrame[0] = VTXDEV_MSP,
 151     mspFrame[1] = vtxSettingsConfig()->band;
 152     mspFrame[2] = vtxSettingsConfig()->channel;
 153     mspFrame[3] = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based
 154     mspFrame[4] = pitmode;
 155     mspFrame[5] = 0; // Freq_L
 156     mspFrame[6] = 0; // Freq_H
 157     mspFrame[7] = (mspVtxStatus == MSP_VTX_STATUS_READY) ? 1 : 0;
 158     mspFrame[8] = isLowPowerDisarmed();
 159     mspFrame[9] = 0; // Pitmode freq Low
 160     mspFrame[10] = 0; // pitmod freq High
 161     mspFrame[11] = 0; // 1 if using vtx table
 162     mspFrame[12] = 0; // vtx table bands or 0
 163     mspFrame[13] = 0; // vtx table channels or 0
 164     mspFrame[14] = 0; // vtx table power levels or 0
 165 }
 166
 167 static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize)
 168 {
 169
 170     LOG_DEBUG(VTX, "msp CrsfPush\r\n");
 171 #ifndef USE_TELEMETRY_CRSF
 172     UNUSED(mspCommand);
 173     UNUSED(mspFrame);
 174     UNUSED(mspFrameSize);
 175 #else
 176     sbuf_t crsfPayloadBuf;
 177     sbuf_t *dst = &crsfPayloadBuf;
 178
 179     uint8_t mspHeader[6] = {0x50, 0, mspCommand & 0xFF, (mspCommand >> 8) & 0xFF, mspFrameSize & 0xFF, (mspFrameSize >> 8) & 0xFF }; // MSP V2 over CRSF header
 180     uint8_t mspChecksum;
 181
 182     mspChecksum = crc8_dvb_s2_update(0, &mspHeader[1], 5); // first character is not checksummable
 183     mspChecksum = crc8_dvb_s2_update(mspChecksum, mspFrame, mspFrameSize);
 184
 185     uint8_t fullMspFrameSize = mspFrameSize + sizeof(mspHeader) + 1;  // add 1 for msp checksum
 186     uint8_t crsfFrameSize = CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_LENGTH_TYPE_CRC + fullMspFrameSize;
 187
 188     uint8_t crsfFrame[crsfFrameSize];
 189
 190     dst->ptr = crsfFrame;
 191     dst->end = ARRAYEND(crsfFrame);
 192
 193     sbufWriteU8(dst, CRSF_SYNC_BYTE);
 194     sbufWriteU8(dst, fullMspFrameSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); // size of CRSF frame (everything except sync and size itself)
 195     sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); // CRSF type
 196     sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER); // response destination is the receiver the vtx connection
 197     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); // origin is always this device
 198     sbufWriteData(dst, mspHeader, sizeof(mspHeader));
 199     sbufWriteData(dst, mspFrame, mspFrameSize);
 200     sbufWriteU8(dst, mspChecksum);
 201     crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
 202     sbufSwitchToReader(dst, crsfFrame);
 203
 204     crsfRxSendTelemetryData(); //give the FC a chance to send outstanding telemetry
 205     crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
 206     crsfRxSendTelemetryData();
 207 #endif
 208 }
 209
 210 static uint16_t packetCounter = 0;
 211
 212 static bool isVtxConfigChanged(void)
 213 {
 214     if(mspVtxStatus == MSP_VTX_STATUS_READY) {
 215         if (mspVtxConfigChanged == true)
 216                 return true;
 217
 218         if (isLowPowerDisarmed() != prevLowPowerDisarmedState) {
 219                 LOG_DEBUG(VTX, "msp vtx config changed (lower power disarm 2)\r\n");
 220                 mspVtxConfigChanged = true;
 221                 prevLowPowerDisarmedState = isLowPowerDisarmed();
 222         }
 223
 224         if (mspConfPowerIndex != vtxSettingsConfig()->power) {
 225                 LOG_DEBUG(VTX, "msp vtx config changed (power 2)\r\n");
 226                 mspVtxConfigChanged = true;
 227         }
 228
 229         if (mspConfBand != vtxSettingsConfig()->band || mspConfChannel != vtxSettingsConfig()->channel) {
 230                 LOG_DEBUG(VTX, "msp vtx config changed (band and channel 2)\r\n");
 231                 mspVtxConfigChanged = true;
 232         }
 233
 234         return mspVtxConfigChanged;
 235     }
 236
 237     return false;
 238 }
 239
 240 static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
 241 {
 242     UNUSED(vtxDevice);
 243
 244     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD);
 245     uint8_t frame[15];
 246
 247     switch (mspVtxStatus) {
 248     case MSP_VTX_STATUS_OFFLINE:
 249         LOG_DEBUG(VTX, "msp MspProcess: OFFLINE\r\n");
 250         // wait for MSP communication from the VTX
 251 #ifdef USE_CMS
 252         //mspCmsUpdateStatusString();
 253 #endif
 254         break;
 255     case MSP_VTX_STATUS_READY:
 256         LOG_DEBUG(VTX, "msp MspProcess: READY\r\n");
 257         // send an update if stuff has changed with 200ms period
 258         if ((isVtxConfigChanged()) && cmp32(currentTimeUs, mspVtxLastTimeUs) >= MSP_VTX_REQUEST_PERIOD_US) {
 259
 260             LOG_DEBUG(VTX, "msp-vtx: vtxInfo Changed\r\n");
 261             prepareMspFrame(frame);
 262
 263             if (isCrsfPortConfig(portConfig)) {
 264                 mspCrsfPush(MSP_VTX_CONFIG, frame, sizeof(frame));
 265             } else {
 266                 mspPort_t *port = getMspOsdPort();
 267                 if(port != NULL && port->port) {
 268                     LOG_DEBUG(VTX, "msp-vtx: mspSerialPushPort\r\n");
 269                     int sent = mspSerialPushPort(MSP_VTX_CONFIG, frame, sizeof(frame), port, MSP_V2_NATIVE);
 270                     if (sent <= 0) {
 271                         break;
 272                     }
 273                 }
 274             }
 275             packetCounter++;
 276             mspVtxLastTimeUs = currentTimeUs;
 277             mspVtxConfigChanged = false;
 278
 279 #ifdef USE_CMS
 280             //mspCmsUpdateStatusString();
 281 #endif
 282         }
 283         break;
 284     default:
 285         mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
 286         break;
 287     }
 288
 289 #if 0
 290     DEBUG_SET(DEBUG_VTX_MSP, 0, packetCounter);
 291     DEBUG_SET(DEBUG_VTX_MSP, 1, isCrsfPortConfig(portConfig));
 292     DEBUG_SET(DEBUG_VTX_MSP, 2, isLowPowerDisarmed());
 293 #if defined(USE_MSP_OVER_TELEMETRY)
 294     DEBUG_SET(DEBUG_VTX_MSP, 3, isCrsfPortConfig(portConfig) ? getMspTelemetryDescriptor() : getMspSerialPortDescriptor(mspVtxPortIdentifier));
 295 #else
 296     DEBUG_SET(DEBUG_VTX_MSP, 3, getMspSerialPortDescriptor(mspVtxPortIdentifier));
 297 #endif
 298 #endif
 299 }
 300
 301 static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice)
 302 {
 303     //LOG_DEBUG(VTX, "msp GetDeviceType\r\n");
 304     UNUSED(vtxDevice);
 305     return VTXDEV_MSP;
 306 }
 307
 308 static bool vtxMspIsReady(const vtxDevice_t *vtxDevice)
 309 {
 310     //LOG_DEBUG(VTX, "msp vtxIsReady: %s\r\n", (vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY) ? "Y": "N");
 311     return vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY;
 312 }
 313
 314 static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
 315 {
 316     LOG_DEBUG(VTX, "msp SetBandAndChannel\r\n");
 317     UNUSED(vtxDevice);
 318     if (band != mspConfBand || channel != mspConfChannel) {
 319         LOG_DEBUG(VTX, "msp vtx config changed (band and channel)\r\n");
 320         mspVtxConfigChanged = true;
 321     }
 322     mspConfBand = band;
 323     mspConfChannel = channel;
 324 }
 325
 326 static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
 327 {
 328     LOG_DEBUG(VTX, "msp SetPowerByIndex\r\n");
 329     UNUSED(vtxDevice);
 330
 331     if (index > 0 && (index < VTX_MSP_TABLE_MAX_POWER_LEVELS))
 332     {
 333         if (index != mspConfPowerIndex)
 334         {
 335             LOG_DEBUG(VTX, "msp vtx config changed (power by index)\r\n");
 336             mspVtxConfigChanged = true;
 337         }
 338         mspConfPowerIndex = index;
 339     }
 340 }
 341
 342 static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
 343 {
 344     LOG_DEBUG(VTX, "msp SetPitMode\r\n");
 345     UNUSED(vtxDevice);
 346     if (onoff != mspConfPitMode) {
 347         LOG_DEBUG(VTX, "msp vtx config changed (pitmode)\r\n");
 348         mspVtxConfigChanged = true;
 349     }
 350     mspConfPitMode = onoff;
 351 }
 352
 353 #if 0
 354 static void vtxMspSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
 355 {
 356     UNUSED(vtxDevice);
 357     if (freq != mspConfFreq) {
 358         mspVtxConfigChanged = true;
 359     }
 360     mspConfFreq = freq;
 361 }
 362 #endif
 363
 364
 365
 366
 367 static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
 368 {
 369     if (!vtxMspIsReady(vtxDevice)) {
 370         return false;
 371     }
 372
 373     *pBand = vtxSettingsConfig()->band;
 374     *pChannel = vtxSettingsConfig()->channel;
 375
 376     //LOG_DEBUG(VTX, "msp GetBandAndChannel: %02x:%02x\r\n", vtxSettingsConfig()->band, vtxSettingsConfig()->channel);
 377
 378     return true;
 379 }
 380
 381 static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
 382 {
 383     if (!vtxMspIsReady(vtxDevice)) {
 384         return false;
 385     }
 386
 387     uint8_t power = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power;
 388     // Special case, power not set
 389     if (power > VTX_MSP_TABLE_MAX_POWER_LEVELS) {
 390         *pIndex = 0;
 391         //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex);
 392         return true;
 393     }
 394
 395     *pIndex = power;
 396
 397     //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex);
 398     return true;
 399 }
 400
 401 static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
 402 {
 403     LOG_DEBUG(VTX, "msp GetFreq\r\n");
 404     if (!vtxMspIsReady(vtxDevice)) {
 405         return false;
 406     }
 407
 408     *pFreq = 5800;
 409     return true;
 410 }
 411
 412 static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
 413 {
 414     LOG_DEBUG(VTX, "msp GetPower\r\n");
 415     uint8_t powerIndex;
 416
 417     if (!vtxMspGetPowerIndex(vtxDevice, &powerIndex)) {
 418         return false;
 419     }
 420
 421
 422     *pIndex = powerIndex;
 423     *pPowerMw = *pIndex;
 424     return true;
 425 }
 426
 427 static bool vtxMspGetOsdInfo(const  vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
 428 {
 429     LOG_DEBUG(VTX, "msp GetOsdInfo\r\n");
 430     uint8_t powerIndex;
 431     uint16_t powerMw;
 432     uint16_t freq;
 433     uint8_t band, channel;
 434
 435     if (!vtxMspGetBandAndChannel(vtxDevice, &band, &channel)) {
 436         return false;
 437     }
 438
 439     if (!vtxMspGetFreq(vtxDevice, &freq)) {
 440         return false;
 441     }
 442
 443     if (!vtxMspGetPower(vtxDevice, &powerIndex, &powerMw)) {
 444         return false;
 445     }
 446
 447     pOsdInfo->band = band;
 448     pOsdInfo->channel = channel;
 449     pOsdInfo->frequency = freq;
 450     pOsdInfo->powerIndex = powerIndex;
 451     pOsdInfo->powerMilliwatt = powerMw;
 452     pOsdInfo->bandLetter = vtx58BandNames[band][0];
 453     pOsdInfo->bandName = vtx58BandNames[band];
 454     pOsdInfo->channelName = vtx58ChannelNames[channel];
 455     pOsdInfo->powerIndexLetter = '0' + powerIndex;
 456     return true;
 457 }
 458
 459
 460 bool vtxMspInit(void)
 461 {
 462     LOG_DEBUG(VTX, "msp %s\r\n", __FUNCTION__);
 463     // don't bother setting up this device if we don't have MSP vtx enabled
 464     // Port is shared with MSP_OSD
 465     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD);
 466     if (!portConfig) {
 467         return false;
 468     }
 469
 470     mspVtxPortIdentifier = portConfig->identifier;
 471
 472     // XXX Effect of USE_VTX_COMMON should be reviewed, as following call to vtxInit will do nothing if vtxCommonSetDevice is not called.
 473 #if defined(USE_VTX_COMMON)
 474     vtxCommonSetDevice(&vtxMsp);
 475 #endif
 476
 477     mspConfBand = vtxSettingsConfig()->band;
 478     mspConfChannel = vtxSettingsConfig()->channel;
 479     mspConfPowerIndex = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based
 480     vtxCommonGetPitMode(&vtxMsp, &mspConfPitMode);
 481
 482     vtxInit();
 483
 484     return true;
 485 }
 486
 487 static const vtxVTable_t mspVTable = {
 488     .process = vtxMspProcess,
 489     .getDeviceType = vtxMspGetDeviceType,
 490     .isReady = vtxMspIsReady,
 491     .setBandAndChannel = vtxMspSetBandAndChannel,
 492     .setPowerByIndex = vtxMspSetPowerByIndex,
 493     .setPitMode = vtxMspSetPitMode,
 494     //.setFrequency = vtxMspSetFreq,
 495     .getBandAndChannel = vtxMspGetBandAndChannel,
 496     .getPowerIndex = vtxMspGetPowerIndex,
 497     .getFrequency = vtxMspGetFreq,
 498     //.getStatus = vtxMspGetStatus,
 499     .getPower = vtxMspGetPower,
 500     //.serializeCustomDeviceStatus = NULL,
 501     .getOsdInfo = vtxMspGetOsdInfo,
 502 };
 503
 504 static mspResult_e mspVtxProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
 505 {
 506     //LOG_DEBUG(VTX, "msp VTX_MSP_PROCESS\r\n");
 507     UNUSED(mspPostProcessFn);
 508
 509     sbuf_t *dst = &reply->buf;
 510     sbuf_t *src = &cmd->buf;
 511
 512     const unsigned int dataSize = sbufBytesRemaining(src);
 513     UNUSED(dst);
 514     UNUSED(src);
 515
 516     // Start initializing the reply message
 517     reply->cmd = cmd->cmd;
 518     reply->result = MSP_RESULT_ACK;
 519
 520     vtxDevice_t *vtxDevice = vtxCommonDevice();
 521     if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_MSP) {
 522         LOG_DEBUG(VTX, "msp wrong vtx\r\n");
 523         return MSP_RESULT_ERROR;
 524     }
 525
 526     switch (cmd->cmd)
 527     {
 528         case MSP_VTXTABLE_BAND:
 529         {
 530             LOG_DEBUG(VTX, "msp MSP_VTXTABLE_BAND\r\n");
 531             uint8_t deviceType = vtxCommonGetDeviceType(vtxDevice);
 532             if (deviceType == VTXDEV_MSP)
 533             {
 534                 /*
 535                 char bandName[MSP_VTX_TABLE_BAND_NAME_LENGTH + 1];
 536                 memset(bandName, 0, MSP_VTX_TABLE_BAND_NAME_LENGTH + 1);
 537                 uint16_t frequencies[MSP_VTX_TABLE_MAX_CHANNELS];
 538                 const uint8_t band = sbufReadU8(src);
 539                 const uint8_t bandNameLength = sbufReadU8(src);
 540                 for (int i = 0; i < bandNameLength; i++) {
 541                     const char nameChar = sbufReadU8(src);
 542                     if (i < MSP_VTX_TABLE_BAND_NAME_LENGTH) {
 543                         bandName[i] = toupper(nameChar);
 544                     }
 545                 }
 546                 const char bandLetter = toupper(sbufReadU8(src));
 547                 const bool isFactoryBand = (bool)sbufReadU8(src);
 548                 const uint8_t channelCount = sbufReadU8(src);
 549                 for (int i = 0; i < channelCount; i++)
 550                 {
 551                     const uint16_t frequency = sbufReadU16(src);
 552                     if (i < vtxTableConfig()->channels)
 553                     {
 554                         frequencies[i] = frequency;
 555                     }
 556                 }
 557                 */
 558
 559                 setMspVtxDeviceStatusReady(1);
 560             }
 561             break;
 562         }
 563         case MSP_VTXTABLE_POWERLEVEL:
 564         {
 565             LOG_DEBUG(VTX, "msp MSP_VTXTABLE_POWERLEVEL\r\n");
 566
 567                 /*
 568                 char powerLevelLabel[VTX_TABLE_POWER_LABEL_LENGTH + 1];
 569                 memset(powerLevelLabel, 0, VTX_TABLE_POWER_LABEL_LENGTH + 1);
 570                 const uint8_t powerLevel = sbufReadU8(src);
 571                 const uint16_t powerValue = sbufReadU16(src);
 572                 const uint8_t powerLevelLabelLength = sbufReadU8(src);
 573                 for (int i = 0; i < powerLevelLabelLength; i++)
 574                 {
 575                     const char labelChar = sbufReadU8(src);
 576                     if (i < VTX_TABLE_POWER_LABEL_LENGTH)
 577                     {
 578                         powerLevelLabel[i] = toupper(labelChar);
 579                     }
 580                 }
 581                 */
 582                 setMspVtxDeviceStatusReady(1);
 583         }
 584         break;
 585         case MSP_VTX_CONFIG:
 586         {
 587             LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG received\r\n");
 588             LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG VTXDEV_MSP\r\n");
 589             uint8_t pitmode = 0;
 590             vtxCommonGetPitMode(vtxDevice, &pitmode);
 591
 592             // VTXDEV_MSP,
 593             sbufWriteU8(dst, VTXDEV_MSP);
 594             // band;
 595             sbufWriteU8(dst, vtxSettingsConfig()->band);
 596             // channel;
 597             sbufWriteU8(dst, vtxSettingsConfig()->channel);
 598             // power; // index based
 599             sbufWriteU8(dst, vtxSettingsConfig()->power);
 600             // pit mode;
 601             // Freq_L
 602             sbufWriteU8(dst, 0);
 603             // Freq_H
 604             sbufWriteU8(dst, 0);
 605             // vtx status
 606             sbufWriteU8(dst, 1);
 607             // lowPowerDisarm
 608
 609             sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
 610             // Pitmode freq Low
 611             sbufWriteU8(dst, 0);
 612             // pitmod freq High
 613             sbufWriteU8(dst, 0);
 614             // 1 if using vtx table
 615             sbufWriteU8(dst, 0);
 616             // vtx table bands or 0
 617             sbufWriteU8(dst, 0);
 618             // vtx table channels or 0
 619             sbufWriteU8(dst, 0);
 620
 621             setMspVtxDeviceStatusReady(1);
 622             break;
 623         }
 624         case MSP_SET_VTX_CONFIG:
 625             LOG_DEBUG(VTX, "msp MSP_SET_VTX_CONFIG\r\n");
 626             if (dataSize == 15)
 627             {
 628                 if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN)
 629                 {
 630                     for (int i = 0; i < 15; ++i)
 631                     {
 632                         uint8_t data = sbufReadU8(src);
 633                         switch (i)
 634                         {
 635                         case 1:
 636                             vtxSettingsConfigMutable()->band = data;
 637                             break;
 638                         case 2:
 639                             vtxSettingsConfigMutable()->channel = data;
 640                             break;
 641                         case 3:
 642                             vtxSettingsConfigMutable()->power = data;
 643                             break;
 644                         case 4:
 645                             vtxCommonSetPitMode(vtxDevice, data);
 646                             break;
 647                         case 7:
 648                             // vtx ready
 649                             break;
 650                         case 8:
 651                             vtxSettingsConfigMutable()->lowPowerDisarm = data;
 652                             break;
 653                         }
 654                     }
 655                 }
 656
 657                 setMspVtxDeviceStatusReady(1);
 658                 break;
 659             }
 660             LOG_DEBUG(VTX, "msp MSP_RESULT_ERROR\r\n");
 661             return MSP_RESULT_ERROR;
 662         default:
 663             // debug[1]++;
 664             // debug[2] = cmd->cmd;
 665             if(cmd->cmd != MSP_STATUS && cmd->cmd != MSP_STATUS_EX && cmd->cmd != MSP_RC) {
 666                 LOG_DEBUG(VTX, "msp default: %02x\r\n", cmd->cmd);
 667             }
 668             reply->result = MSP_RESULT_ERROR;
 669             break;
 670     }
 671
 672     // Process DONT_REPLY flag
 673     if (cmd->flags & MSP_FLAG_DONT_REPLY)
 674     {
 675         reply->result = MSP_RESULT_NO_REPLY;
 676     }
 677
 678     return reply->result;
 679 }
 680
 681 void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
 682 {
 683     UNUSED(mspProcessCommandFn);
 684     // Check if VTX is ready
 685     /*
 686     if (mspVtxStatus != MSP_VTX_STATUS_READY) {
 687         LOG_DEBUG(VTX, "msp VTX NOT READY, skipping\r\n");
 688         return;
 689     }
 690     */
 691
 692     mspPort_t *port = getMspOsdPort();
 693
 694     if(port) {
 695         mspSerialProcessOnePort(port, MSP_SKIP_NON_MSP_DATA, mspVtxProcessMspCommand);
 696     }
 697
 698 }
 699
 700
 701 #endif