AFG/dump1090
3
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

Restructure decoding to use a helper function to extract bit ranges

Browse source 
With forced inlining this is about as fast, and it is much less
errorprone than the twisty little maze of handcoded bitshifts that
it was before.

(notably, at least one error - in the ACAS RI field - has been fixed)
This commit is contained in:
Oliver Jowett 2016-09-01 22:10:17 +01:00
parent 303d3c3fef
commit a1fdc07db2
1 changed files with 202 additions and 136 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

332
mode_s.c
View file

@ -53,6 +53,8 @@
/* for PRIX64 */ /* for PRIX64 */
#include <inttypes.h> #include <inttypes.h>
#include <assert.h>
// //
// ===================== Mode S detection and decoding =================== // ===================== Mode S detection and decoding ===================
// //
@ -218,6 +220,67 @@ static int correct_aa_field(uint32_t *addr, struct errorinfo *ei)
return addr_errors; return addr_errors;
} }
// The first bit (MSB of the first byte) is numbered 1, for consistency
// with how the specs number them.
// Extract one bit from a message.
static inline __attribute__((always_inline)) unsigned getbit(unsigned char *data, unsigned bitnum)
{
unsigned bi = bitnum - 1;
unsigned by = bi >> 3;
unsigned mask = 1 << (7 - (bi & 7));
return (data[by] & mask) != 0;
}
// Extract some bits (firstbit .. lastbit inclusive) from a message.
static inline __attribute__((always_inline)) unsigned getbits(unsigned char *data, unsigned firstbit, unsigned lastbit)
{
unsigned fbi = firstbit - 1;
unsigned lbi = lastbit - 1;
unsigned nbi = (lastbit - firstbit + 1);
unsigned fby = fbi >> 3;
unsigned lby = lbi >> 3;
unsigned nby = (lby - fby) + 1;
unsigned shift = 7 - (lbi & 7);
unsigned topmask = 0xFF >> (fbi & 7);
assert (fbi <= lbi);
assert (nbi <= 32);
assert (nby <= 5);
if (nby == 5) {
return
((data[fby] & topmask) << (32 - shift)) |
(data[fby + 1] << (24 - shift)) |
(data[fby + 2] << (16 - shift)) |
(data[fby + 3] << (8 - shift)) |
(data[fby + 4] >> shift);
} else if (nby == 4) {
return
((data[fby] & topmask) << (24 - shift)) |
(data[fby + 1] << (16 - shift)) |
(data[fby + 2] << (8 - shift)) |
(data[fby + 3] >> shift);
} else if (nby == 3) {
return
((data[fby] & topmask) << (16 - shift)) |
(data[fby + 1] << (8 - shift)) |
(data[fby + 2] >> shift);
} else if (nby == 2) {
return
((data[fby] & topmask) << (8 - shift)) |
(data[fby + 1] >> shift);
} else if (nby == 1) {
return
(data[fby] & topmask) >> shift;
} else {
return 0;
}
}
// Score how plausible this ModeS message looks. // Score how plausible this ModeS message looks.
// The more positive, the more reliable the message is // The more positive, the more reliable the message is
@ -254,7 +317,7 @@ int scoreModesMessage(unsigned char *msg, int validbits)
if (validbits < 56) if (validbits < 56)
return -2; return -2;
msgtype = msg[0] >> 3; // Downlink Format msgtype = getbits(msg, 1, 5); // Downlink Format
msgbits = modesMessageLenByType(msgtype); msgbits = modesMessageLenByType(msgtype);
if (validbits < msgbits) if (validbits < msgbits)
@ -283,7 +346,7 @@ int scoreModesMessage(unsigned char *msg, int validbits)
case 11: // All-call reply case 11: // All-call reply
iid = crc & 0x7f; iid = crc & 0x7f;
crc = crc & 0xffff80; crc = crc & 0xffff80;
addr = (msg[1] << 16) | (msg[2] << 8) | (msg[3]); addr = getbits(msg, 9, 32);
ei = modesChecksumDiagnose(crc, msgbits); ei = modesChecksumDiagnose(crc, msgbits);
if (!ei) if (!ei)
@ -318,7 +381,7 @@ int scoreModesMessage(unsigned char *msg, int validbits)
return -2; // can't correct errors return -2; // can't correct errors
// fix any errors in the address field // fix any errors in the address field
addr = (msg[1] << 16) | (msg[2] << 8) | (msg[3]); addr = getbits(msg, 9, 32);
correct_aa_field(&addr, ei); correct_aa_field(&addr, ei);
if (icaoFilterTest(addr)) if (icaoFilterTest(addr))
@ -372,7 +435,7 @@ int decodeModesMessage(struct modesMessage *mm, unsigned char *msg)
msg = mm->msg; msg = mm->msg;
// Get the message type ASAP as other operations depend on this // Get the message type ASAP as other operations depend on this
mm->msgtype = msg[0] >> 3; // Downlink Format mm->msgtype = getbits(msg, 1, 5); // Downlink Format
mm->msgbits = modesMessageLenByType(mm->msgtype); mm->msgbits = modesMessageLenByType(mm->msgtype);
mm->crc = modesChecksum(msg, mm->msgbits); mm->crc = modesChecksum(msg, mm->msgbits);
mm->correctedbits = 0; mm->correctedbits = 0;
@ -429,7 +492,7 @@ int decodeModesMessage(struct modesMessage *mm, unsigned char *msg)
// check whether the corrected message looks sensible // check whether the corrected message looks sensible
// we are conservative here: only accept corrected messages that // we are conservative here: only accept corrected messages that
// match an existing aircraft. // match an existing aircraft.
addr = (msg[1] << 16) | (msg[2] << 8) | (msg[3]); addr = getbits(msg, 9, 32);
if (!icaoFilterTest(addr)) { if (!icaoFilterTest(addr)) {
return -1; return -1;
} }
@ -450,10 +513,10 @@ int decodeModesMessage(struct modesMessage *mm, unsigned char *msg)
return -2; // couldn't fix it return -2; // couldn't fix it
} }
addr1 = (msg[1] << 16) | (msg[2] << 8) | (msg[3]); addr1 = getbits(msg, 9, 32);
mm->correctedbits = ei->errors; mm->correctedbits = ei->errors;
modesChecksumFix(msg, ei); modesChecksumFix(msg, ei);
addr2 = (msg[1] << 16) | (msg[2] << 8) | (msg[3]); addr2 = getbits(msg, 9, 32);
// we are conservative here: only accept corrected messages that // we are conservative here: only accept corrected messages that
// match an existing aircraft. // match an existing aircraft.
@ -494,12 +557,12 @@ int decodeModesMessage(struct modesMessage *mm, unsigned char *msg)
// AA (Address announced) // AA (Address announced)
if (mm->msgtype == 11 || mm->msgtype == 17 || mm->msgtype == 18) { if (mm->msgtype == 11 || mm->msgtype == 17 || mm->msgtype == 18) {
mm->AA = mm->addr = (msg[1] << 16) | (msg[2] << 8) | (msg[3]); mm->AA = mm->addr = getbits(msg, 9, 32);
} }
// AC (Altitude Code) // AC (Altitude Code)
if (mm->msgtype == 0 || mm->msgtype == 4 || mm->msgtype == 16 || mm->msgtype == 20) { if (mm->msgtype == 0 || mm->msgtype == 4 || mm->msgtype == 16 || mm->msgtype == 20) {
mm->AC = ((msg[2] << 8) | msg[3]) & 0x1FFF; mm->AC = getbits(msg, 20, 32);
if (mm->AC) { // Only attempt to decode if a valid (non zero) altitude is present if (mm->AC) { // Only attempt to decode if a valid (non zero) altitude is present
mm->altitude = decodeAC13Field(mm->AC, &mm->altitude_unit); mm->altitude = decodeAC13Field(mm->AC, &mm->altitude_unit);
if (mm->altitude != INVALID_ALTITUDE) if (mm->altitude != INVALID_ALTITUDE)
@ -512,7 +575,7 @@ int decodeModesMessage(struct modesMessage *mm, unsigned char *msg)
// CA (Capability) // CA (Capability)
if (mm->msgtype == 11 || mm->msgtype == 17) { if (mm->msgtype == 11 || mm->msgtype == 17) {
mm->CA = (msg[0] & 0x07); mm->CA = getbits(msg, 6, 8);
switch (mm->CA) { switch (mm->CA) {
case 0: case 0:
@ -535,22 +598,22 @@ int decodeModesMessage(struct modesMessage *mm, unsigned char *msg)
// CC (Cross-link capability) // CC (Cross-link capability)
if (mm->msgtype == 0) { if (mm->msgtype == 0) {
mm->CC = (msg[0] & 0x02) ? 1 : 0; mm->CC = getbit(msg, 7);
} }
// CF (Control field) // CF (Control field)
if (mm->msgtype == 18) { if (mm->msgtype == 18) {
mm->CF = msg[0] & 7; mm->CF = getbits(msg, 5, 8);
} }
// DR (Downlink Request) // DR (Downlink Request)
if (mm->msgtype == 4 || mm->msgtype == 5 || mm->msgtype == 20 || mm->msgtype == 21) { if (mm->msgtype == 4 || mm->msgtype == 5 || mm->msgtype == 20 || mm->msgtype == 21) {
mm->DR = (msg[1] >> 3) & 0x1F; mm->DR = getbits(msg, 9, 13);
} }
// FS (Flight Status) // FS (Flight Status)
if (mm->msgtype == 4 || mm->msgtype == 5 || mm->msgtype == 20 || mm->msgtype == 21) { if (mm->msgtype == 4 || mm->msgtype == 5 || mm->msgtype == 20 || mm->msgtype == 21) {
mm->FS = msg[0] & 7; mm->FS = getbits(msg, 6, 8);
mm->alert_valid = 1; mm->alert_valid = 1;
mm->spi_valid = 1; mm->spi_valid = 1;
@ -588,7 +651,7 @@ int decodeModesMessage(struct modesMessage *mm, unsigned char *msg)
// ID (Identity) // ID (Identity)
if (mm->msgtype == 5 || mm->msgtype == 21) { if (mm->msgtype == 5 || mm->msgtype == 21) {
// Gillham encoded Squawk // Gillham encoded Squawk
mm->ID = ((msg[2] << 8) | msg[3]) & 0x1FFF; mm->ID = getbits(msg, 20, 32);
if (mm->ID) { if (mm->ID) {
mm->squawk = decodeID13Field(mm->ID); mm->squawk = decodeID13Field(mm->ID);
mm->squawk_valid = 1; mm->squawk_valid = 1;
@ -597,7 +660,7 @@ int decodeModesMessage(struct modesMessage *mm, unsigned char *msg)
// KE (Control, ELM) // KE (Control, ELM)
if (mm->msgtype >= 24 && mm->msgtype <= 31) { if (mm->msgtype >= 24 && mm->msgtype <= 31) {
mm->KE = (msg[0] & 0x10) ? 1 : 0; mm->KE = getbit(msg, 4);
} }
// MB (messsage, Comm-B) // MB (messsage, Comm-B)
@ -624,27 +687,27 @@ int decodeModesMessage(struct modesMessage *mm, unsigned char *msg)
// ND (number of D-segment, Comm-D) // ND (number of D-segment, Comm-D)
if (mm->msgtype >= 24 && mm->msgtype <= 31) { if (mm->msgtype >= 24 && mm->msgtype <= 31) {
mm->ND = msg[0] & 0x0F; mm->ND = getbits(msg, 5, 8);
} }
// RI (Reply information, ACAS) // RI (Reply information, ACAS)
if (mm->msgtype == 0 || mm->msgtype == 16) { if (mm->msgtype == 0 || mm->msgtype == 16) {
mm->RI = ((msg[2] & 0x07) << 1) | ((msg[3] >> 7) & 0x01); mm->RI = getbits(msg, 14, 17);
} }
// SL (Sensitivity level, ACAS) // SL (Sensitivity level, ACAS)
if (mm->msgtype == 0 || mm->msgtype == 16) { if (mm->msgtype == 0 || mm->msgtype == 16) {
mm->SL = (msg[1] >> 5) & 0x07; mm->SL = getbits(msg, 9, 11);
} }
// UM (Utility Message) // UM (Utility Message)
if (mm->msgtype == 4 || mm->msgtype == 5 || mm->msgtype == 20 || mm->msgtype == 21) { if (mm->msgtype == 4 || mm->msgtype == 5 || mm->msgtype == 20 || mm->msgtype == 21) {
mm->UM = ((msg[1] & 0x07) << 3) | ((msg[2] >> 5) & 0x07); mm->UM = getbits(msg, 14, 19);
} }
// VS (Vertical Status) // VS (Vertical Status)
if (mm->msgtype == 0 || mm->msgtype == 16) { if (mm->msgtype == 0 || mm->msgtype == 16) {
mm->VS = (msg[0] & 0x04) ? 1 : 0; mm->VS = getbit(msg, 6);
if (mm->VS) if (mm->VS)
mm->airground = AG_GROUND; mm->airground = AG_GROUND;
else else
@ -676,8 +739,8 @@ static void decodeBDS20(struct modesMessage *mm)
uint32_t chars1, chars2; uint32_t chars1, chars2;
unsigned char *msg = mm->msg; unsigned char *msg = mm->msg;
chars1 = (msg[5] << 16) | (msg[6] << 8) | (msg[7]); chars1 = getbits(msg, 41, 64);
chars2 = (msg[8] << 16) | (msg[9] << 8) | (msg[10]); chars2 = getbits(msg, 65, 88);
// A common failure mode seems to be to intermittently send // A common failure mode seems to be to intermittently send
// all zeros. Catch that here. // all zeros. Catch that here.
@ -701,10 +764,9 @@ static void decodeBDS20(struct modesMessage *mm)
static void decodeExtendedSquitter(struct modesMessage *mm) static void decodeExtendedSquitter(struct modesMessage *mm)
{ {
unsigned char *msg = mm->msg;
unsigned char *me = mm->ME; unsigned char *me = mm->ME;
int metype = mm->metype = msg[4] >> 3; // Extended squitter message type int metype = mm->metype = getbits(me, 1, 5);
int mesub = mm->mesub = (metype == 29 ? ((msg[4]&6)>>1) : (msg[4] & 7)); // Extended squitter message subtype int mesub = mm->mesub = (metype == 29 ? getbits(me, 6, 7) : getbits(me, 6, 8)); // Extended squitter message subtype
int check_imf = 0; int check_imf = 0;
@ -727,7 +789,7 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
// TODO: decode me. // TODO: decode me.
// For now we only look at the IMF bit. // For now we only look at the IMF bit.
mm->source = SOURCE_TISB; mm->source = SOURCE_TISB;
if (msg[4] & 0x80) if (getbit(me, 1))
mm->addr |= MODES_NON_ICAO_ADDRESS; mm->addr |= MODES_NON_ICAO_ADDRESS;
return; return;
@ -751,8 +813,8 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
// Aircraft Identification and Category // Aircraft Identification and Category
uint32_t chars1, chars2; uint32_t chars1, chars2;
chars1 = (msg[5] << 16) | (msg[6] << 8) | (msg[7]); chars1 = getbits(me, 9, 32);
chars2 = (msg[8] << 16) | (msg[9] << 8) | (msg[10]); chars2 = getbits(me, 33, 56);
// A common failure mode seems to be to intermittently send // A common failure mode seems to be to intermittently send
// all zeros. Catch that here. // all zeros. Catch that here.
@ -778,29 +840,30 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
} }
case 19: { // Airborne Velocity Message case 19: { // Airborne Velocity Message
if (check_imf && (msg[5] & 0x80)) if (check_imf && getbit(me, 9))
mm->addr |= MODES_NON_ICAO_ADDRESS; mm->addr |= MODES_NON_ICAO_ADDRESS;
if ( (mesub >= 1) && (mesub <= 4) ) { if ( (mesub >= 1) && (mesub <= 4) ) {
int vert_rate = ((msg[8] & 0x07) << 6) | (msg[9] >> 2); int vert_rate = getbits(me, 38, 46);
if (vert_rate) { if (vert_rate) {
--vert_rate; --vert_rate;
if (msg[8] & 0x08) if (getbit(me, 37)) {
{vert_rate = 0 - vert_rate;} vert_rate = 0 - vert_rate;
}
mm->vert_rate = vert_rate * 64; mm->vert_rate = vert_rate * 64;
mm->vert_rate_valid = 1; mm->vert_rate_valid = 1;
} }
mm->vert_rate_source = (msg[8] & 0x10 ? ALTITUDE_GNSS : ALTITUDE_BARO); mm->vert_rate_source = (getbit(me, 36) ? ALTITUDE_GNSS : ALTITUDE_BARO);
} }
if ((mesub == 1) || (mesub == 2)) { if ((mesub == 1) || (mesub == 2)) {
unsigned ew_raw = ((msg[5] & 0x03) << 8) | msg[6]; unsigned ew_raw = getbits(me, 15, 24);
unsigned ns_raw = ((msg[7] & 0x7F) << 3) | (msg[8] >> 5); unsigned ns_raw = getbits(me, 26, 35);
if (ew_raw && ns_raw) { if (ew_raw && ns_raw) {
int ew_vel = (ew_raw - 1) * ((msg[5] & 0x04) ? -1 : 1) * ((mesub == 2) ? 4 : 1); int ew_vel = (ew_raw - 1) * (getbit(me, 14) ? -1 : 1) * ((mesub == 2) ? 4 : 1);
int ns_vel = (ns_raw - 1) * ((msg[7] & 0x80) ? -1 : 1) * ((mesub == 2) ? 4 : 1); int ns_vel = (ns_raw - 1) * (getbit(me, 25) ? -1 : 1) * ((mesub == 2) ? 4 : 1);
// Compute velocity and angle from the two speed components // Compute velocity and angle from the two speed components
mm->speed = (unsigned) sqrt((ns_vel * ns_vel) + (ew_vel * ew_vel) + 0.5); mm->speed = (unsigned) sqrt((ns_vel * ns_vel) + (ew_vel * ew_vel) + 0.5);
@ -819,27 +882,28 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
mm->speed_source = SPEED_GROUNDSPEED; mm->speed_source = SPEED_GROUNDSPEED;
} }
} else if (mesub == 3 || mesub == 4) { } else if (mesub == 3 || mesub == 4) {
unsigned airspeed = ((msg[7] & 0x7f) << 3) | (msg[8] >> 5); unsigned airspeed = getbits(me, 26, 35);
if (airspeed) { if (airspeed) {
--airspeed; --airspeed;
if (mesub == 4) { // If (supersonic) unit is 4 kts if (mesub == 4) { // If (supersonic) unit is 4 kts
airspeed *= 4; airspeed *= 4;
} }
mm->speed = airspeed; mm->speed = airspeed;
mm->speed_source = (msg[7] & 0x80) ? SPEED_TAS : SPEED_IAS; mm->speed_source = getbit(me, 25) ? SPEED_TAS : SPEED_IAS;
mm->speed_valid = 1; mm->speed_valid = 1;
} }
if (msg[5] & 0x04) { if (getbit(me, 14)) {
mm->heading = ((((msg[5] & 0x03) << 8) | msg[6]) * 45) >> 7; mm->heading = getbits(me, 15, 24);
mm->heading_source = HEADING_MAGNETIC; mm->heading_source = HEADING_MAGNETIC;
mm->heading_valid = 1; mm->heading_valid = 1;
} }
} }
if (msg[10] & 0x7f) { unsigned raw_delta = getbits(me, 50, 56);
if (raw_delta) {
mm->gnss_delta_valid = 1; mm->gnss_delta_valid = 1;
mm->gnss_delta = ((msg[10] & 0x80) ? -25 : 25) * ((msg[10] & 0x7f) - 1); mm->gnss_delta = (raw_delta - 1) * (getbit(me, 49) ? -25 : 25);
} }
break; break;
@ -849,28 +913,27 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
// Ground position // Ground position
int movement; int movement;
if (check_imf && (msg[6] & 0x08)) if (check_imf && getbit(me, 21))
mm->addr |= MODES_NON_ICAO_ADDRESS; mm->addr |= MODES_NON_ICAO_ADDRESS;
mm->airground = AG_GROUND; // definitely. mm->airground = AG_GROUND; // definitely.
mm->cpr_lat = ((msg[6] & 3) << 15) | (msg[7] << 7) | (msg[8] >> 1); mm->cpr_lat = getbits(me, 23, 39);
mm->cpr_lon = ((msg[8] & 1) << 16) | (msg[9] << 8) | (msg[10]); mm->cpr_lon = getbits(me, 40, 56);
if (mm->msg[6] & 0x04) mm->cpr_odd = getbit(me, 22);
mm->cpr_odd = 1;
mm->cpr_nucp = (14 - metype); mm->cpr_nucp = (14 - metype);
mm->cpr_valid = 1; mm->cpr_valid = 1;
movement = ((msg[4] << 4) | (msg[5] >> 4)) & 0x007F; movement = getbits(me, 6, 12);
if ((movement) && (movement < 125)) { if ((movement) && (movement < 125)) {
mm->speed_valid = 1; mm->speed_valid = 1;
mm->speed = decodeMovementField(movement); mm->speed = decodeMovementField(movement);
mm->speed_source = SPEED_GROUNDSPEED; mm->speed_source = SPEED_GROUNDSPEED;
} }
if (msg[5] & 0x08) { if (getbit(me, 13)) {
mm->heading_valid = 1; mm->heading_valid = 1;
mm->heading_source = HEADING_TRUE; mm->heading_source = HEADING_TRUE;
mm->heading = ((((msg[5] << 4) | (msg[6] >> 4)) & 0x007F) * 45) >> 4; mm->heading = getbits(me, 14, 20) * 360 / 128;
} }
break; break;
@ -879,9 +942,9 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
case 0: // Airborne position, baro altitude only case 0: // Airborne position, baro altitude only
case 9: case 10: case 11: case 12: case 13: case 14: case 15: case 16: case 17: case 18: // Airborne position, baro case 9: case 10: case 11: case 12: case 13: case 14: case 15: case 16: case 17: case 18: // Airborne position, baro
case 20: case 21: case 22: { // Airborne position, GNSS altitude (HAE or MSL) case 20: case 21: case 22: { // Airborne position, GNSS altitude (HAE or MSL)
int AC12Field = ((msg[5] << 4) | (msg[6] >> 4)) & 0x0FFF; int AC12Field = getbits(me, 9, 20);
if (check_imf && (msg[4] & 0x01)) if (check_imf && getbit(me, 8))
mm->addr |= MODES_NON_ICAO_ADDRESS; mm->addr |= MODES_NON_ICAO_ADDRESS;
if (metype == 0) { if (metype == 0) {
@ -890,8 +953,8 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
// Catch some common failure modes and don't mark them as valid // Catch some common failure modes and don't mark them as valid
// (so they won't be used for positioning) // (so they won't be used for positioning)
mm->cpr_lat = ((msg[6] & 3) << 15) | (msg[7] << 7) | (msg[8] >> 1); mm->cpr_lat = getbits(me, 23, 39);
mm->cpr_lon = ((msg[8] & 1) << 16) | (msg[9] << 8) | (msg[10]); mm->cpr_lon = getbits(me, 40, 56);
if (AC12Field == 0 && mm->cpr_lon == 0 && (mm->cpr_lat & 0x0fff) == 0 && mm->metype == 15) { if (AC12Field == 0 && mm->cpr_lon == 0 && (mm->cpr_lat & 0x0fff) == 0 && mm->metype == 15) {
// Seen from at least: // Seen from at least:
@ -904,8 +967,7 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
} else { } else {
// Otherwise, assume it's valid. // Otherwise, assume it's valid.
mm->cpr_valid = 1; mm->cpr_valid = 1;
if (mm->msg[6] & 0x04) mm->cpr_odd = getbit(me, 22);
mm->cpr_odd = 1;
if (metype == 18 || metype == 22) if (metype == 18 || metype == 22)
mm->cpr_nucp = 0; mm->cpr_nucp = 0;
@ -930,7 +992,7 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
case 23: { // Test message case 23: { // Test message
if (mesub == 7) { // (see 1090-WP-15-20) if (mesub == 7) { // (see 1090-WP-15-20)
int ID13Field = (((msg[5] << 8) | msg[6]) & 0xFFF1)>>3; int ID13Field = getbits(me, 9, 21);
if (ID13Field) { if (ID13Field) {
mm->squawk_valid = 1; mm->squawk_valid = 1;
mm->squawk = decodeID13Field(ID13Field); mm->squawk = decodeID13Field(ID13Field);
@ -944,25 +1006,30 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
case 28: { // Extended Squitter Aircraft Status case 28: { // Extended Squitter Aircraft Status
if (mesub == 1) { // Emergency status squawk field if (mesub == 1) { // Emergency status squawk field
int ID13Field = (((msg[5] << 8) | msg[6]) & 0x1FFF); int ID13Field = getbits(me, 12, 24);
if (ID13Field) { if (ID13Field) {
mm->squawk_valid = 1; mm->squawk_valid = 1;
mm->squawk = decodeID13Field(ID13Field); mm->squawk = decodeID13Field(ID13Field);
} }
if (check_imf && (msg[10] & 0x01)) if (check_imf && getbit(me, 56))
mm->addr |= MODES_NON_ICAO_ADDRESS; mm->addr |= MODES_NON_ICAO_ADDRESS;
} }
break; break;
} }
case 29: // Aircraft Trajectory Intent case 29: // Aircraft Trajectory Intent
if (mesub == 1) { // Target state and status, V2 if (check_imf && getbit(me, 51))
mm->tss.valid = 1; mm->addr |= MODES_NON_ICAO_ADDRESS;
mm->tss.sil_type = (me[0] & 0x01) ? SIL_PER_SAMPLE : SIL_PER_HOUR;
mm->tss.altitude_type = (me[1] & 0x80) ? TSS_ALTITUDE_FMS : TSS_ALTITUDE_MCP;
unsigned alt_bits = ((me[1] << 4) | (me[2] >> 4)) & 0x7FF; if (mesub == 0) { // Target state and status, V1
// TODO: need RTCA/DO-260A
} else if (mesub == 1) { // Target state and status, V2
mm->tss.valid = 1;
mm->tss.sil_type = getbit(me, 8) ? SIL_PER_SAMPLE : SIL_PER_HOUR;
mm->tss.altitude_type = getbit(me, 9) ? TSS_ALTITUDE_FMS : TSS_ALTITUDE_MCP;
unsigned alt_bits = getbits(me, 10, 20);
if (alt_bits == 0) { if (alt_bits == 0) {
mm->tss.altitude_valid = 0; mm->tss.altitude_valid = 0;
} else { } else {
@ -970,7 +1037,7 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
mm->tss.altitude = (alt_bits - 1) * 32; mm->tss.altitude = (alt_bits - 1) * 32;
} }
unsigned baro_bits = ((me[2] << 5) | (me[3] >> 3)) & 0x1FF; unsigned baro_bits = getbits(me, 21, 29);
if (baro_bits == 0) { if (baro_bits == 0) {
mm->tss.baro_valid = 0; mm->tss.baro_valid = 0;
} else { } else {
@ -978,26 +1045,25 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
mm->tss.baro = 800.0 + (baro_bits - 1) * 0.8; mm->tss.baro = 800.0 + (baro_bits - 1) * 0.8;
} }
mm->tss.heading_valid = (me[3] & 0x04) != 0; mm->tss.heading_valid = getbit(me, 30);
if (mm->tss.heading_valid) { if (mm->tss.heading_valid) {
// two's complement -180..+180, which is conveniently // two's complement -180..+180, which is conveniently
// also the same as unsigned 0..360 // also the same as unsigned 0..360
unsigned heading_bits = ((me[3] << 7) | (me[4] >> 1)) & 0x1ff; mm->tss.heading = getbits(me, 31, 39) * 180 / 256;
mm->tss.heading = heading_bits * 180 / 256;
} }
mm->tss.nac_p = ((me[4] << 3) | (me[5] >> 5)) & 0x0f; mm->tss.nac_p = getbits(me, 40, 43);
mm->tss.nic_baro = (me[5] & 0x10) ? 1 : 0; mm->tss.nic_baro = getbit(me, 44);
mm->tss.sil = (me[5] >> 2) & 0x03; mm->tss.sil = getbits(me, 45, 46);
mm->tss.mode_valid = (me[5] & 0x02) ? 1 : 0; mm->tss.mode_valid = getbit(me, 47);
if (mm->tss.mode_valid) { if (mm->tss.mode_valid) {
mm->tss.mode_autopilot = (me[5] & 0x01) != 0; mm->tss.mode_autopilot = getbit(me, 48);
mm->tss.mode_vnav = (me[6] & 0x80) != 0; mm->tss.mode_vnav = getbit(me, 49);
mm->tss.mode_alt_hold = (me[6] & 0x40) != 0; mm->tss.mode_alt_hold = getbit(me, 50);
mm->tss.mode_approach = (me[6] & 0x10) != 0; mm->tss.mode_approach = getbit(me, 52);
} }
mm->tss.acas_operational = (me[6] & 0x08) != 0; mm->tss.acas_operational = getbit(me, 53);
} }
break; break;
@ -1005,93 +1071,93 @@ static void decodeExtendedSquitter(struct modesMessage *mm)
break; break;
case 31: // Aircraft Operational Status case 31: // Aircraft Operational Status
if (check_imf && (msg[10] & 0x01)) if (check_imf && getbit(me, 56))
mm->addr |= MODES_NON_ICAO_ADDRESS; mm->addr |= MODES_NON_ICAO_ADDRESS;
if (mm->mesub == 0 || mm->mesub == 1) { if (mm->mesub == 0 || mm->mesub == 1) {
mm->opstatus.valid = 1; mm->opstatus.valid = 1;
mm->opstatus.version = (me[5] >> 5) & 0x07; mm->opstatus.version = getbits(me, 41, 43);
switch (mm->opstatus.version) { switch (mm->opstatus.version) {
case 0: case 0:
break; break;
case 1: case 1:
if ((me[3] & 0xC0) == 0) { if (getbits(me, 25, 26) == 0) {
mm->opstatus.om_acas_ra = (me[3] & 0x20) != 0; mm->opstatus.om_acas_ra = getbit(me, 27);
mm->opstatus.om_ident = (me[3] & 0x10) != 0; mm->opstatus.om_ident = getbit(me, 28);
mm->opstatus.om_atc = (me[3] & 0x08) != 0; mm->opstatus.om_atc = getbit(me, 29);
} }
if (mm->mesub == 0 && (me[1] & 0xCC) == 0) { if (mm->mesub == 0 && getbits(me, 9, 10) == 0 && getbits(me, 13, 14) == 0) {
// airborne // airborne
mm->opstatus.cc_acas = (me[1] & 0x20) == 0; mm->opstatus.cc_acas = !getbit(me, 11);
mm->opstatus.cc_cdti = (me[1] & 0x10) != 0; mm->opstatus.cc_cdti = getbit(me, 12);
mm->opstatus.cc_arv = (me[1] & 0x02) != 0; mm->opstatus.cc_arv = getbit(me, 15);
mm->opstatus.cc_ts = (me[1] & 0x01) != 0; mm->opstatus.cc_ts = getbit(me, 16);
mm->opstatus.cc_tc = (me[2] >> 6) & 0x03; mm->opstatus.cc_tc = getbits(me, 17, 18);
} else if (mm->mesub == 1 && (me[1] & 0xCC) == 0) { } else if (mm->mesub == 1 && getbits(me, 9, 10) == 0 && getbits(me, 13, 14) == 0) {
// surface // surface
mm->opstatus.cc_poa = (me[1] & 0x20) != 0; mm->opstatus.cc_poa = getbit(me, 11);
mm->opstatus.cc_cdti = (me[1] & 0x10) != 0; mm->opstatus.cc_cdti = getbit(me, 12);
mm->opstatus.cc_b2_low = (me[1] & 0x02) != 0; mm->opstatus.cc_b2_low = getbit(me, 15);
mm->opstatus.cc_lw_valid = 1; mm->opstatus.cc_lw_valid = 1;
mm->opstatus.cc_lw = me[2] & 0x0F; mm->opstatus.cc_lw = getbits(me, 21, 24);
} }
mm->opstatus.nic_supp_a = (me[5] & 0x10) ? 1 : 0; mm->opstatus.nic_supp_a = getbit(me, 44);
mm->opstatus.nac_p = me[5] & 0x0F; mm->opstatus.nac_p = getbits(me, 45, 48);
mm->opstatus.sil = (me[6] >> 4) & 0x03; mm->opstatus.sil = getbits(me, 51, 52);
if (mm->mesub == 0) { if (mm->mesub == 0) {
mm->opstatus.nic_baro = (me[6] & 0x08) ? 1 : 0; mm->opstatus.nic_baro = getbit(me, 53);
} else { } else {
mm->opstatus.track_angle = (me[6] & 0x08) ? ANGLE_TRACK : ANGLE_HEADING; mm->opstatus.track_angle = getbit(me, 53) ? ANGLE_TRACK : ANGLE_HEADING;
} }
mm->opstatus.hrd = (me[6] & 0x04) ? HEADING_MAGNETIC : HEADING_TRUE; mm->opstatus.hrd = getbit(me, 54) ? HEADING_MAGNETIC : HEADING_TRUE;
break; break;
case 2: case 2:
default: default:
if ((me[3] & 0xC0) == 0) { if (getbits(me, 25, 26) == 0) {
mm->opstatus.om_acas_ra = (me[3] & 0x20) != 0; mm->opstatus.om_acas_ra = getbit(me, 27);
mm->opstatus.om_ident = (me[3] & 0x10) != 0; mm->opstatus.om_ident = getbit(me, 28);
mm->opstatus.om_atc = (me[3] & 0x08) != 0; mm->opstatus.om_atc = getbit(me, 29);
mm->opstatus.om_saf = (me[3] & 0x04) != 0; mm->opstatus.om_saf = getbit(me, 30);
mm->opstatus.om_sda = (me[3] & 0x03); mm->opstatus.om_sda = getbits(me, 31, 32);
} }
if (mm->mesub == 0 && (me[1] & 0xCC) == 0) { if (mm->mesub == 0 && getbits(me, 9, 10) == 0 && getbits(me, 13, 14) == 0) {
// airborne // airborne
mm->opstatus.cc_acas = (me[1] & 0x20) != 0; mm->opstatus.cc_acas = getbit(me, 11);
mm->opstatus.cc_1090_in = (me[1] & 0x10) != 0; mm->opstatus.cc_1090_in = getbit(me, 12);
mm->opstatus.cc_arv = (me[1] & 0x02) != 0; mm->opstatus.cc_arv = getbit(me, 15);
mm->opstatus.cc_ts = (me[1] & 0x01) != 0; mm->opstatus.cc_ts = getbit(me, 16);
mm->opstatus.cc_tc = (me[2] >> 6) & 0x03; mm->opstatus.cc_tc = getbits(me, 17, 18);
mm->opstatus.cc_uat_in = (me[2] & 0x20) != 0; mm->opstatus.cc_uat_in = getbit(me, 19);
} else if (mm->mesub == 1 && (me[1] & 0xCC) == 0) { } else if (mm->mesub == 1 && getbits(me, 9, 10) == 0 && getbits(me, 13, 14) == 0) {
// surface // surface
mm->opstatus.cc_poa = (me[1] & 0x20) != 0; mm->opstatus.cc_poa = getbit(me, 11);
mm->opstatus.cc_1090_in = (me[1] & 0x10) != 0; mm->opstatus.cc_1090_in = getbit(me, 12);
mm->opstatus.cc_b2_low = (me[1] & 0x02) != 0; mm->opstatus.cc_b2_low = getbit(me, 15);
mm->opstatus.cc_uat_in = (me[1] & 0x01) != 0; mm->opstatus.cc_uat_in = getbit(me, 16);
mm->opstatus.cc_nac_v = (me[2] >> 5) & 0x07; mm->opstatus.cc_nac_v = getbits(me, 17, 19);
mm->opstatus.cc_nic_supp_c = (me[2] & 0x10) ? 1 : 0; mm->opstatus.cc_nic_supp_c = getbit(me, 20);
mm->opstatus.cc_lw_valid = 1; mm->opstatus.cc_lw_valid = 1;
mm->opstatus.cc_lw = me[2] & 0x0F; mm->opstatus.cc_lw = getbits(me, 21, 24);
mm->opstatus.cc_antenna_offset = me[3]; mm->opstatus.cc_antenna_offset = getbits(me, 33, 40);
} }
mm->opstatus.nic_supp_a = (me[5] & 0x10) ? 1 : 0; mm->opstatus.nic_supp_a = getbit(me, 44);
mm->opstatus.nac_p = me[5] & 0x0F; mm->opstatus.nac_p = getbits(me, 45, 48);
mm->opstatus.sil = (me[6] >> 4) & 0x03; mm->opstatus.sil = getbits(me, 51, 52);
if (mm->mesub == 0) { if (mm->mesub == 0) {
mm->opstatus.gva = (me[6] >> 6) & 0x03; mm->opstatus.gva = getbits(me, 49, 50);
mm->opstatus.nic_baro = (me[6] & 0x08) ? 1 : 0; mm->opstatus.nic_baro = getbit(me, 53);
} else { } else {
mm->opstatus.track_angle = (me[6] & 0x08) ? ANGLE_TRACK : ANGLE_HEADING; mm->opstatus.track_angle = getbit(me, 53) ? ANGLE_TRACK : ANGLE_HEADING;
} }
mm->opstatus.hrd = (me[6] & 0x04) ? HEADING_MAGNETIC : HEADING_TRUE; mm->opstatus.hrd = getbit(me, 54) ? HEADING_MAGNETIC : HEADING_TRUE;
mm->opstatus.sil_type = (me[6] & 0x02) ? SIL_PER_SAMPLE : SIL_PER_HOUR; mm->opstatus.sil_type = getbit(me, 55) ? SIL_PER_SAMPLE : SIL_PER_HOUR;
break; break;
} }
} }
@ -1107,7 +1173,7 @@ static void decodeCommB(struct modesMessage *mm)
unsigned char *msg = mm->msg; unsigned char *msg = mm->msg;
// This is a bit hairy as we don't know what the requested register was // This is a bit hairy as we don't know what the requested register was
if (msg[4] == 0x20) { // BDS 2,0 Aircraft Identification if (getbits(msg, 33, 40) == 0x20) { // BDS 2,0 Aircraft Identification
decodeBDS20(mm); decodeBDS20(mm);
} }
} }