src/lib/MSPVTX/devMSPVTX.cpp

   1 #include "targets.h"
   2 #include "common.h"
   3 #include "devMSPVTX.h"
   4 #include "devVTXSPI.h"
   5 #include "freqTable.h"
   6 #include "CRSF.h"
   7 #include "msptypes.h"
   8 #include "hwTimer.h"
   9
  10 /**
  11  * Created by phobos-
  12  * Based on OpenVTX implementation - https://github.com/OpenVTx/OpenVTx/blob/master/src/src/mspVtx.c
  13  * Original author: Jye Smith.
  14 **/
  15
  16 #ifdef HAS_MSP_VTX
  17
  18 #define FC_QUERY_PERIOD_MS      200 // poll every 200ms
  19 #define MSP_VTX_FUNCTION_OFFSET 7
  20 #define MSP_VTX_PAYLOAD_OFFSET  11
  21 #define MSP_VTX_TIMEOUT_NO_CONNECTION 5000
  22
  23 typedef enum
  24 {
  25   GET_VTX_TABLE_SIZE = 0,
  26   CHECK_POWER_LEVELS,
  27   CHECK_BANDS,
  28   SET_RCE_PIT_MODE,
  29   SEND_EEPROM_WRITE,
  30   MONITORING,
  31   STOP_MSPVTX,
  32   MSP_STATE_MAX
  33 } mspVtxState_e;
  34
  35 void SendMSPFrameToFC(uint8_t *mspData);
  36
  37 static bool eepromWriteRequired = false;
  38 static bool setRcePitMode = false;
  39 static uint8_t checkingIndex = 0;
  40 static uint8_t pitMode = 0;
  41 static uint8_t power = 0;
  42 static uint8_t channel = 0;
  43 static uint8_t mspState = STOP_MSPVTX;
  44
  45 static void sendCrsfMspToFC(uint8_t *mspFrame, uint8_t mspFrameSize)
  46 {
  47     CRSF::SetExtendedHeaderAndCrc(mspFrame, CRSF_FRAMETYPE_MSP_REQ, mspFrameSize, CRSF_ADDRESS_CRSF_RECEIVER, CRSF_ADDRESS_FLIGHT_CONTROLLER);
  48     SendMSPFrameToFC(mspFrame);
  49 }
  50
  51 static void sendVtxConfigCommand(void)
  52 {
  53     uint8_t vtxConfig[MSP_REQUEST_LENGTH(0)];
  54     CRSF::SetMspV2Request(vtxConfig, MSP_VTX_CONFIG, nullptr, 0);
  55     sendCrsfMspToFC(vtxConfig, MSP_REQUEST_FRAME_SIZE(0));
  56 }
  57
  58 static void sendEepromWriteCommand(void)
  59 {
  60     if (!eepromWriteRequired)
  61     {
  62         return;
  63     }
  64
  65     uint8_t eepromWrite[MSP_REQUEST_LENGTH(0)];
  66     CRSF::SetMspV2Request(eepromWrite, MSP_EEPROM_WRITE, nullptr, 0);
  67     sendCrsfMspToFC(eepromWrite, MSP_REQUEST_FRAME_SIZE(0));
  68
  69     eepromWriteRequired = false;
  70 }
  71
  72 static void clearVtxTable(void)
  73 {
  74     uint8_t payload[MSP_SET_VTX_CONFIG_PAYLOAD_LENGTH] = {
  75         0, // idx LSB
  76         0, // idx MSB
  77         3, // 25mW Power idx
  78         0, // pitmode
  79         0, // lowPowerDisarm
  80         0, // pitModeFreq LSB
  81         0, // pitModeFreq MSB
  82         4, // newBand - Band Fatshark
  83         4, // newChannel - Channel 4
  84         0, // newFreq  LSB
  85         0, // newFreq  MSB
  86         6, // newBandCount
  87         8, // newChannelCount
  88         5, // newPowerCount
  89         1  // vtxtable should be cleared
  90     };
  91
  92     uint8_t request[MSP_REQUEST_LENGTH(MSP_SET_VTX_CONFIG_PAYLOAD_LENGTH)];
  93     CRSF::SetMspV2Request(request, MSP_SET_VTX_CONFIG, payload, MSP_SET_VTX_CONFIG_PAYLOAD_LENGTH);
  94     sendCrsfMspToFC(request, MSP_REQUEST_FRAME_SIZE(MSP_SET_VTX_CONFIG_PAYLOAD_LENGTH));
  95
  96     eepromWriteRequired = true;
  97 }
  98
  99 static void sendRcePitModeCommand(void)
 100 {
 101     uint8_t payload[4] = {
 102         channel, // idx LSB
 103         0, // idx MSB
 104         RACE_MODE, // 25mW Power idx
 105         1 // pitmode
 106     };
 107
 108     uint8_t request[MSP_REQUEST_LENGTH(4)];
 109     CRSF::SetMspV2Request(request, MSP_SET_VTX_CONFIG, payload, 4);
 110     sendCrsfMspToFC(request, MSP_REQUEST_FRAME_SIZE(4));
 111
 112     eepromWriteRequired = true;
 113 }
 114
 115 static void setVtxTableBand(uint8_t band)
 116 {
 117     uint8_t payload[MSP_SET_VTXTABLE_BAND_PAYLOAD_LENGTH];
 118
 119     payload[0] = band;
 120     payload[1] = BAND_NAME_LENGTH;
 121
 122     for (uint8_t i = 0; i < CHANNEL_COUNT; i++)
 123     {
 124         payload[2 + i] = channelFreqLabelByIdx((band - 1) * CHANNEL_COUNT + i);
 125     }
 126
 127     payload[2+CHANNEL_COUNT] = getBandLetterByIdx(band - 1);
 128     payload[3+CHANNEL_COUNT] = IS_FACTORY_BAND;
 129     payload[4+CHANNEL_COUNT] = CHANNEL_COUNT;
 130
 131     for (uint8_t i = 0; i < CHANNEL_COUNT; i++)
 132     {
 133         payload[(5+CHANNEL_COUNT) + (i * 2)] =  getFreqByIdx(((band-1) * CHANNEL_COUNT) + i) & 0xFF;
 134         payload[(6+CHANNEL_COUNT) + (i * 2)] = (getFreqByIdx(((band-1) * CHANNEL_COUNT) + i) >> 8) & 0xFF;
 135     }
 136
 137     uint8_t request[MSP_REQUEST_LENGTH(MSP_SET_VTXTABLE_BAND_PAYLOAD_LENGTH)];
 138     CRSF::SetMspV2Request(request, MSP_SET_VTXTABLE_BAND, payload, MSP_SET_VTXTABLE_BAND_PAYLOAD_LENGTH);
 139     sendCrsfMspToFC(request, MSP_REQUEST_FRAME_SIZE(MSP_SET_VTXTABLE_BAND_PAYLOAD_LENGTH));
 140
 141     eepromWriteRequired = true;
 142 }
 143
 144 static void getVtxTableBand(uint8_t idx)
 145 {
 146     uint8_t payloadLength = 1;
 147     uint8_t payload[1] = {idx};
 148
 149     uint8_t request[MSP_REQUEST_LENGTH(payloadLength)];
 150     CRSF::SetMspV2Request(request, MSP_VTXTABLE_BAND, payload, payloadLength);
 151     sendCrsfMspToFC(request, MSP_REQUEST_FRAME_SIZE(payloadLength));
 152 }
 153
 154 static void setVtxTablePowerLevel(uint8_t idx)
 155 {
 156     uint8_t payload[MSP_SET_VTXTABLE_POWERLEVEL_PAYLOAD_LENGTH];
 157
 158     payload[0] = idx;
 159     payload[1] = powerLevelsLut[idx - 1] & 0xFF;         // powerValue LSB
 160     payload[2] = (powerLevelsLut[idx - 1] >> 8) & 0xFF; // powerValue MSB
 161     payload[3] = POWER_LEVEL_LABEL_LENGTH;
 162     payload[4] = powerLevelsLabel[((idx - 1) * POWER_LEVEL_LABEL_LENGTH) + 0];
 163     payload[5] = powerLevelsLabel[((idx - 1) * POWER_LEVEL_LABEL_LENGTH) + 1];
 164     payload[6] = powerLevelsLabel[((idx - 1) * POWER_LEVEL_LABEL_LENGTH) + 2];
 165
 166     uint8_t request[MSP_REQUEST_LENGTH(MSP_SET_VTXTABLE_POWERLEVEL_PAYLOAD_LENGTH)];
 167     CRSF::SetMspV2Request(request, MSP_SET_VTXTABLE_POWERLEVEL, payload, MSP_SET_VTXTABLE_POWERLEVEL_PAYLOAD_LENGTH);
 168     sendCrsfMspToFC(request, MSP_REQUEST_FRAME_SIZE(MSP_SET_VTXTABLE_POWERLEVEL_PAYLOAD_LENGTH));
 169
 170     eepromWriteRequired = true;
 171 }
 172
 173 static void getVtxTablePowerLvl(uint8_t idx)
 174 {
 175     uint8_t payloadLength = 1;
 176     uint8_t payload[1] = {idx};
 177
 178     uint8_t request[MSP_REQUEST_LENGTH(payloadLength)];
 179     CRSF::SetMspV2Request(request, MSP_VTXTABLE_POWERLEVEL, payload, payloadLength);
 180     sendCrsfMspToFC(request, MSP_REQUEST_FRAME_SIZE(payloadLength));
 181 }
 182
 183 /**
 184  * CRSF frame structure:
 185  * <sync/address><length><type><destination><origin><status><MSP_v2_frame><crsf_crc>
 186  * MSP V2 over CRSF frame:
 187  * <flags><function1><function2><length1><length2><payload1>...<payloadN><msp_crc>
 188  * The biggest payload we have is 29 (MSP_VTXTABLE_BAND).
 189  * The maximum packet size is 42 bytes which fits well enough under maximum of 64.
 190  * Reply always comes with the same function as the request.
 191 **/
 192
 193 void mspVtxProcessPacket(uint8_t *packet)
 194 {
 195     mspVtxConfigPacket_t *vtxConfigPacket;
 196     mspVtxPowerLevelPacket_t *powerLevelPacket;
 197     mspVtxBandPacket_t *bandPacket;
 198
 199     switch (packet[MSP_VTX_FUNCTION_OFFSET])
 200     {
 201     case MSP_VTX_CONFIG:
 202         vtxConfigPacket = (mspVtxConfigPacket_t *)(packet + MSP_VTX_PAYLOAD_OFFSET);
 203
 204         switch (mspState)
 205         {
 206         case GET_VTX_TABLE_SIZE:
 207
 208             //  Store initially received values.  If the VTx Table is correct, only then set these values.
 209             pitMode = vtxConfigPacket->pitmode;
 210             power = vtxConfigPacket->power;
 211
 212             if (power == RACE_MODE && pitMode != 1) // If race mode and not already in PIT, force pit mode on boot and set it in BF.
 213             {
 214                 pitMode = 1;
 215                 setRcePitMode = true;
 216             }
 217
 218             if (vtxConfigPacket->lowPowerDisarm) // Force 0mw on boot because BF doesnt send a low power index.
 219             {
 220                 power = 1;
 221             }
 222
 223             if (power > NUM_POWER_LEVELS)
 224             {
 225                 power = 3; // 25 mW
 226             }
 227
 228             channel = ((vtxConfigPacket->band - 1) * 8) + (vtxConfigPacket->channel - 1);
 229             if (channel >= FREQ_TABLE_SIZE)
 230             {
 231                 channel = 27; // F4 5800MHz
 232             }
 233
 234             if (vtxConfigPacket->bands == getFreqTableBands() && vtxConfigPacket->channels == getFreqTableChannels() && vtxConfigPacket->powerLevels == NUM_POWER_LEVELS)
 235             {
 236                mspState = CHECK_POWER_LEVELS;
 237                break;
 238             }
 239             clearVtxTable();
 240             break;
 241         case SET_RCE_PIT_MODE:
 242             setRcePitMode = false;
 243             mspState = SEND_EEPROM_WRITE;
 244         case MONITORING:
 245             pitMode = vtxConfigPacket->pitmode;
 246
 247             // Set power before freq changes to prevent PLL settling issues and spamming other frequencies.
 248             power = vtxConfigPacket->power;
 249             vtxSPIPitmode = pitMode;
 250             vtxSPIPowerIdx = power;
 251
 252             channel = ((vtxConfigPacket->band - 1) * 8) + (vtxConfigPacket->channel - 1);
 253             if (channel < FREQ_TABLE_SIZE)
 254             {
 255                 vtxSPIFrequency = getFreqByIdx(channel);
 256             }
 257             break;
 258         }
 259         break;
 260
 261     case MSP_VTXTABLE_POWERLEVEL:
 262         powerLevelPacket = (mspVtxPowerLevelPacket_t *)(packet + MSP_VTX_PAYLOAD_OFFSET);
 263
 264         switch (mspState)
 265         {
 266         case CHECK_POWER_LEVELS:
 267             if (powerLevelPacket->powerValue ==  powerLevelsLut[checkingIndex] && powerLevelPacket->powerLabelLength == POWER_LEVEL_LABEL_LENGTH) // Check lengths before trying to check content
 268             {
 269                 if (memcmp(powerLevelPacket->label, powerLevelsLabel + checkingIndex * POWER_LEVEL_LABEL_LENGTH, sizeof(powerLevelPacket->label)) == 0)
 270                 {
 271                     checkingIndex++;
 272                     if (checkingIndex > NUM_POWER_LEVELS - 1)
 273                     {
 274                         checkingIndex = 0;
 275                         mspState = CHECK_BANDS;
 276                     }
 277                     break;
 278                 }
 279             }
 280             setVtxTablePowerLevel(checkingIndex + 1);
 281             break;
 282         }
 283         break;
 284
 285     case MSP_VTXTABLE_BAND:
 286         bandPacket = (mspVtxBandPacket_t *)(packet + MSP_VTX_PAYLOAD_OFFSET);
 287
 288         switch (mspState)
 289         {
 290         case CHECK_BANDS:
 291             if (bandPacket->bandNameLength ==  BAND_NAME_LENGTH && bandPacket->bandLetter == getBandLetterByIdx(checkingIndex) && bandPacket->isFactoryBand == IS_FACTORY_BAND && bandPacket->channels == CHANNEL_COUNT) // Check lengths before trying to check content
 292             {
 293                 if (memcmp(bandPacket->bandName, channelFreqLabel + checkingIndex * CHANNEL_COUNT, sizeof(bandPacket->bandName)) == 0)
 294                 {
 295                     if (memcmp(bandPacket->channel, channelFreqTable + checkingIndex * CHANNEL_COUNT, sizeof(bandPacket->channel)) == 0)
 296                     {
 297                         checkingIndex++;
 298                         if (checkingIndex > getFreqTableBands() - 1)
 299                         {
 300                             mspState = (setRcePitMode ? SET_RCE_PIT_MODE : (eepromWriteRequired ? SEND_EEPROM_WRITE : MONITORING));
 301                             vtxSPIPitmode = pitMode;
 302                             vtxSPIPowerIdx = power;
 303                             vtxSPIFrequency = getFreqByIdx(channel);
 304                         }
 305                         break;
 306                     }
 307                 }
 308             }
 309             setVtxTableBand(checkingIndex + 1);
 310             break;
 311         }
 312         break;
 313
 314     case MSP_SET_VTX_CONFIG:
 315     case MSP_SET_VTXTABLE_BAND:
 316     case MSP_SET_VTXTABLE_POWERLEVEL:
 317         break;
 318
 319     case MSP_EEPROM_WRITE:
 320         mspState = MONITORING;
 321         break;
 322     }
 323 }
 324
 325 static void mspVtxStateUpdate(void)
 326 {
 327     switch (mspState)
 328     {
 329         case GET_VTX_TABLE_SIZE:
 330             sendVtxConfigCommand();
 331             break;
 332         case CHECK_POWER_LEVELS:
 333             getVtxTablePowerLvl(checkingIndex + 1);
 334             break;
 335         case CHECK_BANDS:
 336             getVtxTableBand(checkingIndex + 1);
 337             break;
 338         case SET_RCE_PIT_MODE:
 339             sendRcePitModeCommand();
 340             break;
 341         case SEND_EEPROM_WRITE:
 342             sendEepromWriteCommand();
 343             break;
 344         case MONITORING:
 345             break;
 346         default:
 347             break;
 348     }
 349 }
 350
 351 void disableMspVtx(void)
 352 {
 353     mspState = STOP_MSPVTX;
 354 }
 355
 356 static void initialize()
 357 {
 358     if (OPT_HAS_VTX_SPI)
 359     {
 360         mspState = GET_VTX_TABLE_SIZE;
 361     }
 362 }
 363
 364 static int event(void)
 365 {
 366     if (GPIO_PIN_SPI_VTX_NSS == UNDEF_PIN)
 367     {
 368         return DURATION_NEVER;
 369     }
 370     return DURATION_IMMEDIATELY;
 371 }
 372
 373 static int timeout(void)
 374 {
 375     if (mspState == STOP_MSPVTX || (mspState != MONITORING && millis() > MSP_VTX_TIMEOUT_NO_CONNECTION))
 376     {
 377         return DURATION_NEVER;
 378     }
 379
 380     if (hwTimer::running && !hwTimer::isTick)
 381     {
 382         // Only run code during rx free time or when disconnected.
 383         return DURATION_IMMEDIATELY;
 384     } else {
 385         mspVtxStateUpdate();
 386         return FC_QUERY_PERIOD_MS;
 387     }
 388 }
 389
 390 device_t MSPVTx_device = {
 391     .initialize = initialize,
 392     .start = nullptr,
 393     .event = event,
 394     .timeout = timeout
 395 };
 396
 397 #endif