CRSF_INIT

.clang-format

@@ -0,0 +1,6 @@
+---
+Language:        Cpp
+BasedOnStyle:  Google
+AlignConsecutiveMacros: true
+AlignEscapedNewlines: Right
+...

library.properties

@@ -0,0 +1,11 @@
+name=CRSF
+version=0.0.1
+author=Sam4uk
+maintainer=Sam4uk
+sentence=CRSF
+paragraph=CRSF
+category=Other
+url=
+architectures=*
+includes=CrsfSerial.h
+depends=CRC

src/CrsfSerial.cpp

@@ -0,0 +1,267 @@
+#include "CrsfSerial.h"
+
+// static void hexdump(void *p, size_t len)
+// {
+//     char *data = (char *)p;
+//     while (len > 0)
+//     {
+//         uint8_t linepos = 0;
+//         char* linestart = data;
+//         // Binary part
+//         while (len > 0 && linepos < 16)
+//         {
+//             if (*data < 0x0f)
+//             Serial.write('0');
+//             Serial.print(*data, HEX);
+//             Serial.write(' ');
+//             ++data;
+//             ++linepos;
+//             --len;
+//         }
+
+//         // Spacer to align last line
+//         for (uint8_t i = linepos; i < 16; ++i)
+//             Serial.print("   ");
+
+//         // ASCII part
+//         for (uint8_t i = 0; i < linepos; ++i)
+//             Serial.write((linestart[i] < ' ') ? '.' : linestart[i]);
+//         Serial.println();
+//     }
+// }
+
+CrsfSerial::CrsfSerial(HardwareSerial &port, uint32_t baud)
+    : _port(port),
+      _crc(0xd5),
+      _baud(baud),
+      _lastReceive(0),
+      _lastChannelsPacket(0),
+      _linkIsUp(false),
+      _passthroughBaud(0) {}
+
+void CrsfSerial::begin(uint32_t baud) {
+  if (baud != 0)
+    _port.begin(baud);
+  else
+    _port.begin(_baud);
+}
+
+// Call from main loop to update
+void CrsfSerial::loop() { handleSerialIn(); }
+
+void CrsfSerial::write(uint8_t b) { _port.write(b); }
+
+void CrsfSerial::write(const uint8_t *buf, size_t len) {
+  _port.write(buf, len);
+}
+
+void CrsfSerial::queuePacket(uint8_t addr, uint8_t type, const void *payload,
+                             uint8_t len) {
+  if (getPassthroughMode()) return;
+  if (len > CRSF_MAX_PAYLOAD_LEN) return;
+
+  uint8_t buf[CRSF_MAX_PACKET_SIZE];
+  buf[0] = addr;
+  buf[1] = len + 2;  // type + payload + crc
+  buf[2] = type;
+  memcpy(&buf[3], payload, len);
+  buf[len + 3] = _crc.calc(&buf[2], len + 1);
+
+  write(buf, len + 4);
+}
+
+uint32_t CrsfSerial::getBaud() const { return _baud; };
+
+int CrsfSerial::getChannel(unsigned int ch) const { return _channels[ch - 1]; }
+
+const crsfLinkStatistics_t *CrsfSerial::getLinkStatistics() const {
+  return &_linkStatistics;
+}
+
+const crsf_sensor_gps_t *CrsfSerial::getGpsSensor() const {
+  return &_gpsSensor;
+}
+
+bool CrsfSerial::isLinkUp() const { return _linkIsUp; }
+
+bool CrsfSerial::getPassthroughMode() const { return _passthroughBaud != 0; }
+
+void CrsfSerial::setPassthroughMode(bool val, uint32_t passthroughBaud) {
+  if (val) {
+    // If not requesting any baud change
+    if (passthroughBaud == 0) {
+      // Enter passthrough mode if not yet
+      if (_passthroughBaud == 0) _passthroughBaud = _baud;
+      return;
+    }
+
+    _passthroughBaud = passthroughBaud;
+  } else {
+    // Not in passthrough, can't leave it any harder than we already are
+    if (_passthroughBaud == 0) return;
+
+    // Leaving passthrough, but going back to same baud, just disable
+    if (_passthroughBaud == _baud) {
+      _passthroughBaud = 0;
+      return;
+    }
+
+    _passthroughBaud = 0;
+  }
+
+  // Can only get here if baud is changing, close and reopen the port
+  _port.end();  // assumes flush()
+  begin(_passthroughBaud);
+}
+
+void CrsfSerial::handleSerialIn() {
+  while (_port.available()) {
+    uint8_t b = _port.read();
+    _lastReceive = millis();
+
+    if (getPassthroughMode()) {
+      if (onOobData) onOobData(b);
+      continue;
+    }
+
+    _rxBuf[_rxBufPos++] = b;
+    handleByteReceived();
+
+    if (_rxBufPos == (sizeof(_rxBuf) / sizeof(_rxBuf[0]))) {
+      // Packet buffer filled and no valid packet found, dump the whole thing
+      _rxBufPos = 0;
+    }
+  }
+
+  checkPacketTimeout();
+  checkLinkDown();
+}
+
+void CrsfSerial::handleByteReceived() {
+  bool reprocess;
+  do {
+    reprocess = false;
+    if (_rxBufPos > 1) {
+      uint8_t len = _rxBuf[1];
+      // Sanity check the declared length isn't outside Type +
+      // X{1,CRSF_MAX_PAYLOAD_LEN} + CRC assumes there never will be a CRSF
+      // message that just has a type and no data (X)
+      if (len < 3 || len > (CRSF_MAX_PAYLOAD_LEN + 2)) {
+        shiftRxBuffer(1);
+        reprocess = true;
+      }
+
+      else if (_rxBufPos >= (len + 2)) {
+        uint8_t inCrc = _rxBuf[2 + len - 1];
+        uint8_t crc = _crc.calc(&_rxBuf[2], len - 1);
+        if (crc == inCrc) {
+          processPacketIn(len);
+          shiftRxBuffer(len + 2);
+          reprocess = true;
+        } else {
+          shiftRxBuffer(1);
+          reprocess = true;
+        }
+      }  // if complete packet
+    }    // if pos > 1
+  } while (reprocess);
+}
+
+void CrsfSerial::checkPacketTimeout() {
+  // If we haven't received data in a long time, flush the buffer a byte at a
+  // time (to trigger shiftyByte)
+  if (_rxBufPos > 0 && millis() - _lastReceive > CRSF_PACKET_TIMEOUT_MS)
+    while (_rxBufPos) shiftRxBuffer(1);
+}
+
+void CrsfSerial::checkLinkDown() {
+  if (_linkIsUp && millis() - _lastChannelsPacket > CRSF_FAILSAFE_STAGE1_MS) {
+    if (onLinkDown) onLinkDown();
+    _linkIsUp = false;
+  }
+}
+
+void CrsfSerial::processPacketIn(uint8_t len) {
+  const crsf_header_t *hdr = (crsf_header_t *)_rxBuf;
+  if (hdr->device_addr == CRSF_ADDRESS_FLIGHT_CONTROLLER) {
+    switch (hdr->type) {
+      case CRSF_FRAMETYPE_GPS:
+        packetGps(hdr);
+        break;
+      case CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
+        packetChannelsPacked(hdr);
+        break;
+      case CRSF_FRAMETYPE_LINK_STATISTICS:
+        packetLinkStatistics(hdr);
+        break;
+    }
+  }  // CRSF_ADDRESS_FLIGHT_CONTROLLER
+}
+
+// Shift the bytes in the RxBuf down by cnt bytes
+void CrsfSerial::shiftRxBuffer(uint8_t cnt) {
+  // If removing the whole thing, just set pos to 0
+  if (cnt >= _rxBufPos) {
+    _rxBufPos = 0;
+    return;
+  }
+
+  if (cnt == 1 && onOobData) onOobData(_rxBuf[0]);
+
+  // Otherwise do the slow shift down
+  uint8_t *src = &_rxBuf[cnt];
+  uint8_t *dst = &_rxBuf[0];
+  _rxBufPos -= cnt;
+  uint8_t left = _rxBufPos;
+  while (left--) *dst++ = *src++;
+}
+
+void CrsfSerial::packetChannelsPacked(const crsf_header_t *p) {
+  crsf_channels_t *ch = (crsf_channels_t *)&p->data;
+  _channels[0] = ch->ch0;
+  _channels[1] = ch->ch1;
+  _channels[2] = ch->ch2;
+  _channels[3] = ch->ch3;
+  _channels[4] = ch->ch4;
+  _channels[5] = ch->ch5;
+  _channels[6] = ch->ch6;
+  _channels[7] = ch->ch7;
+  _channels[8] = ch->ch8;
+  _channels[9] = ch->ch9;
+  _channels[10] = ch->ch10;
+  _channels[11] = ch->ch11;
+  _channels[12] = ch->ch12;
+  _channels[13] = ch->ch13;
+  _channels[14] = ch->ch14;
+  _channels[15] = ch->ch15;
+
+  for (unsigned int i = 0; i < CRSF_NUM_CHANNELS; ++i)
+    _channels[i] = map(_channels[i], CRSF_CHANNEL_VALUE_1000,
+                       CRSF_CHANNEL_VALUE_2000, 1000, 2000);
+
+  if (!_linkIsUp && onLinkUp) onLinkUp();
+  _linkIsUp = true;
+  _lastChannelsPacket = millis();
+
+  if (onPacketChannels) onPacketChannels();
+}
+
+void CrsfSerial::packetLinkStatistics(const crsf_header_t *p) {
+  const crsfLinkStatistics_t *link = (crsfLinkStatistics_t *)p->data;
+  memcpy(&_linkStatistics, link, sizeof(_linkStatistics));
+
+  if (onPacketLinkStatistics) onPacketLinkStatistics(&_linkStatistics);
+}
+
+void CrsfSerial::packetGps(const crsf_header_t *p) {
+  const crsf_sensor_gps_t *gps = (crsf_sensor_gps_t *)p->data;
+  _gpsSensor.latitude = be32toh(gps->latitude);
+  _gpsSensor.longitude = be32toh(gps->longitude);
+  _gpsSensor.groundspeed = be16toh(gps->groundspeed);
+  _gpsSensor.heading = be16toh(gps->heading);
+  _gpsSensor.altitude = be16toh(gps->altitude);
+  _gpsSensor.satellites = gps->satellites;
+
+  if (onPacketGps) onPacketGps(&_gpsSensor);
+}
+