From 4198a91478e57c23d817ef62f24709397134093d Mon Sep 17 00:00:00 2001 From: lgomez Date: Tue, 5 Mar 2024 09:34:58 -0600 Subject: [PATCH] -Add VelNED to WHL_AHRS. -Bump version number --- pom.xml | 2 +- .../windhoverlabs/yamcs/gdl90/GDL90Link.java | 91 +++++++++++++++++-- .../windhoverlabs/yamcs/gdl90/WHL_AHRS.java | 46 +++++++--- 3 files changed, 121 insertions(+), 18 deletions(-) diff --git a/pom.xml b/pom.xml index 0f031d5..794417f 100644 --- a/pom.xml +++ b/pom.xml @@ -3,7 +3,7 @@ 4.0.0 com.windhoverlabs yamcs-gdl90 - 1.3.0-SNAPSHOT + 1.4.0-SNAPSHOT jar YAMCS plugin for GDL90 standard. diff --git a/src/main/java/com/windhoverlabs/yamcs/gdl90/GDL90Link.java b/src/main/java/com/windhoverlabs/yamcs/gdl90/GDL90Link.java index 0f3048b..2472c9c 100644 --- a/src/main/java/com/windhoverlabs/yamcs/gdl90/GDL90Link.java +++ b/src/main/java/com/windhoverlabs/yamcs/gdl90/GDL90Link.java @@ -1761,11 +1761,10 @@ private void calcQT(WHL_AHRS ahrs) { } } - org.yamcs.protobuf.Pvalue.ParameterValue pvHorizontalSpeed = - paramsToSend.get("HorizontalSpeed"); + org.yamcs.protobuf.Pvalue.ParameterValue northVel = paramsToSend.get("NorthVel"); - if (pvHorizontalSpeed != null) { - switch (pvHorizontalSpeed.getEngValue().getType()) { + if (northVel != null) { + switch (northVel.getEngValue().getType()) { case AGGREGATE: break; case ARRAY: @@ -1776,12 +1775,92 @@ private void calcQT(WHL_AHRS ahrs) { break; case DOUBLE: // Assumes the PV is in meters/second. Convert to Knots - ahrs.Groundspeed = mpsToKnots((float) pvHorizontalSpeed.getEngValue().getDoubleValue()); + ahrs.northVel = northVel.getEngValue().getDoubleValue(); break; case ENUMERATED: break; case FLOAT: - ahrs.Groundspeed = mpsToKnots(pvHorizontalSpeed.getEngValue().getFloatValue()); + ahrs.northVel = northVel.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 eastVel = paramsToSend.get("EastVel"); + + if (eastVel != null) { + switch (eastVel.getEngValue().getType()) { + case AGGREGATE: + break; + case ARRAY: + break; + case BINARY: + break; + case BOOLEAN: + break; + case DOUBLE: + // Assumes the PV is in meters/second. Convert to Knots + ahrs.eastVel = eastVel.getEngValue().getDoubleValue(); + break; + case ENUMERATED: + break; + case FLOAT: + ahrs.eastVel = eastVel.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 downVel = paramsToSend.get("DownVel"); + + if (downVel != null) { + switch (downVel.getEngValue().getType()) { + case AGGREGATE: + break; + case ARRAY: + break; + case BINARY: + break; + case BOOLEAN: + break; + case DOUBLE: + // Assumes the PV is in meters/second. Convert to Knots + ahrs.downVel = downVel.getEngValue().getDoubleValue(); + break; + case ENUMERATED: + break; + case FLOAT: + ahrs.downVel = downVel.getEngValue().getFloatValue(); break; case NONE: break; diff --git a/src/main/java/com/windhoverlabs/yamcs/gdl90/WHL_AHRS.java b/src/main/java/com/windhoverlabs/yamcs/gdl90/WHL_AHRS.java index b7ddea6..cad9fdd 100644 --- a/src/main/java/com/windhoverlabs/yamcs/gdl90/WHL_AHRS.java +++ b/src/main/java/com/windhoverlabs/yamcs/gdl90/WHL_AHRS.java @@ -57,7 +57,9 @@ public class WHL_AHRS { public double Lon; public double Alt; - public double Groundspeed; + public double northVel; + public double eastVel; + public double downVel; public byte[] toBytes() throws Exception { @@ -138,16 +140,38 @@ public byte[] toBytes() throws Exception { messageStream.write(packedAltBytes[6]); messageStream.write(packedAltBytes[7]); - byte[] GroundspeedAltBytes = - ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(Groundspeed).array(); - messageStream.write(GroundspeedAltBytes[0]); - messageStream.write(GroundspeedAltBytes[1]); - messageStream.write(GroundspeedAltBytes[2]); - messageStream.write(GroundspeedAltBytes[3]); - messageStream.write(GroundspeedAltBytes[4]); - messageStream.write(GroundspeedAltBytes[5]); - messageStream.write(GroundspeedAltBytes[6]); - messageStream.write(GroundspeedAltBytes[7]); + byte[] NorthtBytes = + ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(northVel).array(); + messageStream.write(NorthtBytes[0]); + messageStream.write(NorthtBytes[1]); + messageStream.write(NorthtBytes[2]); + messageStream.write(NorthtBytes[3]); + messageStream.write(NorthtBytes[4]); + messageStream.write(NorthtBytes[5]); + messageStream.write(NorthtBytes[6]); + messageStream.write(NorthtBytes[7]); + + byte[] EastBytes = + ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(eastVel).array(); + messageStream.write(EastBytes[0]); + messageStream.write(EastBytes[1]); + messageStream.write(EastBytes[2]); + messageStream.write(EastBytes[3]); + messageStream.write(EastBytes[4]); + messageStream.write(EastBytes[5]); + messageStream.write(EastBytes[6]); + messageStream.write(EastBytes[7]); + + byte[] DownBytes = + ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(downVel).array(); + messageStream.write(DownBytes[0]); + messageStream.write(DownBytes[1]); + messageStream.write(DownBytes[2]); + messageStream.write(DownBytes[3]); + messageStream.write(DownBytes[4]); + messageStream.write(DownBytes[5]); + messageStream.write(DownBytes[6]); + messageStream.write(DownBytes[7]); byte[] crcData = messageStream.toByteArray(); int crc = CrcTable.crcCompute(crcData, 0, crcData.length);