{"id":307,"date":"2017-10-10T20:54:16","date_gmt":"2017-10-10T20:54:16","guid":{"rendered":"http:\/\/ceptimus.co.uk\/?p=307"},"modified":"2017-10-10T20:54:16","modified_gmt":"2017-10-10T20:54:16","slug":"decoding-frsky-telemetry-data-with-an-arduino-part-two","status":"publish","type":"post","link":"https:\/\/ceptimus.co.uk\/index.php\/2017\/10\/10\/decoding-frsky-telemetry-data-with-an-arduino-part-two\/","title":{"rendered":"Decoding FrSKY telemetry data with an Arduino – Part two"},"content":{"rendered":"

In part one<\/a>, we looked at the standard FrSky ‘D series’ data packets that provide voltage and RSSI information.\u00a0 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.\u00a0 In part three<\/a> I’ll begin to 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.<\/p>\n

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.\u00a0 To try to reduce the confusion a little, I shall refer to the base station and the aircraft.\u00a0 If you’re using a surface vehicle such as a boat, just mentally substitute “boat” or whatever when you see “aircraft”.<\/p>\n

Let’s consider what happens at the base station.\u00a0 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.\u00a0 The 0xFE packets contained the basic A1\/A2\/RSSI data.\u00a0 Now all other data<\/strong> 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.\u00a0 I shall call these second type of data packets, “non-basic-data-packets” or NBDPs for short.<\/p>\n

Structure of a NBDP:<\/h5>\n

0xFD <length> <wasted> [up to six data bytes]<\/p>\n

Following the 0xFD identifier is a <length> byte that indicates (in binary) the number of data bytes in the packet.\u00a0 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.\u00a0 Next comes a wasted byte that contains no useful data.\u00a0 Then follows the actual data – and the number of bytes here is the value that was received in <length><\/p>\n

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()<\/p>\n

void handlePacket(uint8_t *packet) {\n  switch (packet[0]) {\n    case 0xFD:\n      if (packet[1] > 0 && packet[1] <= 6) {\n        for (int i = 0; i < packet[1]; i++) {\n          handleDataByte(packet[3 + i]);\n        }\n      }\n      break;\n    case 0xFE:\n      a1 = packet[1]; \/\/ A1: \n      a2 = packet[2]; \/\/ A2:\n      rssi = packet[3]; \/\/ main (Rx) link quality 100+ is full signal  40 is no signal\n      \/\/ packet[4] secondary (Tx) link quality.  Strength of signal received by Tx so not particularly useful.  Numbers are about double those of RSSI.\n      break;\n  }\n  cli(); timeout = 1000; sei();\n}\n<\/pre>\n
Now let’s consider the data stream format that the handleDataByte() routine has to decode.\u00a0 It’s useful to study this FrSky document<\/a> to understand what follows.<\/p>\n
Data frames coming from the hub are framed (start and end) with 0x5E bytes.\u00a0 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.<\/p>\n
Here’s the first part of the handleDataByte() routine.\u00a0 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:<\/p>\n
void handleDataByte(uint8_t data) {\n  static uint8_t dataPacket[4];\n  static uint8_t FrSkyUserDataMode = 0;\n  static uint8_t FrSkyUserDataLow;\n  static uint8_t FrSkyTelemetryDataID;\n  static bool FrSkyUserDataStuffing;\n  static bool FrSkyUserDataLowFlag; \/\/ flag for low byte of data, which is the first of the following two\n  int16_t i;\n  uint8_t high, low;\n\n  switch (FrSkyUserDataMode) {\n    case 0: \/\/ idle\n      if (data == 0x5E) { \/\/ telemetry hub frames begin with ^ (0x5E)\n        FrSkyUserDataMode = 1; \/\/ expect telemetry hub DataID next\n      }\n      break;\n    case 1: \/\/ expecting telemetry hub DataID\n      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\n        FrSkyUserDataMode = 0; \/\/ back to idle mode\n      }\n      break;\n    case 2: \/\/ expecting two bytes of data\n      if (FrSkyUserDataStuffing) {\n        FrSkyUserDataStuffing = false;\n        if ((data != 0x3D) && (data != 0x3E)) { \/\/ byte stuffing is only valid for (unstuffed) bytes 0x5D or or 0x5E\n          FrSkyUserDataMode = 0; \/\/ back to idle mode\n          break;\n        }\n        else {\n          data ^= 0x20; \/\/ unstuff byte\n        }\n      }\n      else if (data == 0x5D) { \/\/ following byte is stuffed\n        FrSkyUserDataStuffing = true;\n        break;\n      }\n<\/pre>\n
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:<\/p>\n
      if (FrSkyUserDataLowFlag) { \/\/ expecting low byte of data\n        FrSkyUserDataLow = data; \/\/ remember low byte\n        FrSkyUserDataLowFlag = false; \/\/ expect high byte next\n      }\n      else { \/\/ expecting high byte of data\n        switch (FrSkyTelemetryDataID) {\n          case 0x01: \/\/ 1st part of GPS altitude\n          case 0x10: \/\/ 1st part of barimetric altitude\n          case 0x11: \/\/ 1st part of GPS speed\n          case 0x12: \/\/ 1st part of longitude\n          case 0x13: \/\/ 1st part of latitude\n          case 0x14: \/\/ 1st part of GPS course over ground\n          case 0x15: \/\/ 1st part of date\n          case 0x17: \/\/ 1st part of time\n            dataPacket[0] = FrSkyUserDataLow;\n            dataPacket[1] = data; \/\/ store value till last part received\n            break;\n          case 0x02: \/\/ temperature 1\n            Serial.print(\"Temperature 1: \");\n            Serial.print(format(FrSkyUserDataLow, data));\n            Serial.print(\"\\n\");\n            break;\n          case 0x05: \/\/ temperature 2\n            Serial.print(\"Temperature 2: \");\n            Serial.print(format(FrSkyUserDataLow, data));\n            Serial.print(\"\\n\");\n            break;\n          case 0x09: \/\/ second (final) part of GPS altitude\n            Serial.print(\"GPS Altitude: \");\n            Serial.print(format(dataPacket[0], dataPacket[1]));\n            Serial.print(\".\");\n            Serial.print(format(FrSkyUserDataLow, data));\n            Serial.print(\"\\n\");\n            break;\n          case 0x16: \/\/ second (final) part of date\n            Serial.print(\"Date: 20\"); \/\/ assume 21st century\n            Serial.print(format(FrSkyUserDataLow, 0)); \/\/ year\n            Serial.print(\"-\");\n            Serial.print(format(dataPacket[1], 0)); \/\/ month\n            Serial.print(\"-\");\n            Serial.print(format(dataPacket[0], 0)); \/\/ day\n            Serial.print(\"\\n\");\n            break;\n          case 0x18: \/\/ second (final) part of time\n            Serial.print(\"Time: \");\n            Serial.print(format(dataPacket[0], 0)); \/\/ hour\n            Serial.print(\":\");\n            Serial.print(format(dataPacket[1], 0)); \/\/ minute\n            Serial.print(\":\");\n            Serial.print(format(FrSkyUserDataLow, 0)); \/\/ second\n            Serial.print(\"\\n\");\n            break;\n          case 0x19: \/\/ second (final) part of GPS speed\n            Serial.print(\"GPS speed: \");\n            Serial.print(format(dataPacket[0], dataPacket[1]));\n            Serial.print(\".\");\n            Serial.print(format(FrSkyUserDataLow, data));\n            Serial.print(\"\\n\");\n            break;\n          case 0x1A: \/\/ second part of longitude\n          case 0x1B: \/\/ second part of latitude\n            dataPacket[2] = FrSkyUserDataLow;\n            dataPacket[3] = data; \/\/ store value till last part received\n            break;\n          case 0x1C: \/\/ second (final) part of GPS cog\n            Serial.print(\"GPS C.O.G.: \");\n            Serial.print(format(dataPacket[0], dataPacket[1]));\n            Serial.print(\".\");\n            Serial.print(format(FrSkyUserDataLow, data));\n            Serial.print(\"\\n\");\n            break;\n          case 0x21: \/\/ second (final) part of barometric altitude\n            Serial.print(\"Barometric Altitude: \");\n            Serial.print(format(dataPacket[0], dataPacket[1]));\n            Serial.print(\".\");\n            Serial.print(format(FrSkyUserDataLow, data));\n            Serial.print(\"\\n\");\n            break;\n          case 0x22: \/\/ third (final) part of longitude\n            Serial.print(\"Longitude: \");\n            i = (uint16_t)dataPacket[1];\n            i = (i << 8) | dataPacket[0]; \/\/ degrees * 100 + minutes high = (i \/ 100) >> 8; low = (i \/ 100) & 0x00FF;\n            Serial.print(format(low, high));\n            Serial.print(\" deg \");\n            Serial.print(format(i % 100, 0));\n            Serial.print(\".\");\n            Serial.print(format(dataPacket[2], dataPacket[3]));\n            Serial.print(\" min \");\n            Serial.write(FrSkyUserDataLow); \/\/ 'E' or 'W'\n            Serial.print(\"\\n\");\n            break;\n          case 0x23: \/\/ third (final) part of latitude\n            Serial.print(\"Latitude: \");\n            i = (uint16_t)dataPacket[1];\n            i = (i << 8) | dataPacket[0]; \/\/ degrees * 100 + minutes high = (i \/ 100) >> 8; low = (i \/ 100) & 0x00FF;\n            Serial.print(format(low, high));\n            Serial.print(\" deg \");\n            Serial.print(format(i % 100, 0));\n            Serial.print(\".\");\n            Serial.print(format(dataPacket[2], dataPacket[3]));\n            Serial.print(\" min \");\n            Serial.write(FrSkyUserDataLow); \/\/ 'N' or 'S'\n            Serial.print(\"\\n\");\n            break;\n          case 0x24: \/\/ x acceleration\n            Serial.print(\"X acceleration: \");\n            Serial.print(format(FrSkyUserDataLow, data));\n            Serial.print(\"\\n\");\n            break;\n          case 0x25: \/\/ y acceleration\n            Serial.print(\"Y acceleration: \");\n            Serial.print(format(FrSkyUserDataLow, data));\n            Serial.print(\"\\n\");\n            break;\n          case 0x26: \/\/ z acceleration\n            Serial.print(\"Z acceleration: \");\n            Serial.print(format(FrSkyUserDataLow, data));\n            Serial.print(\"\\n\");\n            break;\n          default:\n            break;\n        }\n        FrSkyUserDataMode = 0; \/\/ received and stored both data bytes, so back to idle mode\n      } \/\/ else\n      break;\n    default: \/\/ should never happen\n      FrSkyUserDataMode = 0; \/\/ back to idle mode\n      break;\n  }\n}<\/pre>\n
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.<\/p>\n
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'\n  static char buffer[8];\n  char *p;\n  p = buffer;\n  int16_t i = (int16_t)high;\n  i = (i << 8) | low;\n  if (i < 0) {\n    *p++ = '-';\n    i = -i;\n  }\n  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) {\n    uint8_t digit = '0';\n    while (i >= tenPower) {\n      digit++;\n      i -= tenPower;\n    }\n    *p++ = digit;\n  }\n  *p = '\\0';\n  return buffer;\n}<\/pre>\n
Here’s some sample output.\u00a0 I changed the latitude and longitude values so as not to show my home location:<\/p>\n
RSSI: 87 AD1: 96 AD2: 142\nRSSI: 87 AD1: 96 AD2: 141\nBarometric Altitude: 95.18\nGPS C.O.G.: 00.00\nGPS speed: 00.96\nRSSI: 87 AD1: 96 AD2: 143\nGPS Altitude: 63.00\nDate: 2017-10-10\nTime: 20:01:27\nX acceleration: -60\nY acceleration: 133\nZ acceleration: 963\nLatitude: 53 deg 17.4902 min N\nLongitude: 03 deg 33.1422 min W\nRSSI: 87 AD1: 96 AD2: 141\nTemperature 1: 25\nTemperature 2: 26\nRSSI: 86 AD1: 96 AD2: 143\nRSSI: 87 AD1: 96 AD2: 140\n<\/pre>\n
Note that the RSSI frames arrive more frequently than the other types of data.<\/p>\n
You can download the complete working sketch using this link.<\/a><\/p>\n
Continued in part three…<\/a><\/p>\n","protected":false},"excerpt":{"rendered":"
In part one, we looked at the standard FrSky ‘D series’ data packets that provide voltage and RSSI information.\u00a0 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.\u00a0 In part […]<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[3,4,6],"tags":[],"class_list":["post-307","post","type-post","status-publish","format-standard","hentry","category-arduino","category-programming","category-uav"],"_links":{"self":[{"href":"https:\/\/ceptimus.co.uk\/index.php\/wp-json\/wp\/v2\/posts\/307","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/ceptimus.co.uk\/index.php\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/ceptimus.co.uk\/index.php\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/ceptimus.co.uk\/index.php\/wp-json\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/ceptimus.co.uk\/index.php\/wp-json\/wp\/v2\/comments?post=307"}],"version-history":[{"count":0,"href":"https:\/\/ceptimus.co.uk\/index.php\/wp-json\/wp\/v2\/posts\/307\/revisions"}],"wp:attachment":[{"href":"https:\/\/ceptimus.co.uk\/index.php\/wp-json\/wp\/v2\/media?parent=307"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/ceptimus.co.uk\/index.php\/wp-json\/wp\/v2\/categories?post=307"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/ceptimus.co.uk\/index.php\/wp-json\/wp\/v2\/tags?post=307"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}