private static void startListeners()
{
cancelTokens = new CancellationTokenSource();
CancellationToken token = cancelTokens.Token;
//wait for reply messages from the tello and process.
Task.Factory.StartNew(async() =>
{
while (true)
{
try
{
if (token.IsCancellationRequested)//handle canceling thread.
{
break;
}
var received = await client.Receive();
lastMessageTime = DateTime.Now;//for timeouts
if (connectionState == ConnectionState.Connecting)
{
if (received.Message.StartsWith("conn_ack"))
{
connected = true;
connectionState = ConnectionState.Connected;
//send event
onConnection(connectionState);
startHeartbeat();
requestIframe();
//for(int i=74;i<80;i++)
//queryUnk(i);
//Console.WriteLine("Tello connected!");
continue;
}
}
int cmdId = ((int)received.bytes[5] | ((int)received.bytes[6] << 8));
if (cmdId >= 74 && cmdId < 80)
{
//Console.WriteLine("XXXXXXXXCMD:" + cmdId);
}
if (cmdId == 86)//state command
{
//update
var newState = new FlyData();
newState.set(received.bytes.Skip(9).ToArray());
try
{
//fire update event.
onUpdate(newState);
}
catch (Exception ex)
{
//Fixed. Update errors do not cause disconnect.
Console.WriteLine("onUpdate error:" + ex.Message);
//break;
}
//update current state.
state = newState;
}
if (cmdId == 4185)//att angle response
{
var array = received.bytes.Skip(10).Take(4).ToArray();
float f = BitConverter.ToSingle(array, 0);
Console.WriteLine(f);
}
if (cmdId == 4182)//max hei response
{
//var array = received.bytes.Skip(9).Take(4).Reverse().ToArray();
//float f = BitConverter.ToSingle(array, 0);
//Console.WriteLine(f);
if (received.bytes[10] != 10)
{
}
}
if (cmdId == 26)//wifi str command
{
wifiStrength = received.bytes[9];
if (received.bytes[10] != 0)//Disturb?
{
}
}
if (cmdId == 53)//light str command
{
}
if (cmdId == 98)
{
picFilePath = picPath + DateTime.Now.ToString("yyyy-dd-M--HH-mm-ss") + ".jpg";
var start = 9;
var ftype = received.bytes[start];
start += 1;
picBytesExpected = BitConverter.ToUInt32(received.bytes, start);
if (picBytesExpected > picbuffer.Length)
{
Console.WriteLine("WARNING:Picture Too Big! " + picBytesExpected);
picbuffer = new byte[picBytesExpected];
}
picBytesRecived = 0;
picChunkState = new bool[(picBytesExpected / 1024) + 1]; //calc based on size.
picPieceState = new bool[(picChunkState.Length / 8) + 1];
picExtraPackets = 0; //for debugging.
sendAckFileSize();
}
if (cmdId == 99)//jpeg
{
//var dataStr = BitConverter.ToString(received.bytes.Skip(0).Take(30).ToArray()).Replace("-", " ");
var start = 9;
var fileNum = BitConverter.ToUInt16(received.bytes, start);
start += 2;
var pieceNum = BitConverter.ToUInt32(received.bytes, start);
start += 4;
var seqNum = BitConverter.ToUInt32(received.bytes, start);
start += 4;
var size = BitConverter.ToUInt16(received.bytes, start);
start += 2;
maxPieceNum = Math.Max((int)pieceNum, maxPieceNum);
if (!picChunkState[seqNum])
{
Array.Copy(received.bytes, start, picbuffer, seqNum * 1024, size);
picBytesRecived += size;
picChunkState[seqNum] = true;
for (int p = 0; p < picChunkState.Length / 8; p++)
{
var done = true;
for (int s = 0; s < 8; s++)
{
if (!picChunkState[(p * 8) + s])
{
done = false;
break;
}
}
if (done && !picPieceState[p])
{
picPieceState[p] = true;
sendAckFilePiece(0, fileNum, (UInt32)p);
//Console.WriteLine("\nACK PN:" + p + " " + seqNum);
}
}
if (picFilePath != null && picBytesRecived >= picBytesExpected)
{
sendAckFilePiece(1, 0, (UInt32)maxPieceNum);//todo. Double check this. finalize
sendAckFileDone((int)picBytesExpected);
Console.WriteLine("\nDONE PN:" + pieceNum + " max: " + maxPieceNum);
//Save raw data minus sequence.
using (var stream = new FileStream(picFilePath, FileMode.Append))
{
stream.Write(picbuffer, 0, (int)picBytesExpected);
}
}
}
else
{
picExtraPackets++;//for debugging.
//if(picBytesRecived >= picBytesExpected)
// Console.WriteLine("\nEXTRA PN:"+pieceNum+" max "+ maxPieceNum);
}
}
if (cmdId == 100)
{
}
}
catch (Exception ex)
{
Console.WriteLine("Receive thread error:" + ex.Message);
disconnect();
break;
}
}
}, token);
//video server
var 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();
if (received.bytes[2] == 0 && received.bytes[3] == 0 && received.bytes[4] == 0 && received.bytes[5] == 1)//Wait for first NAL
{
var nal = (received.bytes[6] & 0x1f);
//if (nal != 0x01 && nal!=0x07 && nal != 0x08 && nal != 0x05)
// Console.WriteLine("NAL type:" +nal);
started = true;
}
if (started)
{
onVideoData(received.bytes);
}
}
catch (Exception ex)
{
Console.WriteLine("Video receive thread error:" + ex.Message);
//dont disconnect();
break;
}
}
}, token);
}
private static void updateListeners()
{
try {
var received = client.Receive();
if (received.bytes == null)
{
return;
}
lastMessageTime = DateTime.Now; //for timeouts
if (connectionState == ConnectionState.Connecting)
{
if (received.Message.StartsWith("conn_ack"))
{
connected = true;
connectionState = ConnectionState.Connected;
//send event
onConnection(connectionState);
//startVideoServer();
requestIframe();
//for(int i=74;i<80;i++)
//queryUnk(i);
//Debug.Log("Tello connected!");
return;
}
}
int cmdId = ((int)received.bytes[5] | ((int)received.bytes[6] << 8));
if (cmdId >= 74 && cmdId < 80)
{
//Debug.Log("XXXXXXXXCMD:" + cmdId);
}
if (cmdId == 86) //state command
{
//update
var newState = new FlyData();
newState.set(received.bytes.Skip(9).ToArray());
try {
//fire update event.
onUpdate(newState);
} catch (Exception ex) {
//Fixed. Update errors do not cause disconnect.
Debug.Log("onUpdate error:" + ex.Message);
//break;
}
//update current state.
state = newState;
}
if (cmdId == 4185) //att angle response
{
var array = received.bytes.Skip(10).Take(4).ToArray();
float f = BitConverter.ToSingle(array, 0);
Debug.Log(f);
}
if (cmdId == 4182) //max hei response
{
//var array = received.bytes.Skip(9).Take(4).Reverse().ToArray();
//float f = BitConverter.ToSingle(array, 0);
//Debug.Log(f);
if (received.bytes[10] != 10)
{
}
}
if (cmdId == 26) //wifi str command
{
wifiStrength = received.bytes[9];
if (received.bytes[10] != 0) //Disturb?
{
}
}
if (cmdId == 53) //light str command
{
}
if (cmdId == 98)
{
picFilePath = picPath + DateTime.Now.ToString("yyyy-dd-M--HH-mm-ss") + ".jpg";
var start = 9;
var ftype = received.bytes[start];
start += 1;
picBytesExpected = BitConverter.ToUInt32(received.bytes, start);
if (picBytesExpected > picbuffer.Length)
{
Debug.Log("WARNING:Picture Too Big! " + picBytesExpected);
picbuffer = new byte[picBytesExpected];
}
picBytesRecived = 0;
picChunkState = new bool[(picBytesExpected / 1024) + 1]; //calc based on size.
picPieceState = new bool[(picChunkState.Length / 8) + 1];
picExtraPackets = 0; //for debugging.
sendAckFileSize();
}
if (cmdId == 99) //jpeg
{
//var dataStr = BitConverter.ToString(received.bytes.Skip(0).Take(30).ToArray()).Replace("-", " ");
var start = 9;
var fileNum = BitConverter.ToUInt16(received.bytes, start);
start += 2;
var pieceNum = BitConverter.ToUInt32(received.bytes, start);
start += 4;
var seqNum = BitConverter.ToUInt32(received.bytes, start);
start += 4;
var size = BitConverter.ToUInt16(received.bytes, start);
start += 2;
maxPieceNum = Math.Max((int)pieceNum, maxPieceNum);
if (!picChunkState[seqNum])
{
Array.Copy(received.bytes, start, picbuffer, seqNum * 1024, size);
picBytesRecived += size;
picChunkState[seqNum] = true;
for (int p = 0; p < picChunkState.Length / 8; p++)
{
var done = true;
for (int s = 0; s < 8; s++)
{
if (!picChunkState[(p * 8) + s])
{
done = false;
break;
}
}
if (done && !picPieceState[p])
{
picPieceState[p] = true;
sendAckFilePiece(0, fileNum, (UInt32)p);
//Debug.Log("\nACK PN:" + p + " " + seqNum);
}
}
if (picFilePath != null && picBytesRecived >= picBytesExpected)
{
sendAckFilePiece(1, 0, (UInt32)maxPieceNum); //todo. Double check this. finalize
sendAckFileDone((int)picBytesExpected);
Debug.Log("\nDONE PN:" + pieceNum + " max: " + maxPieceNum);
//Save raw data minus sequence.
using (var stream = new FileStream(picFilePath, FileMode.Append)) {
stream.Write(picbuffer, 0, (int)picBytesExpected);
}
}
}
else
{
picExtraPackets++; //for debugging.
//if(picBytesRecived >= picBytesExpected)
// Debug.Log("\nEXTRA PN:"+pieceNum+" max "+ maxPieceNum);
}
}
if (cmdId == 100)
{
}
} catch (Exception ex) {
Debug.Log("Receive thread error:" + ex.Message);
disconnect();
}
}