Decoding FrSKY telemetry data with an Arduino – Part two

In part one, we looked at the standard FrSky ‘D series’ data packets that provide voltage and RSSI information.  In this part two post we’ll consider the data format used by standard FrSky telemetry add-ons and how to decode the data from those without using a Taranis transmitter or a commercial display unit.  In part three I’ll explain how we can mimic that data format by injecting our own serial data from, say, an Arduino into the receiver’s serial port and then extend that idea to send and decode our own data packet types.

The terminology is confusing in that it’s the radio control “transmitter” that we usually hold in our hands is the receiver of telemetry data, and the radio control “receiver” in the aircraft (or other model) transmits the data.  To try to reduce the confusion a little, I shall refer to the base station and the aircraft.  If you’re using a surface vehicle such as a boat, just mentally substitute “boat” or whatever when you see “aircraft”.

Let’s consider what happens at the base station.  If you’ve read part one of this telemetry blog, you’ll remember that in our handlePacket() routine, we looked for packets that began with an 0xFE byte and ignored any other packet types.  The 0xFE packets contained the basic A1/A2/RSSI data.  Now all other data from all the standard telemetry sensors, plus your own injected serial data, arrive in a second type of packet that begins with an 0xFD character.  I shall call these second type of data packets, “non-basic-data-packets” or NBDPs for short.

Structure of a NBDP:

0xFD <length> <wasted> [up to six data bytes]

Following the 0xFD identifier is a <length> byte that indicates (in binary) the number of data bytes in the packet.  This should always have a value in the range one to six – if it’s outside that range then the packet is bad and can be ignored.  Next comes a wasted byte that contains no useful data.  Then follows the actual data – and the number of bytes here is the value that was received in <length>

So we can modify our existing handlePacket() routine so that it still decodes A1, A2, and RSSI as before, but also recognizes any NBDPs that are received and passes the data within them, a byte at a time, to another routine named handleDataByte()

void handlePacket(uint8_t *packet) {
  switch (packet[0]) {
    case 0xFD:
      if (packet[1] > 0 && packet[1] <= 6) {
        for (int i = 0; i < packet[1]; i++) {
          handleDataByte(packet[3 + i]);
        }
      }
      break;
    case 0xFE:
      a1 = packet[1]; // A1: 
      a2 = packet[2]; // A2:
      rssi = packet[3]; // main (Rx) link quality 100+ is full signal  40 is no signal
      // packet[4] secondary (Tx) link quality.  Strength of signal received by Tx so not particularly useful.  Numbers are about double those of RSSI.
      break;
  }
  cli(); timeout = 1000; sei();
}

Now let’s consider the data stream format that the handleDataByte() routine has to decode.  It’s useful to study this FrSky document to understand what follows.

Data frames coming from the hub are framed (start and end) with 0x5E bytes.  As with the data at the (previously explained) lower level, there may be data bytes within the stream that happen to have the same value 0x5E so another layer of Byte stuffing is employed; inside the packets: 0x5E is replaced by the byte pair 0x5D, 0x3E, and then an actual data byte with value 0x5D also needs to be stuffed and this is replaced by the pair 0x5D, 0x3D.

Here’s the first part of the handleDataByte() routine.  It needs somewhere to store data as it arrives until it has a complete packet, and also has to remember what it’s doing between one call and the next and whether or not stuffing is in force so we have a few static variables:

