public AttitudePacketReceivedEventArgs(Attitude a)
{
attitude = a;
}
void ComProt_AttitudePacketReceived(object sender, CommProtocol.AttitudePacketReceivedEventArgs e)
{
CommPacketRXLight.On = true;
PacketReceived = true;
Att = e.attitude;
compass.Angle = Att.Yaw;
if (Att.Roll > 128)
{
Arthorizon.Roll = -(255 - Att.Roll);
}
else
{
Arthorizon.Roll = Att.Roll;
}
if (Att.Pitch > 128)
{
Arthorizon.Pitch = -(255 - Att.Pitch);
}
else
{
Arthorizon.Pitch = Att.Pitch;
}
}
private void ParseAttitudePacket()
{
/*
Attitude
1 0xA5
0x5A Packet Header
3 0x08 bytes
4 0x74 Report type “Telemetry”
0x5A Report “Attitude”
6 0xPPPP Hex value (0x0000 – 0x0167) representing current pitch angle, can potentially use extra bits for partial degrees
(increases as helicopter pitches forward)
8 0xRRRR Hex value (0x0000 – 0x0167) representing current roll angle, can potentially use extra bits for partial degrees
(increases as helicopter rolls in the starboard direction)
10 0xYYYY Hex value (0x0000 – 0x0167) representing current yaw angle, can potentially use extra bits for partial degrees
(increases as helicopter rotates counter clockwise, as seen from above the helicopter)
12 0xXX //checksum high
0xXX //checksum low
14 0xCC
15 0x33 //footer
*/
if (IncomingDataBuffer.Length == 15 && (int)IncomingDataBuffer[13] == 0xCC && (int)IncomingDataBuffer[14] == 0x33)
{
//calculate checksum
UInt16 sum = 0;
for (int i = 2; i < 11; i++)
{
sum += (ushort)IncomingDataBuffer[i];
}
byte chk1 = (byte)((sum & 0xFF00) >> 8);
byte chk2 = (byte)(sum & 0x00FF);
if (chk1 != (int)IncomingDataBuffer[11] || chk2 != (int)IncomingDataBuffer[12])
{
Invoke(BadPacketReceived, new object[] {this, new BadPacketReceivedEventArgs(IncomingDataBuffer,
string.Format("Invalid attitude packet: invalid checksum: received {0:x4}, expected {1:x4}",
(Convert.ToUInt16((int)IncomingDataBuffer[11]) << 8) + (int)IncomingDataBuffer[12], sum))});
ClearBuffer();
}
else
{
//checksum ok
Attitude a = new Attitude();
a.Pitch = (short)(((int)IncomingDataBuffer[5] << 8) + (int)IncomingDataBuffer[6]);
a.Roll = (short)(((int)IncomingDataBuffer[7] << 8) + (int)IncomingDataBuffer[8]);
a.Yaw = (short)(((int)IncomingDataBuffer[9] << 8) + (int)IncomingDataBuffer[10]);
//invoke the event
ClearBuffer();
Invoke(AttitudePacketReceived, new object[] { this, new AttitudePacketReceivedEventArgs(a) });
}
}
else if (IncomingDataBuffer.Length >= 15)
{
Invoke(BadPacketReceived,
new object[] { this, new BadPacketReceivedEventArgs(IncomingDataBuffer, "Bad attitude packet received") });
ClearBuffer();
}
}
void cp_AttitudePacketReceived(object sender, CommProtocol.AttitudePacketReceivedEventArgs e)
{
ConsecutiveBadPackets = 0;
attitude = e.attitude;
textBox1.AppendText("Attitude packet received: PITCH: " + attitude.Pitch + " ROLL: " + attitude.Roll + " YAW: " + attitude.Yaw + "\r\n");
if (!ManualMode)
{
InsertRowToReceived_packetsTable("Attitude", "PITCH: " + attitude.Pitch + " ROLL: " + attitude.Roll + " YAW: " + attitude.Yaw);
}
}