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.

7 thoughts on “Decoding FrSKY telemetry data with an Arduino – Part two”

  1. This is really useful, thanks! Just what I’ve been looking for to help with my Mavic pro clone I’m building. Using an Arduino nano and DHT module for my controller. Looking forward to part three!

  2. Dearest
    I was just trying to exploit the FrSky transmission to send my data and to use a voice structure and a friend provided these pages I found
    very interesting, congratulations, it would be nice to have the third part ……. is it possible?

      1. Thanks for the answer ……. I look anxiously on the third part, meanwhile I’ve already done all the session of the commands and voice responses.
        I would like to ask if the sketch of the character provides the reception of data from the HUB or the TX module where it usually connects the display …. I imagine it in the latter …….
        Thanks again and see you soon
        Silvano G.

        1. Hello,
          I test the sketch and confirming that works, this made me happy.
          There is only one problem, how to reset the barometric height to 0 meters transmitting 50 meters ????

          Thanks for answering
          Silvano G.
          silvano.g1@alice.it

  3. Dear Ceptimus,

    Thanks a lot for your nice tutorials.
    I have a question regarding Raspberry Pi board and xm1000 mote and would appreciate if you could help.

    I have been working with Raspberry pi and xm1000 mote for the past couple of months.
    I can read sensor (xm1000) information (e.g. temperature) as well as take a picture or shoot a video using pi camera.
    The code to read sensor information is written in C and the code for the camera is in python (You also provided a code to work with camera on C).

    first I want to check the temperature and then if it is above a threshold I would like to turn on the camera.

    For example:

    I read the temperature HERE.
    if (temp > 30)
    {
    HERE I need to run the code to turn on the camera
    }
    But I am dealing with errors for the past couple of weeks.

    I would appreciate if you can help.

    Thanks in advance.

    1. Hi,

      Thanks for your nice comment. I don’t have an XM1000 mote myself – and I’ve never tried using those Advanticsys modules. If you know the problem isn’t with your code that talks to the XM1000 then I could probably help with the other camera part. To try it out we could use the temperature sensor that’s built in to the Pi’s chip and then I don’t need an XM1000. Of course, the temperature sensor in the Pi measures how hot the chip is running, not the ambient temperature – but we could display the chip’s temperature and then you could change it by just blowing on the chip or putting a bit of cardboard over it to let it warm up – and we could use that to trigger the camera code. Would that help?

Leave a Reply

Your email address will not be published. Required fields are marked *