radio/src/telemetry/multi.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20 #include "opentx.h"
  21 #include "telemetry.h"
  22 #include "multi.h"
  23
  24 #define MULTI_CHAN_BITS 11
  25
  26 extern uint8_t g_moduleIdx;
  27
  28 enum MultiPacketTypes : uint8_t
  29 {
  30   MultiStatus = 1,
  31   FrSkySportTelemetry,
  32   FrSkyHubTelemetry,
  33   SpektrumTelemetry,
  34   DSMBindPacket,
  35   FlyskyIBusTelemetry,
  36   ConfigCommand,
  37   InputSync,
  38   FrskySportPolling,
  39   HitecTelemetry,
  40   SpectrumScannerPacket,
  41   FlyskyIBusTelemetryAC,
  42   MultiRxChannels,
  43   HottTelemetry,
  44 };
  45
  46 enum MultiBufferState : uint8_t
  47 {
  48   NoProtocolDetected,
  49   MultiFirstByteReceived,
  50   ReceivingMultiProtocol,
  51   ReceivingMultiStatus,
  52   SpektrumTelemetryFallback,
  53   FrskyTelemetryFallback,
  54   FrskyTelemetryFallbackFirstByte,
  55   FrskyTelemetryFallbackNextBytes,
  56   FlyskyTelemetryFallback,
  57   HitecTelemetryFallback,
  58   MultiStatusOrFrskyData
  59 };
  60
  61
  62 #if defined(INTERNAL_MODULE_MULTI)
  63
  64 static MultiModuleStatus multiModuleStatus[NUM_MODULES] = {MultiModuleStatus(), MultiModuleStatus()};
  65 static MultiModuleSyncStatus multiSyncStatus[NUM_MODULES] = {MultiModuleSyncStatus(), MultiModuleSyncStatus()};
  66 static uint8_t multiBindStatus[NUM_MODULES] = {MULTI_NORMAL_OPERATION, MULTI_NORMAL_OPERATION};
  67
  68 static MultiBufferState multiTelemetryBufferState[NUM_MODULES];
  69 static uint16_t multiTelemetryLastRxTS[NUM_MODULES];
  70
  71 MultiModuleStatus &getMultiModuleStatus(uint8_t module)
  72 {
  73   return multiModuleStatus[module];
  74 }
  75
  76 MultiModuleSyncStatus &getMultiSyncStatus(uint8_t module)
  77 {
  78   return multiSyncStatus[module];
  79 }
  80
  81 uint8_t getMultiBindStatus(uint8_t module)
  82 {
  83   return multiBindStatus[module];
  84 }
  85
  86 void setMultiBindStatus(uint8_t module, uint8_t bindStatus)
  87 {
  88   multiBindStatus[module] = bindStatus;
  89 }
  90
  91 MultiBufferState getMultiTelemetryBufferState(uint8_t module)
  92 {
  93   return multiTelemetryBufferState[module];
  94 }
  95
  96 void setMultiTelemetryBufferState(uint8_t module, MultiBufferState state)
  97 {
  98   multiTelemetryBufferState[module] = state;
  99 }
 100
 101 static uint16_t& getMultiTelemetryLastRxTS(uint8_t module)
 102 {
 103   return multiTelemetryLastRxTS[module];
 104 }
 105
 106 // Use additional telemetry buffer
 107 uint8_t intTelemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE];
 108 uint8_t intTelemetryRxBufferCount;
 109
 110 #else // !INTERNAL_MODULE_MULTI
 111
 112 static MultiModuleStatus multiModuleStatus;
 113 static MultiModuleSyncStatus multiSyncStatus;
 114 static uint8_t multiBindStatus = MULTI_NORMAL_OPERATION;
 115
 116 static MultiBufferState multiTelemetryBufferState;
 117 static uint16_t multiTelemetryLastRxTS;
 118
 119 MultiModuleStatus& getMultiModuleStatus(uint8_t)
 120 {
 121   return multiModuleStatus;
 122 }
 123
 124 MultiModuleSyncStatus& getMultiSyncStatus(uint8_t)
 125 {
 126   return multiSyncStatus;
 127 }
 128
 129 uint8_t getMultiBindStatus(uint8_t)
 130 {
 131   return multiBindStatus;
 132 }
 133
 134 void setMultiBindStatus(uint8_t, uint8_t bindStatus)
 135 {
 136   multiBindStatus = bindStatus;
 137 }
 138
 139 MultiBufferState getMultiTelemetryBufferState(uint8_t)
 140 {
 141   return multiTelemetryBufferState;
 142 }
 143
 144 void setMultiTelemetryBufferState(uint8_t, MultiBufferState state)
 145 {
 146   multiTelemetryBufferState = state;
 147 }
 148
 149 uint16_t& getMultiTelemetryLastRxTS(uint8_t module)
 150 {
 151   return multiTelemetryLastRxTS;
 152 }
 153
 154 #endif // INTERNAL_MODULE_MULTI
 155
 156
 157 static MultiBufferState guessProtocol(uint8_t module)
 158 {
 159   uint32_t moduleIdx = EXTERNAL_MODULE;
 160 #if defined(INTERNAL_MODULE_MULTI)
 161   if (isModuleMultimodule(INTERNAL_MODULE)) {
 162     moduleIdx = INTERNAL_MODULE;
 163   }
 164 #endif
 165
 166   if (g_model.moduleData[moduleIdx].getMultiProtocol() == MODULE_SUBTYPE_MULTI_DSM2)
 167     return SpektrumTelemetryFallback;
 168   else if (g_model.moduleData[module].getMultiProtocol() == MODULE_SUBTYPE_MULTI_FS_AFHDS2A)
 169     return FlyskyTelemetryFallback;
 170   else
 171     return FrskyTelemetryFallback;
 172 }
 173
 174 static void processMultiScannerPacket(const uint8_t *data)
 175 {
 176   uint8_t cur_channel = data[0];
 177   if (moduleState[g_moduleIdx].mode == MODULE_MODE_SPECTRUM_ANALYSER) {
 178     for (uint8_t channel = 0; channel <5; channel++) {
 179       uint8_t power = max<int>(0,(data[channel+1] - 34) >> 1); // remove everything below -120dB
 180
 181 #if LCD_W == 480
 182       coord_t x = cur_channel*2;
 183       if (x < LCD_W) {
 184         reusableBuffer.spectrumAnalyser.bars[x] = power;
 185         reusableBuffer.spectrumAnalyser.bars[x+1] = power;
 186         if (power > reusableBuffer.spectrumAnalyser.max[x]) {
 187           reusableBuffer.spectrumAnalyser.max[x] = power;
 188           reusableBuffer.spectrumAnalyser.max[x+1] = power;
 189         }
 190 #elif LCD_W == 212
 191         coord_t x = cur_channel;
 192       if (x <= LCD_W) {
 193         reusableBuffer.spectrumAnalyser.bars[x] = power;
 194         if (power > reusableBuffer.spectrumAnalyser.max[x]) {
 195           reusableBuffer.spectrumAnalyser.max[x] = power;
 196         }
 197 #else
 198       coord_t x = cur_channel/2 + 1;
 199       if (x <= LCD_W) {
 200         reusableBuffer.spectrumAnalyser.bars[x] = power;
 201         if (power > reusableBuffer.spectrumAnalyser.max[x]) {
 202           reusableBuffer.spectrumAnalyser.max[x] = power;
 203         }
 204 #endif
 205       }
 206       if (++cur_channel > MULTI_SCANNER_MAX_CHANNEL)
 207         cur_channel = 0;
 208     }
 209   }
 210 }
 211
 212 static void processMultiStatusPacket(const uint8_t * data, uint8_t module, uint8_t len)
 213 {
 214   MultiModuleStatus &status = getMultiModuleStatus(module);
 215
 216   // At least two status packets without bind flag
 217   bool wasBinding = status.isBinding();
 218
 219   status.lastUpdate = get_tmr10ms();
 220   status.flags = data[0];
 221   status.major = data[1];
 222   status.minor = data[2];
 223   status.revision = data[3];
 224   status.patch = data[4];
 225   if (len < 6) {
 226     status.ch_order = 0xFF;
 227   }
 228   else {
 229     status.ch_order = data[5];
 230     if (len >= 24) {
 231       status.protocolNext = data[6];
 232       status.protocolPrev = data[7];
 233       memcpy(status.protocolName, &data[8], 7);
 234       status.protocolName[7] = 0;
 235       status.protocolSubNbr = data[15] & 0x0F;
 236       memcpy(status.protocolSubName, &data[16], 8);
 237       status.protocolSubName[8] = 0;
 238       status.optionDisp = data[15] >> 4;
 239     }
 240     else {
 241       status.protocolName[0] = 0;
 242     }
 243   }
 244   if (getMultiModuleStatus(module).requiresFailsafeCheck) {
 245     getMultiModuleStatus(module).requiresFailsafeCheck = false;
 246     if (getMultiModuleStatus(module).supportsFailsafe() &&  g_model.moduleData[module].failsafeMode == FAILSAFE_NOT_SET)
 247       POPUP_WARNING(STR_NO_FAILSAFE);
 248   }
 249
 250   if (wasBinding && !status.isBinding() && getMultiBindStatus(module) == MULTI_BIND_INITIATED)
 251     setMultiBindStatus(module, MULTI_BIND_FINISHED);
 252 }
 253
 254 static void processMultiSyncPacket(const uint8_t * data, uint8_t module)
 255 {
 256   MultiModuleSyncStatus &status = getMultiSyncStatus(module);
 257
 258   status.lastUpdate = get_tmr10ms();
 259   status.interval = data[4];
 260   status.target = data[5];
 261 #if !defined(PPM_PIN_SERIAL)
 262   auto oldlag = status.inputLag;
 263   (void) oldlag;
 264 #endif
 265
 266   status.calcAdjustedRefreshRate(data[0] << 8 | data[1], data[2] << 8 | data[3]);
 267
 268 #if !defined(PPM_PIN_SERIAL)
 269   TRACE("MP ADJ: rest: %d, lag %04d, diff: %04d  target: %d, interval: %d, Refresh: %d, intAdjRefresh: %d, adjRefresh %d\r\n",
 270         module == EXTERNAL_MODULE ? extmodulePulsesData.dsm2.rest : 0,
 271         status.inputLag, oldlag - status.inputLag, status.target, status.interval, status.refreshRate, status.adjustedRefreshRate / 50,
 272         status.getAdjustedRefreshRate());
 273 #endif
 274 }
 275
 276 #if defined(PCBTARANIS) || defined(PCBHORUS)
 277 static void processMultiRxChannels(const uint8_t * data, uint8_t len)
 278 {
 279   if (g_model.trainerData.mode != TRAINER_MODE_MULTI)
 280     return;
 281
 282   //uint8_t pps  = data[0];
 283   //uint8_t rssi = data[1];
 284   int ch    = max(data[2], (uint8_t)0);
 285   int maxCh = min(ch + data[3], MAX_TRAINER_CHANNELS);
 286
 287   uint32_t bits = 0;
 288   uint8_t  bitsavailable = 0;
 289   uint8_t  byteIdx = 4;
 290
 291   while (ch < maxCh) {
 292     while (bitsavailable < MULTI_CHAN_BITS && byteIdx < len) {
 293       bits |= (uint32_t)(data[byteIdx++]) << (uint32_t)bitsavailable;
 294       bitsavailable += 8;
 295     }
 296
 297     int value = bits & ((1 << MULTI_CHAN_BITS) - 1);
 298     bitsavailable -= MULTI_CHAN_BITS;
 299     bits >>= MULTI_CHAN_BITS;
 300
 301     ppmInput[ch] = (value - 1024) * 500 / 800;
 302     ch++;
 303
 304     if (byteIdx >= len)
 305       break;
 306   }
 307
 308   if (ch == maxCh)
 309     ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT;
 310 }
 311 #endif
 312
 313 static void processMultiTelemetryPaket(const uint8_t * packet, uint8_t module)
 314 {
 315   uint8_t type = packet[0];
 316   uint8_t len = packet[1];
 317   const uint8_t * data = packet + 2;
 318
 319   // Switch type
 320   switch (type) {
 321     case MultiStatus:
 322       if (len >= 5)
 323         processMultiStatusPacket(data, module, len);
 324       break;
 325
 326     case DSMBindPacket:
 327       if (len >= 10)
 328         processDSMBindPacket(module, data);
 329       break;
 330
 331     case SpektrumTelemetry:
 332       // processSpektrumPacket expects data[0] to be the telemetry indicator 0xAA but does not check it,
 333       // just send one byte of our header instead
 334       if (len >= 17)
 335         processSpektrumPacket(data - 1);
 336       else
 337         TRACE("[MP] Received spektrum telemetry len %d < 17", len);
 338       break;
 339
 340     case FlyskyIBusTelemetry:
 341       if (len >= 28)
 342         processFlySkyPacket(data);
 343       else
 344         TRACE("[MP] Received IBUS telemetry len %d < 28", len);
 345       break;
 346
 347     case FlyskyIBusTelemetryAC:
 348       if (len >= 28)
 349         processFlySkyPacketAC(data);
 350       else
 351         TRACE("[MP] Received IBUS telemetry AC len %d < 28", len);
 352       break;
 353
 354     case HitecTelemetry:
 355       if (len >= 8)
 356         processHitecPacket(data);
 357       else
 358         TRACE("[MP] Received Hitec telemetry len %d < 8", len);
 359       break;
 360
 361     case HottTelemetry:
 362       if (len >= 14)
 363         processHottPacket(data);
 364       else
 365         TRACE("[MP] Received HoTT telemetry len %d < 14", len);
 366       break;
 367
 368     case FrSkyHubTelemetry:
 369       if (len >= 4)
 370         frskyDProcessPacket(data);
 371       else
 372         TRACE("[MP] Received Frsky HUB telemetry len %d < 4", len);
 373       break;
 374
 375     case FrSkySportTelemetry:
 376       if (len >= 4)
 377         sportProcessTelemetryPacket(data);
 378       else
 379         TRACE("[MP] Received sport telemetry len %d < 4", len);
 380       break;
 381
 382     case InputSync:
 383       if (len >= 6)
 384         processMultiSyncPacket(data, module);
 385       else
 386         TRACE("[MP] Received input sync len %d < 6", len);
 387       break;
 388
 389     case ConfigCommand:
 390       // Just an ack to our command, ignore for now
 391       break;
 392
 393 #if defined(LUA)
 394     case FrskySportPolling:
 395       if (len >= 1 && outputTelemetryBuffer.destination == TELEMETRY_ENDPOINT_SPORT && data[0] == outputTelemetryBuffer.sport.physicalId) {
 396         TRACE("MP Sending sport data out.");
 397         sportSendBuffer(outputTelemetryBuffer.data, outputTelemetryBuffer.size);
 398       }
 399       break;
 400 #endif
 401     case SpectrumScannerPacket:
 402       if (len == 6)
 403         processMultiScannerPacket(data);
 404       else
 405         TRACE("[MP] Received spectrum scanner len %d != 6", len);
 406       break;
 407
 408 #if defined(PCBTARANIS) || defined(PCBHORUS)
 409     case MultiRxChannels:
 410       if (len >= 4)
 411         processMultiRxChannels(data, len);
 412       else
 413         TRACE("[MP] Received RX channels len %d < 4", len);
 414       break;
 415 #endif
 416
 417     default:
 418       TRACE("[MP] Unkown multi packet type 0x%02X, len %d", type, len);
 419       break;
 420   }
 421 }
 422
 423 #define MIN_REFRESH_RATE      5500
 424
 425 void MultiModuleSyncStatus::calcAdjustedRefreshRate(uint16_t newRefreshRate, uint16_t newInputLag)
 426 {
 427   // Check how far off we are from our target, positive means we are too slow, negative we are too fast
 428   int lagDifference = newInputLag - inputLag;
 429
 430   // The refresh rate that we target
 431   // Below is least common multiple of MIN_REFRESH_RATE and requested rate
 432   uint16_t targetRefreshRate = (uint16_t) (newRefreshRate * ((MIN_REFRESH_RATE / (newRefreshRate - 1)) + 1));
 433
 434   // Overflow, reverse sample
 435   if (lagDifference < -targetRefreshRate / 2)
 436     lagDifference = -lagDifference;
 437
 438
 439   // Reset adjusted refresh if rate has changed
 440   if (newRefreshRate != refreshRate) {
 441     refreshRate = newRefreshRate;
 442     adjustedRefreshRate = targetRefreshRate;
 443     if (adjustedRefreshRate >= 30000)
 444       adjustedRefreshRate /= 2;
 445
 446     // Our refresh rate in ps
 447     adjustedRefreshRate *= 1000;
 448     return;
 449   }
 450
 451   // Caluclate how many samples went into the reported input Lag (*10)
 452   int numsamples = interval * 10000 / targetRefreshRate;
 453
 454   // Convert lagDifference to ps
 455   lagDifference = lagDifference * 1000;
 456
 457   // Calculate the time we intentionally were late/early
 458   if (inputLag > target * 10 + 30)
 459     lagDifference += numsamples * 500;
 460   else if (inputLag < target * 10 - 30)
 461     lagDifference -= numsamples * 500;
 462
 463   // Caculate the time in ps each frame is to slow (positive), fast(negative)
 464   int perframeps = lagDifference * 10 / numsamples;
 465
 466   if (perframeps > 20000)
 467     perframeps = 20000;
 468
 469   if (perframeps < -20000)
 470     perframeps = -20000;
 471
 472   adjustedRefreshRate = (adjustedRefreshRate + perframeps);
 473
 474   // Safeguards
 475   if (adjustedRefreshRate < MIN_REFRESH_RATE * 1000)
 476     adjustedRefreshRate = MIN_REFRESH_RATE * 1000;
 477   if (adjustedRefreshRate > 30 * 1000 * 1000)
 478     adjustedRefreshRate = 30 * 1000 * 1000;
 479
 480   inputLag = newInputLag;
 481 }
 482
 483 static uint8_t counter;
 484
 485 const uint16_t MultiModuleSyncStatus::getAdjustedRefreshRate()
 486 {
 487   if (!isValid() || refreshRate == 0)
 488     return 18000;
 489
 490   counter = (uint8_t) (counter + 1 % 10);
 491   uint16_t rate = (uint16_t) ((adjustedRefreshRate + counter * 50) / 500);
 492   // Check how far off we are from our target, positive means we are too slow, negative we are too fast
 493   if (inputLag > target * 10 + 30)
 494     return (uint16_t) (rate - 1);
 495   else if (inputLag < target * 10 - 30)
 496     return (uint16_t) (rate + 1);
 497   else
 498     return rate;
 499 }
 500
 501 void MultiModuleSyncStatus::getRefreshString(char * statusText)
 502 {
 503   if (!isValid()) {
 504     return;
 505   }
 506
 507   char * tmp = statusText;
 508 #if defined(DEBUG)
 509   *tmp++ = 'L';
 510   tmp = strAppendUnsigned(tmp, inputLag, 5);
 511   tmp = strAppend(tmp, "us R ");
 512   tmp = strAppendUnsigned(tmp, (uint32_t) (adjustedRefreshRate / 1000), 5);
 513   tmp = strAppend(tmp, "us");
 514 #else
 515   tmp = strAppend(tmp, "Sync at ");
 516   tmp = strAppendUnsigned(tmp, (uint32_t) (adjustedRefreshRate / 1000000));
 517   tmp = strAppend(tmp, " ms");
 518 #endif
 519 }
 520
 521 void MultiModuleStatus::getStatusString(char * statusText) const
 522 {
 523   if (!isValid()) {
 524 #if defined(PCBTARANIS) || defined(PCBHORUS)
 525 #if !defined(INTERNAL_MODULE_MULTI)
 526     if (isSportLineUsedByInternalModule())
 527       strcpy(statusText, STR_DISABLE_INTERNAL);
 528     else
 529 #endif
 530 #endif
 531     strcpy(statusText, STR_MODULE_NO_TELEMETRY);
 532     return;
 533   }
 534   if (!protocolValid()) {
 535     strcpy(statusText, STR_PROTOCOL_INVALID);
 536     return;
 537   }
 538   else if (!serialMode()) {
 539     strcpy(statusText, STR_MODULE_NO_SERIAL_MODE);
 540     return;
 541   }
 542   else if (!inputDetected()) {
 543     strcpy(statusText, STR_MODULE_NO_INPUT);
 544     return;
 545   }
 546   else if (isWaitingforBind()) {
 547     strcpy(statusText, STR_MODULE_WAITFORBIND);
 548     return;
 549   }
 550
 551   if (major == 1 && minor < 3 && SLOW_BLINK_ON_PHASE) {
 552     strcpy(statusText, STR_MODULE_UPGRADE);
 553   }
 554   else {
 555     char * tmp = statusText;
 556     *tmp++ = 'V';
 557     tmp = strAppendUnsigned(tmp, major);
 558     *tmp++ = '.';
 559     tmp = strAppendUnsigned(tmp, minor);
 560     *tmp++ = '.';
 561     tmp = strAppendUnsigned(tmp, revision);
 562     *tmp++ = '.';
 563     tmp = strAppendUnsigned(tmp, patch);
 564
 565     if (isBinding()) {
 566       strcpy(tmp, " " TR_MODULE_BINDING);
 567     }
 568     else if (ch_order != 0xFF) {
 569       uint8_t temp = ch_order;
 570       *tmp++ = ' ';
 571       *(tmp + (temp & 0x03)) = 'A';
 572       temp >>= 2;
 573       *(tmp + (temp & 0x03)) = 'E';
 574       temp >>= 2;
 575       *(tmp + (temp & 0x03)) = 'T';
 576       temp >>= 2;
 577       *(tmp + (temp & 0x03)) = 'R';
 578       *(tmp + 4) = '\0';
 579     }
 580   }
 581 }
 582
 583 static uint8_t * getRxBuffer(uint8_t moduleIdx)
 584 {
 585 #if defined(INTERNAL_MODULE_MULTI)
 586   if (moduleIdx == INTERNAL_MODULE)
 587     return intTelemetryRxBuffer;
 588 #endif
 589   return telemetryRxBuffer;
 590 }
 591
 592 static uint8_t &getRxBufferCount(uint8_t moduleIdx)
 593 {
 594 #if defined(INTERNAL_MODULE_MULTI)
 595   if (moduleIdx == INTERNAL_MODULE)
 596     return intTelemetryRxBufferCount;
 597 #endif
 598   return telemetryRxBufferCount;
 599 }
 600
 601 static void processMultiTelemetryByte(const uint8_t data, uint8_t module)
 602 {
 603   uint8_t * rxBuffer = getRxBuffer(module);
 604   uint8_t &rxBufferCount = getRxBufferCount(module);
 605
 606   if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
 607     rxBuffer[rxBufferCount++] = data;
 608   }
 609   else {
 610     TRACE("[MP] array size %d error", rxBufferCount);
 611     setMultiTelemetryBufferState(module, NoProtocolDetected);
 612   }
 613
 614   // Length field does not count the header
 615   if (rxBufferCount >= 2 && rxBuffer[1] == rxBufferCount - 2) {
 616     // debug print the content of the packet
 617 #if 0
 618     debugPrintf("[MP] Packet type %02X len 0x%02X: ",
 619                 rxBuffer[0], rxBuffer[1]);
 620     for (int i=0; i<(rxBufferCount+3)/4; i++) {
 621       debugPrintf("[%02X%02X %02X%02X] ", rxBuffer[i*4+2], rxBuffer[i*4 + 3],
 622                   rxBuffer[i*4 + 4], rxBuffer[i*4 + 5]);
 623     }
 624     debugPrintf("\r\n");
 625 #endif
 626     // Packet is complete, process it
 627     processMultiTelemetryPaket(rxBuffer, module);
 628     setMultiTelemetryBufferState(module, NoProtocolDetected);
 629   }
 630 }
 631
 632 void processMultiTelemetryData(uint8_t data, uint8_t module)
 633 {
 634   uint8_t * rxBuffer = getRxBuffer(module);
 635   uint8_t &rxBufferCount = getRxBufferCount(module);
 636
 637   uint16_t &lastRxTS = getMultiTelemetryLastRxTS(module);
 638   uint16_t nowMs = (uint16_t)RTOS_GET_MS();
 639   if (nowMs - lastRxTS > 15)
 640     setMultiTelemetryBufferState(module, NoProtocolDetected);
 641   lastRxTS = nowMs;
 642
 643   // debugPrintf("State: %d, byte received %02X, buflen: %d\r\n", getMultiTelemetryBufferState(module), data, rxBufferCount);
 644
 645   switch (getMultiTelemetryBufferState(module)) {
 646     case NoProtocolDetected:
 647       if (data == 'M') {
 648         setMultiTelemetryBufferState(module, MultiFirstByteReceived);
 649       }
 650       else if (data == 0xAA || data == 0x7e) {
 651         setMultiTelemetryBufferState(module, guessProtocol(module));
 652
 653         // Process the first byte by the protocol
 654         processMultiTelemetryData(data, module);
 655       }
 656       else {
 657         TRACE("[MP] invalid start byte 0x%02X", data);
 658       }
 659       break;
 660
 661     case FrskyTelemetryFallback:
 662       setMultiTelemetryBufferState(module, FrskyTelemetryFallbackFirstByte);
 663       processFrskyTelemetryData(data);
 664       break;
 665
 666     case FrskyTelemetryFallbackFirstByte:
 667       if (data == 'M') {
 668         setMultiTelemetryBufferState(module, MultiStatusOrFrskyData);
 669       }
 670       else {
 671         processFrskyTelemetryData(data);
 672         if (data != 0x7e)
 673           setMultiTelemetryBufferState(module, FrskyTelemetryFallbackNextBytes);
 674       }
 675
 676       break;
 677
 678     case FrskyTelemetryFallbackNextBytes:
 679       processFrskyTelemetryData(data);
 680       if (data == 0x7e) {
 681         // end of packet or start of new packet
 682         setMultiTelemetryBufferState(module, FrskyTelemetryFallbackFirstByte);
 683       }
 684       break;
 685
 686     case FlyskyTelemetryFallback:
 687       processFlySkyTelemetryData(data, rxBuffer, rxBufferCount);
 688       if (rxBufferCount == 0) {
 689         setMultiTelemetryBufferState(module, NoProtocolDetected);
 690       }
 691       break;
 692
 693     case SpektrumTelemetryFallback:
 694       processSpektrumTelemetryData(module, data, rxBuffer, rxBufferCount);
 695       if (rxBufferCount == 0) {
 696         setMultiTelemetryBufferState(module, NoProtocolDetected);
 697       }
 698       break;
 699
 700     case MultiFirstByteReceived:
 701       rxBufferCount = 0;
 702       if (data == 'P') {
 703         setMultiTelemetryBufferState(module, ReceivingMultiProtocol);
 704       }
 705       else if (data >= 5 && data <= 10) {
 706         // Protocol indented for er9x/ersky9, accept only 5-10 as packet length to have
 707         // a bit of validation
 708         setMultiTelemetryBufferState(module, ReceivingMultiStatus);
 709         processMultiTelemetryData(data, module);
 710       }
 711       else {
 712         TRACE("[MP] invalid second byte 0x%02X", data);
 713         setMultiTelemetryBufferState(module, NoProtocolDetected);
 714       }
 715       break;
 716
 717     case ReceivingMultiProtocol:
 718       processMultiTelemetryByte(data, module);
 719       break;
 720
 721     case MultiStatusOrFrskyData:
 722       // Check len byte if it makes sense for multi
 723       if (data >= 5 && data <= 10) {
 724         setMultiTelemetryBufferState(module, ReceivingMultiStatus);
 725         rxBufferCount = 0;
 726       }
 727       else {
 728         setMultiTelemetryBufferState(module, FrskyTelemetryFallbackNextBytes);
 729         processMultiTelemetryData('M', module);
 730       }
 731       processMultiTelemetryData(data, module);
 732       break;
 733
 734     case ReceivingMultiStatus:
 735       if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
 736         rxBuffer[rxBufferCount++] = data;
 737         if (rxBufferCount > 5 && rxBuffer[0] == rxBufferCount - 1) {
 738           processMultiStatusPacket(rxBuffer + 1, module, rxBuffer[0]);
 739           rxBufferCount = 0;
 740           setMultiTelemetryBufferState(module, NoProtocolDetected);
 741         }
 742         if (rxBufferCount > 24) {
 743           // too long ignore
 744           TRACE("Overlong multi status packet detected ignoring, wanted %d", rxBuffer[0]);
 745           rxBufferCount = 0;
 746           setMultiTelemetryBufferState(module, NoProtocolDetected);
 747         }
 748       }
 749       else {
 750         TRACE("[MP] array size %d error", rxBufferCount);
 751         setMultiTelemetryBufferState(module, NoProtocolDetected);
 752       }
 753       break;
 754   }
 755 }
 756