예제 #1
0
		public short FL, FR, BR, BL;	// front-left, front-right, back-right, back-left motor outputs

		public void ReadFrom( Packet p )
		{
			FL = p.GetShort();
			FR = p.GetShort();
			BR = p.GetShort();
			BL = p.GetShort();
		}
예제 #2
0
		public void ReadFrom( Packet p )
		{
			Thro = p.GetShort();
			Aile = p.GetShort();
			Elev = p.GetShort();
			Rudd = p.GetShort();
			Gear = p.GetShort();
			Aux1 = p.GetShort();
			Aux2 = p.GetShort();
			Aux3 = p.GetShort();
			BatteryVolts = p.GetShort();
		}
예제 #3
0
		public void ReadFrom( Packet p )
		{
			Temp = p.GetShort();
			GyroX = p.GetShort();
			GyroY = p.GetShort();
			GyroZ = p.GetShort();
			AccelX = p.GetShort();
			AccelY = p.GetShort();
			AccelZ = p.GetShort();
			MagX = p.GetShort();
			MagY = p.GetShort();
			MagZ = p.GetShort();
		}
		private void ProcessByte( byte b )
		{
			if(sigByteIndex < 2)
			{
				if(sigByteIndex == 0)
				{
					if(b == 0x55)
					{
						sigByteIndex++;
						packetByteIndex = 0;
					}
					return;
				}


				if(sigByteIndex == 1)
				{
					if(b == 0xAA)
					{
						sigByteIndex++;
						packetByteIndex = 0;
					}
					else
					{
						sigByteIndex = 0;
					}
					return;
				}
			}


			switch(packetByteIndex)
			{
				case 0:
					currentPacket = new Packet();
					currentPacket.mode = b;
					if(b > 0x20)
					{	// No such mode - bad data
						sigByteIndex = 0;
						packetByteIndex = 0;
						return;
					}
					packetByteIndex++;
					return;

				case 1:
					if(b != 0)
					{	// No such mode - bad data
						sigByteIndex = 0;
						packetByteIndex = 0;
						return;
					}
					packetByteIndex++;
					currentChecksum = Checksum( 0, (UInt16)0xaa55 );
					currentChecksum = Checksum( currentChecksum, (UInt16)currentPacket.mode );
					return;

				case 2:
					currentPacket.len = (short)b;
					packetByteIndex++;
					return;

				case 3:
					currentPacket.len |= (short)(b << 8);
					currentChecksum = Checksum( currentChecksum, (UInt16)currentPacket.len );

					if(currentPacket.len > 1024)
					{	// unreasonable packet size (> 1kb)
						sigByteIndex = 0;
						packetByteIndex = 0;
						return;
					}

					currentPacket.len -= 6;		// Subtract off signature and header size
					if(currentPacket.len < 1)
					{	// Can't have a zero length packet - bad data
						sigByteIndex = 0;
						packetByteIndex = 0;
						return;
					}
					currentPacket.data = new byte[currentPacket.len];
					packetByteIndex++;
					return;

				default:
					currentPacket.data[packetByteIndex - 4] = b;
					break;
			}

			packetByteIndex++;
			if(packetByteIndex == (currentPacket.data.Length + 4))	// length + header bytes
			{
				sigByteIndex = 0;
				packetByteIndex = 0;

				// Validate the checksum
				// only keep the packet if they match
				int len = currentPacket.data.Length - 2;

				UInt16 check = Checksum( currentChecksum, currentPacket.data, len );
				UInt16 sourceCheck = (UInt16)(currentPacket.data[len] | (currentPacket.data[len + 1] << 8));

				if(check == sourceCheck)
				{
					packetsArray[head] = currentPacket;

					lock(packetsArray)
					{
						head = (head + 1) % packetsArray.Length;

						if(tail == head) {
							tail = (tail + 1) % packetsArray.Length;		// Throw away oldest data if we fill the buffer
						}
					}
				}
			}
		}
예제 #5
0
		public int Alt, AltTemp, AltiEst;							// Altimeter = 12 bytes


		public void ReadFrom( Packet p )
		{
			Pitch = p.GetInt();
			Roll = p.GetInt();
			Yaw = p.GetInt();

			Alt = p.GetInt();
			AltTemp = p.GetInt();
			AltiEst = p.GetInt();
		}
예제 #6
0
		public void ReadFrom( Packet p )
		{
			Version   = p.GetShort();
			MinCycles = p.GetShort();
			MaxCycles = p.GetShort();
			AvgCycles = p.GetShort();
			Counter = p.GetInt();		// basically a sequence value
		}
예제 #7
0
		private void ProcessByte( byte b )
		{
			if(sigByteIndex < 2)
			{
				if(b == 0x77)
				{
					sigByteIndex++;
					packetByteIndex = 0;
					return;
				}
				else
				{
					sigByteIndex = 0;
					packetByteIndex = 0;
					return;
				}
			}

			switch( packetByteIndex )
			{
				case 0:
					currentPacket = new Packet();
					currentPacket.mode = b;
					packetByteIndex++;
					return;

				case 1:
					currentPacket.len = b;
					currentPacket.data = new byte[currentPacket.len];
					packetByteIndex++;
					return;

				default:
					currentPacket.data[packetByteIndex-2] = b;
					break;
			}

			packetByteIndex++;
			if(packetByteIndex == (currentPacket.data.Length + 2))
			{
				sigByteIndex = 0;
				packetByteIndex = 0;
				packetsArray[head] = currentPacket;

				lock(packetsArray)
				{
					head = (head+1) % packetsArray.Length;

					if(tail == head) {
						tail = (tail + 1) % packetsArray.Length;		// Throw away oldest data if we fill the buffer
					}
				}
			}
		}