From 58d9d561df35f88884b2959d2cf322f1ee69e3cd Mon Sep 17 00:00:00 2001 From: Arturs Artamonovs Date: Wed, 26 Jun 2024 08:29:38 +0100 Subject: Decoding lat/lon seems to work --- ADSBDecoder/PositionDecoder.swift | 221 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 221 insertions(+) create mode 100644 ADSBDecoder/PositionDecoder.swift (limited to 'ADSBDecoder/PositionDecoder.swift') 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 = [] + + 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 + } + + +} -- cgit v1.2.3