Пример #1
0
        protected void GenerateDroneCommand()
        {
            lock (ThisLock)
            {
                if (FlyVector.IsNull())
                {
                    return;
                }

                CommandTuple cmdTuple = new CommandTuple(1, 0, 2);
                int          roll     = Clamp(FlyVector.Roll, VectorMin, VectorMax);
                int          pitch    = Clamp(FlyVector.Pitch, VectorMin, VectorMax);
                int          yaw      = Clamp(FlyVector.Yaw, VectorMin, VectorMax);
                int          gaz      = Clamp(FlyVector.Gaz, VectorMin, VectorMax);

                CommandParam cmdParam = new CommandParam();

                cmdParam.AddData((byte)FlyVector.Flag);
                cmdParam.AddData((sbyte)roll);
                cmdParam.AddData((sbyte)pitch);
                cmdParam.AddData((sbyte)yaw);
                cmdParam.AddData((sbyte)gaz);
                cmdParam.AddData((uint)0);

                SendSinglePcmd(cmdTuple, cmdParam);

                Thread.Sleep(100);
            }
        }
Пример #2
0
        protected bool?SendParam(CommandTuple cmdTuple, CommandParam cmdParam, bool ack = true)
        {
            // ReSharper disable once InconsistentNaming
            string ACK = ack ? "SEND_WITH_ACK" : "SEND_NO_ACK";
            // ReSharper disable once InconsistentNaming
            string DataACK = ack ? "DATA_WITH_ACK" : "DATA_NO_ACK";
            // ReSharper disable once StringLiteralTypo
            string fmt = "<BBBIBBH" + cmdParam.Format();

            SequenceCounter[ACK] = (SequenceCounter[ACK] + 1) % 256;

            Command cmd = new Command();

            cmd.InsertData((byte)DataTypesByName[DataACK]);
            cmd.InsertData((byte)BufferIds[ACK]);
            cmd.InsertData((byte)SequenceCounter[ACK]);
            cmd.InsertData((uint)StructConverter.PacketSize(fmt));

            cmd.InsertTuple(cmdTuple);
            cmd.InsertParam(cmdParam);

            if (ack)
            {
                return(SendCommandAck(cmd.Export(fmt), SequenceCounter["SEND_WITH_ACK"]));
            }

            SendCommandNoAck(cmd.Export(fmt));
            return(null);
        }
Пример #3
0
        public void Land(int timeout)
        {
            // We do not want to fly while landing
            lock (ThisLock)
            {
                FlyVector.ResetVector();

                Logger.Debug("Landing...");
                CommandTuple cmdTuple = new CommandTuple(1, 0, 3);

                SendNoParam(cmdTuple);

                Stopwatch sw = new Stopwatch();
                sw.Start();
                while (FlyingState.GetState() != FlyingState.State.Landed && sw.ElapsedMilliseconds < timeout)
                {
                    if (FlyingState.GetState() == FlyingState.State.Emergency)
                    {
                        break;
                    }

                    SmartSleep(100);
                }

                sw.Stop();
            }
        }
Пример #4
0
        public void TakeOff(int timeout)
        {
            Logger.Debug("Performing takeoff...");
            CommandTuple cmdTuple = new CommandTuple(1, 0, 1);

            if (FlyingState.GetState() == FlyingState.State.Landed || FlyingState.GetState() == FlyingState.State.UnKn0wn)
            {
                SendNoParam(cmdTuple);
            }
            else
            {
                return;
            }

            Stopwatch sw = new Stopwatch();

            sw.Start();
            while (FlyingState.GetState() != FlyingState.State.Hovering && FlyingState.GetState() != FlyingState.State.Flying && sw.ElapsedMilliseconds < timeout)
            {
                if (FlyingState.GetState() == FlyingState.State.Emergency)
                {
                    break;
                }

                SmartSleep(100);
            }

            sw.Stop();
        }
Пример #5
0
        public void StopVideo()
        {
            CommandTuple cmdTuple = new CommandTuple(1, 21, 0);
            CommandParam cmdParam = new CommandParam();

            cmdParam.AddData((byte)0);  // Disable Video

            SendParam(cmdTuple, cmdParam);
        }
Пример #6
0
        protected bool SendNoParam(CommandTuple cmdTuple)
        {
            SequenceCounter["SEND_WITH_ACK"] = (SequenceCounter["SEND_WITH_ACK"] + 1) % 256;
            // ReSharper disable once StringLiteralTypo
            const string fmt = "<BBBIBBH";

            Command cmd = new Command();

            cmd.InsertData((byte)DataTypesByName["DATA_WITH_ACK"]);
            cmd.InsertData((byte)BufferIds["SEND_WITH_ACK"]);
            cmd.InsertData((byte)SequenceCounter["SEND_WITH_ACK"]);
            cmd.InsertData((uint)StructConverter.PacketSize(fmt));
            cmd.InsertTuple(cmdTuple);

            return(SendCommandAck(cmd.Export(fmt), SequenceCounter["SEND_WITH_ACK"]));
        }
Пример #7
0
        // ReSharper disable once IdentifierTypo
        protected bool SendSinglePcmd(CommandTuple cmdTuple, CommandParam cmdParam)
        {
            SequenceCounter["SEND_NO_ACK"] = (SequenceCounter["SEND_NO_ACK"] + 1) % 256;
            // ReSharper disable once StringLiteralTypo
            string fmt = "<BBBIBBH" + cmdParam.Format();

            Command cmd = new Command();

            cmd.InsertData((byte)DataTypesByName["DATA_NO_ACK"]);
            cmd.InsertData((byte)BufferIds["SEND_NO_ACK"]);
            cmd.InsertData((byte)SequenceCounter["SEND_NO_ACK"]);
            cmd.InsertData((uint)StructConverter.PacketSize(fmt));

            cmd.InsertTuple(cmdTuple);
            cmd.InsertParam(cmdParam);

            return(SafeSend(cmd.Export(fmt)));
        }
Пример #8
0
        public void FlatTrim(int duration)
        {
            if (duration > 0)
            {
                FlatTrimChanged.Reset();
            }

            CommandTuple cmdTuple = new CommandTuple(1, 0, 0);

            SendNoParam(cmdTuple);

            // Wait for FlatTrimChanged  or duration
            if (duration > 0)
            {
                Stopwatch sw = new Stopwatch();
                sw.Start();
                while (!FlatTrimChanged.Updated && sw.ElapsedMilliseconds < duration)
                {
                    SmartSleep(100);
                }

                sw.Stop();
            }
        }
Пример #9
0
        public void AskForStateUpdate()
        {
            CommandTuple cmdTuple = new CommandTuple(0, 4, 0);

            SendNoParam(cmdTuple);
        }