WindhoverLabs / yamcs_gdl90 / #38

Committed 26 Nov 2024 07:42PM UTC coverage: 0.0%. Remained the same

Build # #38

Build Type

Committed by

Commit Message

-Upgrade yamcs to 5.9.8

Pull Request Pull Request #3: Binary Mode

Run Details

0 of 973 new or added lines in 7 files covered. (0.0%)

20 existing lines in 4 files now uncovered.

0 of 2374 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/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 com.windhoverlabs.yamcs.gdl90.GDL90Link.AHRS_MODE;
import java.io.ByteArrayOutputStream;
import java.nio.ByteBuffer;

/**
 * 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 ForeFlight
 */
public class AHRS {

  byte FlagByte = 0x7E;
  public static final byte MessageID = 0x65;
  public static final byte AHRSSubMessageID = 0x01;
  public double Roll;
  public double Pitch;
  public double Heading;
  public int IndicatedAirspeed;
  public int TrueAirspeed;

  public AHRSHeadingType HeadingType;

  public AHRS_MODE ahrsMode;

  public byte[] toBytes() throws Exception {

    ByteArrayOutputStream messageStream = new ByteArrayOutputStream();

    messageStream.write(MessageID);
    messageStream.write(AHRSSubMessageID);

    int packedRoll = packDegrees(Roll);

    byte[] packedRollBytes = ByteBuffer.allocate(4).putInt(packedRoll).array();
    messageStream.write(packedRollBytes[2]);
    messageStream.write(packedRollBytes[3]);

    int packedPitch = packDegrees(Pitch);

    byte[] packedPitchBytes = ByteBuffer.allocate(4).putInt(packedPitch).array();
    messageStream.write(packedPitchBytes[2]);
    messageStream.write(packedPitchBytes[3]);
    int packedHeading = packDegrees(FFB_PackForeFlightHeading((Heading)));

    byte[] packedHeadingBytes = ByteBuffer.allocate(4).putInt(packedHeading).array();

    byte iaByte = packedHeadingBytes[2];
    switch (HeadingType) {
      case TRUE_HEADING:
        iaByte = (byte) (iaByte | (0 << 7));

        break;
      case MAGNETIC:
        iaByte = (byte) (iaByte | (1 << 7));
        break;
      default:
        break;
    }
    messageStream.write(iaByte);
    messageStream.write(packedHeadingBytes[3]);

    byte[] IndicatedAirspeedBytes = ByteBuffer.allocate(4).putInt(IndicatedAirspeed).array();
    messageStream.write(IndicatedAirspeedBytes[2]);
    messageStream.write(IndicatedAirspeedBytes[3]);

    byte[] TrueAirspeedBytes = ByteBuffer.allocate(4).putInt(TrueAirspeed).array();
    messageStream.write(TrueAirspeedBytes[2]);
    messageStream.write(TrueAirspeedBytes[3]);

    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).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 static int setNibble(int num, int nibble, int which) {
    int shiftNibble = nibble << (4 * which);
    int shiftMask = 0x0000000F << (4 * which);
    return (num & ~shiftMask) | shiftNibble;
  }

  public int packLatLong(double LatLon) {

    Double doubleVal = LatLon;

    int valLon = (int) (doubleVal / (180.0 / 8388608.0));

    return valLon;
  }

  public int packAltitude(int altFt) {
    return (int) ((1000 + altFt) / 25);
  }

  public int packDegrees(double deg) {
    int tenth = ((int) ((deg % ((int) deg)) * 10));
    return ((int) ((deg * 10)) + tenth);
  }

  //  public int packDegrees(double deg) {
  //    return ((int) ((deg * 10)));
  //  }

