Decoding lat/lon seems to work

1 files changed, 221 insertions, 0 deletions

diff --git a/ADSBDecoder/PositionDecoder.swift b/ADSBDecoder/PositionDecoder.swift
new file mode 100644
index 0000000..7d77221
--- /dev/null
+++ b/ADSBDecoder/PositionDecoder.swift
@@ -0,0 +1,221 @@
+//
+//  PositionDecoder.swift
+//  ADSBDecoder
+//
+//  Created by Jacky Jack on 14/06/2024.
+//
+
+import Foundation
+import Collections
+
+struct PositionData {
+    var cpr_lat: UInt32
+    var cpr_long: UInt32
+    var even: Bool
+    var cprtime: Int64
+}
+
+func cpr_mod(_ a: Int, _ b: Int) -> Int {
+    var res:Int = a % b;
+    if (res < 0) {
+        res += b
+    }
+    return res;
+}
+
+/* The NL function uses the precomputed table from 1090-WP-9-14 */
+func cpr_NLFunction(_ _lat: Double) -> Int {
+    var lat: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}
+}
+
+func cpr_NFunction(_ lat: Double, _ isodd: Int) -> Int {
+    var nl:Int = cpr_NLFunction(lat) - isodd;
+    if (nl < 1) {
+        nl = 1
+    }
+    return nl;
+}
+
+func cpr_DlonFunction(_ lat: Double, _ isodd: Int) -> Double {
+    return 360.0 / Double(cpr_NFunction(lat, isodd));
+}
+
+func mstime() -> Int64 {
+    var tv:timeval = timeval(tv_sec: 0, tv_usec: 0)
+    var mst:Int64=0;
+
+    gettimeofday(&tv, nil);
+    mst = Int64(tv.tv_sec)*1000;
+    mst += Int64(tv.tv_usec)/1000;
+    return mst;
+}
+
+class PositionDecoder {
+    
+    var long: Double = 0.0
+    var lat: Double = 0.0
+    var ready: Bool = false
+    var newposition: Bool = false
+    
+    //initialise first values so this values is nonsence
+    var queue: Deque<PositionData> = []
+    
+    init() {
+        
+    }
+    
+    func addEvenPosition(_ cpr_lat: UInt32,_ cpr_long: UInt32, _ cprtime: Int64) {
+        queue.append(PositionData(cpr_lat:cpr_lat, cpr_long:cpr_long, even:true, cprtime: cprtime))
+    }
+    
+    func addOddPosition(_ cpr_lat: UInt32,_ cpr_long: UInt32, _ cprtime: Int64) {
+        queue.append(PositionData(cpr_lat:cpr_lat,cpr_long:cpr_long,even:false, cprtime: cprtime))
+    }
+    
+    func getPosition() -> (Double, Double)? {
+        if (ready) {
+            return (lat, long)
+        }
+        return nil
+    }
+    
+    //TODO: allways adds to queue newer frees the queue
+    func calcPosition() -> Bool {
+        if (queue.count > 2) {
+            let el1 = queue[queue.count-1]
+            let el2 = queue[queue.count-2]
+            if (el1.even == el2.even) {
+                //ready = false
+                return false
+            }
+        } else {
+            ready = false
+            print("Position queue to short to calculate values")
+            return false
+        }
+        
+        //last to elements are evan and odd
+        let el1 = queue[queue.count-1]
+        let el2 = queue[queue.count-2]
+        let cpr_even = el1.even ? el1 : el2
+        let cpr_odd  = (!el1.even) ? el1 : el2
+        
+        print("Position queue is ready to calculate location \(cpr_even) \(cpr_odd)")
+        // from here https://github.com/antirez/dump1090/blob/master/dump1090.c#L1718
+        let AirDlat0:Double = 360.0/60.0
+        let AirDlat1:Double = 360.0/59.0
+        
+        let lat0 = Int(cpr_even.cpr_lat)
+        let lat1 = Int(cpr_odd.cpr_lat)
+        let lon0 = Int(cpr_even.cpr_long)
+        let lon1 = Int(cpr_odd.cpr_long)
+        
+        //latitude index
+        let j:Int = Int(floor(((59.0*Double(lat0) - 60.0*Double(lat1)) / 131072.0) + 0.5))
+        
+        var rlat0:Double = AirDlat0 * (Double(cpr_mod(j,60)) + Double(lat0) / 131072.0)
+        var rlat1:Double = AirDlat1 * (Double(cpr_mod(j,59)) + Double(lat1) / 131072.0)
+        
+        if (rlat0 >= 270.0) {
+            rlat0 -= 360.0
+        }
+        if (rlat1 >= 270.0) {
+            rlat1 -= 360.0
+        }
+        
+        /* Check that both are in the same latitude zone, or abort. */
+        if (cpr_NLFunction(rlat0) != cpr_NLFunction(rlat1)) {
+            print("Not same lat?!")
+            return false
+        }
+        
+        /* Compute ni and the longitude index m */
+        if (cpr_even.cprtime > cpr_odd.cprtime) {
+                /* Use even packet. */
+                let ni:Int = cpr_NFunction(rlat0,0);
+                let m:Int = Int(
+                    floor((((Double(lon0) * (Double(cpr_NLFunction(rlat0))-1.0)) -
+                                    (Double(lon1) * Double(cpr_NLFunction(rlat0)))) / 131072.0) + 0.5)
+                );
+                self.long = Double(cpr_DlonFunction(rlat0,0)) * (Double(cpr_mod(m,ni))+Double(lon0)/131072.0);
+                self.lat = rlat0;
+            } else {
+                /* Use odd packet. */
+                let ni:Int = cpr_NFunction(rlat1,1);
+                let m:Int = Int(floor((((Double(lon0) * (Double(cpr_NLFunction(rlat1))-1.0)) -
+                                (Double(lon1) * Double(cpr_NLFunction(rlat1)))) / 131072.0) + 0.5));
+                self.long = cpr_DlonFunction(rlat1,1) * (Double(cpr_mod(m,ni))+Double(lon1)/131072.0);
+                self.lat = rlat1;
+            }
+        if (self.long > 180) {
+            self.long -= 360
+        }
+        
+        ready = true
+        return true
+    }
+    
+    
+}