From ed4d48177200703ba5cc5d7e0be40fd15da7e3a0 Mon Sep 17 00:00:00 2001 From: Oliver Jowett Date: Tue, 20 Jan 2015 18:41:44 +0000 Subject: [PATCH] Import CPR changes from experimental branch. --- Makefile | 18 ++- cpr.c | 349 ++++++++++++++++++++++++++++++++++++++++++++++++++ cpr.h | 39 ++++++ cprtests.c | 303 +++++++++++++++++++++++++++++++++++++++++++ dump1090.h | 6 +- interactive.c | 221 +++++++++++++++++++++++++++----- mode_s.c | 317 --------------------------------------------- stats.c | 21 +++ stats.h | 8 ++ 9 files changed, 925 insertions(+), 357 deletions(-) create mode 100644 cpr.c create mode 100644 cpr.h create mode 100644 cprtests.c diff --git a/Makefile b/Makefile index d10a09e..b526a31 100644 --- a/Makefile +++ b/Makefile @@ -19,13 +19,19 @@ CC=gcc all: dump1090 view1090 %.o: %.c dump1090.h - $(CC) $(CPPFLAGS) $(CFLAGS) $(EXTRACFLAGS) -c $< + $(CC) $(CPPFLAGS) $(CFLAGS) $(EXTRACFLAGS) -c $< -o $@ -dump1090: dump1090.o anet.o interactive.o mode_ac.o mode_s.o net_io.o crc.o demod_2000.o demod_2400.o stats.o - $(CC) -g -o dump1090 $^ $(LIBS) $(LIBS_RTL) $(LDFLAGS) +dump1090: dump1090.o anet.o interactive.o mode_ac.o mode_s.o net_io.o crc.o demod_2000.o demod_2400.o stats.o cpr.o + $(CC) -g -o $@ $^ $(LIBS) $(LIBS_RTL) $(LDFLAGS) -view1090: view1090.o anet.o interactive.o mode_ac.o mode_s.o net_io.o crc.o stats.o - $(CC) -g -o view1090 $^ $(LIBS) $(LDFLAGS) +view1090: view1090.o anet.o interactive.o mode_ac.o mode_s.o net_io.o crc.o stats.o cpr.o + $(CC) -g -o $@ $^ $(LIBS) $(LDFLAGS) clean: - rm -f *.o dump1090 view1090 + rm -f *.o dump1090 view1090 cprtests + +test: cprtests + $(PWD)/cprtests + +cprtests: cpr.o cprtests.o + $(CC) -g -o $@ $^ $(LIBS) $(LDFLAGS) diff --git a/cpr.c b/cpr.c new file mode 100644 index 0000000..d641670 --- /dev/null +++ b/cpr.c @@ -0,0 +1,349 @@ +// Part of dump1090, a Mode S message decoder for RTLSDR devices. +// +// cpr.c - Compact Position Reporting decoder and tests +// +// Copyright (c) 2014,2015 Oliver Jowett +// +// This file is free software: you may copy, redistribute and/or modify it +// under the terms of the GNU General Public License as published by the +// Free Software Foundation, either version 2 of the License, or (at your +// option) any later version. +// +// This file is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +// This file incorporates work covered by the following copyright and +// permission notice: +// +// Copyright (C) 2012 by Salvatore Sanfilippo +// +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include +#include + +// +//========================================================================= +// +// Always positive MOD operation, used for CPR decoding. +// +static int cprModInt(int a, int b) { + int res = a % b; + if (res < 0) res += b; + return res; +} + +static double cprModDouble(double a, double b) { + double res = fmod(a, b); + if (res < 0) res += b; + return res; +} + +// +//========================================================================= +// +// The NL function uses the precomputed table from 1090-WP-9-14 +// +static int cprNLFunction(double lat) { + if (lat < 0) lat = -lat; // Table is simmetric about the equator + 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; +} +// +//========================================================================= +// +static int cprNFunction(double lat, int fflag) { + int nl = cprNLFunction(lat) - (fflag ? 1 : 0); + if (nl < 1) nl = 1; + return nl; +} +// +//========================================================================= +// +static double cprDlonFunction(double lat, int fflag, int surface) { + return (surface ? 90.0 : 360.0) / cprNFunction(lat, fflag); +} +// +//========================================================================= +// +// 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. +// +int decodeCPRairborne(int even_cprlat, int even_cprlon, + int odd_cprlat, int odd_cprlon, + int fflag, + double *out_lat, double *out_lon) +{ + double AirDlat0 = 360.0 / 60.0; + double AirDlat1 = 360.0 / 59.0; + double lat0 = even_cprlat; + double lat1 = odd_cprlat; + double lon0 = even_cprlon; + double lon1 = odd_cprlon; + + double rlat, rlon; + + // Compute the Latitude Index "j" + int j = (int) floor(((59*lat0 - 60*lat1) / 131072) + 0.5); + double rlat0 = AirDlat0 * (cprModInt(j,60) + lat0 / 131072); + double rlat1 = AirDlat1 * (cprModInt(j,59) + lat1 / 131072); + + if (rlat0 >= 270) rlat0 -= 360; + if (rlat1 >= 270) rlat1 -= 360; + + // Check to see that the latitude is in range: -90 .. +90 + if (rlat0 < -90 || rlat0 > 90 || rlat1 < -90 || rlat1 > 90) + return (-2); // bad data + + // Check that both are in the same latitude zone, or abort. + if (cprNLFunction(rlat0) != cprNLFunction(rlat1)) + return (-1); // positions crossed a latitude zone, try again later + + // Compute ni and the Longitude Index "m" + if (fflag) { // Use odd packet. + int ni = cprNFunction(rlat1,1); + int m = (int) floor((((lon0 * (cprNLFunction(rlat1)-1)) - + (lon1 * cprNLFunction(rlat1))) / 131072.0) + 0.5); + rlon = cprDlonFunction(rlat1, 1, 0) * (cprModInt(m, ni)+lon1/131072); + rlat = rlat1; + } else { // Use even packet. + int ni = cprNFunction(rlat0,0); + int m = (int) floor((((lon0 * (cprNLFunction(rlat0)-1)) - + (lon1 * cprNLFunction(rlat0))) / 131072) + 0.5); + rlon = cprDlonFunction(rlat0, 0, 0) * (cprModInt(m, ni)+lon0/131072); + rlat = rlat0; + } + + // Renormalize to -180 .. +180 + rlon -= floor( (rlon + 180) / 360 ) * 360; + + *out_lat = rlat; + *out_lon = rlon; + + return 0; +} + +int decodeCPRsurface(double reflat, double reflon, + int even_cprlat, int even_cprlon, + int odd_cprlat, int odd_cprlon, + int fflag, + double *out_lat, double *out_lon) +{ + double AirDlat0 = 90.0 / 60.0; + double AirDlat1 = 90.0 / 59.0; + double lat0 = even_cprlat; + double lat1 = odd_cprlat; + double lon0 = even_cprlon; + double lon1 = odd_cprlon; + double rlon, rlat; + + // Compute the Latitude Index "j" + int j = (int) floor(((59*lat0 - 60*lat1) / 131072) + 0.5); + double rlat0 = AirDlat0 * (cprModInt(j,60) + lat0 / 131072); + double rlat1 = AirDlat1 * (cprModInt(j,59) + lat1 / 131072); + + // Pick the quadrant that's closest to the reference location - + // this is not necessarily the same quadrant that contains the + // reference location. + // + // There are also only two valid quadrants: -90..0 and 0..90; + // no correct message would try to encoding a latitude in the + // ranges -180..-90 and 90..180. + // + // If the computed latitude is more than 45 degrees north of + // the reference latitude (using the northern hemisphere + // solution), then the southern hemisphere solution will be + // closer to the refernce latitude. + // + // e.g. reflat=0, rlat=44, use rlat=44 + // reflat=0, rlat=46, use rlat=46-90 = -44 + // reflat=40, rlat=84, use rlat=84 + // reflat=40, rlat=86, use rlat=86-90 = -4 + // reflat=-40, rlat=4, use rlat=4 + // reflat=-40, rlat=6, use rlat=6-90 = -84 + + if ( (rlat0 - reflat) > 45 ) rlat0 -= 90; + if ( (rlat1 - reflat) > 45 ) rlat1 -= 90; + + // Check to see that the latitude is in range: -90 .. +90 + if (rlat0 < -90 || rlat0 > 90 || rlat1 < -90 || rlat1 > 90) + return (-2); // bad data + + // Check that both are in the same latitude zone, or abort. + if (cprNLFunction(rlat0) != cprNLFunction(rlat1)) + return (-1); // positions crossed a latitude zone, try again later + + // Compute ni and the Longitude Index "m" + if (fflag) { // Use odd packet. + int ni = cprNFunction(rlat1,1); + int m = (int) floor((((lon0 * (cprNLFunction(rlat1)-1)) - + (lon1 * cprNLFunction(rlat1))) / 131072.0) + 0.5); + rlon = cprDlonFunction(rlat1, 1, 1) * (cprModInt(m, ni)+lon1/131072); + rlat = rlat1; + } else { // Use even packet. + int ni = cprNFunction(rlat0,0); + int m = (int) floor((((lon0 * (cprNLFunction(rlat0)-1)) - + (lon1 * cprNLFunction(rlat0))) / 131072) + 0.5); + rlon = cprDlonFunction(rlat0, 0, 1) * (cprModInt(m, ni)+lon0/131072); + rlat = rlat0; + } + + // Pick the quadrant that's closest to the reference location - + // this is not necessarily the same quadrant that contains the + // reference location. Unlike the latitude case, all four + // quadrants are valid. + + // if reflon is more than 45 degrees away, move some multiple of 90 degrees towards it + rlon += floor( (reflon - rlon + 45) / 90 ) * 90; // this might move us outside (-180..+180), we fix this below + + // Renormalize to -180 .. +180 + rlon -= floor( (rlon + 180) / 360 ) * 360; + + *out_lat = rlat; + *out_lon = rlon; + return 0; +} + +// +//========================================================================= +// +// This algorithm comes from: +// 1090-WP29-07-Draft_CPR101 (which also defines decodeCPR() ) +// +// Despite what the earlier comment here said, we should *not* be using trunc(). +// See Figure 5-5 / 5-6 and note that floor is applied to (0.5 + fRP - fEP), not +// directly to (fRP - fEP). Eq 38 is correct. +// +int decodeCPRrelative(double reflat, double reflon, + int cprlat, int cprlon, + int fflag, int surface, + double *out_lat, double *out_lon) +{ + double AirDlat; + double AirDlon; + double fractional_lat = cprlat / 131072.0; + double fractional_lon = cprlon / 131072.0; + double rlon, rlat; + int j,m; + + AirDlat = (surface ? 90.0 : 360.0) / (fflag ? 59.0 : 60.0); + + // Compute the Latitude Index "j" + j = (int) (floor(reflat/AirDlat) + + floor(0.5 + cprModDouble(reflat, AirDlat)/AirDlat - fractional_lat)); + rlat = AirDlat * (j + fractional_lat); + if (rlat >= 270) rlat -= 360; + + // Check to see that the latitude is in range: -90 .. +90 + if (rlat < -90 || rlat > 90) { + return (-1); // Time to give up - Latitude error + } + + // Check to see that answer is reasonable - ie no more than 1/2 cell away + if (fabs(rlat - reflat) > (AirDlat/2)) { + return (-1); // Time to give up - Latitude error + } + + // Compute the Longitude Index "m" + AirDlon = cprDlonFunction(rlat, fflag, surface); + m = (int) (floor(reflon/AirDlon) + + floor(0.5 + cprModDouble(reflon, AirDlon)/AirDlon - fractional_lon)); + rlon = AirDlon * (m + fractional_lon); + if (rlon > 180) rlon -= 360; + + // Check to see that answer is reasonable - ie no more than 1/2 cell away + if (fabs(rlon - reflon) > (AirDlon/2)) + return (-1); // Time to give up - Longitude error + + *out_lat = rlat; + *out_lon = rlon; + return (0); +} diff --git a/cpr.h b/cpr.h new file mode 100644 index 0000000..1cb413b --- /dev/null +++ b/cpr.h @@ -0,0 +1,39 @@ +// Part of dump1090, a Mode S message decoder for RTLSDR devices. +// +// cpr.h - Compact Position Reporting prototypes +// +// Copyright (c) 2014,2015 Oliver Jowett +// +// This file is free software: you may copy, redistribute and/or modify it +// under the terms of the GNU General Public License as published by the +// Free Software Foundation, either version 2 of the License, or (at your +// option) any later version. +// +// This file is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef DUMP1090_CPR_H +#define DUMP1090_CPR_H + +int decodeCPRairborne(int even_cprlat, int even_cprlon, + int odd_cprlat, int odd_cprlon, + int fflag, + double *out_lat, double *out_lon); + +int decodeCPRsurface(double reflat, double reflon, + int even_cprlat, int even_cprlon, + int odd_cprlat, int odd_cprlon, + int fflag, + double *out_lat, double *out_lon); + +int decodeCPRrelative(double reflat, double reflon, + int cprlat, int cprlon, + int fflag, int surface, + double *out_lat, double *out_lon); + +#endif diff --git a/cprtests.c b/cprtests.c new file mode 100644 index 0000000..182aa19 --- /dev/null +++ b/cprtests.c @@ -0,0 +1,303 @@ +// Part of dump1090, a Mode S message decoder for RTLSDR devices. +// +// cprtests.c - tests for CPR decoder +// +// Copyright (c) 2014,2015 Oliver Jowett +// +// This file is free software: you may copy, redistribute and/or modify it +// under the terms of the GNU General Public License as published by the +// Free Software Foundation, either version 2 of the License, or (at your +// option) any later version. +// +// This file is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + + +#include +#include + +#include "cpr.h" + +// Global, airborne CPR test data: +static const struct { + int even_cprlat, even_cprlon; // input: raw CPR values, even message + int odd_cprlat, odd_cprlon; // input: raw CPR values, odd message + int even_result; // verify: expected result from decoding with fflag=0 (even message is latest) + double even_rlat, even_rlon; // verify: expected position from decoding with fflag=0 (even message is latest) + int odd_result; // verify: expected result from decoding with fflag=1 (odd message is latest) + double odd_rlat, odd_rlon; // verify: expected position from decoding with fflag=1 (odd message is latest) +} cprGlobalAirborneTests[] = { + { 80536, 9432, 61720, 9192, 0, 51.686646, 0.700156, 0, 51.686763, 0.701294 }, + { 80534, 9413, 61714, 9144, 0, 51.686554, 0.698745, 0, 51.686484, 0.697632 }, + + // todo: more positions, bad data +}; + +// Global, surface CPR test data: +static const struct { + double reflat, reflon; // input: reference location for decoding + int even_cprlat, even_cprlon; // input: raw CPR values, even message + int odd_cprlat, odd_cprlon; // input: raw CPR values, odd message + int even_result; // verify: expected result from decoding with fflag=0 (even message is latest) + double even_rlat, even_rlon; // verify: expected position from decoding with fflag=0 (even message is latest) + int odd_result; // verify: expected result from decoding with fflag=1 (odd message is latest) + double odd_rlat, odd_rlon; // verify: expected position from decoding with fflag=1 (odd message is latest) +} cprGlobalSurfaceTests[] = { + // The real position received here was on the Cambridge (UK) airport apron at 52.21N 0.177E + // We mess with the reference location to check that the right quadrant is used. + + // longitude quadrants: + { 52.00, -180.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601 - 180.0, 0, 52.209976, 0.176507 - 180.0 }, + { 52.00, -140.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601 - 180.0, 0, 52.209976, 0.176507 - 180.0 }, + { 52.00, -130.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601 - 90.0, 0, 52.209976, 0.176507 - 90.0 }, + { 52.00, -50.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601 - 90.0, 0, 52.209976, 0.176507 - 90.0 }, + { 52.00, -40.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601, 0, 52.209976, 0.176507 }, + { 52.00, -10.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601, 0, 52.209976, 0.176507 }, + { 52.00, 0.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601, 0, 52.209976, 0.176507 }, + { 52.00, 10.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601, 0, 52.209976, 0.176507 }, + { 52.00, 40.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601, 0, 52.209976, 0.176507 }, + { 52.00, 50.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601 + 90.0, 0, 52.209976, 0.176507 + 90.0 }, + { 52.00, 130.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601 + 90.0, 0, 52.209976, 0.176507 + 90.0 }, + { 52.00, 140.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601 - 180.0, 0, 52.209976, 0.176507 - 180.0 }, + { 52.00, 180.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601 - 180.0, 0, 52.209976, 0.176507 - 180.0 }, + + // latitude quadrants (but only 2). The decoded longitude also changes because the cell size changes with latitude + { 90.00, 0.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601, 0, 52.209976, 0.176507 }, + { 52.00, 0.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601, 0, 52.209976, 0.176507 }, + { 8.00, 0.00, 105730, 9259, 29693, 8997, 0, 52.209984, 0.176601, 0, 52.209976, 0.176507 }, + { 7.00, 0.00, 105730, 9259, 29693, 8997, 0, 52.209984 - 90.0, 0.135269, 0, 52.209976 - 90.0, 0.134299 }, + { -52.00, 0.00, 105730, 9259, 29693, 8997, 0, 52.209984 - 90.0, 0.135269, 0, 52.209976 - 90.0, 0.134299 }, + { -90.00, 0.00, 105730, 9259, 29693, 8997, 0, 52.209984 - 90.0, 0.135269, 0, 52.209976 - 90.0, 0.134299 }, +}; + +// Relative CPR test data: +static const struct { + double reflat, reflon; // input: reference location for decoding + int cprlat, cprlon; // input: raw CPR values, even or odd message + int fflag; // input: fflag in raw message + int surface; // input: decode as air (0) or surface (1) position + int result; // verify: expected result + double rlat, rlon; // verify: expected position +} cprRelativeTests[] = { + // + // AIRBORNE + // + + { 52.00, 0.00, 80536, 9432, 0, 0, 0, 51.686646, 0.700156 }, // even, airborne + { 52.00, 0.00, 61720, 9192, 1, 0, 0, 51.686763, 0.701294 }, // odd, airborne + { 52.00, 0.00, 80534, 9413, 0, 0, 0, 51.686554, 0.698745 }, // even, airborne + { 52.00, 0.00, 61714, 9144, 1, 0, 0, 51.686484, 0.697632 }, // odd, airborne + + // test moving the receiver around a bit + // We cannot move it more than 1/2 cell away before ambiguity happens. + + // latitude must be within about 3 degrees (cell size is 360/60 = 6 degrees) + { 48.70, 0.00, 80536, 9432, 0, 0, 0, 51.686646, 0.700156 }, // even, airborne + { 48.70, 0.00, 61720, 9192, 1, 0, 0, 51.686763, 0.701294 }, // odd, airborne + { 48.70, 0.00, 80534, 9413, 0, 0, 0, 51.686554, 0.698745 }, // even, airborne + { 48.70, 0.00, 61714, 9144, 1, 0, 0, 51.686484, 0.697632 }, // odd, airborne + { 54.60, 0.00, 80536, 9432, 0, 0, 0, 51.686646, 0.700156 }, // even, airborne + { 54.60, 0.00, 61720, 9192, 1, 0, 0, 51.686763, 0.701294 }, // odd, airborne + { 54.60, 0.00, 80534, 9413, 0, 0, 0, 51.686554, 0.698745 }, // even, airborne + { 54.60, 0.00, 61714, 9144, 1, 0, 0, 51.686484, 0.697632 }, // odd, airborne + + // longitude must be within about 4.8 degrees at this latitude + { 52.00, 5.40, 80536, 9432, 0, 0, 0, 51.686646, 0.700156 }, // even, airborne + { 52.00, 5.40, 61720, 9192, 1, 0, 0, 51.686763, 0.701294 }, // odd, airborne + { 52.00, 5.40, 80534, 9413, 0, 0, 0, 51.686554, 0.698745 }, // even, airborne + { 52.00, 5.40, 61714, 9144, 1, 0, 0, 51.686484, 0.697632 }, // odd, airborne + { 52.00, -4.10, 80536, 9432, 0, 0, 0, 51.686646, 0.700156 }, // even, airborne + { 52.00, -4.10, 61720, 9192, 1, 0, 0, 51.686763, 0.701294 }, // odd, airborne + { 52.00, -4.10, 80534, 9413, 0, 0, 0, 51.686554, 0.698745 }, // even, airborne + { 52.00, -4.10, 61714, 9144, 1, 0, 0, 51.686484, 0.697632 }, // odd, airborne + + // + // SURFACE + // + + // Surface position on the Cambridge (UK) airport apron at 52.21N 0.18E + { 52.00, 0.00, 105730, 9259, 0, 1, 0, 52.209984, 0.176601 }, // even, surface + { 52.00, 0.00, 29693, 8997, 1, 1, 0, 52.209976, 0.176507 }, // odd, surface + + // test moving the receiver around a bit + // We cannot move it more than 1/2 cell away before ambiguity happens. + + // latitude must be within about 0.75 degrees (cell size is 90/60 = 1.5 degrees) + { 51.46, 0.00, 105730, 9259, 0, 1, 0, 52.209984, 0.176601 }, // even, surface + { 51.46, 0.00, 29693, 8997, 1, 1, 0, 52.209976, 0.176507 }, // odd, surface + { 52.95, 0.00, 105730, 9259, 0, 1, 0, 52.209984, 0.176601 }, // even, surface + { 52.95, 0.00, 29693, 8997, 1, 1, 0, 52.209976, 0.176507 }, // odd, surface + + // longitude must be within about 1.25 degrees at this latitude + { 52.00, 1.40, 105730, 9259, 0, 1, 0, 52.209984, 0.176601 }, // even, surface + { 52.00, 1.40, 29693, 8997, 1, 1, 0, 52.209976, 0.176507 }, // odd, surface + { 52.00, -1.05, 105730, 9259, 0, 1, 0, 52.209984, 0.176601 }, // even, surface + { 52.00, -1.05, 29693, 8997, 1, 1, 0, 52.209976, 0.176507 }, // odd, surface + +}; + +static int testCPRGlobalAirborne() { + int ok = 1; + unsigned i; + for (i = 0; i < sizeof(cprGlobalAirborneTests)/sizeof(cprGlobalAirborneTests[0]); ++i) { + double rlat = 0, rlon = 0; + int res; + + res = decodeCPRairborne(cprGlobalAirborneTests[i].even_cprlat, cprGlobalAirborneTests[i].even_cprlon, + cprGlobalAirborneTests[i].odd_cprlat, cprGlobalAirborneTests[i].odd_cprlon, + 0, + &rlat, &rlon); + if (res != cprGlobalAirborneTests[i].even_result + || fabs(rlat - cprGlobalAirborneTests[i].even_rlat) > 1e-6 + || fabs(rlon - cprGlobalAirborneTests[i].even_rlon) > 1e-6) { + ok = 0; + fprintf(stderr, + "testCPRGlobalAirborne[%d,EVEN]: FAIL: decodeCPRairborne(%d,%d,%d,%d,EVEN) failed:\n" + " result %d (expected %d)\n" + " lat %.6f (expected %.6f)\n" + " lon %.6f (expected %.6f)\n", + i, + cprGlobalAirborneTests[i].even_cprlat, cprGlobalAirborneTests[i].even_cprlon, + cprGlobalAirborneTests[i].odd_cprlat, cprGlobalAirborneTests[i].odd_cprlon, + res, cprGlobalAirborneTests[i].even_result, + rlat, cprGlobalAirborneTests[i].even_rlat, + rlon, cprGlobalAirborneTests[i].even_rlon); + } else { + fprintf(stderr, "testCPRGlobalAirborne[%d,EVEN]: PASS\n", i); + } + + res = decodeCPRairborne(cprGlobalAirborneTests[i].even_cprlat, cprGlobalAirborneTests[i].even_cprlon, + cprGlobalAirborneTests[i].odd_cprlat, cprGlobalAirborneTests[i].odd_cprlon, + 1, + &rlat, &rlon); + if (res != cprGlobalAirborneTests[i].odd_result + || fabs(rlat - cprGlobalAirborneTests[i].odd_rlat) > 1e-6 + || fabs(rlon - cprGlobalAirborneTests[i].odd_rlon) > 1e-6) { + ok = 0; + fprintf(stderr, + "testCPRGlobalAirborne[%d,ODD]: FAIL: decodeCPRairborne(%d,%d,%d,%d,ODD) failed:\n" + " result %d (expected %d)\n" + " lat %.6f (expected %.6f)\n" + " lon %.6f (expected %.6f)\n", + i, + cprGlobalAirborneTests[i].even_cprlat, cprGlobalAirborneTests[i].even_cprlon, + cprGlobalAirborneTests[i].odd_cprlat, cprGlobalAirborneTests[i].odd_cprlon, + res, cprGlobalAirborneTests[i].odd_result, + rlat, cprGlobalAirborneTests[i].odd_rlat, + rlon, cprGlobalAirborneTests[i].odd_rlon); + } else { + fprintf(stderr, "testCPRGlobalAirborne[%d,ODD]: PASS\n", i); + } + } + + return ok; +} + +static int testCPRGlobalSurface() { + int ok = 1; + unsigned i; + for (i = 0; i < sizeof(cprGlobalSurfaceTests)/sizeof(cprGlobalSurfaceTests[0]); ++i) { + double rlat = 0, rlon = 0; + int res; + + res = decodeCPRsurface(cprGlobalSurfaceTests[i].reflat, cprGlobalSurfaceTests[i].reflon, + cprGlobalSurfaceTests[i].even_cprlat, cprGlobalSurfaceTests[i].even_cprlon, + cprGlobalSurfaceTests[i].odd_cprlat, cprGlobalSurfaceTests[i].odd_cprlon, + 0, + &rlat, &rlon); + if (res != cprGlobalSurfaceTests[i].even_result + || fabs(rlat - cprGlobalSurfaceTests[i].even_rlat) > 1e-6 + || fabs(rlon - cprGlobalSurfaceTests[i].even_rlon) > 1e-6) { + ok = 0; + fprintf(stderr, + "testCPRGlobalSurface[%d]: FAIL: decodeCPRsurface(%.6f,%.6f,%d,%d,%d,%d,EVEN) failed:\n" + " result %d (expected %d)\n" + " lat %.6f (expected %.6f)\n" + " lon %.6f (expected %.6f)\n", + i, + cprGlobalSurfaceTests[i].reflat, cprGlobalSurfaceTests[i].reflon, + cprGlobalSurfaceTests[i].even_cprlat, cprGlobalSurfaceTests[i].even_cprlon, + cprGlobalSurfaceTests[i].odd_cprlat, cprGlobalSurfaceTests[i].odd_cprlon, + res, cprGlobalSurfaceTests[i].even_result, + rlat, cprGlobalSurfaceTests[i].even_rlat, + rlon, cprGlobalSurfaceTests[i].even_rlon); + } else { + fprintf(stderr, "testCPRGlobalSurface[%d,EVEN]: PASS\n", i); + } + + res = decodeCPRsurface(cprGlobalSurfaceTests[i].reflat, cprGlobalSurfaceTests[i].reflon, + cprGlobalSurfaceTests[i].even_cprlat, cprGlobalSurfaceTests[i].even_cprlon, + cprGlobalSurfaceTests[i].odd_cprlat, cprGlobalSurfaceTests[i].odd_cprlon, + 1, + &rlat, &rlon); + if (res != cprGlobalSurfaceTests[i].odd_result + || fabs(rlat - cprGlobalSurfaceTests[i].odd_rlat) > 1e-6 + || fabs(rlon - cprGlobalSurfaceTests[i].odd_rlon) > 1e-6) { + ok = 0; + fprintf(stderr, + "testCPRGlobalSurface[%d,ODD]: FAIL: decodeCPRsurface(%.6f,%.6f,%d,%d,%d,%d,ODD) failed:\n" + " result %d (expected %d)\n" + " lat %.6f (expected %.6f)\n" + " lon %.6f (expected %.6f)\n", + i, + cprGlobalSurfaceTests[i].reflat, cprGlobalSurfaceTests[i].reflon, + cprGlobalSurfaceTests[i].even_cprlat, cprGlobalSurfaceTests[i].even_cprlon, + cprGlobalSurfaceTests[i].odd_cprlat, cprGlobalSurfaceTests[i].odd_cprlon, + res, cprGlobalSurfaceTests[i].odd_result, + rlat, cprGlobalSurfaceTests[i].odd_rlat, + rlon, cprGlobalSurfaceTests[i].odd_rlon); + } else { + fprintf(stderr, "testCPRGlobalSurface[%d,ODD]: PASS\n", i); + } + } + + return ok; +} + +static int testCPRRelative() { + int ok = 1; + unsigned i; + for (i = 0; i < sizeof(cprRelativeTests)/sizeof(cprRelativeTests[0]); ++i) { + double rlat = 0, rlon = 0; + int res; + + res = decodeCPRrelative(cprRelativeTests[i].reflat, cprRelativeTests[i].reflon, + cprRelativeTests[i].cprlat, cprRelativeTests[i].cprlon, + cprRelativeTests[i].fflag, cprRelativeTests[i].surface, + &rlat, &rlon); + if (res != cprRelativeTests[i].result + || fabs(rlat - cprRelativeTests[i].rlat) > 1e-6 + || fabs(rlon - cprRelativeTests[i].rlon) > 1e-6) { + ok = 0; + fprintf(stderr, + "testCPRRelative[%d]: FAIL: decodeCPRrelative(%.6f,%.6f,%d,%d,%d,%d) failed:\n" + " result %d (expected %d)\n" + " lat %.6f (expected %.6f)\n" + " lon %.6f (expected %.6f)\n", + i, + cprRelativeTests[i].reflat, cprRelativeTests[i].reflon, + cprRelativeTests[i].cprlat, cprRelativeTests[i].cprlon, + cprRelativeTests[i].fflag, cprRelativeTests[i].surface, + res, cprRelativeTests[i].result, + rlat, cprRelativeTests[i].rlat, + rlon, cprRelativeTests[i].rlon); + } else { + fprintf(stderr, "testCPRRelative[%d]: PASS\n", i); + } + } + + return ok; +} + +int main(int __attribute__ ((unused)) argc, char __attribute__ ((unused)) **argv) { + int ok = 1; + ok = testCPRGlobalAirborne() && ok; + ok = testCPRGlobalSurface() && ok; + ok = testCPRRelative() && ok; + return ok ? 0 : 1; +} diff --git a/dump1090.h b/dump1090.h index 2cf8819..aac473a 100644 --- a/dump1090.h +++ b/dump1090.h @@ -179,6 +179,7 @@ #define MODES_ACFLAGS_FS_VALID (1<<13) // Aircraft Flight Status is known #define MODES_ACFLAGS_NSEWSPD_VALID (1<<14) // Aircraft EW and NS Speed is known #define MODES_ACFLAGS_LATLON_REL_OK (1<<15) // Indicates it's OK to do a relative CPR +#define MODES_ACFLAGS_REL_CPR_USED (1<<16) // Lat/lon derived from relative CPR #define MODES_ACFLAGS_LLEITHER_VALID (MODES_ACFLAGS_LLEVEN_VALID | MODES_ACFLAGS_LLODD_VALID) #define MODES_ACFLAGS_LLBOTH_VALID (MODES_ACFLAGS_LLEVEN_VALID | MODES_ACFLAGS_LLODD_VALID) @@ -224,11 +225,14 @@ #define MODES_NOTUSED(V) ((void) V) +// Include subheaders after all the #defines are in place + #include "anet.h" #include "crc.h" #include "demod_2000.h" #include "demod_2400.h" #include "stats.h" +#include "cpr.h" //======================== structure declarations ========================= @@ -467,8 +471,6 @@ void decodeModesMessage (struct modesMessage *mm, unsigned char *msg); void displayModesMessage(struct modesMessage *mm); void useModesMessage (struct modesMessage *mm); void computeMagnitudeVector(uint16_t *pData); -int decodeCPR (struct aircraft *a, int fflag, int surface); -int decodeCPRrelative (struct aircraft *a, int fflag, int surface); void modesInitErrorInfo (); // // Functions exported from interactive.c diff --git a/interactive.c b/interactive.c index 50471e5..98f2613 100644 --- a/interactive.c +++ b/interactive.c @@ -195,13 +195,199 @@ void interactiveUpdateAircraftModeS() { // // Receive new messages and populate the interactive mode with more info // + +// Distance between points on a spherical earth. +// This has up to 0.5% error because the earth isn't actually spherical +// (but we don't use it in situations where that matters) +static double greatcircle(double lat0, double lon0, double lat1, double lon1) +{ + lat0 = lat0 * M_PI / 180.0; + lon0 = lon0 * M_PI / 180.0; + lat1 = lat1 * M_PI / 180.0; + lon1 = lon1 * M_PI / 180.0; + return 6371e3 * acos(sin(lat0) * sin(lat1) + cos(lat0) * cos(lat1) * cos(fabs(lon0 - lon1))); +} + +static int doGlobalCPR(struct aircraft *a, int fflag, int surface) +{ + int result; + double lat=0, lon=0; + + if (surface) { + // surface global CPR + // find reference location + double reflat, reflon; + + if (a->bFlags & MODES_ACFLAGS_LATLON_REL_OK) { // Ok to try aircraft relative first + reflat = a->lat; + reflon = a->lon; + } else if (Modes.bUserFlags & MODES_USER_LATLON_VALID) { + reflat = Modes.fUserLat; + reflon = Modes.fUserLon; + } else { + // No local reference, give up + return (-1); + } + + result = decodeCPRsurface(reflat, reflon, + a->even_cprlat, a->even_cprlon, + a->odd_cprlat, a->odd_cprlon, + fflag, + &lat, &lon); + } else { + // airborne global CPR + result = decodeCPRairborne(a->even_cprlat, a->even_cprlon, + a->odd_cprlat, a->odd_cprlon, + fflag, + &lat, &lon); + } + + if (result < 0) + return result; + + // check max range + if (Modes.maxRange > 0 && (Modes.bUserFlags & MODES_USER_LATLON_VALID)) { + double range = greatcircle(Modes.fUserLat, Modes.fUserLon, lat, lon); + if (range > Modes.maxRange) + return (-2); // we consider an out-of-range value to be bad data + } + + a->lat = lat; + a->lon = lon; + return 0; +} + +static int doLocalCPR(struct aircraft *a, int fflag, int surface, time_t now) +{ + // relative CPR + // find reference location + double reflat, reflon, lat=0, lon=0; + double range_limit = 0; + int result; + + if (a->bFlags & MODES_ACFLAGS_LATLON_REL_OK) { + int elapsed = (int)(now - a->seenLatLon); + if (elapsed < 0) elapsed = 0; + + reflat = a->lat; + reflon = a->lon; + + // impose a range limit based on 2000km/h speed + range_limit = 5e3 + (2000e3 * elapsed / 3600); // 5km + 2000km/h + } else if (!surface && (Modes.bUserFlags & MODES_USER_LATLON_VALID)) { + reflat = Modes.fUserLat; + reflon = Modes.fUserLon; + + // The cell size is at least 360NM, giving a nominal + // max range of 180NM (half a cell). + // + // If the receiver range is more than half a cell + // then we must limit this range further to avoid + // ambiguity. (e.g. if we receive a position report + // at 200NM distance, this may resolve to a position + // at (200-360) = 160NM in the wrong direction) + if (Modes.maxRange > 1852*180) + range_limit = (1852*360) - Modes.maxRange; + } else { + // No local reference, give up + return (-1); + } + + result = decodeCPRrelative(reflat, reflon, + fflag ? a->odd_cprlat : a->even_cprlat, + fflag ? a->odd_cprlon : a->even_cprlon, + fflag, surface, + &lat, &lon); + if (result < 0) + return result; + // check range limit + if (range_limit > 0) { + double range = greatcircle(reflat, reflon, lat, lon); + if (range > range_limit) + return (-1); + } + + // check max range + if (Modes.maxRange > 0 && (Modes.bUserFlags & MODES_USER_LATLON_VALID)) { + double range = greatcircle(Modes.fUserLat, Modes.fUserLon, lat, lon); + if (range > Modes.maxRange) + return (-2); // we consider an out-of-range value to be bad data + } + + a->lat = lat; + a->lon = lon; + return 0; +} + +static void updatePosition(struct aircraft *a, struct modesMessage *mm, time_t now) +{ + int location_result = -1; + + if (mm->bFlags & MODES_ACFLAGS_LLODD_VALID) { + a->odd_cprlat = mm->raw_latitude; + a->odd_cprlon = mm->raw_longitude; + a->odd_cprtime = mstime(); + } else { + a->even_cprlat = mm->raw_latitude; + a->even_cprlon = mm->raw_longitude; + a->even_cprtime = mstime(); + } + + // If we have enough recent data, try global CPR + if (((mm->bFlags | a->bFlags) & MODES_ACFLAGS_LLEITHER_VALID) == MODES_ACFLAGS_LLBOTH_VALID && abs((int)(a->even_cprtime - a->odd_cprtime)) <= 10000) { + location_result = doGlobalCPR(a, (mm->bFlags & MODES_ACFLAGS_LLODD_VALID), (mm->bFlags & MODES_ACFLAGS_AOG)); + if (location_result == -2) { + // Global CPR failed because an airborne position produced implausible results. + // This is bad data. Discard both odd and even messages and wait for a fresh pair. + // Also disable aircraft-relative positions until we have a new good position (but don't discard the + // recorded position itself) + Modes.stats_current.cpr_global_bad++; + mm->bFlags &= ~(MODES_ACFLAGS_LATLON_VALID | MODES_ACFLAGS_LLODD_VALID | MODES_ACFLAGS_LLEVEN_VALID); + a->bFlags &= ~(MODES_ACFLAGS_LATLON_REL_OK | MODES_ACFLAGS_LLODD_VALID | MODES_ACFLAGS_LLEVEN_VALID); + return; + } else if (location_result == -1) { + // No local reference for surface position available, or the two messages crossed a zone. + // Nonfatal, try again later. + Modes.stats_current.cpr_global_skipped++; + } else { + Modes.stats_current.cpr_global_ok++; + } + } + + // Otherwise try relative CPR. + if (location_result == -1) { + location_result = doLocalCPR(a, (mm->bFlags & MODES_ACFLAGS_LLODD_VALID), (mm->bFlags & MODES_ACFLAGS_AOG), now); + if (location_result == -1) { + Modes.stats_current.cpr_local_skipped++; + } else { + Modes.stats_current.cpr_local_ok++; + mm->bFlags |= MODES_ACFLAGS_REL_CPR_USED; + } + } + + if (location_result == 0) { + // If we sucessfully decoded, back copy the results to mm so that we can print them in list output + mm->bFlags |= MODES_ACFLAGS_LATLON_VALID; + mm->fLat = a->lat; + mm->fLon = a->lon; + + // Update aircraft state + a->bFlags |= (MODES_ACFLAGS_LATLON_VALID | MODES_ACFLAGS_LATLON_REL_OK); + a->seenLatLon = a->seen; + a->timestampLatLon = a->timestamp; + } +} + struct aircraft *interactiveReceiveData(struct modesMessage *mm) { struct aircraft *a, *aux; + time_t now; // Return if (checking crc) AND (not crcok) AND (not fixed) if (Modes.check_crc && (mm->crcok == 0) && (mm->correctedbits == 0)) return NULL; + now = time(NULL); + // Lookup our aircraft or create a new one a = interactiveFindAircraft(mm->addr); if (!a) { // If it's a currently unknown aircraft.... @@ -216,7 +402,7 @@ struct aircraft *interactiveReceiveData(struct modesMessage *mm) { * since the aircraft that is currently on head sent a message, * othewise with multiple aircrafts at the same time we have an * useless shuffle of positions on the screen. */ - if (0 && Modes.aircrafts != a && (time(NULL) - a->seen) >= 1) { + if (0 && Modes.aircrafts != a && (now - a->seen) >= 1) { aux = Modes.aircrafts; while(aux->next != a) aux = aux->next; /* Now we are a node before the aircraft to remove. */ @@ -228,7 +414,7 @@ struct aircraft *interactiveReceiveData(struct modesMessage *mm) { } a->signalLevel[a->messages & 7] = mm->signalLevel;// replace the 8th oldest signal strength - a->seen = time(NULL); + a->seen = now; a->timestamp = mm->timestampMsg; a->messages++; @@ -282,36 +468,7 @@ struct aircraft *interactiveReceiveData(struct modesMessage *mm) { // If we've got a new cprlat or cprlon if (mm->bFlags & MODES_ACFLAGS_LLEITHER_VALID) { - int location_ok = 0; - - if (mm->bFlags & MODES_ACFLAGS_LLODD_VALID) { - a->odd_cprlat = mm->raw_latitude; - a->odd_cprlon = mm->raw_longitude; - a->odd_cprtime = mstime(); - } else { - a->even_cprlat = mm->raw_latitude; - a->even_cprlon = mm->raw_longitude; - a->even_cprtime = mstime(); - } - - // If we have enough recent data, try global CPR - if (((mm->bFlags | a->bFlags) & MODES_ACFLAGS_LLEITHER_VALID) == MODES_ACFLAGS_LLBOTH_VALID && abs((int)(a->even_cprtime - a->odd_cprtime)) <= 10000) { - if (decodeCPR(a, (mm->bFlags & MODES_ACFLAGS_LLODD_VALID), (mm->bFlags & MODES_ACFLAGS_AOG)) == 0) { - location_ok = 1; - } - } - - // Otherwise try relative CPR. - if (!location_ok && decodeCPRrelative(a, (mm->bFlags & MODES_ACFLAGS_LLODD_VALID), (mm->bFlags & MODES_ACFLAGS_AOG)) == 0) { - location_ok = 1; - } - - //If we sucessfully decoded, back copy the results to mm so that we can print them in list output - if (location_ok) { - mm->bFlags |= MODES_ACFLAGS_LATLON_VALID; - mm->fLat = a->lat; - mm->fLon = a->lon; - } + updatePosition(a, mm, now); } // Update the aircrafts a->bFlags to reflect the newly received mm->bFlags; diff --git a/mode_s.c b/mode_s.c index 4543bfa..5d689a4 100644 --- a/mode_s.c +++ b/mode_s.c @@ -949,324 +949,7 @@ void useModesMessage(struct modesMessage *mm) { if (Modes.net) {modesQueueOutput(mm);} } } -// -//========================================================================= -// -// Always positive MOD operation, used for CPR decoding. -// -int cprModInt(int a, int b) { - int res = a % b; - if (res < 0) res += b; - return res; -} -double cprModDouble(double a, double b) { - double res = fmod(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 < 0) lat = -lat; // Table is simmetric about the equator - 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(double lat, int fflag) { - int nl = cprNLFunction(lat) - (fflag ? 1 : 0); - if (nl < 1) nl = 1; - return nl; -} -// -//========================================================================= -// -double cprDlonFunction(double lat, int fflag, int surface) { - return (surface ? 90.0 : 360.0) / cprNFunction(lat, fflag); -} - -// Distance between points on a spherical earth. -// This has up to 0.5% error because the earth isn't actually spherical -// (but we don't use it in situations where that matters) -static double greatcircle(double lat0, double lon0, double lat1, double lon1) -{ - lat0 = lat0 * M_PI / 180.0; - lon0 = lon0 * M_PI / 180.0; - lat1 = lat1 * M_PI / 180.0; - lon1 = lon1 * M_PI / 180.0; - return 6371e3 * acos(sin(lat0) * sin(lat1) + cos(lat0) * cos(lat1) * cos(fabs(lon0 - lon1))); -} - -// -//========================================================================= -// -// 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. -// -int decodeCPR(struct aircraft *a, int fflag, int surface) { - double AirDlat0 = (surface ? 90.0 : 360.0) / 60.0; - double AirDlat1 = (surface ? 90.0 : 360.0) / 59.0; - 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 = (int) floor(((59*lat0 - 60*lat1) / 131072) + 0.5); - double rlat0 = AirDlat0 * (cprModInt(j,60) + lat0 / 131072); - double rlat1 = AirDlat1 * (cprModInt(j,59) + lat1 / 131072); - - time_t now = time(NULL); - double surface_rlat = MODES_USER_LATITUDE_DFLT; - double surface_rlon = MODES_USER_LONGITUDE_DFLT; - - double rlat, rlon; - - if (surface) { - // If we're on the ground, make sure we have a (likely) valid Lat/Lon - if ((a->bFlags & MODES_ACFLAGS_LATLON_VALID) && (((int)(now - a->seenLatLon)) < Modes.interactive_display_ttl)) { - surface_rlat = a->lat; - surface_rlon = a->lon; - } else if (Modes.bUserFlags & MODES_USER_LATLON_VALID) { - surface_rlat = Modes.fUserLat; - surface_rlon = Modes.fUserLon; - } else { - // No local reference, give up - return (-1); - } - - // Pick the quadrant that's closest to the reference location - - // this is not necessarily the same quadrant that contains the - // reference location. Note there are only two valid quadrants - // here (northern/southern hemisphere) - if ( (rlat0 - surface_rlat) > 45 ) rlat0 -= 90; - if ( (rlat1 - surface_rlat) > 45 ) rlat1 -= 90; - } else { - if (rlat0 >= 270) rlat0 -= 360; - if (rlat1 >= 270) rlat1 -= 360; - } - - // Check to see that the latitude is in range: -90 .. +90 - if (rlat0 < -90 || rlat0 > 90 || rlat1 < -90 || rlat1 > 90) - return (-1); - - // Check that both are in the same latitude zone, or abort. - if (cprNLFunction(rlat0) != cprNLFunction(rlat1)) - return (-1); - - // Compute ni and the Longitude Index "m" - if (fflag) { // Use odd packet. - int ni = cprNFunction(rlat1,1); - int m = (int) floor((((lon0 * (cprNLFunction(rlat1)-1)) - - (lon1 * cprNLFunction(rlat1))) / 131072.0) + 0.5); - rlon = cprDlonFunction(rlat1, 1, surface) * (cprModInt(m, ni)+lon1/131072); - rlat = rlat1; - } else { // Use even packet. - int ni = cprNFunction(rlat0,0); - int m = (int) floor((((lon0 * (cprNLFunction(rlat0)-1)) - - (lon1 * cprNLFunction(rlat0))) / 131072) + 0.5); - rlon = cprDlonFunction(rlat0, 0, surface) * (cprModInt(m, ni)+lon0/131072); - rlat = rlat0; - } - - if (surface) { - // Pick the quadrant that's closest to the reference location - - // this is not necessarily the same quadrant that contains the - // reference location. - rlon += floor( (surface_rlon - rlon + 45) / 90 ) * 90; // if we are >45 degrees away, move towards the reference position - rlon -= floor( (rlon + 180) / 360 ) * 360; // normalize to (-180..+180) - } else if (rlon > 180) { - rlon -= 360; - } - - // check max range - if (Modes.maxRange > 0 && (Modes.bUserFlags & MODES_USER_LATLON_VALID)) { - double range = greatcircle(Modes.fUserLat, Modes.fUserLon, rlat, rlon); - if (range > Modes.maxRange) - return (-2); // we consider an out-of-range value to be bad data - } - - a->lat = rlat; - a->lon = rlon; - a->seenLatLon = a->seen; - a->timestampLatLon = a->timestamp; - a->bFlags |= (MODES_ACFLAGS_LATLON_VALID | MODES_ACFLAGS_LATLON_REL_OK); - - return 0; -} -// -//========================================================================= -// -// This algorithm comes from: -// 1090-WP29-07-Draft_CPR101 (which also defines decodeCPR() ) -// -// Despite what the earlier comment here said, we should *not* be using trunc(). -// See Figure 5-5 / 5-6 and note that floor() is applied to (0.5 + fRP - fEP), not -// directly to (fRP - fEP). Eq 38 is correct. and we should use floor(). -int decodeCPRrelative(struct aircraft *a, int fflag, int surface) { - double AirDlat; - double AirDlon; - double lat; - double lon; - double lonr, latr; - double rlon, rlat; - double range_limit = 0; - int j,m; - - if (a->bFlags & MODES_ACFLAGS_LATLON_REL_OK) { // Ok to try aircraft relative first - int elapsed = (int)(time(NULL) - a->seenLatLon); - - latr = a->lat; - lonr = a->lon; - - // impose a range limit based on 2000km/h speed - range_limit = 5e3 + (2000e3 * elapsed / 3600); // 5km + 2000km/h - } else if (Modes.bUserFlags & MODES_USER_LATLON_VALID) { // Try ground station relative next - latr = Modes.fUserLat; - lonr = Modes.fUserLon; - - // The cell size is at least 360NM, giving a nominal - // max range of 180NM (half a cell). - // - // If the receiver range is more than half a cell - // then we must limit this range further to avoid - // ambiguity. (e.g. if we receive a position report - // at 200NM distance, this may resolve to a position - // at (200-360) = 160NM in the wrong direction) - if (Modes.maxRange > 1852*180) - range_limit = (1852*360) - Modes.maxRange; - } else { - return (-1); // Exit with error - can't do relative if we don't have ref. - } - - if (fflag) { // odd - AirDlat = (surface ? 90.0 : 360.0) / 59.0; - lat = a->odd_cprlat; - lon = a->odd_cprlon; - } else { // even - AirDlat = (surface ? 90.0 : 360.0) / 60.0; - lat = a->even_cprlat; - lon = a->even_cprlon; - } - - // Compute the Latitude Index "j" - j = (int) (floor(latr/AirDlat) + - floor(0.5 + cprModDouble(latr, AirDlat)/AirDlat - lat/131072)); - rlat = AirDlat * (j + lat/131072); - if (rlat >= 270) rlat -= 360; - - // Check to see that the latitude is in range: -90 .. +90 - if (rlat < -90 || rlat > 90) { - a->bFlags &= ~MODES_ACFLAGS_LATLON_REL_OK; // This will cause a quick exit next time if no global has been done - return (-1); // Time to give up - Latitude error - } - - // Check to see that answer is reasonable - ie no more than 1/2 cell away - if (fabs(rlat - latr) > (AirDlat/2)) { - a->bFlags &= ~MODES_ACFLAGS_LATLON_REL_OK; // This will cause a quick exit next time if no global has been done - return (-1); // Time to give up - Latitude error - } - - // Compute the Longitude Index "m" - AirDlon = cprDlonFunction(rlat, fflag, surface); - m = (int) (floor(lonr/AirDlon) + - floor(0.5 + cprModDouble(lonr, AirDlon)/AirDlon - lon/131072)); - rlon = AirDlon * (m + lon/131072); - if (rlon > 180) rlon -= 360; - - // Check to see that answer is reasonable - ie no more than 1/2 cell away - if (fabs(rlon - lonr) > (AirDlon/2)) { - a->bFlags &= ~MODES_ACFLAGS_LATLON_REL_OK; // This will cause a quick exit next time if no global has been done - return (-1); // Time to give up - Longitude error - } - - // check range limit - if (range_limit > 0) { - double range = greatcircle(latr, lonr, rlat, rlon); - if (range > range_limit) - return (-1); - } - - // check max range - if (Modes.maxRange > 0 && (Modes.bUserFlags & MODES_USER_LATLON_VALID)) { - double range = greatcircle(Modes.fUserLat, Modes.fUserLon, rlat, rlon); - if (range > Modes.maxRange) - return (-2); // we consider an out-of-range value to be bad data - } - - a->lat = rlat; - a->lon = rlon; - - a->seenLatLon = a->seen; - a->timestampLatLon = a->timestamp; - a->bFlags |= (MODES_ACFLAGS_LATLON_VALID | MODES_ACFLAGS_LATLON_REL_OK); - return (0); -} // // ===================== Mode S detection and decoding =================== // diff --git a/stats.c b/stats.c index 1cf0c6a..cdf3598 100644 --- a/stats.c +++ b/stats.c @@ -120,6 +120,19 @@ void display_stats(struct stats *st) { printf("%d total usable messages\n", st->messages_total); + printf("%d global CPR attempts with valid positions\n" + "%d global CPR attempts with bad data\n" + "%d global CPR attempts with insufficient data\n" + "%d local CPR attempts with valid positions\n" + "%d local CPR attempts with insufficient data\n" + "%d CPR messages that look like transponder failures filtered\n", + st->cpr_global_ok, + st->cpr_global_bad, + st->cpr_global_skipped, + st->cpr_local_ok, + st->cpr_local_skipped, + st->cpr_filtered); + fflush(stdout); } @@ -192,5 +205,13 @@ void add_stats(const struct stats *st1, const struct stats *st2, struct stats *t // total messages: target->messages_total = st1->messages_total + st2->messages_total; + + // CPR decoding: + target->cpr_global_ok = st1->cpr_global_ok + st2->cpr_global_ok; + target->cpr_global_bad = st1->cpr_global_bad + st2->cpr_global_bad; + target->cpr_global_skipped = st1->cpr_global_skipped + st2->cpr_global_skipped; + target->cpr_local_ok = st1->cpr_local_ok + st2->cpr_local_ok; + target->cpr_local_skipped = st1->cpr_local_skipped + st2->cpr_local_skipped; + target->cpr_filtered = st1->cpr_filtered + st2->cpr_filtered; } diff --git a/stats.h b/stats.h index 7be07ab..46d65b2 100644 --- a/stats.h +++ b/stats.h @@ -97,6 +97,14 @@ struct stats { // total messages: unsigned int messages_total; + + // CPR decoding: + unsigned int cpr_global_ok; + unsigned int cpr_global_bad; + unsigned int cpr_global_skipped; + unsigned int cpr_local_ok; + unsigned int cpr_local_skipped; + unsigned int cpr_filtered; }; void add_stats(const struct stats *st1, const struct stats *st2, struct stats *target);