diff --git a/TelloLib/Tello.cs b/TelloLib/Tello.cs index 69c0e59..33171b4 100644 --- a/TelloLib/Tello.cs +++ b/TelloLib/Tello.cs @@ -1,4 +1,4 @@ -using System; +using System; using System.Collections.Generic; using System.IO; using System.Linq; @@ -24,8 +24,8 @@ public class Tello public delegate void videoUpdateDeligate(byte[] data); public static event videoUpdateDeligate onVideoData; - public static string picPath;//todo redo this. - public static string picFilePath;//todo redo this. + public static string picPath;//todo redo this. + public static string picFilePath;//todo redo this. public static int picMode = 0;//pic or vid aspect ratio. public static int iFrameRate = 5;//How often to ask for iFrames in 50ms. Ie 2=10x 5=4x 10=2xSecond 5 = 4xSecond @@ -38,12 +38,14 @@ public enum ConnectionState Connecting, Connected, Paused,//used to keep from disconnecting when starved for input. - UnPausing//Transition. Never stays in this state. + UnPausing//Transition. Never stays in this state. } public static ConnectionState connectionState = ConnectionState.Disconnected; private static CancellationTokenSource cancelTokens = new CancellationTokenSource();//used to cancel listeners + private static CancellationTokenSource masterCancelTokens = new CancellationTokenSource(); + public static void takeOff() { var packet = new byte[] { 0xcc, 0x58, 0x00, 0x7c, 0x68, 0x54, 0x00, 0xe4, 0x01, 0xc2, 0x16 }; @@ -334,7 +336,7 @@ public static void sendAckLog(short cmd,ushort id) client.Send(packet); } - //this might not be working right + //this might not be working right public static void sendAckLogConfig(short cmd, ushort id,int n2) { // crc typ cmdL cmdH seqL seqH unk idL idH n2L n2H n2L n2H crc crc @@ -401,7 +403,7 @@ private static void disconnect() } connectionState = ConnectionState.Disconnected; - + } private static void connect() { @@ -445,12 +447,14 @@ public static void connectionSetPause(bool bPause) private static UInt32 picExtraPackets; public static bool picDownloading; private static int maxPieceNum = 0; + private static UdpListener videoServer; + private static void startListeners() { cancelTokens = new CancellationTokenSource(); CancellationToken token = cancelTokens.Token; - //wait for reply messages from the tello and process. + //wait for reply messages from the tello and process. Task.Factory.StartNew(async () => { while (true) @@ -559,10 +563,10 @@ private static void startListeners() if(picBytesExpected>picbuffer.Length) { Console.WriteLine("WARNING:Picture Too Big! " + picBytesExpected); - picbuffer = new byte[picBytesExpected]; + picbuffer = new byte[picBytesExpected]; } picBytesRecived = 0; - picChunkState = new bool[(picBytesExpected/1024)+1]; //calc based on size. + picChunkState = new bool[(picBytesExpected/1024)+1]; //calc based on size. picPieceState = new bool[(picChunkState.Length / 8)+1]; picExtraPackets = 0;//for debugging. picDownloading = true; @@ -617,7 +621,7 @@ private static void startListeners() sendAckFileDone((int)picBytesExpected); //HACK. - //Send file done cmdId to the update listener so it knows the picture is done. + //Send file done cmdId to the update listener so it knows the picture is done. //hack. onUpdate(100); //hack. @@ -648,7 +652,7 @@ private static void startListeners() } - //send command to listeners. + //send command to listeners. try { //fire update event. @@ -673,7 +677,8 @@ private static void startListeners() } }, token); //video server - var videoServer = new UdpListener(6038); + if (videoServer == null) + videoServer = new UdpListener(6038); //var videoServer = new UdpListener(new IPEndPoint(IPAddress.Parse("192.168.10.2"), 6038)); Task.Factory.StartNew(async () => { @@ -681,9 +686,9 @@ private static void startListeners() var started = false; while (true) - { + { try - { + { if (token.IsCancellationRequested)//handle canceling thread. break; var received = await videoServer.Receive(); @@ -695,17 +700,15 @@ private static void startListeners() started = true; } if (started) - { + { onVideoData(received.bytes); } - } - catch (Exception ex) - { + } catch (Exception ex) { Console.WriteLine("Video receive thread error:" + ex.Message); - + //dont disconnect(); -// break; + // break; } } }, token); @@ -765,8 +768,16 @@ private static void startHeartbeat() }, token); } + + public static void stopConnecting() { + masterCancelTokens.Cancel(); + } + public static void startConnecting() { + masterCancelTokens = new CancellationTokenSource(); + CancellationToken token = masterCancelTokens.Token; + //Thread to handle connecting. Task.Factory.StartNew(async () => { @@ -775,6 +786,13 @@ public static void startConnecting() { try { + if (token.IsCancellationRequested) { + if (cancelTokens != null) + cancelTokens.Cancel(); + break; + + } + switch (connectionState) { case ConnectionState.Disconnected: @@ -782,7 +800,7 @@ public static void startConnecting() lastMessageTime = DateTime.Now; startListeners(); - + break; case ConnectionState.Connecting: case ConnectionState.Connected: @@ -794,7 +812,7 @@ public static void startConnecting() } break; case ConnectionState.Paused: - lastMessageTime = DateTime.Now;//reset timeout so we have time to recover if enabled. + lastMessageTime = DateTime.Now;//reset timeout so we have time to recover if enabled. break; } Thread.Sleep(500); @@ -886,9 +904,9 @@ public static void sendControllerUpdate() }; //Create joystick packet from floating point axis. - //Center = 0.0. - //Up/Right =1.0. - //Down/Left=-1.0. + //Center = 0.0. + //Up/Right =1.0. + //Down/Left=-1.0. private static byte[] createJoyPacket(float fRx, float fRy, float fLx, float fLy, float speed) { //template joy packet. @@ -898,7 +916,7 @@ private static byte[] createJoyPacket(float fRx, float fRy, float fLx, float fLy short axis2 = (short)(660.0F * fRy + 1024.0F);//RightY down =364 up =-364 short axis3 = (short)(660.0F * fLy + 1024.0F);//LeftY down =364 up =-364 short axis4 = (short)(660.0F * fLx + 1024.0F);//LeftX left =364 right =-364 - short axis5 = (short)(660.0F * speed + 1024.0F);//Speed. + short axis5 = (short)(660.0F * speed + 1024.0F);//Speed. if (speed > 0.1f) axis5 = 0x7fff; @@ -911,7 +929,7 @@ private static byte[] createJoyPacket(float fRx, float fRy, float fLx, float fLy packet[13] = ((byte)(int)(packedAxis >> 32 & 0xFF)); packet[14] = ((byte)(int)(packedAxis >> 40 & 0xFF)); - //Add time info. + //Add time info. var now = DateTime.Now; packet[15] = (byte)now.Hour; packet[16] = (byte)now.Minute; @@ -921,7 +939,7 @@ private static byte[] createJoyPacket(float fRx, float fRy, float fLx, float fLy CRC.calcUCRC(packet, 4);//Not really needed. - //calc crc for packet. + //calc crc for packet. CRC.calcCrc(packet, packet.Length); return packet; @@ -1046,7 +1064,7 @@ public void set(byte[] data) //Parse some of the interesting info from the tello log stream public void parseLog(byte[] data) { - int pos = 0; + int pos = 0; //A packet can contain more than one record. while (pos < data.Length-2)//-2 for CRC bytes at end of packet. @@ -1205,4 +1223,3 @@ public override string ToString() } } - \ No newline at end of file