WindhoverLabs / yamcs_gdl90 / #29

Committed 29 Feb 2024 08:22PM UTC coverage: 0.0%. Remained the same

Build # #29

Build Type

push

Committed by

Commit Message

-Set true/magnetic heading correctly for ForeFlight.

Run Details

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

1 existing line in 1 file now uncovered.

0 of 2231 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]);

    //    Heading = -80;
    //    Heading = 147;
    //    Heading = 0b10110111110;
    int packedHeading = packDegrees(FFB_PackForeFlightHeading((Heading)));

    //    packedHeading = packedHeading |  0b00000101101111100000000000000000;

    //    packedHeading = packedHeading | 0b00000000000000000000000000000001;

    //    packedHeading = packedHeading | 1 << 31;

    //    packedHeading = 0xffffffff;

    //    0x01C2 = 450
    //    int packedHeading = packDegrees(45);

    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]);	×
80
81	// Heading = -80;
82	// Heading = 147;
83	// Heading = 0b10110111110;
UNCOV 84	int packedHeading = packDegrees(FFB_PackForeFlightHeading((Heading)));	×
85
86	// packedHeading = packedHeading \| 0b00000101101111100000000000000000;
87
88	// packedHeading = packedHeading \| 0b00000000000000000000000000000001;
89
90	// packedHeading = packedHeading \| 1 << 31;
91
92	// packedHeading = 0xffffffff;
93
94	// 0x01C2 = 450
95	// int packedHeading = packDegrees(45);
96
97	byte[] packedHeadingBytes = ByteBuffer.allocate(4).putInt(packedHeading).array();	×
98
99	byte iaByte = packedHeadingBytes[2];	×
NEW 100	switch (HeadingType) {	×
101	case TRUE_HEADING:
NEW 102	iaByte = (byte) (iaByte \| (0 << 7));	×
103
NEW 104	break;	×
105	case MAGNETIC:
NEW 106	iaByte = (byte) (iaByte \| (1 << 7));	×
107
NEW 108	break;	×
109	default:
110	break;
111	}
112	messageStream.write(iaByte);	×
113	messageStream.write(packedHeadingBytes[3]);	×
114
115	byte[] IndicatedAirspeedBytes = ByteBuffer.allocate(4).putInt(IndicatedAirspeed).array();	×
116	messageStream.write(IndicatedAirspeedBytes[2]);	×
117	messageStream.write(IndicatedAirspeedBytes[3]);	×
118
119	byte[] TrueAirspeedBytes = ByteBuffer.allocate(4).putInt(TrueAirspeed).array();	×
120	messageStream.write(TrueAirspeedBytes[2]);	×
121	messageStream.write(TrueAirspeedBytes[3]);	×
122
123	byte[] crcData = messageStream.toByteArray();	×
124	int crc = CrcTable.crcCompute(crcData, 0, crcData.length);	×
125	//
126	// Go through message data and escape characters as per the spec
127	// ....
128	//
129
130	byte[] crcBytes = ByteBuffer.allocate(4).putInt(crc).array();	×
131	messageStream.write(crcBytes[3]);	×
132	messageStream.write(crcBytes[2]);	×
133
134	ByteArrayOutputStream messageStreamOut =	×
135	OwnshipGeoAltitude.escapeBytes(messageStream.toByteArray());	×
136
137	ByteBuffer bbOut = ByteBuffer.allocate(messageStreamOut.toByteArray().length + 2).put(FlagByte);	×
138
139	bbOut.put(messageStreamOut.toByteArray());	×
140
141	bbOut.put(FlagByte);	×
142
143	byte[] dataOut = bbOut.array();	×
144
145	messageStream.toByteArray();	×
146
147	return dataOut;	×
148	}
149
150	public static int setNibble(int num, int nibble, int which) {
151	int shiftNibble = nibble << (4 * which);	×
152	int shiftMask = 0x0000000F << (4 * which);	×
153	return (num & ~shiftMask) \| shiftNibble;	×
154	}
155
156	public int packLatLong(double LatLon) {
157
158	Double doubleVal = LatLon;	×
159
160	int valLon = (int) (doubleVal / (180.0 / 8388608.0));	×
161
162	return valLon;	×
163	}
164
165	public int packAltitude(int altFt) {
166	return (int) ((1000 + altFt) / 25);	×
167	}
168
169	public int packDegrees(double deg) {
170	int tenth = ((int) ((deg % ((int) deg)) * 10));	×
171	return ((int) ((deg * 10)) + tenth);	×
172	}
173
174	// public int packDegrees(double deg) {
175	// return ((int) ((deg * 10)));
176	// }
177
178	public double FFB_PackForeFlightHeading(double heading) {
179
180	// Connvert heading of [-180, 180] to [-360,360]
181	double PackedHeading = heading;	×
182	if (heading < 0) {	×
183	PackedHeading = 360 + heading;	×
184	} else {
185	PackedHeading = heading;	×
186	}
187	return (PackedHeading);	×
188	}
189	}

WindhoverLabs / yamcs_gdl90 / #29

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