-Add VelNED to WHL_AHRS.

-Bump version number
WindhoverLabs · Mar 5, 2024 · 4198a91 · 4198a91
1 parent 4f2c645
commit 4198a91
Show file tree

Hide file tree

Showing 3 changed files with 121 additions and 18 deletions.
diff --git a/pom.xml b/pom.xml
@@ -3,7 +3,7 @@
 	<modelVersion>4.0.0</modelVersion>
 	<groupId>com.windhoverlabs</groupId>
 	<artifactId>yamcs-gdl90</artifactId>
-	<version>1.3.0-SNAPSHOT</version>
+	<version>1.4.0-SNAPSHOT</version>
 
 	<packaging>jar</packaging>
 	<name>YAMCS plugin for GDL90 standard.</name>

diff --git 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
@@ -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);