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

@@ -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,29 +2321,28 @@ int cprNFunction(double lat, int isodd) {
     return nl;
 }
 
-double cprDlonFunction(double lat, int isodd) {
+double cprDlonFunction(double lat, int isodd, int surface) {
-    return 360.0 / cprNFunction(lat, isodd);
+    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) {
+void decodeCPR(struct aircraft *a, int fflag, int surface) {
-    const double AirDlat0 = 360.0 / 60;
+    double AirDlat0 = (surface ? 90.0 : 360.0) / 60.0;
-    const double AirDlat1 = 360.0 / 59;
+    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" */
+    // 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);
@@ -2351,29 +2350,96 @@ void decodeCPR(struct aircraft *a) {
     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 */
+    // Compute ni and the Longitude Index "m"
-    if (a->even_cprtime > a->odd_cprtime) {
+    if (fflag) { // Use odd packet.
-        /* 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. */
         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
+            // Try relative CPR first
-             * the position. */
+            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);
+                      decodeCPR(a, mm->fflag, 0);
+                }
             }
         } else if (mm->metype == 19) {
             if (mm->mesub == 1 || mm->mesub == 2) {