diff --git a/dump1090.c b/dump1090.c index 4749a78..1d60596 100644 --- a/dump1090.c +++ b/dump1090.c @@ -85,8 +85,17 @@ struct aircraft { char flight[9]; /* Flight number */ int altitude; /* Altitude */ int speed; /* Velocity computed from EW and NS components. */ + int track; /* Angle of flight. */ time_t seen; /* Time at which the last packet was received. */ long messages; /* Number of Mode S messages received. */ + /* Encoded latitude and longitude as extracted by odd and even + * CPR encoded messages. */ + int odd_cprlat; + int odd_cprlon; + int even_cprlat; + int even_cprlon; + double lat, lon; /* Coordinated obtained from CPR encoded data. */ + time_t odd_cprtime, even_cprtime; struct aircraft *next; /* Next aircraft in our linked list. */ }; @@ -157,14 +166,14 @@ struct modesMessage { int heading_is_valid; int heading; int aircraft_type; - int fflag; /* Odd or Even CPR message? */ + int fflag; /* 1 = Odd, 0 = Even CPR message. */ int tflag; /* UTC synchronized? */ int raw_latitude; /* Non decoded latitude */ int raw_longitude; /* Non decoded longitude */ char flight[9]; /* 8 chars flight number. */ - int ew_dir; /* E/W direction. */ + int ew_dir; /* 0 = East, 1 = West. */ int ew_velocity; /* E/W velocity. */ - int ns_dir; /* N/S direction. */ + int ns_dir; /* 0 = North, 1 = South. */ int ns_velocity; /* N/S velocity. */ int vert_rate_source; /* Vertical rate source. */ int vert_rate_sign; /* Vertical rate sign. */ @@ -867,8 +876,24 @@ void decodeModesMessage(struct modesMessage *mm, unsigned char *msg) { mm->vert_rate_source = (msg[8]&0x10) >> 4; mm->vert_rate_sign = (msg[8]&0x8) >> 5; mm->vert_rate = ((msg[8]&7) << 6) | ((msg[9]&0xfc) >> 2); + /* Compute velocity and angle from the two speed + * components. */ mm->velocity = sqrt(mm->ns_velocity*mm->ns_velocity+ mm->ew_velocity*mm->ew_velocity); + if (mm->velocity) { + int ewv = mm->ew_velocity; + int nsv = mm->ns_velocity; + double heading; + + if (mm->ew_dir) ewv *= -1; + if (mm->ns_dir) nsv *= -1; + heading = atan2(ewv,nsv); + + /* Convert to degrees. */ + mm->heading = heading * 360 / (M_PI*2); + /* We don't want negative values but a 0-360 scale. */ + if (mm->heading < 0) mm->heading += 360; + } } else if (mm->mesub == 3 || mm->mesub == 4) { mm->heading_is_valid = msg[5] & (1<<2); mm->heading = (360.0/128) * (((msg[5] & 3) << 5) | @@ -1207,6 +1232,15 @@ struct aircraft *interactiveCreateAircraft(uint32_t addr) { a->flight[0] = '\0'; a->altitude = 0; a->speed = 0; + a->track = 0; + a->odd_cprlat = 0; + a->odd_cprlon = 0; + a->odd_cprtime = 0; + a->even_cprlat = 0; + a->even_cprlon = 0; + a->even_cprtime = 0; + a->lat = 0; + a->lon = 0; a->seen = time(NULL); a->messages = 0; a->next = NULL; @@ -1225,6 +1259,130 @@ struct aircraft *interactiveFindAircraft(uint32_t addr) { return NULL; } +/* Always positive MOD operation, used for CPR decoding. */ +int cprModFunction(int a, int b) { + int res = a % b; + if (res < 0) res += b; + return res; +} + +/* The NL function uses the precomputed table from 1090-WP-9-14 */ +int cprNLFunction(double lat) { + if (lat < 10.47047130) return 59; + if (lat < 14.82817437) return 58; + if (lat < 18.18626357) return 57; + if (lat < 21.02939493) return 56; + if (lat < 23.54504487) return 55; + if (lat < 25.82924707) return 54; + if (lat < 27.93898710) return 53; + if (lat < 29.91135686) return 52; + if (lat < 31.77209708) return 51; + if (lat < 33.53993436) return 50; + if (lat < 35.22899598) return 49; + if (lat < 36.85025108) return 48; + if (lat < 38.41241892) return 47; + if (lat < 39.92256684) return 46; + if (lat < 41.38651832) return 45; + if (lat < 42.80914012) return 44; + if (lat < 44.19454951) return 43; + if (lat < 45.54626723) return 42; + if (lat < 46.86733252) return 41; + if (lat < 48.16039128) return 40; + if (lat < 49.42776439) return 39; + if (lat < 50.67150166) return 38; + if (lat < 51.89342469) return 37; + if (lat < 53.09516153) return 36; + if (lat < 54.27817472) return 35; + if (lat < 55.44378444) return 34; + if (lat < 56.59318756) return 33; + if (lat < 57.72747354) return 32; + if (lat < 58.84763776) return 31; + if (lat < 59.95459277) return 30; + if (lat < 61.04917774) return 29; + if (lat < 62.13216659) return 28; + if (lat < 63.20427479) return 27; + if (lat < 64.26616523) return 26; + if (lat < 65.31845310) return 25; + if (lat < 66.36171008) return 24; + if (lat < 67.39646774) return 23; + if (lat < 68.42322022) return 22; + if (lat < 69.44242631) return 21; + if (lat < 70.45451075) return 20; + if (lat < 71.45986473) return 19; + if (lat < 72.45884545) return 18; + if (lat < 73.45177442) return 17; + if (lat < 74.43893416) return 16; + if (lat < 75.42056257) return 15; + if (lat < 76.39684391) return 14; + if (lat < 77.36789461) return 13; + if (lat < 78.33374083) return 12; + if (lat < 79.29428225) return 11; + if (lat < 80.24923213) return 10; + if (lat < 81.19801349) return 9; + if (lat < 82.13956981) return 8; + if (lat < 83.07199445) return 7; + if (lat < 83.99173563) return 6; + if (lat < 84.89166191) return 5; + if (lat < 85.75541621) return 4; + if (lat < 86.53536998) return 3; + if (lat < 87.00000000) return 2; + else return 1; +} + +int cprNFunction(int i, int isodd) { + int nl = cprNLFunction(i) - isodd; + if (nl < 1) nl = 1; + return nl; +} + +double cprDlonFunction(int i, int isodd) { + return 360.0 / cprNFunction(i, isodd); +} + +/* This algorithm comes from: + * http://www.lll.lu/~edward/edward/adsb/DecodingADSBposition.html. + * + * + * A few remarks: + * 1) 131072 is 2^17 since CPR latitude and longitude are encoded in 17 bits. + * 2) We assume that we always received the odd packet as last packet for + * simplicity. This may provide a position that is less fresh of a few + * seconds. + */ +void decodeCPR(struct aircraft *a) { + const double AirDlat0 = 6; + const double AirDlat1 = 360.0 / 59; + double lat0 = a->even_cprlat; + double lat1 = a->odd_cprlat; + double lon0 = a->even_cprlon; + double lon1 = a->odd_cprlon; + + /* Compute the Latitude Index "j" */ + int j = ((59*lat0 - 60*lat1) / 131072) + 0.5; + double rlat0 = AirDlat0 * (cprModFunction(j,60) + lat0 / 131072); + double rlat1 = AirDlat1 * (cprModFunction(j,59) + lat1 / 131072); + + /* Check that both are in the same latitude zone, or abort. */ + if (cprNLFunction(rlat0) != cprNLFunction(rlat1)) return; + + /* Compute ni and the longitude index m */ + if (a->even_cprtime > a->odd_cprtime) { + /* Use even packet. */ + int ni = cprNFunction(rlat0,0); + int m = (((lon0 * (cprNLFunction(rlat0)-1)) - + (lon1 * cprNLFunction(rlat0))) / 131072) + 0.5; + a->lon = cprDlonFunction(rlat0,0) * (cprModFunction(m,ni)+lon0/131072); + a->lat = rlat0; + } else { + /* Use odd packet. */ + int ni = cprNFunction(rlat1,1); + int m = (((lon0 * (cprNLFunction(rlat1)-1)) - + (lon1 * cprNLFunction(rlat1))) / 131072) + 0.5; + a->lon = cprDlonFunction(rlat1,1) * (cprModFunction(m,ni)+lon1/131072); + a->lat = rlat1; + } +} + /* Receive new messages and populate the interactive mode with more info. */ void interactiveReceiveData(struct modesMessage *mm) { uint32_t addr; @@ -1268,9 +1426,23 @@ void interactiveReceiveData(struct modesMessage *mm) { memcpy(a->flight, mm->flight, sizeof(a->flight)); } else if (mm->metype >= 9 && mm->metype <= 18) { a->altitude = mm->altitude; + if (mm->fflag) { + a->odd_cprlat = mm->raw_latitude; + a->odd_cprlon = mm->raw_longitude; + a->odd_cprtime = a->seen; /* Current time. */ + } else { + a->even_cprlat = mm->raw_latitude; + a->even_cprlon = mm->raw_longitude; + a->even_cprtime = a->seen; /* Current time. */ + } + /* If the two data is less than 10 seconds apart, compute + * the position. */ + if (abs(a->even_cprtime - a->odd_cprtime) <= 10) + decodeCPR(a); } else if (mm->metype == 19) { if (mm->mesub == 1 || mm->mesub == 2) { a->speed = mm->velocity; + a->track = mm->heading; } } } @@ -1288,9 +1460,10 @@ void interactiveShowData(void) { progress[3] = '\0'; printf("\x1b[H\x1b[2J"); /* Clear the screen */ - printf("Hex Flight Altitude Speed Messages Seen %s\n", - progress); - printf("-------------------------------------------------------\n"); + printf( +"Hex Flight Altitude Speed Lat Lon Track Messages Seen %s\n" +"--------------------------------------------------------------------------------\n", + progress); while(a && count < Modes.interactive_rows) { int altitude = a->altitude, speed = a->speed; @@ -1301,8 +1474,9 @@ void interactiveShowData(void) { speed *= 1.852; } - printf("%-6s %-8s %-9d %-9d %-9ld %d sec ago\n", - a->hexaddr, a->flight, altitude, speed, a->messages, + printf("%-6s %-8s %-9d %-7d %-7.03f %-7.03f %-3d %-9ld %d sec\n", + a->hexaddr, a->flight, altitude, speed, + a->lat, a->lon, a->track, a->messages, (int)(now - a->seen)); a = a->next; count++;