private static void startVideoServer()
{
CancellationToken token = cancelTokens.Token;
//video server
videoServerTask = Task.Factory.StartNew(async() => {
var videoServer = new UdpListener(6038);
//Debug.Log("video:1");
var started = false;
int fragmentIndex = 0;
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)
// Debug.Log("NAL type:" +nal);
started = true;
fragmentIndex = 0;
}
else
{
fragmentIndex++;
}
if (started)
{
onVideoData(fragmentIndex, received.bytes);
}
} catch (Exception ex) {
Debug.Log("Video receive thread error:" + ex.Message);
//dont disconnect();
break;
}
}
}, token);
}
private void StartVideoServer(CancellationToken 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?.Invoke(received.Bytes);
}
}
catch (Exception ex)
{
Log(ex, "Video receive thread error:");
}
}
}, token);
}
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
state.set(received.bytes.Skip(9).ToArray());
}
if (cmdId == 4176)//log header
{
//just ack.
var id = BitConverter.ToUInt16(received.bytes, 9);
sendAckLog((short)cmdId, id);
//Console.WriteLine(id);
}
if (cmdId == 4177)//log data
{
state.parseLog(received.bytes.Skip(10).ToArray());
}
if (cmdId == 4178)//log config
{
//todo. this doesnt seem to be working.
//var id = BitConverter.ToUInt16(received.bytes, 9);
//var n2 = BitConverter.ToInt32(received.bytes, 11);
//sendAckLogConfig((short)cmdId, id,n2);
//var dataStr = BitConverter.ToString(received.bytes.Skip(14).Take(10).ToArray()).Replace("-", " ")/*+" "+pos*/;
//Console.WriteLine(dataStr);
}
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)//start jpeg.
{
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.
picDownloading = true;
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)
{
picDownloading = false;
sendAckFilePiece(1, 0, (UInt32)maxPieceNum);//todo. Double check this. finalize
sendAckFileDone((int)picBytesExpected);
//HACK.
//Send file done cmdId to the update listener so it knows the picture is done.
//hack.
onUpdate(100);
//hack.
//This is a hack because it is faking a message. And not a very good fake.
//HACK.
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)
{
}
//send command to listeners.
try
{
//fire update event.
onUpdate(cmdId);
}
catch (Exception ex)
{
//Fixed. Update errors do not cause disconnect.
Console.WriteLine("onUpdate error:" + ex.Message);
//break;
}
}
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);
}