void handleDataByte(uint8_t data) {
  static uint8_t dataPacket[4];
  static uint8_t FrSkyUserDataMode = 0;
  static uint8_t FrSkyUserDataLow;
  static uint8_t FrSkyTelemetryDataID;
  static bool FrSkyUserDataStuffing;
  static bool FrSkyUserDataLowFlag; // flag for low byte of data, which is the first of the following two
  int16_t i;
  uint8_t high, low;

  switch (FrSkyUserDataMode) {
    case 0: // idle
      if (data == 0x5E) { // telemetry hub frames begin with ^ (0x5E)
        FrSkyUserDataMode = 1; // expect telemetry hub DataID next
      }
      break;
    case 1: // expecting telemetry hub DataID
      if (data < 0x3C) { // store DataID (address) FrSkyUserDataStuffing = false; FrSkyTelemetryDataID = data; FrSkyUserDataMode = 2; // expect two bytes of data next FrSkyUserDataLowFlag = true; // flag for low byte of data, which is the first of the following two } else if (data != 0x5E) { // the header byte 0x5E may occur twice running as it is also used as an 'end of frame' so remain in mode 1. Otherwise DataID was > 0x3B, so invalid
        FrSkyUserDataMode = 0; // back to idle mode
      }
      break;
    case 2: // expecting two bytes of data
      if (FrSkyUserDataStuffing) {
        FrSkyUserDataStuffing = false;
        if ((data != 0x3D) && (data != 0x3E)) { // byte stuffing is only valid for (unstuffed) bytes 0x5D or or 0x5E
          FrSkyUserDataMode = 0; // back to idle mode
          break;
        }
        else {
          data ^= 0x20; // unstuff byte
        }
      }
      else if (data == 0x5D) { // following byte is stuffed
        FrSkyUserDataStuffing = true;
        break;
      }

That first part handles the the basics and the remainder of the routine is a big switch statement that stores data till complete packets are available and then displays the contents:

      if (FrSkyUserDataLowFlag) { // expecting low byte of data
        FrSkyUserDataLow = data; // remember low byte
        FrSkyUserDataLowFlag = false; // expect high byte next
      }
      else { // expecting high byte of data
        switch (FrSkyTelemetryDataID) {
          case 0x01: // 1st part of GPS altitude
          case 0x10: // 1st part of barimetric altitude
          case 0x11: // 1st part of GPS speed
          case 0x12: // 1st part of longitude
          case 0x13: // 1st part of latitude
          case 0x14: // 1st part of GPS course over ground
          case 0x15: // 1st part of date
          case 0x17: // 1st part of time
            dataPacket[0] = FrSkyUserDataLow;
            dataPacket[1] = data; // store value till last part received
            break;
          case 0x02: // temperature 1
            Serial.print("Temperature 1: ");
            Serial.print(format(FrSkyUserDataLow, data));
            Serial.print("\n");
            break;
          case 0x05: // temperature 2
            Serial.print("Temperature 2: ");
            Serial.print(format(FrSkyUserDataLow, data));
            Serial.print("\n");
            break;
          case 0x09: // second (final) part of GPS altitude
            Serial.print("GPS Altitude: ");
            Serial.print(format(dataPacket[0], dataPacket[1]));
            Serial.print(".");
            Serial.print(format(FrSkyUserDataLow, data));
            Serial.print("\n");
            break;
          case 0x16: // second (final) part of date
            Serial.print("Date: 20"); // assume 21st century
            Serial.print(format(FrSkyUserDataLow, 0)); // year
            Serial.print("-");
            Serial.print(format(dataPacket[1], 0)); // month
            Serial.print("-");
            Serial.print(format(dataPacket[0], 0)); // day
            Serial.print("\n");
            break;
          case 0x18: // second (final) part of time
            Serial.print("Time: ");
            Serial.print(format(dataPacket[0], 0)); // hour
            Serial.print(":");
            Serial.print(format(dataPacket[1], 0)); // minute
            Serial.print(":");
            Serial.print(format(FrSkyUserDataLow, 0)); // second
            Serial.print("\n");
            break;
          case 0x19: // second (final) part of GPS speed
            Serial.print("GPS speed: ");
            Serial.print(format(dataPacket[0], dataPacket[1]));
            Serial.print(".");
            Serial.print(format(FrSkyUserDataLow, data));
            Serial.print("\n");
            break;
          case 0x1A: // second part of longitude
          case 0x1B: // second part of latitude
            dataPacket[2] = FrSkyUserDataLow;
            dataPacket[3] = data; // store value till last part received
            break;
          case 0x1C: // second (final) part of GPS cog
            Serial.print("GPS C.O.G.: ");
            Serial.print(format(dataPacket[0], dataPacket[1]));
            Serial.print(".");
            Serial.print(format(FrSkyUserDataLow, data));
            Serial.print("\n");
            break;
          case 0x21: // second (final) part of barometric altitude
            Serial.print("Barometric Altitude: ");
            Serial.print(format(dataPacket[0], dataPacket[1]));
            Serial.print(".");
            Serial.print(format(FrSkyUserDataLow, data));
            Serial.print("\n");
            break;
          case 0x22: // third (final) part of longitude
            Serial.print("Longitude: ");
            i = (uint16_t)dataPacket[1];
            i = (i << 8) | dataPacket[0]; // degrees * 100 + minutes high = (i / 100) >> 8; low = (i / 100) & 0x00FF;
            Serial.print(format(low, high));
            Serial.print(" deg ");
            Serial.print(format(i % 100, 0));
            Serial.print(".");
            Serial.print(format(dataPacket[2], dataPacket[3]));
            Serial.print(" min ");
            Serial.write(FrSkyUserDataLow); // 'E' or 'W'
            Serial.print("\n");
            break;
          case 0x23: // third (final) part of latitude
            Serial.print("Latitude: ");
            i = (uint16_t)dataPacket[1];
            i = (i << 8) | dataPacket[0]; // degrees * 100 + minutes high = (i / 100) >> 8; low = (i / 100) & 0x00FF;
            Serial.print(format(low, high));
            Serial.print(" deg ");
            Serial.print(format(i % 100, 0));
            Serial.print(".");
            Serial.print(format(dataPacket[2], dataPacket[3]));
            Serial.print(" min ");
            Serial.write(FrSkyUserDataLow); // 'N' or 'S'
            Serial.print("\n");
            break;
          case 0x24: // x acceleration
            Serial.print("X acceleration: ");
            Serial.print(format(FrSkyUserDataLow, data));
            Serial.print("\n");
            break;
          case 0x25: // y acceleration
            Serial.print("Y acceleration: ");
            Serial.print(format(FrSkyUserDataLow, data));
            Serial.print("\n");
            break;
          case 0x26: // z acceleration
            Serial.print("Z acceleration: ");
            Serial.print(format(FrSkyUserDataLow, data));
            Serial.print("\n");
            break;
          default:
            break;
        }
        FrSkyUserDataMode = 0; // received and stored both data bytes, so back to idle mode
      } // else
      break;
    default: // should never happen
      FrSkyUserDataMode = 0; // back to idle mode
      break;
  }
}

