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