From a04e399d7e45a146afe3279d5f02ca6c5e64f387 Mon Sep 17 00:00:00 2001 From: Malcolm Robb Date: Wed, 24 Apr 2013 20:24:46 +0100 Subject: [PATCH] VK1ET : Implement Relative CPR decoding Modifications entirely base on code supplied by John VK1ET. Implements relative CPR Lat/Long decoding for --interactive display --- dump1090.c | 119 ++++++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 94 insertions(+), 25 deletions(-) diff --git a/dump1090.c b/dump1090.c index e236563..f25d1e2 100644 --- a/dump1090.c +++ b/dump1090.c @@ -56,7 +56,7 @@ // MinorVer changes when additional features are added, but not for bug fixes (range 00-99) // DayDate & Year changes for all changes, including for bug fixes. It represent the release date of the update // -#define MODES_DUMP1090_VERSION "1.02.2304.13" +#define MODES_DUMP1090_VERSION "1.02.2404.13" #define MODES_DEFAULT_RATE 2000000 #define MODES_DEFAULT_FREQ 1090000000 @@ -2321,59 +2321,125 @@ int cprNFunction(double lat, int isodd) { return nl; } -double cprDlonFunction(double lat, int isodd) { - return 360.0 / cprNFunction(lat, isodd); +double cprDlonFunction(double lat, int isodd, int surface) { + return (surface ? 90.0 : 360.0) / cprNFunction(lat, 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 = 360.0 / 60; - const double AirDlat1 = 360.0 / 59; +void 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); + // Compute the Latitude Index "j" + int j = (int) floor(((59*lat0 - 60*lat1) / 131072) + 0.5); double rlat0 = AirDlat0 * (cprModFunction(j,60) + lat0 / 131072); double rlat1 = AirDlat1 * (cprModFunction(j,59) + lat1 / 131072); if (rlat0 >= 270) rlat0 -= 360; if (rlat1 >= 270) rlat1 -= 360; - /* Check that both are in the same latitude zone, or abort. */ + // 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 = (int) floor((((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. */ + // 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); - a->lon = cprDlonFunction(rlat1,1) * (cprModFunction(m,ni)+lon1/131072); + a->lon = cprDlonFunction(rlat1,1,surface) * (cprModFunction(m,ni)+lon1/131072); a->lat = rlat1; + } else { // Use even packet. + int ni = cprNFunction(rlat0,0); + int m = (int) floor((((lon0 * (cprNLFunction(rlat0)-1)) - + (lon1 * cprNLFunction(rlat0))) / 131072) + 0.5); + a->lon = cprDlonFunction(rlat0, 0, surface) * (cprModFunction(m, ni)+lon0/131072); + a->lat = rlat0; } if (a->lon > 180) a->lon -= 360; + a->sbsflags |= MODES_SBS_LAT_LONG_FRESH; } +/* This algorithm comes from: + * 1090-WP29-07-Draft_CPR101 (which also defines decodeCPR() ) + * + * There is an error in this document related to CPR relative decode. + * Should use trunc() rather than the floor() function in Eq 38 and related for deltaZI. + * floor() returns integer less than argument + * trunc() returns integer closer to zero than argument. + * Note: text of document describes trunc() functionality for deltaZI calculation + * but the formulae use floor(). + */ +int decodeCPRrelative(struct aircraft *a, int fflag, int surface, double latr, double lonr) { + double AirDlat; + double AirDlon; + double lat; + double lon; + double rlon, rlat; + int j,m; + + // If not passed a lat/long, we must be using aircraft relative + if ( (latr == 0) && (lonr == 0) ) { + latr = a->lat; + lonr = a->lon; + } + if ( (latr == 0) && (lonr == 0) ) + 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) + + trunc(0.5 + cprModFunction((int)latr, (int)AirDlat)/AirDlat - lat/131072)); + rlat = AirDlat * (j + lat/131072); + if (rlat >= 270) rlat -= 360; + + // Check to see that answer is reasonable - ie no more than 1/2 cell away + if (fabs(rlat - a->lat) > (AirDlat/2)) { + a->lat = a->lon = 0; // 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, 0); + m = (int) (floor(lonr/AirDlon) + + trunc(0.5 + cprModFunction((int)lonr, (int)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 - a->lon) > (AirDlon/2)) { + a->lat = a->lon = 0; // This will cause a quick exit next time if no global has been done + return (-1); // Time to give up - Longitude error + } + + a->lat = rlat; + a->lon = rlon; + a->sbsflags |= MODES_SBS_LAT_LONG_FRESH; + + return (0); +} + /* Receive new messages and populate the interactive mode with more info. */ struct aircraft *interactiveReceiveData(struct modesMessage *mm) { uint32_t addr; @@ -2443,10 +2509,13 @@ struct aircraft *interactiveReceiveData(struct modesMessage *mm) { a->even_cprlon = mm->raw_longitude; a->even_cprtime = mstime(); } - /* If the two data is less than 10 seconds apart, compute - * the position. */ - if (abs((int)(a->even_cprtime - a->odd_cprtime)) <= 10000) { - decodeCPR(a); + // Try relative CPR first + if (decodeCPRrelative(a, mm->fflag, 0, 0, 0)) { + // If it fails then try global if the two data are less than 10 seconds apart, compute + // the position. + if (abs((int)(a->even_cprtime - a->odd_cprtime)) <= 10000) { + decodeCPR(a, mm->fflag, 0); + } } } else if (mm->metype == 19) { if (mm->mesub == 1 || mm->mesub == 2) {