コード例 #1
0
		public NavData(ARDrone Drone)
		{
			this.drone = Drone;
			this.drone.StatusChanged += new EventHandler<DroneStatusChangedEventArgs>(DroneStatusChanged);
			statusData = new bool[32];
			dataHeader = new NavDataHeader();
			dataDemo = new NavDataDemo();
			
			timeStamp = 0;
			dTime = 66;
			
			initTimer = new System.Timers.Timer(200);
			initTimer.AutoReset = true;
			initTimer.Elapsed += delegate { SendInit(); };
		}
コード例 #2
0
		public void DataReceived(object sender, DataReceivedEventArgs e)
		{
			if (e.Source.Address.Equals( this.drone.IPAddress))
			{
				MemoryStream ms = new MemoryStream(e.Data);
				byte[] rawHeader = new byte[16]; //navData header size
				ms.Read(rawHeader, 0 , rawHeader.Length);
				unsafe { fixed (byte* header = &rawHeader[0]) { dataHeader = *(NavDataHeader*)header; } }
				if (dataHeader.Header == 0x55667788)
				{
					SetStatus();
					while (ms.Position < ms.Length)
					{
						byte[] rawOptionHeader = new byte[4];
						ms.Read(rawOptionHeader, 0, rawOptionHeader.Length);
						ushort tag = BitConverter.ToUInt16(rawOptionHeader,0);
						ushort size = BitConverter.ToUInt16(rawOptionHeader,2);
						byte[] rawOptionBody = new byte[size-4];
						ms.Read(rawOptionBody, 0, rawOptionBody.Length);
						switch (tag)
						{
							case 0xFFFF:
								long chksum = BitConverter.ToUInt32(rawOptionBody,0);
								break;
							case 0: //navdata_demo
								unsafe { fixed (byte* optionBody = &rawOptionBody[0]) { dataDemo = *(NavDataDemo*)optionBody; } }
								OnNavDataReceived(EventArgs.Empty);
								break;
							case 1: //time tag
								uint newTime = BitConverter.ToUInt32(rawOptionBody, 0);
								int _dTime = (int)(newTime - timeStamp);
								if (_dTime > 0)
									dTime = _dTime;
								timeStamp = newTime;
								break;
							default: // TODO: approx 20 more navdata options
								break;
						}
					}
				}
			}
		}