The above routine makes calls to format() which takes two bytes of data representing a 16-bit value and formats the output as an ASCII decimal number.

char* format(uint8_t low, uint8_t high) { // format the 16-bit integer formed by high:low in ASCII decimal.  Negative values have a leading '-' values < 10 have one leading '0'
  static char buffer[8];
  char *p;
  p = buffer;
  int16_t i = (int16_t)high;
  i = (i << 8) | low;
  if (i < 0) {
    *p++ = '-';
    i = -i;
  }
  if (i < 9) { *p++ = '0'; } for (int16_t tenPower = i > 9999 ? 10000 : i > 999 ? 1000 : i > 99 ? 100 : i > 9 ? 10 : 1; tenPower > 0; tenPower /= 10) {
    uint8_t digit = '0';
    while (i >= tenPower) {
      digit++;
      i -= tenPower;
    }
    *p++ = digit;
  }
  *p = '\0';
  return buffer;
}

Here’s some sample output.  I changed the latitude and longitude values so as not to show my home location:

RSSI: 87 AD1: 96 AD2: 142
RSSI: 87 AD1: 96 AD2: 141
Barometric Altitude: 95.18
GPS C.O.G.: 00.00
GPS speed: 00.96
RSSI: 87 AD1: 96 AD2: 143
GPS Altitude: 63.00
Date: 2017-10-10
Time: 20:01:27
X acceleration: -60
Y acceleration: 133
Z acceleration: 963
Latitude: 53 deg 17.4902 min N
Longitude: 03 deg 33.1422 min W
RSSI: 87 AD1: 96 AD2: 141
Temperature 1: 25
Temperature 2: 26
RSSI: 86 AD1: 96 AD2: 143
RSSI: 87 AD1: 96 AD2: 140