  public double FFB_PackForeFlightHeading(double heading) {

    // Connvert heading of [-180, 180] to [-360,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 com.windhoverlabs.yamcs.gdl90.GDL90Link.AHRS_MODE;
37	import java.io.ByteArrayOutputStream;
38	import java.nio.ByteBuffer;
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 ForeFlight
46	*/
47	public class AHRS {	×
48
49	byte FlagByte = 0x7E;	×
50	public static final byte MessageID = 0x65;
51	public static final byte AHRSSubMessageID = 0x01;
52	public double Roll;
53	public double Pitch;
54	public double Heading;
55	public int IndicatedAirspeed;
56	public int TrueAirspeed;
57
58	public AHRSHeadingType HeadingType;
59
60	public AHRS_MODE ahrsMode;
61
62	public byte[] toBytes() throws Exception {
63
64	ByteArrayOutputStream messageStream = new ByteArrayOutputStream();	×
65
66	messageStream.write(MessageID);	×
67	messageStream.write(AHRSSubMessageID);	×
68
69	int packedRoll = packDegrees(Roll);	×
70
71	byte[] packedRollBytes = ByteBuffer.allocate(4).putInt(packedRoll).array();	×
72	messageStream.write(packedRollBytes[2]);	×
73	messageStream.write(packedRollBytes[3]);	×
74
75	int packedPitch = packDegrees(Pitch);	×
76
77	byte[] packedPitchBytes = ByteBuffer.allocate(4).putInt(packedPitch).array();	×
78	messageStream.write(packedPitchBytes[2]);	×
79	messageStream.write(packedPitchBytes[3]);	×
NEW 80	int packedHeading = packDegrees(FFB_PackForeFlightHeading((Heading)));	×
81
82	byte[] packedHeadingBytes = ByteBuffer.allocate(4).putInt(packedHeading).array();	×
83
NEW 84	byte iaByte = packedHeadingBytes[2];	×
NEW 85	switch (HeadingType) {	×
86	case TRUE_HEADING:
NEW 87	iaByte = (byte) (iaByte \| (0 << 7));	×
88
NEW 89	break;	×
90	case MAGNETIC:
NEW 91	iaByte = (byte) (iaByte \| (1 << 7));	×
NEW 92	break;	×
93	default:
94	break;
95	}
NEW 96	messageStream.write(iaByte);	×
UNCOV 97	messageStream.write(packedHeadingBytes[3]);	×
98
99	byte[] IndicatedAirspeedBytes = ByteBuffer.allocate(4).putInt(IndicatedAirspeed).array();	×
100	messageStream.write(IndicatedAirspeedBytes[2]);	×
101	messageStream.write(IndicatedAirspeedBytes[3]);	×
102
103	byte[] TrueAirspeedBytes = ByteBuffer.allocate(4).putInt(TrueAirspeed).array();	×
104	messageStream.write(TrueAirspeedBytes[2]);	×
105	messageStream.write(TrueAirspeedBytes[3]);	×
106
107	byte[] crcData = messageStream.toByteArray();	×
108	int crc = CrcTable.crcCompute(crcData, 0, crcData.length);	×
109	//
110	// Go through message data and escape characters as per the spec
111	// ....
112	//
113
114	byte[] crcBytes = ByteBuffer.allocate(4).putInt(crc).array();	×
115	messageStream.write(crcBytes[3]);	×
116	messageStream.write(crcBytes[2]);	×
117
118	ByteArrayOutputStream messageStreamOut =	×
119	OwnshipGeoAltitude.escapeBytes(messageStream.toByteArray());	×
120
121	ByteBuffer bbOut = ByteBuffer.allocate(messageStreamOut.toByteArray().length + 2).put(FlagByte);	×
122
123	bbOut.put(messageStreamOut.toByteArray());	×
124
125	bbOut.put(FlagByte);	×
126
127	byte[] dataOut = bbOut.array();	×
128
129	messageStream.toByteArray();	×
130
131	return dataOut;	×
132	}
133
134	public static int setNibble(int num, int nibble, int which) {
135	int shiftNibble = nibble << (4 * which);	×
136	int shiftMask = 0x0000000F << (4 * which);	×
137	return (num & ~shiftMask) \| shiftNibble;	×
138	}
139
140	public int packLatLong(double LatLon) {
141
142	Double doubleVal = LatLon;	×
143
144	int valLon = (int) (doubleVal / (180.0 / 8388608.0));	×
145
146	return valLon;	×
147	}
148
149	public int packAltitude(int altFt) {
150	return (int) ((1000 + altFt) / 25);	×
151	}
152
153	public int packDegrees(double deg) {
NEW 154	int tenth = ((int) ((deg % ((int) deg)) * 10));	×
NEW 155	return ((int) ((deg * 10)) + tenth);	×
156	}
157
158	// public int packDegrees(double deg) {
159	// return ((int) ((deg * 10)));
160	// }
161
162	public double FFB_PackForeFlightHeading(double heading) {
163
164	// Connvert heading of [-180, 180] to [-360,360]
NEW 165	double PackedHeading = heading;	×
NEW 166	if (heading < 0) {	×
NEW 167	PackedHeading = 360 + heading;	×
168	} else {
NEW 169	PackedHeading = heading;	×
170	}
NEW 171	return (PackedHeading);	×
172	}
173	}

WindhoverLabs / yamcs_gdl90 / #38

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