src/lib/MSPVTX/devMSPVTX.cpp

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