Note that the RSSI frames arrive more frequently than the other types of data.

You can download the complete working sketch using this link.

Decoding FrSKY telemetry data with an Arduino

I used a FrSKY D4R-II receiver and a DHT DIY Tx module with an Arduino Pro Mini for the purpose of this post.  These FrSKY products use the older “D mode” telemetry and the DHT module is now discontinued, but I’ll be updating this post to deal with the newer S.Port Telemetry soon.

 

There are still lots of people using the older “D mode” equipment and in any case the Arduino code shows the general approach to decoding serial data on any digital pin.  The code presented should work on any of the ATmega-based Arduinos: Uno, Nano, Pro Mini and so on…

First thing to be aware of is that the Telemetry data where it emerges from the transmitter module on the Txd pin is designed to be connected to a PC-type RS232 port.  It works at voltage levels of about +6V swinging to -6V.  Don’t connect it directly to your Arduino!  It could damage either the module or the Arduino!

Fortunately the only ‘interface’ needed to connect the signal to the Arduino is a humble 100k resistor.  This limits the maximum current flow to 60 microamps and the ATmega’s protection diodes on each I/O pin then safely clamp this to the Arduino’s 0V to 5V range.  This is a method approved by the chip manufacturers, Atmel – indeed they even suggest that mains voltages can be connected to the chip through a suitable limiting resistor in an application note: http://www.atmel.com/Images/Atmel-2508-Zero-Cross-Detector_ApplicationNote_AVR182.pdf

So for the purpose of this post we connect the signal from Txd through a 100k resistor to Arduino Pin 11 (you can choose any digital or analogue pin with small modifications to the code), and you need to connect the GND pin from the module to GND on your Arduino.

We use setup() to initialize an interrupt handler to respond to changes of signal on pin 11, to initialize a few variables and start a Serial port where we can display things:

void setup(void) {
  pinMode(11, INPUT); // serial input (Digital 11 is Port B, bit 3)
  PCMSK0 = 0x08; // set mask to allow (only) digital pin 11 pinchange interrupts on Port B
  PCICR |= 0x01; // allow pinchange interrupts for Port B
  rssi = a1 = a2 = 0;
  timeout = 1000; // ms
  Serial.begin(115200);
  oldMillis = millis();
}

The interrupt handler decodes the incoming serial characters (remember the mark and space levels are swapped compared to standard Arduino serial).  Don’t bother understanding this code unless you’re specially interested – all it does is decode individual 8-bit characters arriving and call handleRxChar() to handle each individual character that arrives:

ISR(PCINT0_vect) { // Port B pinchange interrupt
  static uint8_t prev = 0;
  static uint8_t currChar = 0;
  static uint8_t currBits = 0;
  uint8_t now = TCNT0;
  sei(); // allow other interrupts once time has been captured
  uint8_t bits = (((uint8_t)(now - prev)) + 13) / 26; // at 9600 baud with 4 uS TCNT, one bit is 1E6 / (4 * 9600) = 26.04 counts, so half a bit is ~13 counts
  if (!bits || bits > 9) { // detect 0 bits when gap between characters and TCNT0 wrapped exactly, also limit bits to 9 to prevent overflow of (currBits + bits)
    bits = 9;
  }
  prev = now;
  if (!(PINB & 0x08)) { // -ve edge, but inverted RS232 so preceding data bits were low
    if (!currBits) { // start of char so bits includes the start bit (start bit is low)
      currBits = 1; // currBits count does includes the start bit
      bits--; // but don't include start bit in character decode
    }
    currChar >>= bits; // received low bits represent '0' in character.  LSB arrives first, MSB last
    currBits += bits;
  }
  else if (currBits) { // preceding data bits were high
    if (currBits + bits > 9) { // trim stop bit if receiving character values >= 128 (MSB will be high and stop bit is also high)
      bits = 9 - currBits;
    }
    currChar = (0xFF00 | currChar) >> bits; // received high bits represent '1' in character.  LSB arrives first, MSB last
    currBits += bits;
  }
  if (currBits > 8) { // start bit plus character received
    uint8_t rxChar = currChar; // don't know if interrupt is re-entrant - if not then this won't help but if it is then allows next bit(s) to be received while handling char
    currChar = currBits = 0;
    handleRxChar(rxChar);
  }
}

