Exemplo n.º 1
0
        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);
        }
Exemplo n.º 2
0
        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();
            }
        }