Skip to content

Add Unity compatibility for PR #18

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
comoc wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Add Unity compatibility for PR #18

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
77 changes: 47 additions & 30 deletions TelloLib/Tello.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
using System;
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
Expand All @@ -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
Expand All @@ -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 };
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -401,7 +403,7 @@ private static void disconnect()
}

connectionState = ConnectionState.Disconnected;

}
private static void connect()
{
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -648,7 +652,7 @@ private static void startListeners()

}

//send command to listeners.
//send command to listeners.
try
{
//fire update event.
Expand All @@ -673,17 +677,18 @@ 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 () => {
//Console.WriteLine("video:1");
var started = false;

while (true)
{
{
try
{
{
if (token.IsCancellationRequested)//handle canceling thread.
break;
var received = await videoServer.Receive();
Expand All @@ -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);
Expand Down Expand Up @@ -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 () =>
{
Expand All @@ -775,14 +786,21 @@ public static void startConnecting()
{
try
{
if (token.IsCancellationRequested) {
if (cancelTokens != null)
cancelTokens.Cancel();
break;

}

switch (connectionState)
{
case ConnectionState.Disconnected:
connect();
lastMessageTime = DateTime.Now;

startListeners();

break;
case ConnectionState.Connecting:
case ConnectionState.Connected:
Expand All @@ -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);
Expand Down Expand Up @@ -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.
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -1205,4 +1223,3 @@ public override string ToString()

}
}