Now we come to the FrSKY specific stuff – FrSKY data arrives in 9-byte packets framed by hex 7E characters. Of course the value 7E might be included in the data, but can’t be sent directly as it would be mistaken as a framing byte. To get around this, FrSKY uses a technique called byte stuffing where 7E is replaced by the pair 7D 5E. That means that 7D now also can’t be sent directly as a data byte as it would be mistaken for the character introducing byte stuffing – so 7D is replaced by the pair 7D 5D. This code replaces any pair of byte stuffing codes with the single byte original and when it sees a full 9-byte data packet framed with 7E it passes that packet on to handlePacket() for further processing:

void handleRxChar(uint8_t b) { // decode FrSky basic telemetry data
  static uint8_t packetPosition = 0;
  static uint8_t packet[9];
  static bool byteStuffing = false;

  if (b == 0x7E) { // framing character
    if (packetPosition == 9) {
      handlePacket(packet);
    }
    packetPosition = 0;
    return;
  }

  if (b == 0x7D) {
    byteStuffing = true;
    return;
  }

  if (byteStuffing) {
    byteStuffing = false;
    if (b != 0x5E && b != 0x5D) {
      packetPosition = 0;
      return;
    }
    b ^= 0x20;
  }

  if (packetPosition > 8) {
    packetPosition = 0;
  }
  else {
    packet[packetPosition++] = b;
  }
}

There may be several different packet types when telemetry sensors such as an altimeter are present, but for now we’ll ignore all but the most basic type of packet. This packet type with a signature byte of FE is always present and contains the RSSI (received signal strength indication) and the two analogue voltages AD1, and AD2. As well as the signal strength received by the receiver, this packet also contains the signal strength received by the transmitter – but we’re not usually interested in that – we’re interested in how well the receiver can “hear” the transmitter and not the other way round! The handlePacket function in this example just extracts the byte values for rssi, a1, and a2 and makes them available as global variables.

void handlePacket(uint8_t *packet) {
  if (packet[0] == 0xFE) { // not interested in telemetry data other than the most basic kind for this application
    a1 = packet[1]; // A1: 
    a2 = packet[2]; // A2:
    rssi = packet[3]; // main (Rx) link quality 100+ is full signal  40 is no signal
    // packet[4] secondary (Tx) link quality.  Strength of signal received by Tx so not particularly useful.  Numbers are about double those of RSSI.
    cli(); timeout = 1000; sei();
  }
}

All that’s left is to display any received data by printing it to the Serial port so it can be monitored by the Arduino terminal (Ctrl-Shift-M and set the baud rate to 115200). The loop() here uses the millis() millisecond counter to only update the serial display twice per second:

void loop(void) {
  uint32_t now = millis();
  int32_t ms = now - oldMillis; // milliseconds elapsed since last loop
  oldMillis = now;
  
  if (timeout <= (int16_t)ms) {
    timeout = a1 = a2 = rssi = 0;
  } else {
    timeout -= ms;
  }

  if (ms >= millisTillNextPrint) { // time to print
    if (!rssi) {
      Serial.println("No telemetry data being received");
    } else {
      Serial.print("RSSI: ");
      Serial.print(rssi);
      Serial.print(" AD1: ");
      Serial.print(a1);
      Serial.print(" AD2: ");
      Serial.println(a2);
    }
    millisTillNextPrint += 500 - ms;
  } else {
    millisTillNextPrint -= ms;
  }
}

