public PreFlightPacketReceivedEventArgs(PreFlightPacketData PFP)
{
this.PFP = PFP;
}
void ComProt_PreFlightPacketReceived(object sender, CommProtocol.PreFlightPacketReceivedEventArgs e)
{
CommPacketRXLight.On = true;
PacketReceived = true;
pfp = e.PFP;
}
private void ParsePreFlightPacket()
{
/*
Pre flight packet
1 0xA5
0x5A Packet Header
3 0x14 bytes
4 0x74 Report type “Telemetry”
0x50 Report “pre flight packet”
6 0xDD Degrees (0-180) Latitude
0xMM Minutes (0-59) Latitude
0xSSSS Seconds (0-59) Latitude.
10 0xNS 0x4E = North
0x53 = South
11 0xDD Degrees (0-180) Longitude
0xMM Minutes (0-59) Longitude
0xSSSS Seconds (0-59) Longitude. See above for more details.
15 0xEW 0x45 = East
0x57 = West
16 0xAAAA Altitude reported by GPS
18 0xSSSS Altitude reported by Sonar
20 0xVVVV Battery Voltage
22 0xTTTT Battery Temperature
24 0xSS 8-bit bit-field representing sensor status
25 0xXX checksum high
0xXX checksum low
27 0xCC
28 0x33 footer
*/
if (IncomingDataBuffer.Length == 28 && (int)IncomingDataBuffer[26] == 0xCC && (int)IncomingDataBuffer[27] == 0x33)
{
//calculate checksum
UInt16 sum = 0;
for (int i = 2; i < 24; i++)
{
sum += (ushort)IncomingDataBuffer[i];
}
byte chk1 = (byte)((sum & 0xFF00) >> 8);
byte chk2 = (byte)(sum & 0x00FF);
if (chk1 != (int)IncomingDataBuffer[24] || chk2 != (int)IncomingDataBuffer[25])
{
Invoke(BadPacketReceived, new object[] {this, new BadPacketReceivedEventArgs(IncomingDataBuffer,
string.Format("Invalid pre-flight packet: invalid checksum: received {0:x4}, expected {1:x4}",
(Convert.ToUInt16((int)IncomingDataBuffer[24]) << 8) + (int)IncomingDataBuffer[25], sum))});
ClearBuffer();
}
else
{
//checksum ok
PreFlightPacketData PFP = new PreFlightPacketData();
PFP.Lat = new Latitude();
PFP.Lat.Degrees = (byte)IncomingDataBuffer[5];
PFP.Lat.Minutes = (byte)IncomingDataBuffer[6];
PFP.Lat.FractionalMinutes = (ushort)(((ushort)IncomingDataBuffer[7] << 8) + (ushort)IncomingDataBuffer[8]);
if ((byte)IncomingDataBuffer[9] == 0x4E)
{
PFP.Lat.North = true;
}
else if ((byte)IncomingDataBuffer[9] == 0x57)
{
PFP.Lat.North = false;
}
else
{
Invoke(BadPacketReceived, new object[] {
this, new BadPacketReceivedEventArgs(IncomingDataBuffer,
"Invalid packet: The north south byte of the Latitude in a received pre-flight packet is invalid.") });
ClearBuffer();
return;
}
PFP.Long = new Longitude();
PFP.Long.Degrees = (byte)IncomingDataBuffer[10];
PFP.Long.Minutes = (byte)IncomingDataBuffer[11];
PFP.Long.FractionalMinutes = (ushort)(((ushort)IncomingDataBuffer[12] <<8) +(ushort)IncomingDataBuffer[13]);
if ((byte)IncomingDataBuffer[14] == 0x45)
{
PFP.Long.East = true;
}
else if ((byte)IncomingDataBuffer[14] == 0x57)
{
PFP.Long.East = false;
}
else
{
Invoke(BadPacketReceived, new object[] {
this, new BadPacketReceivedEventArgs(IncomingDataBuffer,
"Invalid packet: The east west byte of the Latitude in a received pre-flight packet is invalid.") });
ClearBuffer();
return;
}
PFP.GPSAltitude = (ushort)(((int)IncomingDataBuffer[15]<<8) + (int)IncomingDataBuffer[16]);
PFP.SonarAltitude = (ushort)(((int)IncomingDataBuffer[17] << 8) + (int)IncomingDataBuffer[18]);
PFP.BatteryVoltage = (short)(((int)IncomingDataBuffer[19] << 8) + (int)IncomingDataBuffer[20]);
PFP.BatteryTemp = (short)(((int)IncomingDataBuffer[21] << 8) + (int)IncomingDataBuffer[22]);
PFP.SensorStatus = (byte)IncomingDataBuffer[23];
//invoke the event
ClearBuffer();
Invoke(PreFlightPacketReceived, new object[] { this, new PreFlightPacketReceivedEventArgs(PFP) });
}
}
else if (IncomingDataBuffer.Length >= 28)
{
Invoke(BadPacketReceived, new object[] { this,
new BadPacketReceivedEventArgs(IncomingDataBuffer,
"Bad packet footer for pre-flight packet") });
ClearBuffer();
}
}
void cp_PreFlightPacketReceived(object sender, CommProtocol.PreFlightPacketReceivedEventArgs e)
{
ConsecutiveBadPackets = 0;
PFP = e.PFP;
textBox1.AppendText(" battery temperature: " + PFP.BatteryTemp +
" GPS Altitude: " + PFP.GPSAltitude +
" Lat degrees: " + PFP.Lat.Degrees +
" Lat minutes: " + PFP.Lat.Minutes +
" Lat minutes remainder: " + (float)(PFP.Lat.FractionalMinutes)/1000.0f +
" North degrees: " + PFP.Lat.North +
" Long degrees: " + PFP.Long.Degrees +
" Long minutes: " + PFP.Long.Minutes +
" Long minutes remainder: " + (float)(PFP.Long.FractionalMinutes) /1000.0f+
" East degrees: " + PFP.Long.East +
" sensor status: " + string.Format("{0:x2}", PFP.SensorStatus) +
" sonar altitude: " + PFP.SonarAltitude
+ "\r\n");
if (!ManualMode)
{
InsertRowToReceived_packetsTable("PreFlightPacket",
" battery temperature: " + PFP.BatteryTemp +
" GPS Altitude: " + PFP.GPSAltitude +
" Lat degrees: " + PFP.Lat.Degrees +
" Lat minutes: " + PFP.Lat.Minutes +
" Lat minutes remainder: " + (float)(PFP.Lat.FractionalMinutes) / 1000.0f +
" North degrees: " + PFP.Lat.North +
" Long degrees: " + PFP.Long.Degrees +
" Long minutes: " + PFP.Long.Minutes +
" Long minutes remainder: " + (float)(PFP.Long.FractionalMinutes) / 1000.0f +
" East degrees: " + PFP.Long.East +
" sensor status: " + string.Format("{0:x2}", PFP.SensorStatus) +
" sonar altitude: " + PFP.SonarAltitude);
}
}