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); }
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); }