public ANPacket67(ANPacket packet)
{
delay = BitConverter.ToSingle(packet.data, 0);
speed = BitConverter.ToSingle(packet.data, 4);
distanceTravelled = BitConverter.ToSingle(packet.data, 8);
reverseDetectionSupported = (packet.data[0] & 0x01) != 0;
}
private void serialPort1_DataReceived(object sender, System.IO.Ports.SerialDataReceivedEventArgs e)
{
try
{
anPacketDecoder.bufferLength += serialPort1.Read(anPacketDecoder.buffer, anPacketDecoder.bufferLength, anPacketDecoder.buffer.Length - anPacketDecoder.bufferLength);
ANPacket packet = null;
while ((packet = anPacketDecoder.packetDecode()) != null)
{
switch (packet.id)
{
case ANPacket.PACKET_ID_ODOMETER:
if (packet.length == 13)
{
ANPacket67 anPacket67 = new ANPacket67(packet);
this.Invoke((MethodInvoker) delegate
{
richTextBox1.AppendText("Received Odometer Packet\n");
textBoxDelay.Text = anPacket67.delay.ToString();
textBoxSpeed.Text = anPacket67.speed.ToString();
textBoxDistance.Text = anPacket67.distanceTravelled.ToString();
checkBoxReverseDetection.Checked = anPacket67.reverseDetectionSupported;
});
}
break;
}
}
}
catch { }
}
public ANPacket28(ANPacket packet)
{
accelerometers = new float[3];
accelerometers[0] = BitConverter.ToSingle(packet.data, 0);
accelerometers[1] = BitConverter.ToSingle(packet.data, 4);
accelerometers[2] = BitConverter.ToSingle(packet.data, 8);
gyroscopes = new float[3];
gyroscopes[0] = BitConverter.ToSingle(packet.data, 12);
gyroscopes[1] = BitConverter.ToSingle(packet.data, 16);
gyroscopes[2] = BitConverter.ToSingle(packet.data, 20);
magnetometers = new float[3];
magnetometers[0] = BitConverter.ToSingle(packet.data, 24);
magnetometers[1] = BitConverter.ToSingle(packet.data, 28);
magnetometers[2] = BitConverter.ToSingle(packet.data, 32);
imuTemperature = BitConverter.ToSingle(packet.data, 36);
pressure = BitConverter.ToSingle(packet.data, 40);
pressureTemperature = BitConverter.ToSingle(packet.data, 44);
ArrayList arr = new ArrayList();
/* for(int i=0;i<50;i++)
* {
* arr.Add(pressureTemperature);
* Console.WriteLine("PT =" + arr[i]);
* sum += (int) arr[i];
* }
* Console.WriteLine("AVG =" + (sum / 50));*/
}
public ANPacket packetDecode()
{
ANPacket packet = null;
int decodeIterator = 0;
while (decodeIterator + HEADER_SIZE <= bufferLength)
{
byte headerLRC = buffer[decodeIterator++];
if (headerLRC == calculateHeaderLRC(buffer, decodeIterator))
{
byte id = buffer[decodeIterator++];
byte length = buffer[decodeIterator++];
UInt16 crc = buffer[decodeIterator++];
crc |= (UInt16)(buffer[decodeIterator++] << 8);
if (decodeIterator + length > bufferLength)
{
decodeIterator -= 5;
break;
}
if (crc == calculateCRC16(buffer, decodeIterator, length))
{
packet = new ANPacket(length, id);
for (int i = 0; i < length; i++)
{
packet.data[i] = buffer[decodeIterator + i];
}
decodeIterator += length;
break;
}
else
{
Console.WriteLine("packet crc error, id = " + id + " length = " + length + " crc = " + crc + " it = " + decodeIterator);
}
}
}
if (decodeIterator < bufferLength)
{
if (decodeIterator > 0)
{
for (int i = 0; i < bufferLength - decodeIterator; i++)
{
buffer[i] = buffer[decodeIterator + i];
}
bufferLength -= decodeIterator;
}
}
else
{
bufferLength = 0;
}
return(packet);
}
public static byte[] packetEncode(ANPacket packet)
{
byte[] data = new byte[packet.length + HEADER_SIZE];
int crc = calculateCRC16(packet.data, 0, packet.length);
data[1] = (byte)packet.id;
data[2] = (byte)packet.length;
data[3] = (byte)(crc & 0xFF);
data[4] = (byte)((crc >> 8) & 0xFF);
data[0] = calculateHeaderLRC(data, 1);
for (int i = 0; i < packet.length; i++)
{
data[HEADER_SIZE + i] = packet.data[i];
}
return data;
}
public static byte[] packetEncode(ANPacket packet)
{
byte[] data = new byte[packet.length + HEADER_SIZE];
int crc = calculateCRC16(packet.data, 0, packet.length);
data[1] = (byte)packet.id;
data[2] = (byte)packet.length;
data[3] = (byte)(crc & 0xFF);
data[4] = (byte)((crc >> 8) & 0xFF);
data[0] = calculateHeaderLRC(data, 1);
for (int i = 0; i < packet.length; i++)
{
data[HEADER_SIZE + i] = packet.data[i];
}
return(data);
}
public ANPacket28(ANPacket packet)
{
accelerometers = new float[3];
accelerometers[0] = BitConverter.ToSingle(packet.data, 0);
accelerometers[1] = BitConverter.ToSingle(packet.data, 4);
accelerometers[2] = BitConverter.ToSingle(packet.data, 8);
gyroscopes = new float[3];
gyroscopes[0] = BitConverter.ToSingle(packet.data, 12);
gyroscopes[1] = BitConverter.ToSingle(packet.data, 16);
gyroscopes[2] = BitConverter.ToSingle(packet.data, 20);
magnetometers = new float[3];
magnetometers[0] = BitConverter.ToSingle(packet.data, 24);
magnetometers[1] = BitConverter.ToSingle(packet.data, 28);
magnetometers[2] = BitConverter.ToSingle(packet.data, 32);
imuTemperature = BitConverter.ToSingle(packet.data, 36);
pressure = BitConverter.ToSingle(packet.data, 40);
pressureTemperature = BitConverter.ToSingle(packet.data, 44);
}
public ANPacket28(ANPacket packet)
{
accelerometers = new float[3];
accelerometers[0] = BitConverter.ToSingle(packet.data, 0);
accelerometers[1] = BitConverter.ToSingle(packet.data, 4);
accelerometers[2] = BitConverter.ToSingle(packet.data, 8);
gyroscopes = new float[3];
gyroscopes[0] = BitConverter.ToSingle(packet.data, 12);
gyroscopes[1] = BitConverter.ToSingle(packet.data, 16);
gyroscopes[2] = BitConverter.ToSingle(packet.data, 20);
magnetometers = new float[3];
magnetometers[0] = BitConverter.ToSingle(packet.data, 24);
magnetometers[1] = BitConverter.ToSingle(packet.data, 28);
magnetometers[2] = BitConverter.ToSingle(packet.data, 32);
imuTemperature = BitConverter.ToSingle(packet.data, 36);
pressure = BitConverter.ToSingle(packet.data, 40);
pressureTemperature = BitConverter.ToSingle(packet.data, 44);
}
private void serialPort1_DataReceived(object sender, System.IO.Ports.SerialDataReceivedEventArgs e)
{
try
{
anPacketDecoder.bufferLength += serialPort1.Read(anPacketDecoder.buffer, anPacketDecoder.bufferLength, anPacketDecoder.buffer.Length - anPacketDecoder.bufferLength);
ANPacket packet = null;
while ((packet = anPacketDecoder.packetDecode()) != null)
{
switch (packet.id)
{
case ANPacket.PACKET_ID_SYSTEM_STATE:
if (packet.length == 100)
{
ANPacket20 anPacket20 = new ANPacket20(packet);
this.Invoke((MethodInvoker) delegate
{
richTextBox1.AppendText("Received System State Packet\n");
textBoxLatitude.Text = (anPacket20.position[0] * 180 / Math.PI).ToString();
textBoxLongitude.Text = (anPacket20.position[1] * 180 / Math.PI).ToString();
textBoxHeight.Text = anPacket20.position[2].ToString();
textBoxRoll.Text = (anPacket20.orientation[0] * 180 / Math.PI).ToString();
textBoxPitch.Text = (anPacket20.orientation[1] * 180 / Math.PI).ToString();
textBoxYaw.Text = (anPacket20.orientation[2] * 180 / Math.PI).ToString();
});
}
break;
case ANPacket.PACKET_ID_RAW_SENSORS:
if (packet.length == 48)
{
ANPacket28 anPacket28 = new ANPacket28(packet);
this.Invoke((MethodInvoker) delegate
{
richTextBox1.AppendText("Received Raw Sensors Packet\n");
});
}
break;
}
}
}
catch { }
}
public ANPacket packetDecode()
{
ANPacket packet = null;
int decodeIterator = 0;
while (decodeIterator + HEADER_SIZE <= bufferLength)
{
byte headerLRC = buffer[decodeIterator++];
if (headerLRC == calculateHeaderLRC(buffer, decodeIterator))
{
byte id = buffer[decodeIterator++];
byte length = buffer[decodeIterator++];
UInt16 crc = buffer[decodeIterator++];
crc |= (UInt16)(buffer[decodeIterator++] << 8);
if (decodeIterator + length > bufferLength)
{
decodeIterator -= 5;
break;
}
if (crc == calculateCRC16(buffer, decodeIterator, length))
{
packet = new ANPacket(length, id);
for (int i = 0; i < length; i++)
{
packet.data[i] = buffer[decodeIterator + i];
}
decodeIterator += length;
break;
}
else
{
Console.WriteLine("packet crc error, id = " + id + " length = " + length + " crc = " + crc + " it = " + decodeIterator);
}
}
}
if (decodeIterator < bufferLength)
{
if (decodeIterator > 0)
{
for (int i = 0; i < bufferLength - decodeIterator; i++)
{
buffer[i] = buffer[decodeIterator + i];
}
bufferLength -= decodeIterator;
}
}
else bufferLength = 0;
return packet;
}
public ANPacket20(ANPacket packet)
{
if (packet.data[0] != 0)
{
statusAlert = true;
if ((packet.data[0] & 0x01) != 0)
{
systemFailure = true;
}
if ((packet.data[0] & 0x02) != 0)
{
accelerometerSensorFailure = true;
}
if ((packet.data[0] & 0x04) != 0)
{
gyroscopeSensorFailure = true;
}
if ((packet.data[0] & 0x08) != 0)
{
magnetometerSensorFailure = true;
}
if ((packet.data[0] & 0x10) != 0)
{
pressureSensorFailure = true;
}
if ((packet.data[0] & 0x20) != 0)
{
gnssFailure = true;
}
if ((packet.data[0] & 0x40) != 0)
{
accelerometerOverRange = true;
}
if ((packet.data[0] & 0x80) != 0)
{
gyroscopeOverRange = true;
}
}
if (packet.data[1] != 0)
{
statusAlert = true;
if ((packet.data[1] & 0x01) != 0)
{
magnetometerOverRange = true;
}
if ((packet.data[1] & 0x02) != 0)
{
pressureOverRange = true;
}
if ((packet.data[1] & 0x04) != 0)
{
minimumTemperatureAlarm = true;
}
if ((packet.data[1] & 0x08) != 0)
{
maximumTemperatureAlarm = true;
}
if ((packet.data[1] & 0x10) != 0)
{
lowVoltageAlarm = true;
}
if ((packet.data[1] & 0x20) != 0)
{
highVoltageAlarm = true;
}
if ((packet.data[1] & 0x40) != 0)
{
gnssAntennaAlarm = true;
}
if ((packet.data[1] & 0x80) != 0)
{
dataOverflowAlarm = true;
}
}
if ((packet.data[2] & 0x01) != 0)
{
orientationInitialised = true;
}
if ((packet.data[2] & 0x02) != 0)
{
navigationInitialised = true;
}
if ((packet.data[2] & 0x04) != 0)
{
headingInitialised = true;
}
if ((packet.data[2] & 0x08) != 0)
{
utcTimeValid = true;
}
gnssFixType = (packet.data[2] & 0x70) >> 4;
if (gnssFixType > GNSS_NO_FIX)
{
gnssFix = true;
}
if ((packet.data[2] & 0x80) != 0)
{
event1Flag = true;
}
if ((packet.data[3] & 0x01) != 0)
{
event2Flag = true;
}
if ((packet.data[3] & 0x02) != 0)
{
internalGnssActive = true;
}
if ((packet.data[3] & 0x04) != 0)
{
magnetometersActive = true;
}
if ((packet.data[3] & 0x08) != 0)
{
velocityHeadingActive = true;
}
if ((packet.data[3] & 0x10) != 0)
{
atmosphericAltitudeActive = true;
}
if ((packet.data[3] & 0x20) != 0)
{
externalPositionActive = true;
}
if ((packet.data[3] & 0x40) != 0)
{
externalVelocityActive = true;
}
if ((packet.data[3] & 0x80) != 0)
{
externalHeadingActive = true;
}
utc_time = BitConverter.ToUInt32(packet.data, 4);
microseconds = BitConverter.ToUInt32(packet.data, 8);
time = new DateTime(utc_time * 10000000 + microseconds * 10);
position = new double[3];
position[0] = BitConverter.ToDouble(packet.data, 12);
position[1] = BitConverter.ToDouble(packet.data, 20);
position[2] = BitConverter.ToDouble(packet.data, 28);
velocity = new float[3];
velocity[0] = BitConverter.ToSingle(packet.data, 36);
velocity[1] = BitConverter.ToSingle(packet.data, 40);
velocity[2] = BitConverter.ToSingle(packet.data, 44);
acceleration = new float[3];
acceleration[0] = BitConverter.ToSingle(packet.data, 48);
acceleration[1] = BitConverter.ToSingle(packet.data, 52);
acceleration[2] = BitConverter.ToSingle(packet.data, 56);
gForce = BitConverter.ToSingle(packet.data, 60);
orientation = new float[3];
orientation[0] = BitConverter.ToSingle(packet.data, 64);
orientation[1] = BitConverter.ToSingle(packet.data, 68);
orientation[2] = BitConverter.ToSingle(packet.data, 72);
angularVelocity = new float[3];
angularVelocity[0] = BitConverter.ToSingle(packet.data, 76);
angularVelocity[1] = BitConverter.ToSingle(packet.data, 80);
angularVelocity[2] = BitConverter.ToSingle(packet.data, 84);
positionStandardDeviation = new float[3];
positionStandardDeviation[0] = BitConverter.ToSingle(packet.data, 88);
positionStandardDeviation[1] = BitConverter.ToSingle(packet.data, 92);
positionStandardDeviation[2] = BitConverter.ToSingle(packet.data, 96);
}
private void serialPort1_DataReceived(object sender, System.IO.Ports.SerialDataReceivedEventArgs e)
{
try
{
anPacketDecoder.bufferLength += serialPort1.Read(anPacketDecoder.buffer, anPacketDecoder.bufferLength, anPacketDecoder.buffer.Length - anPacketDecoder.bufferLength);
ANPacket packet = null;
while ((packet = anPacketDecoder.packetDecode()) != null)
{
switch (packet.id)
{
/* case ANPacket.PACKET_ID_SYSTEM_STATE:
* if (packet.length == 100)
* {
* ANPacket20 anPacket20 = new ANPacket20(packet);
* this.Invoke((MethodInvoker)delegate
* {
* richTextBox1.AppendText("Received System State Packet\n");
* textBoxLatitude.Text = (anPacket20.position[0] * 180 / Math.PI).ToString();
* textBoxLongitude.Text = (anPacket20.position[1] * 180 / Math.PI).ToString();
* textBoxHeight.Text = anPacket20.position[2].ToString();
* textBoxRoll.Text = (anPacket20.orientation[0] * 180 / Math.PI).ToString();
* textBoxPitch.Text = (anPacket20.orientation[1] * 180 / Math.PI).ToString();
* textBoxYaw.Text = (anPacket20.orientation[2] * 180 / Math.PI).ToString();
* });
* }
* break;*/
case ANPacket.PACKET_ID_RAW_SENSORS:
if (packet.length == 48)
{
float sum = 0f;
ANPacket28 anPacket28 = new ANPacket28(packet);
float[] arr = new float[1000];
List <float> arr1 = new List <float>();
List <float> arr2 = new List <float>();
List <float> arr3 = new List <float>();
this.Invoke((MethodInvoker) delegate
{
richTextBox1.AppendText("Received Raw Sensors Packet\n");
String mystring1 = anPacket28.accelerometers[0].ToString();
String mystring2 = anPacket28.accelerometers[1].ToString();
String mystring3 = anPacket28.accelerometers[2].ToString();
textBox1.Text = (mystring1);
textBox3.Text = (mystring2);
textBox4.Text = (mystring3);
String mystring4 = anPacket28.gyroscopes[0].ToString();
String mystring5 = anPacket28.gyroscopes[1].ToString();
String mystring6 = anPacket28.gyroscopes[2].ToString();
textBox2.Text = (mystring4);
textBox5.Text = (mystring5);
textBox6.Text = (mystring6);
String mystring7 = anPacket28.magnetometers[0].ToString();
String mystring8 = anPacket28.magnetometers[1].ToString();
String mystring9 = anPacket28.magnetometers[2].ToString();
textBox9.Text = (mystring7);
textBox8.Text = (mystring8);
textBox7.Text = (mystring9);
String mystring10 = anPacket28.pressure.ToString();
textBox10.Text = (mystring10);
String mystring11 = anPacket28.pressureTemperature.ToString();
textBox11.Text = (mystring11);
//Thread.Sleep(90);
/* if (anPacket28.pressureTemperature == 0)
* Console.WriteLine("Come Out");
* else
* {
*
*
* Console.WriteLine("AVG =" + arr.Average());
*
*
* }*/
//arr.Add(anPacket28.magnetometers[0]);
/* for(int i=0;i<1000;i++)
* {
* arr[i] = anPacket28.magnetometers[0];
* sum =sum +arr[i];
* }
* float avg = sum / 1000;
* Console.WriteLine("Avg ="+(sum / 1000));
*
* Console.WriteLine("Count =" + arr[20]);*/
for (int i = 0; i < 1000; i++)
{
arr1.Add(anPacket28.magnetometers[0]);
sum = arr1.Sum();
}
float avg = (sum / arr1.Count);
Console.WriteLine(arr1.Count);
String mystr = avg.ToString();
textBox16.Text = mystr;
/* float avg = (sum / 500);
* Console.WriteLine(arr1.Count);
*
*
*
* String mystr = avg.ToString();
* textBox16.Text = mystr;*/
});
}
break;
case ANPacket.PACKET_ID_SYSTEM_STATE:
if (packet.length == 100)
{
ANPacket20 anPacket20 = new ANPacket20(packet);
this.Invoke((MethodInvoker) delegate
{
richTextBox1.AppendText("Received System State Packet\n");
textBoxLatitude.Text = (anPacket20.position[0] * 180 / Math.PI).ToString();
textBoxLongitude.Text = (anPacket20.position[1] * 180 / Math.PI).ToString();
textBoxHeight.Text = anPacket20.position[2].ToString();
textBoxRoll.Text = (anPacket20.orientation[0] * 180 / Math.PI).ToString();
textBoxPitch.Text = (anPacket20.orientation[1] * 180 / Math.PI).ToString();
textBoxYaw.Text = (anPacket20.orientation[2] * 180 / Math.PI).ToString();
String mystring1 = anPacket20.position[0].ToString();
textBox15.Text = mystring1;
String mystring2 = anPacket20.position[1].ToString();
textBox17.Text = mystring2;
String mystring3 = anPacket20.position[2].ToString();
textBox18.Text = mystring3;
String mystring4 = anPacket20.velocity[0].ToString();
String mystring5 = anPacket20.velocity[1].ToString();
String mystring6 = anPacket20.velocity[2].ToString();
textBox12.Text = mystring4;
textBox13.Text = mystring5;
textBox14.Text = mystring6;
});
}
break;
}
}
}
catch { }
}
public ANPacket20(ANPacket packet)
{
if (packet.data[0] != 0)
{
statusAlert = true;
if ((packet.data[0] & 0x01) != 0) systemFailure = true;
if ((packet.data[0] & 0x02) != 0) accelerometerSensorFailure = true;
if ((packet.data[0] & 0x04) != 0) gyroscopeSensorFailure = true;
if ((packet.data[0] & 0x08) != 0) magnetometerSensorFailure = true;
if ((packet.data[0] & 0x10) != 0) pressureSensorFailure = true;
if ((packet.data[0] & 0x20) != 0) gnssFailure = true;
if ((packet.data[0] & 0x40) != 0) accelerometerOverRange = true;
if ((packet.data[0] & 0x80) != 0) gyroscopeOverRange = true;
}
if (packet.data[1] != 0)
{
statusAlert = true;
if ((packet.data[1] & 0x01) != 0) magnetometerOverRange = true;
if ((packet.data[1] & 0x02) != 0) pressureOverRange = true;
if ((packet.data[1] & 0x04) != 0) minimumTemperatureAlarm = true;
if ((packet.data[1] & 0x08) != 0) maximumTemperatureAlarm = true;
if ((packet.data[1] & 0x10) != 0) lowVoltageAlarm = true;
if ((packet.data[1] & 0x20) != 0) highVoltageAlarm = true;
if ((packet.data[1] & 0x40) != 0) gnssAntennaAlarm = true;
if ((packet.data[1] & 0x80) != 0) dataOverflowAlarm = true;
}
if ((packet.data[2] & 0x01) != 0) orientationInitialised = true;
if ((packet.data[2] & 0x02) != 0) navigationInitialised = true;
if ((packet.data[2] & 0x04) != 0) headingInitialised = true;
if ((packet.data[2] & 0x08) != 0) utcTimeValid = true;
gnssFixType = (packet.data[2] & 0x70)>>4;
if(gnssFixType > GNSS_NO_FIX) gnssFix = true;
if ((packet.data[2] & 0x80) != 0) event1Flag = true;
if ((packet.data[3] & 0x01) != 0) event2Flag = true;
if ((packet.data[3] & 0x02) != 0) internalGnssActive = true;
if ((packet.data[3] & 0x04) != 0) magnetometersActive = true;
if ((packet.data[3] & 0x08) != 0) velocityHeadingActive = true;
if ((packet.data[3] & 0x10) != 0) atmosphericAltitudeActive = true;
if ((packet.data[3] & 0x20) != 0) externalPositionActive = true;
if ((packet.data[3] & 0x40) != 0) externalVelocityActive = true;
if ((packet.data[3] & 0x80) != 0) externalHeadingActive = true;
utc_time = BitConverter.ToUInt32(packet.data, 4);
microseconds = BitConverter.ToUInt32(packet.data, 8);
time = new DateTime(utc_time * 10000000 + microseconds * 10);
position = new double[3];
position[0] = BitConverter.ToDouble(packet.data, 12);
position[1] = BitConverter.ToDouble(packet.data, 20);
position[2] = BitConverter.ToDouble(packet.data, 28);
velocity = new float[3];
velocity[0] = BitConverter.ToSingle(packet.data, 36);
velocity[1] = BitConverter.ToSingle(packet.data, 40);
velocity[2] = BitConverter.ToSingle(packet.data, 44);
acceleration = new float[3];
acceleration[0] = BitConverter.ToSingle(packet.data, 48);
acceleration[1] = BitConverter.ToSingle(packet.data, 52);
acceleration[2] = BitConverter.ToSingle(packet.data, 56);
gForce = BitConverter.ToSingle(packet.data, 60);
orientation = new float[3];
orientation[0] = BitConverter.ToSingle(packet.data, 64);
orientation[1] = BitConverter.ToSingle(packet.data, 68);
orientation[2] = BitConverter.ToSingle(packet.data, 72);
angularVelocity = new float[3];
angularVelocity[0] = BitConverter.ToSingle(packet.data, 76);
angularVelocity[1] = BitConverter.ToSingle(packet.data, 80);
angularVelocity[2] = BitConverter.ToSingle(packet.data, 84);
positionStandardDeviation = new float[3];
positionStandardDeviation[0] = BitConverter.ToSingle(packet.data, 88);
positionStandardDeviation[1] = BitConverter.ToSingle(packet.data, 92);
positionStandardDeviation[2] = BitConverter.ToSingle(packet.data, 96);
}
