/// <summary>
/// Constructer for the HeadingSpeedAltitudePacketReceivedEventArgs class, needs a filled HeadingSpeedAltitude struct
/// </summary>
/// <param name="HSA"></param>
public HeadingSpeedAltitudePacketReceivedEventArgs(HeadingSpeedAltitude HSA)
{
this.HSA = HSA;
}
void ComProt_HeadingSpeedAltitudePacketReceived(object sender, CommProtocol.HeadingSpeedAltitudePacketReceivedEventArgs e)
{
CommPacketRXLight.On = true;
PacketReceived = true;
HSA = e.HSA;
SpeedGauge.Value = HSA.Speed;
HeadingGauge.Value = HSA.Heading;
AltitudeGauge.Value = HSA.Altitude;
}
private void ParseHeadingSpeedAltitudePacket()
{
/*
Heading/Speed/Altitude:
1 0xA5 SOT1
0x5A SOT2
3 0x07 Number of bytes
0x74 Report type “Telemetry”
0x48 Report “Heading, Speed, and Altitude”
6 0xHHHH Hex value (0x0000 – 0x0167) representing current heading. Can potentially use extra bits for partial degrees.
8 0xSS Hex value representing helicopter speed (m/s)
9 0xAAAA 2-byte value representing altitude in meters
11 0xXX Checksum 1
0xXX Checksum 2
13 0xCC EOT1
14 0x33 EOT2
*/
if (IncomingDataBuffer.Length == 14 && (int)IncomingDataBuffer[12] == 0xCC && (int)IncomingDataBuffer[13] == 0x33)
{
//calculate checksum
UInt16 sum = 0;
for (int i = 2; i < 10; i++)
{
sum += (ushort)IncomingDataBuffer[i];
}
byte chk1 = (byte)((sum & 0x0000FF00) >> 8);
byte chk2 = (byte)(sum & 0x000000FF);
if (chk1 != (int)IncomingDataBuffer[10] || chk2 != (int)IncomingDataBuffer[11])
{
Invoke(BadPacketReceived, new object[] {this,
new BadPacketReceivedEventArgs(IncomingDataBuffer,
string.Format("Invalid heading speed altitude packet: invalid checksum: received {0:x4}, expected {1:x4}",
(Convert.ToUInt16((int)IncomingDataBuffer[10]) << 8) + (int)IncomingDataBuffer[11], sum))});
ClearBuffer();
}
else
{
//good checksum
HeadingSpeedAltitude HSA = new HeadingSpeedAltitude();
HSA.Heading = Convert.ToUInt16(((int)IncomingDataBuffer[5] << 8) + (int)IncomingDataBuffer[6]);
HSA.Speed = Convert.ToByte((int)IncomingDataBuffer[7]);
HSA.Altitude = Convert.ToUInt16(((int)IncomingDataBuffer[8] << 8) + (int)IncomingDataBuffer[9]);
//invoke the event
ClearBuffer();
Invoke(HeadingSpeedAltitudePacketReceived,
new object[] { this, new HeadingSpeedAltitudePacketReceivedEventArgs(HSA)});
}
}
else if (IncomingDataBuffer.Length >= 14)
{
Invoke(BadPacketReceived, new object[] { this, new BadPacketReceivedEventArgs(IncomingDataBuffer, "Bad heading/speed/altitude packetreceived") });
ClearBuffer();
}
}
void cp_HeadingSpeedAltitudePacketReceived(object sender, CommProtocol.HeadingSpeedAltitudePacketReceivedEventArgs e)
{
ConsecutiveBadPackets = 0;
HSA = e.HSA;
textBox1.AppendText("Heading/Speed/Altitude packet received: Altitude: " + HSA.Altitude + " Heading: " + HSA.Heading + " Speed: " + HSA.Speed + "\r\n");
if (!ManualMode)
{
InsertRowToReceived_packetsTable("HSA", "Altitude: " + HSA.Altitude + " Heading: " + HSA.Heading + " Speed: " + HSA.Speed);
}
}
