diff --git a/gradle.properties b/gradle.properties index a8f0d77..c47ae13 100644 --- a/gradle.properties +++ b/gradle.properties @@ -1,5 +1,5 @@ GROUP=aero.t2s -VERSION_NAME=0.2.5-SNAPSHOT +VERSION_NAME=0.2.6-SNAPSHOT POM_ARTIFACT_ID=mode-s POM_NAME=Mode-S/ADS-B (1090Mhz) diff --git a/src/main/java/aero/t2s/modes/CprPosition.java b/src/main/java/aero/t2s/modes/CprPosition.java index 5326368..184710c 100644 --- a/src/main/java/aero/t2s/modes/CprPosition.java +++ b/src/main/java/aero/t2s/modes/CprPosition.java @@ -1,9 +1,28 @@ package aero.t2s.modes; +import java.time.Instant; + public class CprPosition { private double lat; private double lon; - private int time; + private boolean valid; + private long time; + + public CprPosition() { + this.lat = 0.0; + this.lon = 0.0; + this.valid = false; + } + public CprPosition(double lat, double lon) { + setLatLon(lat ,lon); + } + + public void setLatLon(double lat, double lon) { + this.lat = lat; + this.lon = lon; + this.time = Instant.now().toEpochMilli(); + this.valid = true; + } public void setLat(double lat) { this.lat = lat; @@ -21,15 +40,19 @@ public double getLon() { return lon; } - public void setTime(int time) { + public void setTime(long time) { this.time = time; } - public int getTime() { + public long getTime() { return time; } public boolean isValid() { - return lat != 0d && lon != 0; + return valid; + } + + public boolean isExpired() { + return time < Instant.now().minusSeconds(10).toEpochMilli(); } } diff --git a/src/main/java/aero/t2s/modes/ModeSHandler.java b/src/main/java/aero/t2s/modes/ModeSHandler.java index aae0391..dafdb65 100644 --- a/src/main/java/aero/t2s/modes/ModeSHandler.java +++ b/src/main/java/aero/t2s/modes/ModeSHandler.java @@ -1,6 +1,7 @@ package aero.t2s.modes; import aero.t2s.modes.decoder.df.DownlinkFormat; +import aero.t2s.modes.decoder.df.df17.AirbornePosition; import java.util.function.Consumer; @@ -42,10 +43,10 @@ protected short[] toData(final String input) throws EmptyMessageException, ModeA } public void start() { - + AirbornePosition.start(); } public void stop() { - + AirbornePosition.stop(); } } diff --git a/src/main/java/aero/t2s/modes/Track.java b/src/main/java/aero/t2s/modes/Track.java index dc0c3e7..f76da27 100644 --- a/src/main/java/aero/t2s/modes/Track.java +++ b/src/main/java/aero/t2s/modes/Track.java @@ -11,6 +11,7 @@ public class Track { private Altitude altitude = new Altitude(); private double lat; private double lon; + private boolean positionAvailable = false; private int vx; private int vy; private double gs; @@ -172,7 +173,13 @@ public boolean isGroundBit() { return groundBit; } + public void setLatLon(double lat, double lon) { + this.lat = lat; + this.lon = lon; + this.positionAvailable = true; + } public void setLat(double lat) { + //TODO How do we know if position really is available if we only set the lat? Can we remove this method? this.lat = lat; } @@ -181,6 +188,7 @@ public double getLat() { } public void setLon(double lon) { + //TODO How do we know if position really is available if we only set the lon? Can we remove this method? this.lon = lon; } @@ -245,7 +253,7 @@ public int getModeA() { } public boolean isPositionAvailable() { - return lat != 0 & lon != 0; + return positionAvailable; } public void setGeometricHeightOffset(int geometricHeightOffset) { diff --git a/src/main/java/aero/t2s/modes/decoder/Decoder.java b/src/main/java/aero/t2s/modes/decoder/Decoder.java index ab2792c..b485453 100644 --- a/src/main/java/aero/t2s/modes/decoder/Decoder.java +++ b/src/main/java/aero/t2s/modes/decoder/Decoder.java @@ -50,7 +50,7 @@ public DownlinkFormat decode(short[] data) throws UnknownDownlinkFormatException df = new DF16(data); break; case 17: - df = new DF17(data, originLat, originLon); + df = new DF17(data); break; case 18: df = new DF18(data); diff --git a/src/main/java/aero/t2s/modes/decoder/df/DF17.java b/src/main/java/aero/t2s/modes/decoder/df/DF17.java index 622caf4..9860eaa 100644 --- a/src/main/java/aero/t2s/modes/decoder/df/DF17.java +++ b/src/main/java/aero/t2s/modes/decoder/df/DF17.java @@ -4,15 +4,10 @@ import aero.t2s.modes.decoder.df.df17.*; public class DF17 extends DownlinkFormat { - private final double originLat; - private final double originLon; - private ExtendedSquitter extendedSquitter; - public DF17(short[] data, double originLat, double originLon) { + public DF17(short[] data) { super(data, IcaoAddress.FROM_MESSAGE); - this.originLat = originLat; - this.originLon = originLon; } @Override @@ -34,7 +29,7 @@ public DF17 decode() { case 20: case 21: case 22: - extendedSquitter = new AirbornePosition(data, originLat, originLon); + extendedSquitter = new AirbornePosition(data, getIcao()); break; case 1: case 2: diff --git a/src/main/java/aero/t2s/modes/decoder/df/df17/AirbornePosition.java b/src/main/java/aero/t2s/modes/decoder/df/df17/AirbornePosition.java index c652f4a..ad59e82 100644 --- a/src/main/java/aero/t2s/modes/decoder/df/df17/AirbornePosition.java +++ b/src/main/java/aero/t2s/modes/decoder/df/df17/AirbornePosition.java @@ -1,15 +1,17 @@ package aero.t2s.modes.decoder.df.df17; import aero.t2s.modes.Track; +import aero.t2s.modes.CprPosition; import aero.t2s.modes.constants.*; +import aero.t2s.modes.decoder.Common; import aero.t2s.modes.registers.Register05; import aero.t2s.modes.registers.Register05V0; import aero.t2s.modes.registers.Register05V2; -public class AirbornePosition extends ExtendedSquitter { - private final double originLat; - private final double originLon; +import java.util.*; +public class AirbornePosition extends ExtendedSquitter { + private final String address; private SurveillanceStatus surveillanceStatus; private int singleAntennaFlag; @@ -17,13 +19,15 @@ public class AirbornePosition extends ExtendedSquitter { private int altitude; private boolean positionAvailable; + private double lat; private double lon; + private static Map cache = new HashMap<>(); + private static Timer cacheCleanup; - public AirbornePosition(short[] data, final double originLat, final double originLon) { + public AirbornePosition(short[] data, String address) { super(data); - this.originLat = originLat; - this.originLon = originLon; + this.address = address; } @Override @@ -43,10 +47,8 @@ public AirbornePosition decode() { return this; } - positionAvailable = true; - int time = (data[6] >>> 3) & 0x1; - boolean cprEven = ((data[6] >>> 2) & 0x1) == 0; + boolean isCprEven = ((data[6] >>> 2) & 0x1) == 0; int cprLat = (data[6] & 0x3) << 15; cprLat = cprLat | (data[7] << 7); @@ -56,7 +58,38 @@ public AirbornePosition decode() { cprLon = cprLon | (data[9] << 8); cprLon = cprLon | data[10]; - calculatePosition(cprEven, ((double)cprLat) / ((double)(1 << 17)), ((double)cprLon) / ((double)(1 << 17)), time); + + if (!cache.containsKey(address)) { + if (!isCprEven) { + return this; + } + + synchronized (cache) { + cache.putIfAbsent(address, new PositionUpdate( + new CprPosition(cprLat / (double)(1 << 17), cprLon / (double)(1 << 17)) + )); + } + } + + PositionUpdate positionUpdate; + synchronized (cache) { + positionUpdate = cache.get(address); + } + if (isCprEven) { + positionUpdate.setEven(new CprPosition(cprLat / (double) (1 << 17), cprLon / (double) (1 << 17))); + } else { + positionUpdate.setOdd(new CprPosition(cprLat / (double) (1 << 17), cprLon / (double) (1 << 17))); + } + + if (positionUpdate.isComplete()) { + calculateGlobal(positionUpdate.even, positionUpdate.odd); + } else if (positionUpdate.isPreviousPositionAvailable() && positionUpdate.isPreviousPositionAvailable()) { + calculateLocal(positionUpdate.odd, true, positionUpdate.previousLat, positionUpdate.previousLon); + } + + if (positionAvailable) { + positionUpdate.setPreviousPosition(this.lat, this.lon); + } return this; } @@ -67,8 +100,9 @@ public void apply(Track track) { track.setSpi(surveillanceStatus == SurveillanceStatus.SPI); track.setTempAlert(surveillanceStatus == SurveillanceStatus.TEMPORARY_ALERT); track.setEmergency(surveillanceStatus == SurveillanceStatus.PERMANENT_ALERT); - track.setLat(lat); - track.setLon(lon); + if (positionAvailable) { + track.setLatLon(lat, lon); + } if (versionChanged(track)) { switch (track.getVersion()) { @@ -119,14 +153,6 @@ public BarometricAltitudeIntegrityCode getNICbaro() { } } - public double getOriginLat() { - return originLat; - } - - public double getOriginLon() { - return originLon; - } - public SurveillanceStatus getSurveillanceStatus() { return surveillanceStatus; } @@ -204,88 +230,77 @@ private AltitudeSource determineAltitudeSource() { return AltitudeSource.GNSS_HAE; } - private void calculatePosition(boolean isEven, double lat, double lon, double time) { -// CprPosition cprEven = track.getCprPosition(true); -// CprPosition cprOdd = track.getCprPosition(false); + private void calculateLocal(CprPosition cpr, boolean isOdd, double previousLat, double previousLon) { + + double dlat = isOdd ? 360.0 / 59.0 : 360.0 / 60.0; + + double j = Math.floor(previousLat / dlat) + Math.floor((previousLat % dlat) / dlat - cpr.getLat() + 0.5); + + double newLat = dlat * (j + previousLat); -// if (! (cprEven.isValid() && cprOdd.isValid())) { - calculateLocal(isEven, lat, lon, time); -// return; -// } + double nl = NL(newLat) - (isOdd ? 1.0 : 0.0); + double dlon = nl > 0 ? 360.0 / nl : 360; + + double m = Math.floor(previousLon / dlon) + Math.floor((previousLon % dlon) / dlon - cpr.getLon() + 0.5); + double newLon = dlon * (m + lon); -// calculateGlobal(track, cprEven, cprOdd); + //TODO Should be a sanity-check here to make sure the calculated position isn't outside receiver origin range + //TODO Should be a sanity-check here to see if the calculated movement since the last update is too far + this.lat = newLat; + this.lon = newLon; } - private void calculateLocal(boolean isEven, double lat, double lon, double time) { - boolean isOdd = !isEven; -// CprPosition cpr = track.getCprPosition(isEven); + private void calculateGlobal(CprPosition cprEven, CprPosition cprOdd) { + double dLat0 = 360.0 / 60.0; + double dLat1 = 360.0 / 59.0; - double dlat = isOdd ? 360.0 / 59.0 : 360.0 / 60.0; + double j = Math.floor(59.0 * cprEven.getLat() - 60.0 * cprOdd.getLat() + 0.5); + + double latEven = dLat0 * (j % 60.0 + cprEven.getLat()); + double latOdd = dLat1 * (j % 59.0 + cprOdd.getLat()); - double j = Math.floor(originLat / dlat) + Math.floor((originLat % dlat) / dlat - lat + 0.5); + if (latEven >= 270.0 && latEven <= 360.0) { + latEven -= 360.0; + } - lat = dlat * (j + lat); + if (latOdd >= 270.0 && latOdd <= 360.0) { + latOdd -= 360.0; + } - double nl = NL(lat) - (isOdd ? 1.0 : 0.0); - double dlon = nl > 0 ? 360.0 / nl : 360; + if (NL(latEven) != NL(latOdd)) { + return; + } - double m = Math.floor(originLon / dlon) + Math.floor((originLon % dlon) / dlon - lon + 0.5); - lon = dlon * (m + lon); + double lat; + double lon; + if (cprEven.getTime() > cprOdd.getTime()) { + double ni = cprN(latEven, 0); + double m = Math.floor(cprEven.getLon() * (NL(latEven) - 1) - cprOdd.getLon() * NL(latEven) + 0.5); + lat = latEven; + lon = (360d / ni) * (m % ni + cprEven.getLon()); + } else { + double ni = cprN(latOdd, 1); + double m = Math.floor(cprEven.getLon() * (NL(latOdd) - 1) - cprOdd.getLon() * NL(latOdd) + 0.5); + + lat = latOdd; + lon = (360d / ni) * (m % ni + cprOdd.getLon()); + } + + if (lon > 180d) { + lon -= 360d; + } + + //TODO Should be a sanity-check here to make sure the calculated position isn't outside receiver origin range, this.lat = lat; this.lon = lon; + this.positionAvailable = true; } + private double cprN(double lat, double isOdd) { + double nl = NL(lat) - isOdd; -// private void calculateGlobal(Track track, CprPosition cprEven, CprPosition cprOdd) { -// double dLat0 = 360.0 / 60.0; -// double dLat1 = 360.0 / 59.0; -// -// double j = Math.floor(59.0 * cprEven.getLat() - 60.0 * cprOdd.getLat() + 0.5); -// -// double latEven = dLat0 * (j % 60.0 + cprEven.getLat()); -// double latOdd = dLat1 * (j % 59.0 + cprOdd.getLat()); -// -// if (latEven >= 270.0 && latEven <= 360.0) { -// latEven -= 360.0; -// } -// -// if (latOdd >= 270.0 && latOdd <= 360.0) { -// latOdd -= 360.0; -// } -// -// if (NL(latEven) != NL(latOdd)) { -// return; -// } -// -// double lat; -// double lon; -// if (cprEven.getTime() > cprOdd.getTime()) { -// double ni = cprN(latEven, 0); -// double m = Math.floor(cprEven.getLon() * (NL(latEven) - 1) - cprOdd.getLon() * NL(latEven) + 0.5); -// -// lat = latEven; -// lon = (360d / ni) * (m % ni + cprEven.getLon()); -// } else { -// double ni = cprN(latOdd, 1); -// double m = Math.floor(cprEven.getLon() * (NL(latOdd) - 1) - cprOdd.getLon() * NL(latOdd) + 0.5); -// -// lat = latOdd; -// lon = (360d / ni) * (m % ni + cprOdd.getLon()); -// } -// -// if (lon > 180d) { -// lon -= 360d; -// } -// -// track.setLat(lat); -// track.setLon(lon); -// } - -// private double cprN(double lat, double isOdd) { -// double nl = NL(lat) - isOdd; -// -// return nl > 1 ? nl : 1; -// } + return nl > 1 ? nl : 1; + } private double NL(double lat) { if (lat == 0) return 59; @@ -305,4 +320,66 @@ private int calculateAltitude(short[] data, int typeCode) { return (n * qBit) - 1000; } + + public static void start() { + AirbornePosition.cache.clear(); + AirbornePosition.cacheCleanup.schedule(new TimerTask() { + @Override + public void run() { + List expired = new LinkedList<>(); + + synchronized (cache) { + cache.entrySet().stream().filter(entry -> entry.getValue().isExpired()).forEach(entry -> expired.add(entry.getKey())); + expired.forEach(cache::remove); + } + } + }, 0, 10_000); + } + + public static void stop() { + AirbornePosition.cacheCleanup.cancel(); + AirbornePosition.cacheCleanup = null; + + AirbornePosition.cache.clear(); + } + + class PositionUpdate { + private CprPosition even; + private CprPosition odd; + + + private boolean previousPositionAvailable = false; + private double previousLat; + private double previousLon; + + public PositionUpdate(CprPosition even) { + this.even = even; + } + + public void setEven(CprPosition even) { + this.even = even; + this.odd = null; + } + + public void setOdd(CprPosition odd) { + this.odd = odd; + } + + public void setPreviousPosition(double lat, double lon) { + this.previousLat = lat; + this.previousLon = lon; + } + + public boolean isPreviousPositionAvailable() { + return this.previousPositionAvailable; + } + + public boolean isComplete() { + return even != null && odd != null; + } + + public boolean isExpired() { + return even.isExpired() || odd.isExpired(); + } + } }