And that’s it! The complete Arduino sketch is attached here: FrSKY_telemetryDecode

In my next post, I’ll expand on the data packet handling to explain how different types of telemetry sensor data are encoded, and how you can even send your own home-brew telemetry data back to the ground by injecting a serial data stream to the receiver on board your model.

Decoding 6 servo channel inputs with an Arduino UNO

Standard radio control receivers drive their connected servos by outputting a train of pulses.  The pulses usually repeat at a 50 Hz rate but the pulse width is the important thing – the standard is a 1.5 ms pulse width for a servo at its center position varying from about 1.0 ms up to 2.0 ms for servo’s nominal travel range. The pulses are positive going, usually about 5V in amplitude.

Searching on the web finds lots of examples of code to read the pulse widths for several servo channels, but for my application the Arduino is looking after several sensors besides the servo signals and also running software serial ports.  The examples I found all gave too much jitter and inaccuracy.

I wired the six channels to the Arduino digital pins 2 to 7.  Pins 0 and 1 are used by the UART (serial communication to PC or other device) so it’s best to avoid those.  Conveniently this means that all six channels are contained on a single Arduino input port, (Port D) so this makes the code to capture the pulse widths very clean.

We enable an interrupt that occurs when any of the 6 pins change state. To do this we set one bit for each of the 6 channels in the PCMSK register.  Then to allow changing input states on Port D to generate interrupts there is just one bit in the PCICR register to set:

PCMSK2 |= 0xFC;
PCICR |= 0x04;

The interrupt handler, declared as ISR(PCINT2_vect) checks each of the six pins. If a pin has changed state since the previous interrupt, we do one of two things:  If the pin has gone high we just remember the current time; if the pin has gone low we store the pulse width by subtracting the remembered time from the current one.  The Arduino has a function that returns microseconds (with a resolution of 4 microseconds on the UNO) convenient for this task.  The microsecond timer wraps around back to zero roughly every 70 minutes but this doesn’t cause any problems – by doing the subtractions using unsigned long integers the pulse widths are still correct even when a ‘wrap around’ occurs.  Here’s the complete program including interrupt handler and simple test output that just prints the current pulse widths to the serial port (PC).

volatile uint8_t prev; // remembers state of input bits from previous interrupt
volatile uint32_t risingEdge[6]; // time of last rising edge for each channel
volatile uint32_t uSec[6]; // the latest measured pulse width for each channel

ISR(PCINT2_vect) { // one or more of pins 2~7 have changed state
  uint32_t now = micros();
  uint8_t curr = PIND; // current state of the 6 input bits
  uint8_t changed = curr ^ prev;
  int channel = 0;
  for (uint8_t mask = 0x04; mask; mask <<= 1) {
    if (changed & mask) { // this pin has changed state
      if (curr & mask) { // +ve edge so remember time
        risingEdge[channel] = now;
      }
      else { // -ve edge so store pulse width
        uSec[channel] = now - risingEdge[channel];
      }
    }
    channel++;
  }
  prev = curr;
}

void setup() {
  Serial.begin(9600);

  for (int pin = 2; pin <= 7; pin++) { // enable pins 2 to 7 as our 6 input bits
    pinMode(pin, INPUT);
  }

  PCMSK2 |= 0xFC; // set the mask to allow those 6 pins to generate interrupts
  PCICR |= 0x04;  // enable interupt for port D
}

void loop() {
  Serial.flush();
  for (int channel = 0; channel < 6; channel++) {
    Serial.print(uSec[channel]);
    Serial.print("\t");
  }
  Serial.println();
}