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