src/lib/CRSF/CRSF.cpp

   1 #include "CRSF.h"
   2 #include "FIFO.h"
   3 #include "telemetry_protocol.h"
   4 #include "logging.h"
   5 #include "helpers.h"
   6
   7 #if defined(PLATFORM_ESP32)
   8 #include "device.h"
   9
  10 HardwareSerial CRSF::Port = HardwareSerial(1);
  11 portMUX_TYPE FIFOmux = portMUX_INITIALIZER_UNLOCKED;
  12 #elif defined(PLATFORM_ESP8266)
  13 HardwareSerial CRSF::Port = Serial;
  14 #elif CRSF_TX_MODULE_STM32
  15 HardwareSerial CRSF::Port(GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX);
  16 #if defined(STM32F3) || defined(STM32F3xx)
  17 #include "stm32f3xx_hal.h"
  18 #include "stm32f3xx_hal_gpio.h"
  19 #elif defined(STM32F1) || defined(STM32F1xx)
  20 #include "stm32f1xx_hal.h"
  21 #include "stm32f1xx_hal_gpio.h"
  22 #endif
  23 #elif defined(TARGET_NATIVE)
  24 HardwareSerial CRSF::Port = Serial;
  25 #endif
  26 Stream *CRSF::PortSecondary;
  27
  28 GENERIC_CRC8 crsf_crc(CRSF_CRC_POLY);
  29
  30 ///Out FIFO to buffer messages///
  31 static FIFO SerialOutFIFO;
  32
  33 volatile uint16_t CRSF::ChannelDataIn[16] = {0};
  34
  35 inBuffer_U CRSF::inBuffer;
  36
  37 volatile crsfPayloadLinkstatistics_s CRSF::LinkStatistics;
  38
  39 #if CRSF_TX_MODULE
  40 #define HANDSET_TELEMETRY_FIFO_SIZE 128 // this is the smallest telemetry FIFO size in ETX with CRSF defined
  41
  42 static FIFO MspWriteFIFO;
  43
  44 void inline CRSF::nullCallback(void) {}
  45
  46 void (*CRSF::disconnected)() = &nullCallback; // called when CRSF stream is lost
  47 void (*CRSF::connected)() = &nullCallback;    // called when CRSF stream is regained
  48
  49 void (*CRSF::RecvParameterUpdate)() = &nullCallback; // called when recv parameter update req, ie from LUA
  50 void (*CRSF::RecvModelUpdate)() = &nullCallback; // called when model id cahnges, ie command from Radio
  51 void (*CRSF::RCdataCallback)() = &nullCallback; // called when there is new RC data
  52
  53 /// UART Handling ///
  54 volatile uint8_t CRSF::SerialInPacketLen = 0; // length of the CRSF packet as measured
  55 volatile uint8_t CRSF::SerialInPacketPtr = 0; // index where we are reading/writing
  56 volatile bool CRSF::CRSFframeActive = false; //since we get a copy of the serial data use this flag to know when to ignore it
  57
  58 uint32_t CRSF::GoodPktsCountResult = 0;
  59 uint32_t CRSF::BadPktsCountResult = 0;
  60
  61 uint8_t CRSF::modelId = 0;
  62 bool CRSF::ForwardDevicePings = false;
  63 volatile uint8_t CRSF::ParameterUpdateData[3] = {0};
  64 volatile bool CRSF::elrsLUAmode = false;
  65
  66 /// OpenTX mixer sync ///
  67 volatile uint32_t CRSF::OpenTXsyncLastSent = 0;
  68 uint32_t CRSF::RequestedRCpacketInterval = 5000; // default to 200hz as per 'normal'
  69 volatile uint32_t CRSF::RCdataLastRecv = 0;
  70 volatile int32_t CRSF::OpenTXsyncOffset = 0;
  71 bool CRSF::OpentxSyncActive = true;
  72
  73 #ifdef FEATURE_OPENTX_SYNC_AUTOTUNE
  74 #define AutoSyncWaitPeriod 2000
  75 uint32_t CRSF::OpenTXsyncOffsetSafeMargin = 1000;
  76 static LPF LPF_OPENTX_SYNC_MARGIN(3);
  77 static LPF LPF_OPENTX_SYNC_OFFSET(3);
  78 uint32_t CRSF::SyncWaitPeriodCounter = 0;
  79 #else
  80 uint32_t CRSF::OpenTXsyncOffsetSafeMargin = 4000; // 400us
  81 #endif
  82
  83 /// UART Handling ///
  84 uint32_t CRSF::GoodPktsCount = 0;
  85 uint32_t CRSF::BadPktsCount = 0;
  86 uint32_t CRSF::UARTwdtLastChecked;
  87
  88 uint8_t CRSF::CRSFoutBuffer[CRSF_MAX_PACKET_LEN] = {0};
  89 uint8_t CRSF::maxPacketBytes = CRSF_MAX_PACKET_LEN;
  90 uint32_t CRSF::maxPeriodBytes = CRSF_MAX_PACKET_LEN;
  91 uint32_t CRSF::TxToHandsetBauds[] = {400000, 115200, 5250000, 3750000, 1870000, 921600};
  92 uint8_t CRSF::UARTcurrentBaudIdx = 0;
  93
  94 bool CRSF::CRSFstate = false;
  95
  96 // for the UART wdt, every 1000ms we change bauds when connect is lost
  97 #define UARTwdtInterval 1000
  98
  99 uint8_t CRSF::MspData[ELRS_MSP_BUFFER] = {0};
 100 uint8_t CRSF::MspDataLength = 0;
 101 #endif // CRSF_TX_MODULE
 102
 103 #ifdef CRSF_RX_MODULE
 104 crsf_channels_s CRSF::PackedRCdataOut;
 105 #endif
 106
 107 void CRSF::Begin()
 108 {
 109     DBGLN("About to start CRSF task...");
 110
 111 #if CRSF_TX_MODULE
 112     UARTwdtLastChecked = millis() + UARTwdtInterval; // allows a delay before the first time the UARTwdt() function is called
 113
 114 #if defined(PLATFORM_ESP32)
 115     // disableCore0WDT(); PAK
 116     portDISABLE_INTERRUPTS();
 117     CRSF::Port.begin(TxToHandsetBauds[UARTcurrentBaudIdx], SERIAL_8N1,
 118                      GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX,
 119                      false, 500);
 120     CRSF::duplex_set_RX();
 121     portENABLE_INTERRUPTS();
 122     flush_port_input();
 123
 124 #elif defined(PLATFORM_ESP8266)
 125     CRSF::Port.flush();
 126     CRSF::Port.updateBaudRate(TxToHandsetBauds[UARTcurrentBaudIdx]);
 127     // Invert RX/TX
 128     USC0(UART0) |= BIT(UCRXI) | BIT(UCTXI);
 129     // No log message because this is our only UART
 130
 131 #elif defined(PLATFORM_STM32)
 132     DBGLN("Start STM32 R9M TX CRSF UART");
 133
 134     CRSF::Port.setTx(GPIO_PIN_RCSIGNAL_TX);
 135     CRSF::Port.setRx(GPIO_PIN_RCSIGNAL_RX);
 136
 137     #if defined(GPIO_PIN_BUFFER_OE) && (GPIO_PIN_BUFFER_OE != UNDEF_PIN)
 138     pinMode(GPIO_PIN_BUFFER_OE, OUTPUT);
 139     digitalWrite(GPIO_PIN_BUFFER_OE, LOW ^ GPIO_PIN_BUFFER_OE_INVERTED); // RX mode default
 140     #elif (GPIO_PIN_RCSIGNAL_TX == GPIO_PIN_RCSIGNAL_RX)
 141     CRSF::Port.setHalfDuplex();
 142     #endif
 143
 144     CRSF::Port.begin(TxToHandsetBauds[UARTcurrentBaudIdx]);
 145
 146 #if defined(TARGET_TX_GHOST)
 147     USART1->CR1 &= ~USART_CR1_UE;
 148     USART1->CR3 |= USART_CR3_HDSEL;
 149     USART1->CR2 |= USART_CR2_RXINV | USART_CR2_TXINV | USART_CR2_SWAP; //inverted/swapped
 150     USART1->CR1 |= USART_CR1_UE;
 151 #endif
 152 #if defined(TARGET_TX_FM30_MINI)
 153     LL_GPIO_SetPinPull(GPIOA, GPIO_PIN_2, LL_GPIO_PULL_DOWN); // default is PULLUP
 154     USART2->CR1 &= ~USART_CR1_UE;
 155     USART2->CR2 |= USART_CR2_RXINV | USART_CR2_TXINV; //inverted
 156     USART2->CR1 |= USART_CR1_UE;
 157 #endif
 158     DBGLN("STM32 CRSF UART LISTEN TASK STARTED");
 159     CRSF::Port.flush();
 160     flush_port_input();
 161 #endif
 162
 163 #endif // CRSF_TX_MODULE
 164
 165     //The master module requires that the serial communication is bidirectional
 166     //The Reciever uses seperate rx and tx pins
 167 }
 168
 169 void CRSF::End()
 170 {
 171 #if CRSF_TX_MODULE
 172     uint32_t startTime = millis();
 173     while (SerialOutFIFO.peek() > 0)
 174     {
 175         handleUARTin();
 176         if (millis() - startTime > 1000)
 177         {
 178             break;
 179         }
 180     }
 181     //CRSF::Port.end(); // don't call seria.end(), it causes some sort of issue with the 900mhz hardware using gpio2 for serial
 182     DBGLN("CRSF UART END");
 183 #endif // CRSF_TX_MODULE
 184 }
 185
 186 void CRSF::flush_port_input(void)
 187 {
 188     // Make sure there is no garbage on the UART at the start
 189     while (CRSF::Port.available())
 190     {
 191         CRSF::Port.read();
 192     }
 193 }
 194
 195 #if CRSF_TX_MODULE
 196 void ICACHE_RAM_ATTR CRSF::sendLinkStatisticsToTX()
 197 {
 198     if (!CRSF::CRSFstate)
 199     {
 200         return;
 201     }
 202
 203     constexpr uint8_t outBuffer[4] = {
 204         LinkStatisticsFrameLength + 4,
 205         CRSF_ADDRESS_RADIO_TRANSMITTER,
 206         LinkStatisticsFrameLength + 2,
 207         CRSF_FRAMETYPE_LINK_STATISTICS
 208     };
 209
 210     uint8_t crc = crsf_crc.calc(outBuffer[3]);
 211     crc = crsf_crc.calc((byte *)&LinkStatistics, LinkStatisticsFrameLength, crc);
 212
 213 #ifdef PLATFORM_ESP32
 214     portENTER_CRITICAL(&FIFOmux);
 215 #endif
 216     SerialOutFIFO.pushBytes(outBuffer, sizeof(outBuffer));
 217     SerialOutFIFO.pushBytes((byte *)&LinkStatistics, LinkStatisticsFrameLength);
 218     SerialOutFIFO.push(crc);
 219 #ifdef PLATFORM_ESP32
 220     portEXIT_CRITICAL(&FIFOmux);
 221 #endif
 222 }
 223
 224 /**
 225  * Build a an extended type packet and queue it in the SerialOutFIFO
 226  * This is just a regular packet with 2 extra bytes with the sub src and target
 227  **/
 228 void CRSF::packetQueueExtended(uint8_t type, void *data, uint8_t len)
 229 {
 230     if (!CRSF::CRSFstate)
 231         return;
 232
 233     uint8_t buf[6] = {
 234         (uint8_t)(len + 6),
 235         CRSF_ADDRESS_RADIO_TRANSMITTER,
 236         (uint8_t)(len + 4),
 237         type,
 238         CRSF_ADDRESS_RADIO_TRANSMITTER,
 239         CRSF_ADDRESS_CRSF_TRANSMITTER
 240     };
 241
 242     // CRC - Starts at type, ends before CRC
 243     uint8_t crc = crsf_crc.calc(&buf[3], sizeof(buf)-3);
 244     crc = crsf_crc.calc((byte *)data, len, crc);
 245
 246 #ifdef PLATFORM_ESP32
 247     portENTER_CRITICAL(&FIFOmux);
 248 #endif
 249     SerialOutFIFO.pushBytes(buf, sizeof(buf));
 250     SerialOutFIFO.pushBytes((byte *)data, len);
 251     SerialOutFIFO.push(crc);
 252 #ifdef PLATFORM_ESP32
 253     portEXIT_CRITICAL(&FIFOmux);
 254 #endif
 255 }
 256
 257 void ICACHE_RAM_ATTR CRSF::sendTelemetryToTX(uint8_t *data)
 258 {
 259     if (data[CRSF_TELEMETRY_LENGTH_INDEX] > CRSF_PAYLOAD_SIZE_MAX)
 260     {
 261         ERRLN("too large");
 262         return;
 263     }
 264
 265     if (CRSF::CRSFstate)
 266     {
 267         data[0] = CRSF_ADDRESS_RADIO_TRANSMITTER;
 268 #ifdef PLATFORM_ESP32
 269         portENTER_CRITICAL(&FIFOmux);
 270 #endif
 271         SerialOutFIFO.push(CRSF_FRAME_SIZE(data[CRSF_TELEMETRY_LENGTH_INDEX])); // length
 272         SerialOutFIFO.pushBytes(data, CRSF_FRAME_SIZE(data[CRSF_TELEMETRY_LENGTH_INDEX]));
 273 #ifdef PLATFORM_ESP32
 274         portEXIT_CRITICAL(&FIFOmux);
 275 #endif
 276     }
 277 }
 278
 279 void ICACHE_RAM_ATTR CRSF::setSyncParams(uint32_t PacketInterval)
 280 {
 281     CRSF::RequestedRCpacketInterval = PacketInterval;
 282 #ifdef FEATURE_OPENTX_SYNC_AUTOTUNE
 283     CRSF::SyncWaitPeriodCounter = millis();
 284     CRSF::OpenTXsyncOffsetSafeMargin = 1000;
 285     LPF_OPENTX_SYNC_OFFSET.init(0);
 286     LPF_OPENTX_SYNC_MARGIN.init(0);
 287 #endif
 288     adjustMaxPacketSize();
 289 }
 290
 291 uint32_t ICACHE_RAM_ATTR CRSF::GetRCdataLastRecv()
 292 {
 293     return CRSF::RCdataLastRecv;
 294 }
 295
 296 void ICACHE_RAM_ATTR CRSF::JustSentRFpacket()
 297 {
 298     CRSF::OpenTXsyncOffset = micros() - CRSF::RCdataLastRecv;
 299
 300     if (CRSF::OpenTXsyncOffset > (int32_t)CRSF::RequestedRCpacketInterval) // detect overrun case when the packet arrives too late and caculate negative offsets.
 301     {
 302         CRSF::OpenTXsyncOffset = -(CRSF::OpenTXsyncOffset % CRSF::RequestedRCpacketInterval);
 303 #ifdef FEATURE_OPENTX_SYNC_AUTOTUNE
 304         // wait until we stablize after changing pkt rate
 305         if (millis() > (CRSF::SyncWaitPeriodCounter + AutoSyncWaitPeriod))
 306         {
 307             CRSF::OpenTXsyncOffsetSafeMargin = LPF_OPENTX_SYNC_MARGIN.update((CRSF::OpenTXsyncOffsetSafeMargin - OpenTXsyncOffset) + 100); // take worst case plus 50us
 308         }
 309 #endif
 310     }
 311
 312 #ifdef FEATURE_OPENTX_SYNC_AUTOTUNE
 313     if (CRSF::OpenTXsyncOffsetSafeMargin > 4000)
 314     {
 315         CRSF::OpenTXsyncOffsetSafeMargin = 4000; // hard limit at no tune default
 316     }
 317     else if (CRSF::OpenTXsyncOffsetSafeMargin < 1000)
 318     {
 319         CRSF::OpenTXsyncOffsetSafeMargin = 1000; // hard limit at no tune default
 320     }
 321 #endif
 322     //DBGLN("%d, %d", CRSF::OpenTXsyncOffset, CRSF::OpenTXsyncOffsetSafeMargin / 10);
 323 }
 324
 325 void CRSF::disableOpentxSync()
 326 {
 327     OpentxSyncActive = false;
 328 }
 329
 330 void CRSF::enableOpentxSync()
 331 {
 332     OpentxSyncActive = true;
 333 }
 334
 335 void ICACHE_RAM_ATTR CRSF::sendSyncPacketToTX() // in values in us.
 336 {
 337     uint32_t now = millis();
 338     if (CRSF::CRSFstate && now >= (OpenTXsyncLastSent + OpenTXsyncPacketInterval))
 339     {
 340         uint32_t packetRate = CRSF::RequestedRCpacketInterval * 10; //convert from us to right format
 341         int32_t offset = CRSF::OpenTXsyncOffset * 10 - CRSF::OpenTXsyncOffsetSafeMargin; // + 400us offset that that opentx always has some headroom
 342
 343         struct otxSyncData {
 344             uint8_t extendedType; // CRSF_FRAMETYPE_OPENTX_SYNC
 345             uint32_t rate; // Big-Endian
 346             uint32_t offset; // Big-Endian
 347         } PACKED;
 348
 349         uint8_t buffer[sizeof(otxSyncData)];
 350         struct otxSyncData * const sync = (struct otxSyncData * const)buffer;
 351
 352         sync->extendedType = CRSF_FRAMETYPE_OPENTX_SYNC;
 353         sync->rate = htobe32(packetRate);
 354         sync->offset = htobe32(offset);
 355
 356         packetQueueExtended(CRSF_FRAMETYPE_RADIO_ID, buffer, sizeof(buffer));
 357
 358         OpenTXsyncLastSent = now;
 359     }
 360 }
 361
 362 void ICACHE_RAM_ATTR CRSF::GetChannelDataIn() // data is packed as 11 bits per channel
 363 {
 364     const volatile crsf_channels_t *rcChannels = &CRSF::inBuffer.asRCPacket_t.channels;
 365     ChannelDataIn[0] = (rcChannels->ch0);
 366     ChannelDataIn[1] = (rcChannels->ch1);
 367     ChannelDataIn[2] = (rcChannels->ch2);
 368     ChannelDataIn[3] = (rcChannels->ch3);
 369     ChannelDataIn[4] = (rcChannels->ch4);
 370     ChannelDataIn[5] = (rcChannels->ch5);
 371     ChannelDataIn[6] = (rcChannels->ch6);
 372     ChannelDataIn[7] = (rcChannels->ch7);
 373     ChannelDataIn[8] = (rcChannels->ch8);
 374     ChannelDataIn[9] = (rcChannels->ch9);
 375     ChannelDataIn[10] = (rcChannels->ch10);
 376     ChannelDataIn[11] = (rcChannels->ch11);
 377     ChannelDataIn[12] = (rcChannels->ch12);
 378     ChannelDataIn[13] = (rcChannels->ch13);
 379     ChannelDataIn[14] = (rcChannels->ch14);
 380     ChannelDataIn[15] = (rcChannels->ch15);
 381 }
 382
 383 bool ICACHE_RAM_ATTR CRSF::ProcessPacket()
 384 {
 385     bool packetReceived = false;
 386
 387     if (CRSFstate == false)
 388     {
 389         CRSFstate = true;
 390         DBGLN("CRSF UART Connected");
 391
 392 #ifdef FEATURE_OPENTX_SYNC_AUTOTUNE
 393         SyncWaitPeriodCounter = millis(); // set to begin wait for auto sync offset calculation
 394         LPF_OPENTX_SYNC_MARGIN.init(0);
 395         LPF_OPENTX_SYNC_OFFSET.init(0);
 396 #endif // FEATURE_OPENTX_SYNC_AUTOTUNE
 397         connected();
 398     }
 399
 400     const uint8_t packetType = CRSF::inBuffer.asRCPacket_t.header.type;
 401     volatile uint8_t *SerialInBuffer = CRSF::inBuffer.asUint8_t;
 402
 403     if (packetType == CRSF_FRAMETYPE_RC_CHANNELS_PACKED)
 404     {
 405         CRSF::RCdataLastRecv = micros();
 406         GetChannelDataIn();
 407         packetReceived = true;
 408     }
 409     // check for all extended frames that are a broadcast or a message to the FC
 410     else if (packetType >= CRSF_FRAMETYPE_DEVICE_PING &&
 411             (SerialInBuffer[3] == CRSF_ADDRESS_FLIGHT_CONTROLLER || SerialInBuffer[3] == CRSF_ADDRESS_BROADCAST || SerialInBuffer[3] == CRSF_ADDRESS_CRSF_RECEIVER))
 412     {
 413         // Some types trigger telemburst to attempt a connection even with telm off
 414         // but for pings (which are sent when the user loads Lua) do not forward
 415         // unless connected
 416         if (ForwardDevicePings || packetType != CRSF_FRAMETYPE_DEVICE_PING)
 417         {
 418             const uint8_t length = CRSF::inBuffer.asRCPacket_t.header.frame_size + 2;
 419             AddMspMessage(length, SerialInBuffer);
 420         }
 421         packetReceived = true;
 422     }
 423
 424     // always execute this check since broadcast needs to be handeled in all cases
 425     if ((SerialInBuffer[3] == CRSF_ADDRESS_CRSF_TRANSMITTER || SerialInBuffer[3] == CRSF_ADDRESS_BROADCAST) &&
 426         (SerialInBuffer[4] == CRSF_ADDRESS_RADIO_TRANSMITTER || SerialInBuffer[4] == CRSF_ADDRESS_ELRS_LUA))
 427     {
 428         elrsLUAmode = SerialInBuffer[4] == CRSF_ADDRESS_ELRS_LUA;
 429
 430         if (packetType == CRSF_FRAMETYPE_COMMAND && SerialInBuffer[5] == SUBCOMMAND_CRSF && SerialInBuffer[6] == COMMAND_MODEL_SELECT_ID)
 431         {
 432             modelId = SerialInBuffer[7];
 433             RecvModelUpdate();
 434         }
 435         else
 436         {
 437             ParameterUpdateData[0] = packetType;
 438             ParameterUpdateData[1] = SerialInBuffer[5];
 439             ParameterUpdateData[2] = SerialInBuffer[6];
 440             RecvParameterUpdate();
 441         }
 442
 443         packetReceived = true;
 444     }
 445
 446     return packetReceived;
 447 }
 448
 449 void CRSF::GetMspMessage(uint8_t **data, uint8_t *len)
 450 {
 451     *len = MspDataLength;
 452     *data = (MspDataLength > 0) ? MspData : nullptr;
 453 }
 454
 455 void CRSF::ResetMspQueue()
 456 {
 457     MspWriteFIFO.flush();
 458     MspDataLength = 0;
 459     memset(MspData, 0, ELRS_MSP_BUFFER);
 460 }
 461
 462 void CRSF::UnlockMspMessage()
 463 {
 464     // current msp message is sent so restore next buffered write
 465     if (MspWriteFIFO.peek() > 0)
 466     {
 467         uint8_t length = MspWriteFIFO.pop();
 468         MspDataLength = length;
 469         MspWriteFIFO.popBytes(MspData, length);
 470     }
 471     else
 472     {
 473         // no msp message is ready to send currently
 474         MspDataLength = 0;
 475         memset(MspData, 0, ELRS_MSP_BUFFER);
 476     }
 477 }
 478
 479 void ICACHE_RAM_ATTR CRSF::AddMspMessage(mspPacket_t* packet)
 480 {
 481     if (packet->payloadSize > ENCAPSULATED_MSP_MAX_PAYLOAD_SIZE)
 482     {
 483         return;
 484     }
 485
 486     const uint8_t totalBufferLen = packet->payloadSize + ENCAPSULATED_MSP_HEADER_CRC_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_NOT_COUNTED_BYTES;
 487     uint8_t outBuffer[ENCAPSULATED_MSP_MAX_FRAME_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_NOT_COUNTED_BYTES];
 488
 489     // CRSF extended frame header
 490     outBuffer[0] = CRSF_ADDRESS_BROADCAST;                                      // address
 491     outBuffer[1] = packet->payloadSize + ENCAPSULATED_MSP_HEADER_CRC_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC; // length
 492     outBuffer[2] = CRSF_FRAMETYPE_MSP_WRITE;                                    // packet type
 493     outBuffer[3] = CRSF_ADDRESS_FLIGHT_CONTROLLER;                              // destination
 494     outBuffer[4] = CRSF_ADDRESS_RADIO_TRANSMITTER;                              // origin
 495
 496     // Encapsulated MSP payload
 497     outBuffer[5] = 0x30;                // header
 498     outBuffer[6] = packet->payloadSize; // mspPayloadSize
 499     outBuffer[7] = packet->function;    // packet->cmd
 500     for (uint8_t i = 0; i < packet->payloadSize; ++i)
 501     {
 502         // copy packet payload into outBuffer
 503         outBuffer[8 + i] = packet->payload[i];
 504     }
 505     // Encapsulated MSP crc
 506     outBuffer[totalBufferLen - 2] = CalcCRCMsp(&outBuffer[6], packet->payloadSize + 2);
 507
 508     // CRSF frame crc
 509     outBuffer[totalBufferLen - 1] = crsf_crc.calc(&outBuffer[2], packet->payloadSize + ENCAPSULATED_MSP_HEADER_CRC_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC - 1);
 510     AddMspMessage(totalBufferLen, outBuffer);
 511 }
 512
 513 void ICACHE_RAM_ATTR CRSF::AddMspMessage(const uint8_t length, volatile uint8_t* data)
 514 {
 515     if (length > ELRS_MSP_BUFFER)
 516     {
 517         return;
 518     }
 519
 520     // store next msp message
 521     if (MspDataLength == 0)
 522     {
 523         for (uint8_t i = 0; i < length; i++)
 524         {
 525             MspData[i] = data[i];
 526         }
 527         MspDataLength = length;
 528     }
 529     // store all write requests since an update does send multiple writes
 530     else
 531     {
 532         MspWriteFIFO.push(length);
 533         for (uint8_t i = 0; i < length; i++)
 534         {
 535             MspWriteFIFO.push(data[i]);
 536         }
 537     }
 538 }
 539
 540 void ICACHE_RAM_ATTR CRSF::handleUARTin()
 541 {
 542     uint8_t *SerialInBuffer = CRSF::inBuffer.asUint8_t;
 543
 544     if (UARTwdt())
 545     {
 546         return;
 547     }
 548
 549     while (CRSF::Port.available())
 550     {
 551         if (CRSFframeActive == false)
 552         {
 553             unsigned char const inChar = CRSF::Port.read();
 554             // stage 1 wait for sync byte //
 555             if (inChar == CRSF_ADDRESS_CRSF_TRANSMITTER ||
 556                 inChar == CRSF_SYNC_BYTE)
 557             {
 558                 // we got sync, reset write pointer
 559                 SerialInPacketPtr = 0;
 560                 SerialInPacketLen = 0;
 561                 CRSFframeActive = true;
 562                 SerialInBuffer[SerialInPacketPtr] = inChar;
 563                 SerialInPacketPtr++;
 564             }
 565         }
 566         else // frame is active so we do the processing
 567         {
 568             // first if things have gone wrong //
 569             if (SerialInPacketPtr > CRSF_MAX_PACKET_LEN - 1)
 570             {
 571                 // we reached the maximum allowable packet length, so start again because shit fucked up hey.
 572                 SerialInPacketPtr = 0;
 573                 SerialInPacketLen = 0;
 574                 CRSFframeActive = false;
 575                 return;
 576             }
 577
 578             // special case where we save the expected pkt len to buffer //
 579             if (SerialInPacketPtr == 1)
 580             {
 581                 unsigned char const inChar = CRSF::Port.read();
 582                 if (inChar <= CRSF_MAX_PACKET_LEN)
 583                 {
 584                     SerialInPacketLen = inChar;
 585                     SerialInBuffer[SerialInPacketPtr] = inChar;
 586                     SerialInPacketPtr++;
 587                 }
 588                 else
 589                 {
 590                     SerialInPacketPtr = 0;
 591                     SerialInPacketLen = 0;
 592                     CRSFframeActive = false;
 593                     return;
 594                 }
 595             }
 596
 597             int toRead = (SerialInPacketLen + 2) - SerialInPacketPtr;
 598             #if defined(PLATFORM_ESP32)
 599             int count = CRSF::Port.read(&SerialInBuffer[SerialInPacketPtr], toRead);
 600             #else
 601             int count = 0;
 602             int avail = CRSF::Port.available();
 603             while (count < toRead && count < avail)
 604             {
 605                 SerialInBuffer[SerialInPacketPtr + count] = CRSF::Port.read();
 606                 count++;
 607             }
 608             #endif
 609             SerialInPacketPtr += count;
 610
 611             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.
 612             {
 613                 char CalculatedCRC = crsf_crc.calc(SerialInBuffer + 2, SerialInPacketPtr - 3);
 614
 615                 if (CalculatedCRC == SerialInBuffer[SerialInPacketPtr-1])
 616                 {
 617                     GoodPktsCount++;
 618                     if (ProcessPacket())
 619                     {
 620                         //delayMicroseconds(50);
 621                         handleUARTout();
 622                         RCdataCallback();
 623                     }
 624                 }
 625                 else
 626                 {
 627                     DBGLN("UART CRC failure");
 628                     // cleanup input buffer
 629                     flush_port_input();
 630                     BadPktsCount++;
 631                 }
 632                 CRSFframeActive = false;
 633                 SerialInPacketPtr = 0;
 634                 SerialInPacketLen = 0;
 635             }
 636         }
 637     }
 638 }
 639
 640 void ICACHE_RAM_ATTR CRSF::handleUARTout()
 641 {
 642     // both static to split up larger packages
 643     static uint8_t packageLengthRemaining = 0;
 644     static uint8_t sendingOffset = 0;
 645
 646     if (OpentxSyncActive)
 647     {
 648         sendSyncPacketToTX(); // calculate mixer sync packet if needed
 649     }
 650
 651     // if partial package remaining, or data in the output FIFO that needs to be written
 652     if (packageLengthRemaining > 0 || SerialOutFIFO.size() > 0) {
 653         duplex_set_TX();
 654
 655         uint32_t periodBytesRemaining = maxPeriodBytes;
 656         while (periodBytesRemaining)
 657         {
 658 #ifdef PLATFORM_ESP32
 659             portENTER_CRITICAL(&FIFOmux); // stops other tasks from writing to the FIFO when we want to read it
 660 #endif
 661             // no package is in transit so get new data from the fifo
 662             if (packageLengthRemaining == 0) {
 663                 packageLengthRemaining = SerialOutFIFO.pop();
 664                 SerialOutFIFO.popBytes(CRSFoutBuffer, packageLengthRemaining);
 665                 sendingOffset = 0;
 666             }
 667 #ifdef PLATFORM_ESP32
 668             portEXIT_CRITICAL(&FIFOmux); // stops other tasks from writing to the FIFO when we want to read it
 669 #endif
 670
 671             // if the package is long we need to split it up so it fits in the sending interval
 672             uint8_t writeLength;
 673             if (packageLengthRemaining > periodBytesRemaining) {
 674                 if (periodBytesRemaining < maxPeriodBytes) {  // only start to send a split packet as the first packet
 675                     break;
 676                 }
 677                 writeLength = periodBytesRemaining;
 678             } else {
 679                 writeLength = packageLengthRemaining;
 680             }
 681
 682             // write the packet out, if it's a large package the offset holds the starting position
 683             CRSF::Port.write(CRSFoutBuffer + sendingOffset, writeLength);
 684             if (CRSF::PortSecondary)
 685                 CRSF::PortSecondary->write(CRSFoutBuffer + sendingOffset, writeLength);
 686
 687             sendingOffset += writeLength;
 688             packageLengthRemaining -= writeLength;
 689             periodBytesRemaining -= writeLength;
 690
 691             // No bytes left to send, exit
 692             if (SerialOutFIFO.size() == 0)
 693                 break;
 694         }
 695         CRSF::Port.flush();
 696         duplex_set_RX();
 697
 698         // make sure there is no garbage on the UART left over
 699         flush_port_input();
 700     }
 701 }
 702
 703 void ICACHE_RAM_ATTR CRSF::duplex_set_RX()
 704 {
 705 #if defined(PLATFORM_ESP32)
 706     ESP_ERROR_CHECK(gpio_set_direction((gpio_num_t)GPIO_PIN_RCSIGNAL_RX, GPIO_MODE_INPUT));
 707     #ifdef UART_INVERTED
 708     gpio_matrix_in((gpio_num_t)GPIO_PIN_RCSIGNAL_RX, U1RXD_IN_IDX, true);
 709     gpio_pulldown_en((gpio_num_t)GPIO_PIN_RCSIGNAL_RX);
 710     gpio_pullup_dis((gpio_num_t)GPIO_PIN_RCSIGNAL_RX);
 711     #else
 712     gpio_matrix_in((gpio_num_t)GPIO_PIN_RCSIGNAL_RX, U1RXD_IN_IDX, false);
 713     gpio_pullup_en((gpio_num_t)GPIO_PIN_RCSIGNAL_RX);
 714     gpio_pulldown_dis((gpio_num_t)GPIO_PIN_RCSIGNAL_RX);
 715     #endif
 716 #elif defined(PLATFORM_ESP8266)
 717     // Enable loopback on UART0 to connect the RX pin to the TX pin
 718     //USC0(UART0) |= BIT(UCLBE);
 719 #elif defined(GPIO_PIN_BUFFER_OE) && (GPIO_PIN_BUFFER_OE != UNDEF_PIN)
 720     digitalWrite(GPIO_PIN_BUFFER_OE, LOW ^ GPIO_PIN_BUFFER_OE_INVERTED);
 721 #elif (GPIO_PIN_RCSIGNAL_TX == GPIO_PIN_RCSIGNAL_RX)
 722     CRSF::Port.enableHalfDuplexRx();
 723 #endif
 724 }
 725
 726 void ICACHE_RAM_ATTR CRSF::duplex_set_TX()
 727 {
 728 #if defined(PLATFORM_ESP32)
 729     gpio_matrix_in((gpio_num_t)-1, U1RXD_IN_IDX, false);
 730     ESP_ERROR_CHECK(gpio_set_pull_mode((gpio_num_t)GPIO_PIN_RCSIGNAL_TX, GPIO_FLOATING));
 731     ESP_ERROR_CHECK(gpio_set_pull_mode((gpio_num_t)GPIO_PIN_RCSIGNAL_RX, GPIO_FLOATING));
 732     ESP_ERROR_CHECK(gpio_set_level((gpio_num_t)GPIO_PIN_RCSIGNAL_TX, 0));
 733     ESP_ERROR_CHECK(gpio_set_direction((gpio_num_t)GPIO_PIN_RCSIGNAL_TX, GPIO_MODE_OUTPUT));
 734     #ifdef UART_INVERTED
 735     gpio_matrix_out((gpio_num_t)GPIO_PIN_RCSIGNAL_TX, U1TXD_OUT_IDX, true, false);
 736     #else
 737     gpio_matrix_out((gpio_num_t)GPIO_PIN_RCSIGNAL_TX, U1TXD_OUT_IDX, false, false);
 738     #endif
 739 #elif defined(PLATFORM_ESP8266)
 740     // Disable loopback to disconnect the RX pin from the TX pin
 741     //USC0(UART0) &= ~BIT(UCLBE);
 742 #elif defined(GPIO_PIN_BUFFER_OE) && (GPIO_PIN_BUFFER_OE != UNDEF_PIN)
 743     digitalWrite(GPIO_PIN_BUFFER_OE, HIGH ^ GPIO_PIN_BUFFER_OE_INVERTED);
 744 #elif (GPIO_PIN_RCSIGNAL_TX == GPIO_PIN_RCSIGNAL_RX)
 745     // writing to the port switches the mode
 746 #endif
 747 }
 748
 749 void ICACHE_RAM_ATTR CRSF::adjustMaxPacketSize()
 750 {
 751     uint32_t UARTrequestedBaud = TxToHandsetBauds[UARTcurrentBaudIdx];
 752     // baud / 10bits-per-byte / 2 windows (1RX, 1TX) / rate * 0.80 (leeway)
 753     maxPeriodBytes = UARTrequestedBaud / 10 / 2 / (1000000/RequestedRCpacketInterval) * 80 / 100;
 754     maxPeriodBytes = maxPeriodBytes > HANDSET_TELEMETRY_FIFO_SIZE ? HANDSET_TELEMETRY_FIFO_SIZE : maxPeriodBytes;
 755     // we need a minimum of 10 bytes otherwise our LUA will not make progress and at 8 we'd get a divide by 0!
 756     maxPeriodBytes = maxPeriodBytes < 10 ? 10 : maxPeriodBytes;
 757     maxPacketBytes = maxPeriodBytes > CRSF_MAX_PACKET_LEN ? CRSF_MAX_PACKET_LEN : maxPeriodBytes;
 758     DBGLN("Adjusted max packet size %u-%u", maxPacketBytes, maxPeriodBytes);
 759 }
 760
 761 bool CRSF::UARTwdt()
 762 {
 763     uint32_t now = millis();
 764     bool retval = false;
 765     if (now >= (UARTwdtLastChecked + UARTwdtInterval))
 766     {
 767         if (BadPktsCount >= GoodPktsCount)
 768         {
 769             DBGLN("Too many bad UART RX packets!");
 770
 771             if (CRSFstate == true)
 772             {
 773                 DBGLN("CRSF UART Disconnected");
 774 #ifdef FEATURE_OPENTX_SYNC_AUTOTUNE
 775                 SyncWaitPeriodCounter = now; // set to begin wait for auto sync offset calculation
 776                 CRSF::OpenTXsyncOffsetSafeMargin = 1000;
 777                 CRSF::OpenTXsyncOffset = 0;
 778                 CRSF::OpenTXsyncLastSent = 0;
 779 #endif
 780                 disconnected();
 781                 CRSFstate = false;
 782             }
 783
 784             UARTcurrentBaudIdx = (UARTcurrentBaudIdx + 1) % ARRAY_SIZE(TxToHandsetBauds);
 785             uint32_t UARTrequestedBaud = TxToHandsetBauds[UARTcurrentBaudIdx];
 786             DBGLN("UART WDT: Switch to: %d baud", UARTrequestedBaud);
 787
 788             adjustMaxPacketSize();
 789
 790             SerialOutFIFO.flush();
 791 #if defined(PLATFORM_ESP8266) || defined(PLATFORM_ESP32)
 792             CRSF::Port.flush();
 793             CRSF::Port.updateBaudRate(UARTrequestedBaud);
 794 #elif defined(TARGET_TX_GHOST)
 795             CRSF::Port.begin(UARTrequestedBaud);
 796             USART1->CR1 &= ~USART_CR1_UE;
 797             USART1->CR3 |= USART_CR3_HDSEL;
 798             USART1->CR2 |= USART_CR2_RXINV | USART_CR2_TXINV | USART_CR2_SWAP; //inverted/swapped
 799             USART1->CR1 |= USART_CR1_UE;
 800 #elif defined(TARGET_TX_FM30_MINI)
 801             CRSF::Port.begin(UARTrequestedBaud);
 802             LL_GPIO_SetPinPull(GPIOA, GPIO_PIN_2, LL_GPIO_PULL_DOWN); // default is PULLUP
 803             USART2->CR1 &= ~USART_CR1_UE;
 804             USART2->CR2 |= USART_CR2_RXINV | USART_CR2_TXINV; //inverted
 805             USART2->CR1 |= USART_CR1_UE;
 806 #else
 807             CRSF::Port.begin(UARTrequestedBaud);
 808 #endif
 809             duplex_set_RX();
 810             // cleanup input buffer
 811             flush_port_input();
 812
 813             retval = true;
 814         }
 815         DBGLN("UART STATS Bad:Good = %u:%u", BadPktsCount, GoodPktsCount);
 816
 817         UARTwdtLastChecked = now;
 818         if (retval)
 819         {
 820             // Speed up the cycling
 821             UARTwdtLastChecked -= 3 * (UARTwdtInterval >> 2);
 822         }
 823
 824         GoodPktsCountResult = GoodPktsCount;
 825         BadPktsCountResult = BadPktsCount;
 826         BadPktsCount = 0;
 827         GoodPktsCount = 0;
 828     }
 829     return retval;
 830 }
 831
 832 #elif CRSF_RX_MODULE // !CRSF_TX_MODULE
 833 bool CRSF::RXhandleUARTout()
 834 {
 835 #if !defined(CRSF_RCVR_NO_SERIAL)
 836     uint8_t peekVal = SerialOutFIFO.peek(); // check if we have data in the output FIFO that needs to be written
 837     if (peekVal > 0)
 838     {
 839         if (SerialOutFIFO.size() > (peekVal))
 840         {
 841             noInterrupts();
 842             uint8_t OutPktLen = SerialOutFIFO.pop();
 843             uint8_t OutData[OutPktLen];
 844             SerialOutFIFO.popBytes(OutData, OutPktLen);
 845             interrupts();
 846             this->_dev->write(OutData, OutPktLen); // write the packet out
 847             return true;
 848         }
 849     }
 850 #endif // CRSF_RCVR_NO_SERIAL
 851     return false;
 852 }
 853
 854 void ICACHE_RAM_ATTR CRSF::sendLinkStatisticsToFC()
 855 {
 856 #if !defined(CRSF_RCVR_NO_SERIAL) && !defined(DEBUG_CRSF_NO_OUTPUT)
 857     constexpr uint8_t outBuffer[4] = {
 858         LinkStatisticsFrameLength + 4,
 859         CRSF_ADDRESS_FLIGHT_CONTROLLER,
 860         LinkStatisticsFrameLength + 2,
 861         CRSF_FRAMETYPE_LINK_STATISTICS
 862     };
 863
 864     uint8_t crc = crsf_crc.calc(outBuffer[3]);
 865     crc = crsf_crc.calc((byte *)&LinkStatistics, LinkStatisticsFrameLength, crc);
 866
 867     SerialOutFIFO.pushBytes(outBuffer, sizeof(outBuffer));
 868     SerialOutFIFO.pushBytes((byte *)&LinkStatistics, LinkStatisticsFrameLength);
 869     SerialOutFIFO.push(crc);
 870
 871     //this->_dev->write(outBuffer, LinkStatisticsFrameLength + 4);
 872 #endif // CRSF_RCVR_NO_SERIAL
 873 }
 874
 875 void ICACHE_RAM_ATTR CRSF::sendRCFrameToFC()
 876 {
 877 #if !defined(CRSF_RCVR_NO_SERIAL) && !defined(DEBUG_CRSF_NO_OUTPUT)
 878     constexpr uint8_t outBuffer[] = {
 879         // No need for length prefix as we aren't using the FIFO
 880         CRSF_ADDRESS_FLIGHT_CONTROLLER,
 881         RCframeLength + 2,
 882         CRSF_FRAMETYPE_RC_CHANNELS_PACKED
 883     };
 884
 885     uint8_t crc = crsf_crc.calc(outBuffer[2]);
 886     crc = crsf_crc.calc((byte *)&PackedRCdataOut, RCframeLength, crc);
 887
 888     //SerialOutFIFO.push(RCframeLength + 4);
 889     //SerialOutFIFO.pushBytes(outBuffer, RCframeLength + 4);
 890     this->_dev->write(outBuffer, sizeof(outBuffer));
 891     this->_dev->write((byte *)&PackedRCdataOut, RCframeLength);
 892     this->_dev->write(crc);
 893 #endif // CRSF_RCVR_NO_SERIAL
 894 }
 895
 896 void ICACHE_RAM_ATTR CRSF::sendMSPFrameToFC(uint8_t* data)
 897 {
 898 #if !defined(CRSF_RCVR_NO_SERIAL) && !defined(DEBUG_CRSF_NO_OUTPUT)
 899     const uint8_t totalBufferLen = CRSF_FRAME_SIZE(data[1]);
 900     if (totalBufferLen <= CRSF_FRAME_SIZE_MAX)
 901     {
 902         data[0] = CRSF_ADDRESS_FLIGHT_CONTROLLER;
 903         this->_dev->write(data, totalBufferLen);
 904     }
 905 #endif // CRSF_RCVR_NO_SERIAL
 906 }
 907
 908 /**
 909  * @brief   Get encoded channel position from PackedRCdataOut
 910  * @param   ch: zero-based channel number
 911  * @return  CRSF-encoded channel position, or 0 if invalid channel
 912  **/
 913 uint16_t CRSF::GetChannelOutput(uint8_t ch)
 914 {
 915     switch (ch)
 916     {
 917         case 0: return PackedRCdataOut.ch0;
 918         case 1: return PackedRCdataOut.ch1;
 919         case 2: return PackedRCdataOut.ch2;
 920         case 3: return PackedRCdataOut.ch3;
 921         case 4: return PackedRCdataOut.ch4;
 922         case 5: return PackedRCdataOut.ch5;
 923         case 6: return PackedRCdataOut.ch6;
 924         case 7: return PackedRCdataOut.ch7;
 925         case 8: return PackedRCdataOut.ch8;
 926         case 9: return PackedRCdataOut.ch9;
 927         case 10: return PackedRCdataOut.ch10;
 928         case 11: return PackedRCdataOut.ch11;
 929         default:
 930             return 0;
 931     }
 932 }
 933
 934 #endif // CRSF_RX_MODULE
 935
 936 void CRSF::GetDeviceInformation(uint8_t *frame, uint8_t fieldCount)
 937 {
 938     deviceInformationPacket_t *device = (deviceInformationPacket_t *)(frame + sizeof(crsf_ext_header_t) + device_name_size);
 939     // Packet starts with device name
 940     memcpy(frame + sizeof(crsf_ext_header_t), device_name, device_name_size);
 941     // Followed by the device
 942     device->serialNo = htobe32(0x454C5253); // ['E', 'L', 'R', 'S'], seen [0x00, 0x0a, 0xe7, 0xc6] // "Serial 177-714694" (value is 714694)
 943     device->hardwareVer = 0; // unused currently by us, seen [ 0x00, 0x0b, 0x10, 0x01 ] // "Hardware: V 1.01" / "Bootloader: V 3.06"
 944     device->softwareVer = 0; // unused currently by us, seen [ 0x00, 0x00, 0x05, 0x0f ] // "Firmware: V 5.15"
 945     device->fieldCnt = fieldCount;
 946     device->parameterVersion = 0;
 947 }
 948
 949 void CRSF::SetExtendedHeaderAndCrc(uint8_t *frame, uint8_t frameType, uint8_t frameSize, uint8_t senderAddr, uint8_t destAddr)
 950 {
 951     crsf_ext_header_t *header = (crsf_ext_header_t *)frame;
 952     header->dest_addr = destAddr;
 953     header->device_addr = destAddr;
 954     header->type = frameType;
 955     header->orig_addr = senderAddr;
 956     header->frame_size = frameSize;
 957
 958     uint8_t crc = crsf_crc.calc(&frame[CRSF_FRAME_NOT_COUNTED_BYTES], header->frame_size - 1, 0);
 959
 960     frame[header->frame_size + CRSF_FRAME_NOT_COUNTED_BYTES - 1] = crc;
 961 }