diff --git a/src/main/java/com/windhoverlabs/yamcs/gdl90/GDL90Link.java b/src/main/java/com/windhoverlabs/yamcs/gdl90/GDL90Link.java index 961173d..1243f44 100644 --- a/src/main/java/com/windhoverlabs/yamcs/gdl90/GDL90Link.java +++ b/src/main/java/com/windhoverlabs/yamcs/gdl90/GDL90Link.java @@ -253,6 +253,11 @@ enum AHRS_MODE { YPR, QT } + + enum AHRS_ENCODING { + WHL, + FF + } /* Configuration Defaults */ private static TupleDefinition gftdef; @@ -308,6 +313,7 @@ enum AHRS_MODE { private int ownshipGeoAltitudeCount = 0; private int foreFlightIDCount = 0; private int AHRSCount = 0; + private int WHLAHRSCount = 0; String GDL90Hostname; Integer appNameMax; @@ -324,6 +330,9 @@ enum AHRS_MODE { private String AHRSStreamName; private Stream AHRSStream; + private String WHLAHRSStreamName; + private Stream WHLAHRSStream; + private String ForeFlightIDStreamName; private Stream ForeFlightIDStream; @@ -364,8 +373,10 @@ enum AHRS_MODE { private int ownShipReportRate; private int ownShipGeoAltitudeRate; private int AHRSRate; + private int WHLAHRSRate; private AHRS_MODE ahrsMode; + private AHRS_ENCODING ahrsEncoding; static { gftdef = new TupleDefinition(); @@ -471,7 +482,9 @@ private void initPVMode() { headingType = AHRSHeadingType.valueOf(headingString); String ahrsModeString = (String) this.config.getMap("pvConfig").get("AHRS_Mode"); - + String ahrsEncodingString = (String) this.config.getMap("pvConfig").get("AHRS_ENCODING"); + // AHRS_ENCODING + // ahrsEncoding = AHRS_ENCODING.valueOf(ahrsEncodingString); ahrsMode = AHRS_MODE.valueOf(ahrsModeString); if (!this.realtime) { @@ -540,6 +553,8 @@ private void initGDL90Timers() { ownShipReportRate = this.config.getInt("ownShipReportRate", 1); ownShipGeoAltitudeRate = this.config.getInt("ownShipGeoAltitudeRate", 1); AHRSRate = this.config.getInt("AHRSRate", 5); + + WHLAHRSRate = this.config.getInt("WHLAHRSRate", 100); scheduler.scheduleAtFixedRate( () -> { if (isRunningAndEnabled()) { @@ -618,6 +633,21 @@ private void initGDL90Timers() { 100, 1000 / AHRSRate, TimeUnit.MILLISECONDS); + + scheduler.scheduleAtFixedRate( + () -> { + if (isRunningAndEnabled()) { + try { + WHLAHRSMessage(); + } catch (IOException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + }, + 100, + 1000 / WHLAHRSRate, + TimeUnit.MILLISECONDS); } private void initStreams() { @@ -652,6 +682,12 @@ private void initStreams() { if (AHRSStreamName != null) { this.AHRSStream = getStream(ydb, AHRSStreamName); } + + WHLAHRSStreamName = this.getConfig().getString("WHLAHRSStreamName", null); + + if (WHLAHRSStreamName != null) { + this.WHLAHRSStream = getStream(ydb, WHLAHRSStreamName); + } } private void initBINARYMode() { @@ -746,12 +782,13 @@ public String getDetailedStatus() { return String.format("DISABLED"); } else { return String.format( - "OK, Sent %d heartbeats, %d OwnshipReports, %d ownShipGeoAltitude(s), %d foreFlightIDs, %d AHRS(s) ", + "OK, Sent %d heartbeats, %d OwnshipReports, %d ownShipGeoAltitude(s), %d foreFlightIDs, %d AHRS(s), %d WHLAHRSCount(s) ", heartBeatCount, ownShipReportCount, ownshipGeoAltitudeCount, foreFlightIDCount, - AHRSCount); + AHRSCount, + WHLAHRSCount); } } @@ -1107,6 +1144,23 @@ private synchronized void AHRSMessage() throws IOException { } } + private synchronized void WHLAHRSMessage() throws IOException { + WHL_AHRS ahrs = newWHLAHRS(); + for (GDL90Device d : gdl90Devices.values()) { + + if (d.alive & !isBlackListed(new HostPortPair(d.host, d.port))) { + try { + d.datagram.setData(ahrs.toBytes()); + } catch (Exception e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + GDL90Socket.send(d.datagram); + reportWHLAHRS(d.datagram.getData()); + } + } + } + private AHRS newAHRS() { AHRS ahrs = new AHRS(); getYPR(ahrs); @@ -1115,6 +1169,12 @@ private AHRS newAHRS() { return ahrs; } + private WHL_AHRS newWHLAHRS() { + WHL_AHRS ahrs = new WHL_AHRS(); + getYPR(ahrs); + return ahrs; + } + private void getYPR(AHRS ahrs) { switch (ahrsMode) { case QT: @@ -1128,6 +1188,19 @@ private void getYPR(AHRS ahrs) { } } + private void getYPR(WHL_AHRS ahrs) { + switch (ahrsMode) { + case QT: + calcQT(ahrs); + break; + case YPR: + calcYPR(ahrs); + break; + default: + break; + } + } + private void calcYPR(AHRS ahrs) { org.yamcs.protobuf.Pvalue.ParameterValue pvRoll = paramsToSend.get("Roll"); @@ -1247,6 +1320,244 @@ private void calcYPR(AHRS ahrs) { } } + private void calcYPR(WHL_AHRS ahrs) { + org.yamcs.protobuf.Pvalue.ParameterValue pvRoll = paramsToSend.get("Roll"); + + org.yamcs.protobuf.Pvalue.ParameterValue pvAltitude = paramsToSend.get("Altitude"); + + org.yamcs.protobuf.Pvalue.ParameterValue pvLatitude = paramsToSend.get("Latitude"); + + if (pvLatitude != null) { + switch (pvLatitude.getEngValue().getType()) { + case AGGREGATE: + break; + case ARRAY: + break; + case BINARY: + break; + case BOOLEAN: + break; + case DOUBLE: + ahrs.Lat = pvLatitude.getEngValue().getDoubleValue(); + break; + case ENUMERATED: + break; + case FLOAT: + ahrs.Lat = pvLatitude.getEngValue().getFloatValue(); + break; + case NONE: + break; + case SINT32: + break; + case SINT64: + break; + case STRING: + break; + case TIMESTAMP: + break; + case UINT32: + break; + case UINT64: + break; + default: + break; + } + } + + org.yamcs.protobuf.Pvalue.ParameterValue pvLongitude = paramsToSend.get("Longitude"); + + if (pvLongitude != null) { + switch (pvLongitude.getEngValue().getType()) { + case AGGREGATE: + break; + case ARRAY: + break; + case BINARY: + break; + case BOOLEAN: + break; + case DOUBLE: + ahrs.Lon = pvLongitude.getEngValue().getDoubleValue(); + break; + case ENUMERATED: + break; + case FLOAT: + ahrs.Lon = pvLongitude.getEngValue().getFloatValue(); + break; + case NONE: + break; + case SINT32: + break; + case SINT64: + break; + case STRING: + break; + case TIMESTAMP: + break; + case UINT32: + break; + case UINT64: + break; + default: + break; + } + } + + if (pvAltitude != null) { + switch (pvAltitude.getEngValue().getType()) { + case AGGREGATE: + break; + case ARRAY: + break; + case BINARY: + break; + case BOOLEAN: + break; + case DOUBLE: + // Meters to feet. Should be made configurable, maybe... + ahrs.Alt = (pvAltitude.getEngValue().getDoubleValue() * 3.28084); + break; + case ENUMERATED: + break; + case FLOAT: + // Meters to feet. Should be made configurable, maybe... + ahrs.Alt = (pvAltitude.getEngValue().getFloatValue() * 3.28084); + break; + case NONE: + break; + case SINT32: + break; + case SINT64: + break; + case STRING: + break; + case TIMESTAMP: + break; + case UINT32: + break; + case UINT64: + break; + default: + break; + } + } + + if (pvRoll != null) { + switch (pvRoll.getEngValue().getType()) { + case AGGREGATE: + break; + case ARRAY: + break; + case BINARY: + break; + case BOOLEAN: + break; + case DOUBLE: + ahrs.Roll = pvRoll.getEngValue().getDoubleValue(); + break; + case ENUMERATED: + break; + case FLOAT: + ahrs.Roll = pvRoll.getEngValue().getFloatValue(); + break; + case NONE: + break; + case SINT32: + break; + case SINT64: + break; + case STRING: + break; + case TIMESTAMP: + break; + case UINT32: + break; + case UINT64: + break; + default: + break; + } + } + + org.yamcs.protobuf.Pvalue.ParameterValue pvPitch = paramsToSend.get("Pitch"); + + if (pvPitch != null) { + switch (pvPitch.getEngValue().getType()) { + case AGGREGATE: + break; + case ARRAY: + break; + case BINARY: + break; + case BOOLEAN: + break; + case DOUBLE: + ahrs.Pitch = pvPitch.getEngValue().getDoubleValue(); + break; + case ENUMERATED: + break; + case FLOAT: + ahrs.Pitch = pvPitch.getEngValue().getFloatValue(); + break; + case NONE: + break; + case SINT32: + break; + case SINT64: + break; + case STRING: + break; + case TIMESTAMP: + break; + case UINT32: + break; + case UINT64: + break; + default: + break; + } + } + + org.yamcs.protobuf.Pvalue.ParameterValue pvAHRS_Heading = paramsToSend.get("AHRS_Heading"); + + if (pvAHRS_Heading != null) { + switch (pvAHRS_Heading.getEngValue().getType()) { + case AGGREGATE: + break; + case ARRAY: + break; + case BINARY: + break; + case BOOLEAN: + break; + case DOUBLE: + ahrs.Heading = pvAHRS_Heading.getEngValue().getDoubleValue(); + break; + case ENUMERATED: + break; + case FLOAT: + ahrs.Heading = pvAHRS_Heading.getEngValue().getFloatValue(); + break; + case NONE: + break; + case SINT32: + break; + case SINT64: + break; + case STRING: + break; + case TIMESTAMP: + break; + case UINT32: + break; + case UINT64: + break; + default: + break; + } + } + } + private void calcQT(AHRS ahrs) { org.yamcs.protobuf.Pvalue.ParameterValue qt = paramsToSend.get("Qt"); QT newQT = new QT(); @@ -1327,6 +1638,205 @@ private void calcQT(AHRS ahrs) { ahrs.Roll = newYPR.roll; } + private void calcQT(WHL_AHRS ahrs) { + org.yamcs.protobuf.Pvalue.ParameterValue qt = paramsToSend.get("Qt"); + + org.yamcs.protobuf.Pvalue.ParameterValue pvAltitude = paramsToSend.get("Altitude"); + + org.yamcs.protobuf.Pvalue.ParameterValue pvLatitude = paramsToSend.get("Latitude"); + + if (pvLatitude != null) { + switch (pvLatitude.getEngValue().getType()) { + case AGGREGATE: + break; + case ARRAY: + break; + case BINARY: + break; + case BOOLEAN: + break; + case DOUBLE: + ahrs.Lat = pvLatitude.getEngValue().getDoubleValue(); + break; + case ENUMERATED: + break; + case FLOAT: + ahrs.Lat = pvLatitude.getEngValue().getFloatValue(); + break; + case NONE: + break; + case SINT32: + break; + case SINT64: + break; + case STRING: + break; + case TIMESTAMP: + break; + case UINT32: + break; + case UINT64: + break; + default: + break; + } + } + + org.yamcs.protobuf.Pvalue.ParameterValue pvLongitude = paramsToSend.get("Longitude"); + + if (pvLongitude != null) { + switch (pvLongitude.getEngValue().getType()) { + case AGGREGATE: + break; + case ARRAY: + break; + case BINARY: + break; + case BOOLEAN: + break; + case DOUBLE: + ahrs.Lon = pvLongitude.getEngValue().getDoubleValue(); + break; + case ENUMERATED: + break; + case FLOAT: + ahrs.Lon = pvLongitude.getEngValue().getFloatValue(); + break; + case NONE: + break; + case SINT32: + break; + case SINT64: + break; + case STRING: + break; + case TIMESTAMP: + break; + case UINT32: + break; + case UINT64: + break; + default: + break; + } + } + + if (pvAltitude != null) { + switch (pvAltitude.getEngValue().getType()) { + case AGGREGATE: + break; + case ARRAY: + break; + case BINARY: + break; + case BOOLEAN: + break; + case DOUBLE: + // Meters to feet. Should be made configurable, maybe... + ahrs.Alt = (pvAltitude.getEngValue().getDoubleValue() * 3.28084); + break; + case ENUMERATED: + break; + case FLOAT: + // Meters to feet. Should be made configurable, maybe... + ahrs.Alt = (pvAltitude.getEngValue().getFloatValue() * 3.28084); + break; + case NONE: + break; + case SINT32: + break; + case SINT64: + break; + case STRING: + break; + case TIMESTAMP: + break; + case UINT32: + break; + case UINT64: + break; + default: + break; + } + } + QT newQT = new QT(); + if (qt != null) { + switch (qt.getEngValue().getType()) { + case AGGREGATE: + break; + case ARRAY: + java.util.List l = qt.getEngValue().getArrayValueList(); + for (int i = 0; i < l.size(); i++) { + switch (l.get(i).getType()) { + case AGGREGATE: + break; + case ARRAY: + break; + case BINARY: + break; + case BOOLEAN: + break; + case DOUBLE: + newQT.data[i] = l.get(i).getDoubleValue(); + break; + case ENUMERATED: + break; + case FLOAT: + newQT.data[i] = l.get(i).getFloatValue(); + case NONE: + break; + case SINT32: + break; + case SINT64: + break; + case STRING: + break; + case TIMESTAMP: + break; + case UINT32: + break; + case UINT64: + break; + default: + break; + } + } + break; + case BINARY: + break; + case BOOLEAN: + break; + case DOUBLE: + break; + case ENUMERATED: + break; + case FLOAT: + break; + case NONE: + break; + case SINT32: + break; + case SINT64: + break; + case STRING: + break; + case TIMESTAMP: + break; + case UINT32: + break; + case UINT64: + break; + default: + break; + } + } + + YPR newYPR = qtToYPR(newQT); + ahrs.Heading = newYPR.yaw; + ahrs.Pitch = newYPR.pitch; + ahrs.Roll = newYPR.roll; + } + public void reportAHRS(byte[] d) { if (this.AHRSStream != null) { @@ -1342,6 +1852,21 @@ public void reportAHRS(byte[] d) { AHRSCount++; } + public void reportWHLAHRS(byte[] d) { + + if (this.WHLAHRSStream != null) { + try { + this.WHLAHRSStream.emitTuple( + new Tuple(gftdef, Arrays.asList(timeService.getMissionTime(), "WHLAHRSStream", d))); + } catch (Exception e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + + WHLAHRSCount++; + } + private synchronized void sendOwnshipGeoAltitude() throws IOException { for (GDL90Device d : gdl90Devices.values()) { @@ -1553,7 +2078,8 @@ public long getDataOutCount() { + ownShipReportCount + ownshipGeoAltitudeCount + foreFlightIDCount - + AHRSCount; + + AHRSCount + + WHLAHRSCount; } @Override @@ -1563,6 +2089,7 @@ public void resetCounters() { ownshipGeoAltitudeCount = 0; foreFlightIDCount = 0; AHRSCount = 0; + WHLAHRSCount = 0; } @Override diff --git a/src/main/java/com/windhoverlabs/yamcs/gdl90/WHL_AHRS.java b/src/main/java/com/windhoverlabs/yamcs/gdl90/WHL_AHRS.java new file mode 100644 index 0000000..e3d0988 --- /dev/null +++ b/src/main/java/com/windhoverlabs/yamcs/gdl90/WHL_AHRS.java @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (c) 2024 Windhover Labs, L.L.C. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Windhover Labs nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *****************************************************************************/ + +package com.windhoverlabs.yamcs.gdl90; + +import java.io.ByteArrayOutputStream; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; + +/** + * As per the spec: + * https://www.faa.gov/sites/faa.gov/files/air_traffic/technology/adsb/archival/GDL90_Public_ICD_RevA.PDF + * Pg 18 + * + *

This message is an extension of the GDL90 protocol by Windhover Labs + */ +public class WHL_AHRS { + + byte FlagByte = 0x7E; + public static final byte MessageID = 0x66; + public static final byte AHRSSubMessageID = 0x00; + public double Roll; + public double Pitch; + public double Heading; + + public double Lat; + public double Lon; + public double Alt; + + public byte[] toBytes() throws Exception { + + ByteArrayOutputStream messageStream = new ByteArrayOutputStream(); + + messageStream.write(MessageID); + messageStream.write(AHRSSubMessageID); + + byte[] packedRollBytes = + ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(Roll).array(); + messageStream.write(packedRollBytes[0]); + messageStream.write(packedRollBytes[1]); + messageStream.write(packedRollBytes[2]); + messageStream.write(packedRollBytes[3]); + messageStream.write(packedRollBytes[4]); + messageStream.write(packedRollBytes[5]); + messageStream.write(packedRollBytes[6]); + messageStream.write(packedRollBytes[7]); + + byte[] packedPitchBytes = + ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(Pitch).array(); + messageStream.write(packedPitchBytes[0]); + messageStream.write(packedPitchBytes[1]); + messageStream.write(packedPitchBytes[2]); + messageStream.write(packedPitchBytes[3]); + messageStream.write(packedPitchBytes[4]); + messageStream.write(packedPitchBytes[5]); + messageStream.write(packedPitchBytes[6]); + messageStream.write(packedPitchBytes[7]); + + // Heading = -80; + double packedHeading = WHL_PackWHLHeading(Heading); + + // 0x01C2 = 450 + // int packedHeading = packDegrees(45); + + byte[] packedHeadingBytes = + ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(packedHeading).array(); + messageStream.write(packedHeadingBytes[0]); + messageStream.write(packedHeadingBytes[1]); + messageStream.write(packedHeadingBytes[2]); + messageStream.write(packedHeadingBytes[3]); + messageStream.write(packedHeadingBytes[4]); + messageStream.write(packedHeadingBytes[5]); + messageStream.write(packedHeadingBytes[6]); + messageStream.write(packedHeadingBytes[7]); + + byte[] packedLatBytes = + ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(Lat).array(); + messageStream.write(packedLatBytes[0]); + messageStream.write(packedLatBytes[1]); + messageStream.write(packedLatBytes[2]); + messageStream.write(packedLatBytes[3]); + messageStream.write(packedLatBytes[4]); + messageStream.write(packedLatBytes[5]); + messageStream.write(packedLatBytes[6]); + messageStream.write(packedLatBytes[7]); + + byte[] packedLonBytes = + ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(Lon).array(); + messageStream.write(packedLonBytes[0]); + messageStream.write(packedLonBytes[1]); + messageStream.write(packedLonBytes[2]); + messageStream.write(packedLonBytes[3]); + messageStream.write(packedLonBytes[4]); + messageStream.write(packedLonBytes[5]); + messageStream.write(packedLonBytes[6]); + messageStream.write(packedLonBytes[7]); + + byte[] packedAltBytes = + ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(Alt).array(); + messageStream.write(packedAltBytes[0]); + messageStream.write(packedAltBytes[1]); + messageStream.write(packedAltBytes[2]); + messageStream.write(packedAltBytes[3]); + messageStream.write(packedAltBytes[4]); + messageStream.write(packedAltBytes[5]); + messageStream.write(packedAltBytes[6]); + messageStream.write(packedAltBytes[7]); + + byte[] crcData = messageStream.toByteArray(); + int crc = CrcTable.crcCompute(crcData, 0, crcData.length); + // + // Go through message data and escape characters as per the spec + // .... + // + + byte[] crcBytes = ByteBuffer.allocate(4).order(ByteOrder.BIG_ENDIAN).putInt(crc).array(); + messageStream.write(crcBytes[3]); + messageStream.write(crcBytes[2]); + + ByteArrayOutputStream messageStreamOut = + OwnshipGeoAltitude.escapeBytes(messageStream.toByteArray()); + + ByteBuffer bbOut = ByteBuffer.allocate(messageStreamOut.toByteArray().length + 2).put(FlagByte); + + bbOut.put(messageStreamOut.toByteArray()); + + bbOut.put(FlagByte); + + byte[] dataOut = bbOut.array(); + + messageStream.toByteArray(); + + return dataOut; + } + + public double WHL_PackWHLHeading(double heading) { + + // Connvert heading of [-180, 180] to [0,360] + double PackedHeading = heading; + if (heading < 0) { + PackedHeading = 360 + heading; + } else { + PackedHeading = heading; + } + return (PackedHeading); + } +}