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 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.
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.
Continued in part three…
Leave a Reply