src/lib/Handset/CRSFHandset.cpp

   1 #include "CRSF.h"
   2 #include "CRSFHandset.h"
   3 #include "FIFO.h"
   4 #include "logging.h"
   5 #include "helpers.h"
   6
   7 #if defined(CRSF_TX_MODULE) && !defined(UNIT_TEST)
   8 #include "device.h"
   9
  10 #if defined(PLATFORM_ESP32)
  11 #include <soc/soc.h>
  12 #include <soc/uart_reg.h>
  13 // UART0 is used since for DupleTX we can connect directly through IO_MUX and not the Matrix
  14 // for better performance, and on other targets (mostly using pin 13), it always uses Matrix
  15 HardwareSerial CRSFHandset::Port(0);
  16 RTC_DATA_ATTR int rtcModelId = 0;
  17 #elif defined(PLATFORM_ESP8266)
  18 HardwareSerial CRSFHandset::Port(0);
  19 #elif defined(PLATFORM_STM32)
  20 HardwareSerial CRSFHandset::Port(GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX);
  21 #if defined(STM32F3) || defined(STM32F3xx)
  22 #include "stm32f3xx_hal.h"
  23 #include "stm32f3xx_hal_gpio.h"
  24 #elif defined(STM32F1) || defined(STM32F1xx)
  25 #include "stm32f1xx_hal.h"
  26 #include "stm32f1xx_hal_gpio.h"
  27 #endif
  28 #elif defined(TARGET_NATIVE)
  29 HardwareSerial CRSFHandset::Port = Serial;
  30 #endif
  31
  32 static constexpr int HANDSET_TELEMETRY_FIFO_SIZE = 128; // this is the smallest telemetry FIFO size in ETX with CRSF defined
  33
  34 /// Out FIFO to buffer messages///
  35 static constexpr auto CRSF_SERIAL_OUT_FIFO_SIZE = 256U;
  36 static FIFO<CRSF_SERIAL_OUT_FIFO_SIZE> SerialOutFIFO;
  37
  38 Stream *CRSFHandset::PortSecondary;
  39
  40 /// UART Handling ///
  41 uint32_t CRSFHandset::GoodPktsCountResult = 0;
  42 uint32_t CRSFHandset::BadPktsCountResult = 0;
  43
  44 uint8_t CRSFHandset::modelId = 0;
  45 bool CRSFHandset::ForwardDevicePings = false;
  46 bool CRSFHandset::elrsLUAmode = false;
  47
  48 /// OpenTX mixer sync ///
  49 static const int32_t OpenTXsyncPacketInterval = 200; // in ms
  50 static const int32_t OpenTXsyncOffsetSafeMargin = 1000; // 100us
  51
  52 /// UART Handling ///
  53 static const int32_t TxToHandsetBauds[] = {400000, 115200, 5250000, 3750000, 1870000, 921600, 2250000};
  54 uint8_t CRSFHandset::UARTcurrentBaudIdx = 6;   // only used for baud-cycling, initialized to the end so the next one we try is the first in the list
  55 uint32_t CRSFHandset::UARTrequestedBaud = 5250000;
  56
  57 // for the UART wdt, every 1000ms we change bauds when connect is lost
  58 static const int UARTwdtInterval = 1000;
  59
  60 void CRSFHandset::Begin()
  61 {
  62     DBGLN("About to start CRSF task...");
  63
  64     UARTwdtLastChecked = millis() + UARTwdtInterval; // allows a delay before the first time the UARTwdt() function is called
  65
  66 #if defined(PLATFORM_ESP32)
  67     portDISABLE_INTERRUPTS();
  68     if (GPIO_PIN_RCSIGNAL_RX != GPIO_PIN_RCSIGNAL_TX)
  69     {
  70         UARTinverted = false; // on a full UART we will start uninverted checking first
  71     }
  72     CRSFHandset::Port.begin(UARTrequestedBaud, SERIAL_8N1,
  73                      GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX,
  74                      false, 500);
  75     CRSFHandset::duplex_set_RX();
  76     portENABLE_INTERRUPTS();
  77     flush_port_input();
  78     if (esp_reset_reason() != ESP_RST_POWERON)
  79     {
  80         modelId = rtcModelId;
  81         if (RecvModelUpdate) RecvModelUpdate();
  82     }
  83 #elif defined(PLATFORM_ESP8266)
  84     // Uses default UART pins
  85     CRSFHandset::Port.begin(UARTrequestedBaud);
  86     // Invert RX/TX (not done, connection is full duplex uninverted)
  87     //USC0(UART0) |= BIT(UCRXI) | BIT(UCTXI);
  88     // No log message because this is our only UART
  89
  90 #elif defined(PLATFORM_STM32)
  91     DBGLN("Start STM32 R9M TX CRSF UART");
  92
  93     CRSFHandset::Port.setTx(GPIO_PIN_RCSIGNAL_TX);
  94     CRSFHandset::Port.setRx(GPIO_PIN_RCSIGNAL_RX);
  95
  96     #if defined(GPIO_PIN_BUFFER_OE) && (GPIO_PIN_BUFFER_OE != UNDEF_PIN)
  97     pinMode(GPIO_PIN_BUFFER_OE, OUTPUT);
  98     digitalWrite(GPIO_PIN_BUFFER_OE, LOW ^ GPIO_PIN_BUFFER_OE_INVERTED); // RX mode default
  99     #elif (GPIO_PIN_RCSIGNAL_TX == GPIO_PIN_RCSIGNAL_RX)
 100     CRSFHandset::Port.setHalfDuplex();
 101     #endif
 102
 103     CRSFHandset::Port.begin(UARTrequestedBaud);
 104
 105 #if defined(TARGET_TX_GHOST)
 106     USART1->CR1 &= ~USART_CR1_UE;
 107     USART1->CR3 |= USART_CR3_HDSEL;
 108     USART1->CR2 |= USART_CR2_RXINV | USART_CR2_TXINV | USART_CR2_SWAP; //inverted/swapped
 109     USART1->CR1 |= USART_CR1_UE;
 110 #elif defined(TARGET_TX_FM30_MINI)
 111     LL_GPIO_SetPinPull(GPIOA, GPIO_PIN_2, LL_GPIO_PULL_DOWN); // default is PULLUP
 112     USART2->CR1 &= ~USART_CR1_UE;
 113     USART2->CR2 |= USART_CR2_RXINV | USART_CR2_TXINV; //inverted
 114     USART2->CR1 |= USART_CR1_UE;
 115 #endif
 116     DBGLN("STM32 CRSF UART LISTEN TASK STARTED");
 117     CRSFHandset::Port.flush();
 118     flush_port_input();
 119 #endif
 120 }
 121
 122 void CRSFHandset::End()
 123 {
 124     uint32_t startTime = millis();
 125     while (SerialOutFIFO.peek() > 0)
 126     {
 127         handleInput();
 128         if (millis() - startTime > 1000)
 129         {
 130             break;
 131         }
 132     }
 133     //CRSFHandset::Port.end(); // don't call serial.end(), it causes some sort of issue with the 900mhz hardware using gpio2 for serial
 134     DBGLN("CRSF UART END");
 135 }
 136
 137 void CRSFHandset::flush_port_input()
 138 {
 139     // Make sure there is no garbage on the UART at the start
 140     while (CRSFHandset::Port.available())
 141     {
 142         CRSFHandset::Port.read();
 143     }
 144 }
 145
 146 void CRSFHandset::makeLinkStatisticsPacket(uint8_t buffer[LinkStatisticsFrameLength + 4])
 147 {
 148     buffer[0] = CRSF_ADDRESS_RADIO_TRANSMITTER;
 149     buffer[1] = LinkStatisticsFrameLength + 2;
 150     buffer[2] = CRSF_FRAMETYPE_LINK_STATISTICS;
 151     for (uint8_t i = 0; i < LinkStatisticsFrameLength; i++)
 152     {
 153         buffer[i + 3] = ((uint8_t *)&CRSF::LinkStatistics)[i];
 154     }
 155     uint8_t crc = crsf_crc.calc(buffer[2]);
 156     buffer[LinkStatisticsFrameLength + 3] = crsf_crc.calc((byte *)&CRSF::LinkStatistics, LinkStatisticsFrameLength, crc);
 157 }
 158
 159 /**
 160  * Build a an extended type packet and queue it in the SerialOutFIFO
 161  * This is just a regular packet with 2 extra bytes with the sub src and target
 162  **/
 163 void CRSFHandset::packetQueueExtended(uint8_t type, void *data, uint8_t len)
 164 {
 165     uint8_t buf[6] = {
 166         (uint8_t)(len + 6),
 167         CRSF_ADDRESS_RADIO_TRANSMITTER,
 168         (uint8_t)(len + 4),
 169         type,
 170         CRSF_ADDRESS_RADIO_TRANSMITTER,
 171         CRSF_ADDRESS_CRSF_TRANSMITTER
 172     };
 173
 174     // CRC - Starts at type, ends before CRC
 175     uint8_t crc = crsf_crc.calc(&buf[3], sizeof(buf)-3);
 176     crc = crsf_crc.calc((byte *)data, len, crc);
 177
 178     SerialOutFIFO.lock();
 179     if (SerialOutFIFO.ensure(buf[0] + 1))
 180     {
 181         SerialOutFIFO.pushBytes(buf, sizeof(buf));
 182         SerialOutFIFO.pushBytes((byte *)data, len);
 183         SerialOutFIFO.push(crc);
 184     }
 185     SerialOutFIFO.unlock();
 186 }
 187
 188 void CRSFHandset::sendTelemetryToTX(uint8_t *data)
 189 {
 190     if (controllerConnected)
 191     {
 192         uint8_t size = CRSF_FRAME_SIZE(data[CRSF_TELEMETRY_LENGTH_INDEX]);
 193         if (size > CRSF_MAX_PACKET_LEN)
 194         {
 195             ERRLN("too large");
 196             return;
 197         }
 198
 199         data[0] = CRSF_ADDRESS_RADIO_TRANSMITTER;
 200         SerialOutFIFO.lock();
 201         if (SerialOutFIFO.ensure(size + 1))
 202         {
 203             SerialOutFIFO.push(size); // length
 204             SerialOutFIFO.pushBytes(data, size);
 205         }
 206         SerialOutFIFO.unlock();
 207     }
 208 }
 209
 210 void ICACHE_RAM_ATTR CRSFHandset::setPacketInterval(int32_t PacketInterval)
 211 {
 212     RequestedRCpacketInterval = PacketInterval;
 213     OpenTXsyncOffset = 0;
 214     OpenTXsyncWindow = 0;
 215     OpenTXsyncWindowSize = std::max((int32_t)1, (int32_t)(20000/RequestedRCpacketInterval));
 216     OpenTXsyncLastSent -= OpenTXsyncPacketInterval;
 217     adjustMaxPacketSize();
 218 }
 219
 220 void ICACHE_RAM_ATTR CRSFHandset::JustSentRFpacket()
 221 {
 222     // read them in this order to prevent a potential race condition
 223     uint32_t last = dataLastRecv;
 224     uint32_t m = micros();
 225     auto delta = (int32_t)(m - last);
 226
 227     if (delta >= (int32_t)RequestedRCpacketInterval)
 228     {
 229         // missing/late packet, force resync
 230         OpenTXsyncOffset = -(delta % RequestedRCpacketInterval) * 10;
 231         OpenTXsyncWindow = 0;
 232         OpenTXsyncLastSent -= OpenTXsyncPacketInterval;
 233 #ifdef DEBUG_OPENTX_SYNC
 234         DBGLN("Missed packet, forced resync (%d)!", delta);
 235 #endif
 236     }
 237     else
 238     {
 239         // The number of packets in the sync window is how many will fit in 20ms.
 240         // This gives quite quite coarse changes for 50Hz, but more fine grained changes at 1000Hz.
 241         OpenTXsyncWindow = std::min(OpenTXsyncWindow + 1, (int32_t)OpenTXsyncWindowSize);
 242         OpenTXsyncOffset = ((OpenTXsyncOffset * (OpenTXsyncWindow-1)) + delta * 10) / OpenTXsyncWindow;
 243     }
 244 }
 245
 246 void CRSFHandset::sendSyncPacketToTX() // in values in us.
 247 {
 248     uint32_t now = millis();
 249     if (controllerConnected && (now - OpenTXsyncLastSent) >= OpenTXsyncPacketInterval)
 250     {
 251         int32_t packetRate = RequestedRCpacketInterval * 10; //convert from us to right format
 252         int32_t offset = OpenTXsyncOffset - OpenTXsyncOffsetSafeMargin; // offset so that opentx always has some headroom
 253 #ifdef DEBUG_OPENTX_SYNC
 254         DBGLN("Offset %d", offset); // in 10ths of us (OpenTX sync unit)
 255 #endif
 256
 257         struct otxSyncData {
 258             uint8_t extendedType; // CRSF_FRAMETYPE_OPENTX_SYNC
 259             uint32_t rate; // Big-Endian
 260             uint32_t offset; // Big-Endian
 261         } PACKED;
 262
 263         uint8_t buffer[sizeof(otxSyncData)];
 264         auto * const sync = (struct otxSyncData * const)buffer;
 265
 266         sync->extendedType = CRSF_FRAMETYPE_OPENTX_SYNC;
 267         sync->rate = htobe32(packetRate);
 268         sync->offset = htobe32(offset);
 269
 270         packetQueueExtended(CRSF_FRAMETYPE_RADIO_ID, buffer, sizeof(buffer));
 271
 272         OpenTXsyncLastSent = now;
 273     }
 274 }
 275
 276 void CRSFHandset::RcPacketToChannelsData() // data is packed as 11 bits per channel
 277 {
 278     // for monitoring arming state
 279     uint32_t prev_AUX1 = ChannelData[4];
 280
 281     auto payload = (uint8_t const * const)&inBuffer.asRCPacket_t.channels;
 282     constexpr unsigned srcBits = 11;
 283     constexpr unsigned dstBits = 11;
 284     constexpr unsigned inputChannelMask = (1 << srcBits) - 1;
 285     constexpr unsigned precisionShift = dstBits - srcBits;
 286
 287     // code from BetaFlight rx/crsf.cpp / bitpacker_unpack
 288     uint8_t bitsMerged = 0;
 289     uint32_t readValue = 0;
 290     unsigned readByteIndex = 0;
 291     for (uint32_t & n : ChannelData)
 292     {
 293         while (bitsMerged < srcBits)
 294         {
 295             uint8_t readByte = payload[readByteIndex++];
 296             readValue |= ((uint32_t) readByte) << bitsMerged;
 297             bitsMerged += 8;
 298         }
 299         //printf("rv=%x(%x) bm=%u\n", readValue, (readValue & inputChannelMask), bitsMerged);
 300         n = (readValue & inputChannelMask) << precisionShift;
 301         readValue >>= srcBits;
 302         bitsMerged -= srcBits;
 303     }
 304
 305     if (prev_AUX1 != ChannelData[4])
 306     {
 307         #if defined(PLATFORM_ESP32)
 308         devicesTriggerEvent();
 309         #endif
 310     }
 311 }
 312
 313 bool CRSFHandset::processInternalCrsfPackage(uint8_t *package)
 314 {
 315     const crsf_ext_header_t *header = (crsf_ext_header_t *)package;
 316     const crsf_frame_type_e packetType = (crsf_frame_type_e)header->type;
 317
 318     // Enter Binding Mode
 319     if (packetType == CRSF_FRAMETYPE_COMMAND
 320         && header->frame_size >= 6 // official CRSF is 7 bytes with two CRCs
 321         && header->dest_addr == CRSF_ADDRESS_CRSF_TRANSMITTER
 322         && header->orig_addr == CRSF_ADDRESS_RADIO_TRANSMITTER
 323         && header->payload[0] == CRSF_COMMAND_SUBCMD_RX
 324         && header->payload[1] == CRSF_COMMAND_SUBCMD_RX_BIND)
 325     {
 326         if (OnBindingCommand) OnBindingCommand();
 327         return true;
 328     }
 329
 330     if (packetType >= CRSF_FRAMETYPE_DEVICE_PING &&
 331         (header->dest_addr == CRSF_ADDRESS_CRSF_TRANSMITTER || header->dest_addr == CRSF_ADDRESS_BROADCAST) &&
 332         (header->orig_addr == CRSF_ADDRESS_RADIO_TRANSMITTER || header->orig_addr == CRSF_ADDRESS_ELRS_LUA))
 333     {
 334         elrsLUAmode = header->orig_addr == CRSF_ADDRESS_ELRS_LUA;
 335
 336         if (packetType == CRSF_FRAMETYPE_COMMAND && header->payload[0] == CRSF_COMMAND_SUBCMD_RX && header->payload[1] == CRSF_COMMAND_MODEL_SELECT_ID)
 337         {
 338             modelId = header->payload[2];
 339             #if defined(PLATFORM_ESP32)
 340             rtcModelId = modelId;
 341             #endif
 342             if (RecvModelUpdate) RecvModelUpdate();
 343         }
 344         else
 345         {
 346             if (RecvParameterUpdate) RecvParameterUpdate(packetType, header->payload[0], header->payload[1]);
 347         }
 348
 349         return true;
 350     }
 351
 352     return false;
 353 }
 354
 355 bool CRSFHandset::ProcessPacket()
 356 {
 357     bool packetReceived = false;
 358
 359     CRSFHandset::dataLastRecv = micros();
 360
 361     if (!controllerConnected)
 362     {
 363         controllerConnected = true;
 364         DBGLN("CRSF UART Connected");
 365         if (connected) connected();
 366     }
 367
 368     const uint8_t packetType = inBuffer.asRCPacket_t.header.type;
 369     uint8_t *SerialInBuffer = inBuffer.asUint8_t;
 370
 371     if (packetType == CRSF_FRAMETYPE_RC_CHANNELS_PACKED)
 372     {
 373         RCdataLastRecv = micros();
 374         RcPacketToChannelsData();
 375         packetReceived = true;
 376     }
 377     // check for all extended frames that are a broadcast or a message to the FC
 378     else if (packetType >= CRSF_FRAMETYPE_DEVICE_PING &&
 379             (SerialInBuffer[3] == CRSF_ADDRESS_FLIGHT_CONTROLLER || SerialInBuffer[3] == CRSF_ADDRESS_BROADCAST || SerialInBuffer[3] == CRSF_ADDRESS_CRSF_RECEIVER))
 380     {
 381         // Some types trigger telemburst to attempt a connection even with telm off
 382         // but for pings (which are sent when the user loads Lua) do not forward
 383         // unless connected
 384         if (ForwardDevicePings || packetType != CRSF_FRAMETYPE_DEVICE_PING)
 385         {
 386             const uint8_t length = inBuffer.asRCPacket_t.header.frame_size + 2;
 387             CRSF::AddMspMessage(length, SerialInBuffer);
 388         }
 389         packetReceived = true;
 390     }
 391
 392     packetReceived |= processInternalCrsfPackage(SerialInBuffer);
 393
 394     return packetReceived;
 395 }
 396
 397 void CRSFHandset::handleInput()
 398 {
 399     uint8_t *SerialInBuffer = inBuffer.asUint8_t;
 400
 401     if (UARTwdt())
 402     {
 403         return;
 404     }
 405
 406     while (CRSFHandset::Port.available())
 407     {
 408         if (!CRSFframeActive)
 409         {
 410             unsigned char const inChar = CRSFHandset::Port.read();
 411             // stage 1 wait for sync byte //
 412             if (inChar == CRSF_ADDRESS_CRSF_TRANSMITTER ||
 413                 inChar == CRSF_SYNC_BYTE)
 414             {
 415                 // we got sync, reset write pointer
 416                 SerialInPacketPtr = 0;
 417                 SerialInPacketLen = 0;
 418                 CRSFframeActive = true;
 419                 SerialInBuffer[SerialInPacketPtr] = inChar;
 420                 SerialInPacketPtr++;
 421             }
 422         }
 423         else // frame is active so we do the processing
 424         {
 425             // first if things have gone wrong //
 426             if (SerialInPacketPtr > CRSF_MAX_PACKET_LEN - 1)
 427             {
 428                 // we reached the maximum allowable packet length, so start again because shit fucked up hey.
 429                 SerialInPacketPtr = 0;
 430                 SerialInPacketLen = 0;
 431                 CRSFframeActive = false;
 432                 return;
 433             }
 434
 435             // special case where we save the expected pkt len to buffer //
 436             if (SerialInPacketPtr == 1)
 437             {
 438                 unsigned char const inChar = CRSFHandset::Port.read();
 439                 if (inChar <= CRSF_MAX_PACKET_LEN)
 440                 {
 441                     SerialInPacketLen = inChar;
 442                     SerialInBuffer[SerialInPacketPtr] = inChar;
 443                     SerialInPacketPtr++;
 444                 }
 445                 else
 446                 {
 447                     SerialInPacketPtr = 0;
 448                     SerialInPacketLen = 0;
 449                     CRSFframeActive = false;
 450                     return;
 451                 }
 452             }
 453
 454             int toRead = (SerialInPacketLen + 2) - SerialInPacketPtr;
 455             #if defined(PLATFORM_ESP32)
 456             auto count = (int)CRSFHandset::Port.read(&SerialInBuffer[SerialInPacketPtr], toRead);
 457             #else
 458             int count = 0;
 459             auto avail = (int)CRSFHandset::Port.available();
 460             while (count < toRead && count < avail)
 461             {
 462                 SerialInBuffer[SerialInPacketPtr + count] = CRSFHandset::Port.read();
 463                 count++;
 464             }
 465             #endif
 466             SerialInPacketPtr += count;
 467
 468             if (SerialInPacketPtr >= (SerialInPacketLen + 2)) // plus 2 because the packlen is referenced from the start of the 'type' flag, IE there are an extra 2 bytes.
 469             {
 470                 uint8_t CalculatedCRC = crsf_crc.calc(SerialInBuffer + 2, SerialInPacketPtr - 3);
 471
 472                 if (CalculatedCRC == SerialInBuffer[SerialInPacketPtr-1])
 473                 {
 474                     GoodPktsCount++;
 475                     if (ProcessPacket())
 476                     {
 477                         //delayMicroseconds(50);
 478                         handleOutput();
 479                         if (RCdataCallback) RCdataCallback();
 480                     }
 481                 }
 482                 else
 483                 {
 484                     DBGLN("UART CRC failure");
 485                     // cleanup input buffer
 486                     flush_port_input();
 487                     BadPktsCount++;
 488                 }
 489                 CRSFframeActive = false;
 490                 SerialInPacketPtr = 0;
 491                 SerialInPacketLen = 0;
 492             }
 493         }
 494     }
 495 }
 496
 497 void CRSFHandset::handleOutput()
 498 {
 499     static uint8_t CRSFoutBuffer[CRSF_MAX_PACKET_LEN] = {0};
 500     // both static to split up larger packages
 501     static uint8_t packageLengthRemaining = 0;
 502     static uint8_t sendingOffset = 0;
 503
 504     if (!controllerConnected)
 505     {
 506         SerialOutFIFO.lock();
 507         SerialOutFIFO.flush();
 508         SerialOutFIFO.unlock();
 509         return;
 510     }
 511
 512     if (packageLengthRemaining == 0 && SerialOutFIFO.size() == 0)
 513     {
 514         sendSyncPacketToTX(); // calculate mixer sync packet if needed
 515     }
 516
 517     // if partial package remaining, or data in the output FIFO that needs to be written
 518     if (packageLengthRemaining > 0 || SerialOutFIFO.size() > 0) {
 519         duplex_set_TX();
 520
 521         uint8_t periodBytesRemaining = maxPeriodBytes;
 522         while (periodBytesRemaining)
 523         {
 524             SerialOutFIFO.lock();
 525             // no package is in transit so get new data from the fifo
 526             if (packageLengthRemaining == 0) {
 527                 packageLengthRemaining = SerialOutFIFO.pop();
 528                 SerialOutFIFO.popBytes(CRSFoutBuffer, packageLengthRemaining);
 529                 sendingOffset = 0;
 530             }
 531             SerialOutFIFO.unlock();
 532
 533             // if the package is long we need to split it up so it fits in the sending interval
 534             uint8_t writeLength;
 535             if (packageLengthRemaining > periodBytesRemaining) {
 536                 if (periodBytesRemaining < maxPeriodBytes) {  // only start to send a split packet as the first packet
 537                     break;
 538                 }
 539                 writeLength = periodBytesRemaining;
 540             } else {
 541                 writeLength = packageLengthRemaining;
 542             }
 543
 544             // write the packet out, if it's a large package the offset holds the starting position
 545             CRSFHandset::Port.write(CRSFoutBuffer + sendingOffset, writeLength);
 546             if (CRSFHandset::PortSecondary)
 547                 CRSFHandset::PortSecondary->write(CRSFoutBuffer + sendingOffset, writeLength);
 548
 549             sendingOffset += writeLength;
 550             packageLengthRemaining -= writeLength;
 551             periodBytesRemaining -= writeLength;
 552
 553             // No bytes left to send, exit
 554             if (SerialOutFIFO.size() == 0)
 555                 break;
 556         }
 557         CRSFHandset::Port.flush();
 558         duplex_set_RX();
 559
 560         // make sure there is no garbage on the UART left over
 561         flush_port_input();
 562     }
 563 }
 564
 565 void CRSFHandset::duplex_set_RX() const
 566 {
 567 #if defined(PLATFORM_ESP32)
 568     if (GPIO_PIN_RCSIGNAL_TX == GPIO_PIN_RCSIGNAL_RX)
 569     {
 570         ESP_ERROR_CHECK(gpio_set_direction((gpio_num_t)GPIO_PIN_RCSIGNAL_RX, GPIO_MODE_INPUT));
 571         if (UARTinverted)
 572         {
 573             gpio_matrix_in((gpio_num_t)GPIO_PIN_RCSIGNAL_RX, U0RXD_IN_IDX, true);
 574             gpio_pulldown_en((gpio_num_t)GPIO_PIN_RCSIGNAL_RX);
 575             gpio_pullup_dis((gpio_num_t)GPIO_PIN_RCSIGNAL_RX);
 576         }
 577         else
 578         {
 579             gpio_matrix_in((gpio_num_t)GPIO_PIN_RCSIGNAL_RX, U0RXD_IN_IDX, false);
 580             gpio_pullup_en((gpio_num_t)GPIO_PIN_RCSIGNAL_RX);
 581             gpio_pulldown_dis((gpio_num_t)GPIO_PIN_RCSIGNAL_RX);
 582         }
 583     }
 584 #elif defined(PLATFORM_ESP8266)
 585     // Enable loopback on UART0 to connect the RX pin to the TX pin (not done, connection is full duplex uninverted)
 586     //USC0(UART0) |= BIT(UCLBE);
 587 #elif defined(GPIO_PIN_BUFFER_OE) && (GPIO_PIN_BUFFER_OE != UNDEF_PIN)
 588     digitalWrite(GPIO_PIN_BUFFER_OE, LOW ^ GPIO_PIN_BUFFER_OE_INVERTED);
 589 #elif (GPIO_PIN_RCSIGNAL_TX == GPIO_PIN_RCSIGNAL_RX)
 590     CRSFHandset::Port.enableHalfDuplexRx();
 591 #endif
 592 }
 593
 594 void CRSFHandset::duplex_set_TX() const
 595 {
 596 #if defined(PLATFORM_ESP32)
 597     if (GPIO_PIN_RCSIGNAL_TX == GPIO_PIN_RCSIGNAL_RX)
 598     {
 599         ESP_ERROR_CHECK(gpio_set_pull_mode((gpio_num_t)GPIO_PIN_RCSIGNAL_TX, GPIO_FLOATING));
 600         ESP_ERROR_CHECK(gpio_set_pull_mode((gpio_num_t)GPIO_PIN_RCSIGNAL_RX, GPIO_FLOATING));
 601         if (UARTinverted)
 602         {
 603             ESP_ERROR_CHECK(gpio_set_level((gpio_num_t)GPIO_PIN_RCSIGNAL_TX, 0));
 604             ESP_ERROR_CHECK(gpio_set_direction((gpio_num_t)GPIO_PIN_RCSIGNAL_TX, GPIO_MODE_OUTPUT));
 605             constexpr uint8_t MATRIX_DETACH_IN_LOW = 0x30; // routes 0 to matrix slot
 606             gpio_matrix_in(MATRIX_DETACH_IN_LOW, U0RXD_IN_IDX, false); // Disconnect RX from all pads
 607             gpio_matrix_out((gpio_num_t)GPIO_PIN_RCSIGNAL_TX, U0TXD_OUT_IDX, true, false);
 608         }
 609         else
 610         {
 611             ESP_ERROR_CHECK(gpio_set_level((gpio_num_t)GPIO_PIN_RCSIGNAL_TX, 1));
 612             ESP_ERROR_CHECK(gpio_set_direction((gpio_num_t)GPIO_PIN_RCSIGNAL_TX, GPIO_MODE_OUTPUT));
 613             constexpr uint8_t MATRIX_DETACH_IN_HIGH = 0x38; // routes 1 to matrix slot
 614             gpio_matrix_in(MATRIX_DETACH_IN_HIGH, U0RXD_IN_IDX, false); // Disconnect RX from all pads
 615             gpio_matrix_out((gpio_num_t)GPIO_PIN_RCSIGNAL_TX, U0TXD_OUT_IDX, false, false);
 616         }
 617     }
 618 #elif defined(PLATFORM_ESP8266)
 619     // Disable loopback to disconnect the RX pin from the TX pin (not done, connection is full duplex uninverted)
 620     //USC0(UART0) &= ~BIT(UCLBE);
 621 #elif defined(GPIO_PIN_BUFFER_OE) && (GPIO_PIN_BUFFER_OE != UNDEF_PIN)
 622     digitalWrite(GPIO_PIN_BUFFER_OE, HIGH ^ GPIO_PIN_BUFFER_OE_INVERTED);
 623 #elif (GPIO_PIN_RCSIGNAL_TX == GPIO_PIN_RCSIGNAL_RX)
 624     // writing to the port switches the mode
 625 #endif
 626 }
 627
 628 void ICACHE_RAM_ATTR CRSFHandset::adjustMaxPacketSize()
 629 {
 630     // baud / 10bits-per-byte / 2 windows (1RX, 1TX) / rate * 0.80 (leeway)
 631     maxPeriodBytes = UARTrequestedBaud / 10 / 2 / (1000000/RequestedRCpacketInterval) * 80 / 100;
 632     maxPeriodBytes = maxPeriodBytes > HANDSET_TELEMETRY_FIFO_SIZE ? HANDSET_TELEMETRY_FIFO_SIZE : maxPeriodBytes;
 633     // we need a minimum of 10 bytes otherwise our LUA will not make progress and at 8 we'd get a divide by 0!
 634     maxPeriodBytes = maxPeriodBytes < 10 ? 10 : maxPeriodBytes;
 635     maxPacketBytes = maxPeriodBytes > CRSF_MAX_PACKET_LEN ? CRSF_MAX_PACKET_LEN : maxPeriodBytes;
 636     DBGLN("Adjusted max packet size %u-%u", maxPacketBytes, maxPeriodBytes);
 637 }
 638
 639 #if defined(PLATFORM_ESP32_S3)
 640 uint32_t CRSFHandset::autobaud()
 641 {
 642     static enum { INIT, MEASURED, INVERTED } state;
 643
 644     if (state == MEASURED)
 645     {
 646         UARTinverted = !UARTinverted;
 647         state = INVERTED;
 648         return UARTrequestedBaud;
 649     }
 650     if (state == INVERTED)
 651     {
 652         UARTinverted = !UARTinverted;
 653         state = INIT;
 654     }
 655
 656     if (REG_GET_BIT(UART_CONF0_REG(0), UART_AUTOBAUD_EN) == 0)
 657     {
 658         REG_WRITE(UART_RX_FILT_REG(0), (4 << UART_GLITCH_FILT_S) | UART_GLITCH_FILT_EN); // enable, glitch filter 4
 659         REG_WRITE(UART_LOWPULSE_REG(0), 4095); // reset register to max value
 660         REG_WRITE(UART_HIGHPULSE_REG(0), 4095); // reset register to max value
 661         REG_SET_BIT(UART_CONF0_REG(0), UART_AUTOBAUD_EN); // enable autobaud
 662         return 400000;
 663     }
 664     if (REG_GET_BIT(UART_CONF0_REG(0), UART_AUTOBAUD_EN) && REG_READ(UART_RXD_CNT_REG(0)) < 300)
 665     {
 666         return 400000;
 667     }
 668
 669     state = MEASURED;
 670
 671     const uint32_t low_period  = REG_READ(UART_LOWPULSE_REG(0));
 672     const uint32_t high_period = REG_READ(UART_HIGHPULSE_REG(0));
 673     REG_CLR_BIT(UART_CONF0_REG(0), UART_AUTOBAUD_EN); // disable autobaud
 674     REG_CLR_BIT(UART_RX_FILT_REG(0), UART_GLITCH_FILT_EN); // disable glitch filtering
 675
 676     DBGLN("autobaud: low %d, high %d", low_period, high_period);
 677     // According to the tecnnical reference
 678     const int32_t calulatedBaud = UART_CLK_FREQ / (low_period + high_period + 2);
 679     int32_t bestBaud = TxToHandsetBauds[0];
 680     for(int i=0 ; i<ARRAY_SIZE(TxToHandsetBauds) ; i++)
 681     {
 682         if (abs(calulatedBaud - bestBaud) > abs(calulatedBaud - TxToHandsetBauds[i]))
 683         {
 684             bestBaud = TxToHandsetBauds[i];
 685         }
 686     }
 687     return bestBaud;
 688 }
 689 #elif defined(PLATFORM_ESP32)
 690 uint32_t CRSFHandset::autobaud()
 691 {
 692     static enum { INIT, MEASURED, INVERTED } state;
 693
 694     if (state == MEASURED) {
 695         UARTinverted = !UARTinverted;
 696         state = INVERTED;
 697         return UARTrequestedBaud;
 698     }
 699     if (state == INVERTED) {
 700         UARTinverted = !UARTinverted;
 701         state = INIT;
 702     }
 703
 704     if (REG_GET_BIT(UART_AUTOBAUD_REG(0), UART_AUTOBAUD_EN) == 0) {
 705         REG_WRITE(UART_AUTOBAUD_REG(0), 4 << UART_GLITCH_FILT_S | UART_AUTOBAUD_EN);    // enable, glitch filter 4
 706         return 400000;
 707     }
 708     if (REG_GET_BIT(UART_AUTOBAUD_REG(0), UART_AUTOBAUD_EN) && REG_READ(UART_RXD_CNT_REG(0)) < 300)
 709     {
 710         return 400000;
 711     }
 712
 713     state = MEASURED;
 714
 715     auto low_period  = (int32_t)REG_READ(UART_LOWPULSE_REG(0));
 716     auto high_period = (int32_t)REG_READ(UART_HIGHPULSE_REG(0));
 717     REG_CLR_BIT(UART_AUTOBAUD_REG(0), UART_AUTOBAUD_EN);   // disable autobaud
 718
 719     DBGLN("autobaud: low %d, high %d", low_period, high_period);
 720     // sample code at https://github.com/espressif/esp-idf/issues/3336
 721     // says baud rate = 80000000/min(UART_LOWPULSE_REG, UART_HIGHPULSE_REG);
 722     // Based on testing use max and add 2 for lowest deviation
 723     int32_t calculatedBaud = 80000000 / (max(low_period, high_period) + 3);
 724     auto bestBaud = TxToHandsetBauds[0];
 725     for(int TxToHandsetBaud : TxToHandsetBauds)
 726     {
 727         if (abs(calculatedBaud - bestBaud) > abs(calculatedBaud - (int32_t)TxToHandsetBaud))
 728         {
 729             bestBaud = (int32_t)TxToHandsetBaud;
 730         }
 731     }
 732     return bestBaud;
 733 }
 734 #else
 735 uint32_t CRSFHandset::autobaud() {
 736     UARTcurrentBaudIdx = (UARTcurrentBaudIdx + 1) % ARRAY_SIZE(TxToHandsetBauds);
 737     return TxToHandsetBauds[UARTcurrentBaudIdx];
 738 }
 739 #endif
 740
 741 bool CRSFHandset::UARTwdt()
 742 {
 743     bool retval = false;
 744 #if !defined(DEBUG_TX_FREERUN)
 745     uint32_t now = millis();
 746     if (now >= (UARTwdtLastChecked + UARTwdtInterval))
 747     {
 748         if (BadPktsCount >= GoodPktsCount || !controllerConnected)
 749         {
 750             DBGLN("Too many bad UART RX packets!");
 751
 752             if (controllerConnected)
 753             {
 754                 DBGLN("CRSF UART Disconnected");
 755                 if (disconnected) disconnected();
 756                 controllerConnected = false;
 757             }
 758
 759             UARTrequestedBaud = autobaud();
 760             if (UARTrequestedBaud != 0)
 761             {
 762                 DBGLN("UART WDT: Switch to: %d baud", UARTrequestedBaud);
 763
 764                 adjustMaxPacketSize();
 765
 766                 SerialOutFIFO.flush();
 767 #if defined(PLATFORM_ESP8266) || defined(PLATFORM_ESP32)
 768                 CRSFHandset::Port.flush();
 769                 CRSFHandset::Port.updateBaudRate(UARTrequestedBaud);
 770 #elif defined(TARGET_TX_GHOST)
 771                 CRSFHandset::Port.begin(UARTrequestedBaud);
 772                 USART1->CR1 &= ~USART_CR1_UE;
 773                 USART1->CR3 |= USART_CR3_HDSEL;
 774                 USART1->CR2 |= USART_CR2_RXINV | USART_CR2_TXINV | USART_CR2_SWAP; //inverted/swapped
 775                 USART1->CR1 |= USART_CR1_UE;
 776 #elif defined(TARGET_TX_FM30_MINI)
 777                 CRSFHandset::Port.begin(UARTrequestedBaud);
 778                 LL_GPIO_SetPinPull(GPIOA, GPIO_PIN_2, LL_GPIO_PULL_DOWN); // default is PULLUP
 779                 USART2->CR1 &= ~USART_CR1_UE;
 780                 USART2->CR2 |= USART_CR2_RXINV | USART_CR2_TXINV; //inverted
 781                 USART2->CR1 |= USART_CR1_UE;
 782 #else
 783                 CRSFHandset::Port.begin(UARTrequestedBaud);
 784 #endif
 785                 duplex_set_RX();
 786                 // cleanup input buffer
 787                 flush_port_input();
 788             }
 789             retval = true;
 790         }
 791 #ifdef DEBUG_OPENTX_SYNC
 792         if (abs((int)((1000000 / (ExpressLRS_currAirRate_Modparams->interval * ExpressLRS_currAirRate_Modparams->numOfSends)) - (int)GoodPktsCount)) > 1)
 793 #endif
 794             DBGLN("UART STATS Bad:Good = %u:%u", BadPktsCount, GoodPktsCount);
 795
 796         UARTwdtLastChecked = now;
 797         if (retval)
 798         {
 799             // Speed up the cycling
 800             UARTwdtLastChecked -= 3 * (UARTwdtInterval >> 2);
 801         }
 802
 803         GoodPktsCountResult = GoodPktsCount;
 804         BadPktsCountResult = BadPktsCount;
 805         BadPktsCount = 0;
 806         GoodPktsCount = 0;
 807     }
 808 #endif
 809     return retval;
 810 }
 811
 812 #endif // CRSF_TX_MODULE