WindhoverLabs / yamcs_gdl90 / #33

Committed 05 Mar 2024 03:34PM UTC coverage: 0.0%. Remained the same

Build # #33

Build Type

push

Committed by

Commit Message

-Add VelNED to WHL_AHRS.
-Bump version number

Run Details

0 of 73 new or added lines in 2 files covered. (0.0%)

1 existing line in 1 file now uncovered.

0 of 2318 relevant lines covered (0.0%)

0.0 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

0.0

/src/main/java/com/windhoverlabs/yamcs/gdl90/WHL_AHRS.java

/****************************************************************************
 *
 *   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
 *
 * <p>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 double northVel;
  public double eastVel;
  public double downVel;

  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[] 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);
    //
    // 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);
  }
}

1	/****************************************************************************
2	*
3	* Copyright (c) 2024 Windhover Labs, L.L.C. All rights reserved.
4	*
5	* Redistribution and use in source and binary forms, with or without
6	* modification, are permitted provided that the following conditions
7	* are met:
8	*
9	* 1. Redistributions of source code must retain the above copyright
10	* notice, this list of conditions and the following disclaimer.
11	* 2. Redistributions in binary form must reproduce the above copyright
12	* notice, this list of conditions and the following disclaimer in
13	* the documentation and/or other materials provided with the
14	* distribution.
15	* 3. Neither the name Windhover Labs nor the names of its
16	* contributors may be used to endorse or promote products derived
17	* from this software without specific prior written permission.
18	*
19	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20	* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21	* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22	* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23	* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24	* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25	* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26	* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27	* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28	* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29	* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30	* POSSIBILITY OF SUCH DAMAGE.
31	*
32	*****************************************************************************/
33
34	package com.windhoverlabs.yamcs.gdl90;
35
36	import java.io.ByteArrayOutputStream;
37	import java.nio.ByteBuffer;
38	import java.nio.ByteOrder;
39
40	/**
41	* As per the spec:
42	* https://www.faa.gov/sites/faa.gov/files/air_traffic/technology/adsb/archival/GDL90_Public_ICD_RevA.PDF
43	* Pg 18
44	*
45	* <p>This message is an extension of the GDL90 protocol by Windhover Labs
46	*/
47	public class WHL_AHRS {	×
48
49	byte FlagByte = 0x7E;	×
50	public static final byte MessageID = 0x66;
51	public static final byte AHRSSubMessageID = 0x00;
52	public double Roll;
53	public double Pitch;
54	public double Heading;
55
56	public double Lat;
57	public double Lon;
58	public double Alt;
59
60	public double northVel;
61	public double eastVel;
62	public double downVel;
63
64	public byte[] toBytes() throws Exception {
65
66	ByteArrayOutputStream messageStream = new ByteArrayOutputStream();	×
67
68	messageStream.write(MessageID);	×
69	messageStream.write(AHRSSubMessageID);	×
70
71	byte[] packedRollBytes =	×
72	ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(Roll).array();	×
73	messageStream.write(packedRollBytes[0]);	×
74	messageStream.write(packedRollBytes[1]);	×
75	messageStream.write(packedRollBytes[2]);	×
76	messageStream.write(packedRollBytes[3]);	×
77	messageStream.write(packedRollBytes[4]);	×
78	messageStream.write(packedRollBytes[5]);	×
79	messageStream.write(packedRollBytes[6]);	×
80	messageStream.write(packedRollBytes[7]);	×
81
82	byte[] packedPitchBytes =	×
83	ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(Pitch).array();	×
84	messageStream.write(packedPitchBytes[0]);	×
85	messageStream.write(packedPitchBytes[1]);	×
86	messageStream.write(packedPitchBytes[2]);	×
87	messageStream.write(packedPitchBytes[3]);	×
88	messageStream.write(packedPitchBytes[4]);	×
89	messageStream.write(packedPitchBytes[5]);	×
90	messageStream.write(packedPitchBytes[6]);	×
91	messageStream.write(packedPitchBytes[7]);	×
92
93	// Heading = -80;
94	double packedHeading = WHL_PackWHLHeading(Heading);	×
95
96	// 0x01C2 = 450
97	// int packedHeading = packDegrees(45);
98
99	byte[] packedHeadingBytes =	×
100	ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(packedHeading).array();	×
101	messageStream.write(packedHeadingBytes[0]);	×
102	messageStream.write(packedHeadingBytes[1]);	×
103	messageStream.write(packedHeadingBytes[2]);	×
104	messageStream.write(packedHeadingBytes[3]);	×
105	messageStream.write(packedHeadingBytes[4]);	×
106	messageStream.write(packedHeadingBytes[5]);	×
107	messageStream.write(packedHeadingBytes[6]);	×
108	messageStream.write(packedHeadingBytes[7]);	×
109
110	byte[] packedLatBytes =	×
111	ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(Lat).array();	×
112	messageStream.write(packedLatBytes[0]);	×
113	messageStream.write(packedLatBytes[1]);	×
114	messageStream.write(packedLatBytes[2]);	×
115	messageStream.write(packedLatBytes[3]);	×
116	messageStream.write(packedLatBytes[4]);	×
117	messageStream.write(packedLatBytes[5]);	×
118	messageStream.write(packedLatBytes[6]);	×
119	messageStream.write(packedLatBytes[7]);	×
120
121	byte[] packedLonBytes =	×
122	ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(Lon).array();	×
123	messageStream.write(packedLonBytes[0]);	×
124	messageStream.write(packedLonBytes[1]);	×
125	messageStream.write(packedLonBytes[2]);	×
126	messageStream.write(packedLonBytes[3]);	×
127	messageStream.write(packedLonBytes[4]);	×
128	messageStream.write(packedLonBytes[5]);	×
129	messageStream.write(packedLonBytes[6]);	×
130	messageStream.write(packedLonBytes[7]);	×
131
132	byte[] packedAltBytes =	×
133	ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(Alt).array();	×
134	messageStream.write(packedAltBytes[0]);	×
135	messageStream.write(packedAltBytes[1]);	×
136	messageStream.write(packedAltBytes[2]);	×
137	messageStream.write(packedAltBytes[3]);	×
138	messageStream.write(packedAltBytes[4]);	×
139	messageStream.write(packedAltBytes[5]);	×
140	messageStream.write(packedAltBytes[6]);	×
141	messageStream.write(packedAltBytes[7]);	×
142
NEW 143	byte[] NorthtBytes =	×
NEW 144	ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(northVel).array();	×
NEW 145	messageStream.write(NorthtBytes[0]);	×
NEW 146	messageStream.write(NorthtBytes[1]);	×
NEW 147	messageStream.write(NorthtBytes[2]);	×
NEW 148	messageStream.write(NorthtBytes[3]);	×
NEW 149	messageStream.write(NorthtBytes[4]);	×
NEW 150	messageStream.write(NorthtBytes[5]);	×
NEW 151	messageStream.write(NorthtBytes[6]);	×
NEW 152	messageStream.write(NorthtBytes[7]);	×
153
NEW 154	byte[] EastBytes =	×
NEW 155	ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(eastVel).array();	×
NEW 156	messageStream.write(EastBytes[0]);	×
NEW 157	messageStream.write(EastBytes[1]);	×
NEW 158	messageStream.write(EastBytes[2]);	×
NEW 159	messageStream.write(EastBytes[3]);	×
NEW 160	messageStream.write(EastBytes[4]);	×
NEW 161	messageStream.write(EastBytes[5]);	×
NEW 162	messageStream.write(EastBytes[6]);	×
NEW 163	messageStream.write(EastBytes[7]);	×
164
NEW 165	byte[] DownBytes =	×
NEW 166	ByteBuffer.allocate(8).order(ByteOrder.BIG_ENDIAN).putDouble(downVel).array();	×
NEW 167	messageStream.write(DownBytes[0]);	×
NEW 168	messageStream.write(DownBytes[1]);	×
NEW 169	messageStream.write(DownBytes[2]);	×
NEW 170	messageStream.write(DownBytes[3]);	×
NEW 171	messageStream.write(DownBytes[4]);	×
NEW 172	messageStream.write(DownBytes[5]);	×
NEW 173	messageStream.write(DownBytes[6]);	×
NEW 174	messageStream.write(DownBytes[7]);	×
175
176	byte[] crcData = messageStream.toByteArray();	×
177	int crc = CrcTable.crcCompute(crcData, 0, crcData.length);	×
178	//
179	// Go through message data and escape characters as per the spec
180	// ....
181	//
182
183	byte[] crcBytes = ByteBuffer.allocate(4).order(ByteOrder.BIG_ENDIAN).putInt(crc).array();	×
184	messageStream.write(crcBytes[3]);	×
185	messageStream.write(crcBytes[2]);	×
186
187	ByteArrayOutputStream messageStreamOut =	×
188	OwnshipGeoAltitude.escapeBytes(messageStream.toByteArray());	×
189
190	ByteBuffer bbOut = ByteBuffer.allocate(messageStreamOut.toByteArray().length + 2).put(FlagByte);	×
191
192	bbOut.put(messageStreamOut.toByteArray());	×
193
194	bbOut.put(FlagByte);	×
195
196	byte[] dataOut = bbOut.array();	×
197
198	messageStream.toByteArray();	×
199
200	return dataOut;	×
201	}
202
203	public double WHL_PackWHLHeading(double heading) {
204
205	// Connvert heading of [-180, 180] to [0,360]
206	double PackedHeading = heading;	×
207	if (heading < 0) {	×
208	PackedHeading = 360 + heading;	×
209	} else {
210	PackedHeading = heading;	×
211	}
212	return (PackedHeading);	×
213	}
214	}

WindhoverLabs / yamcs_gdl90 / #33

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous