MissionPlanner.MAVLinkInterface.doCommand C# (CSharp) Method

        public bool doCommand(byte sysid, byte compid, MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7, bool requireack = true)
        {
            giveComport = true;
            MAVLinkMessage buffer;

            mavlink_command_long_t req = new mavlink_command_long_t();

                req.target_system = sysid;
                req.target_component = compid;

            req.command = (ushort) actionid;

            req.param1 = p1;
            req.param2 = p2;
            req.param3 = p3;
            req.param4 = p4;
            req.param5 = p5;
            req.param6 = p6;
            req.param7 = p7;

            log.InfoFormat("doCommand cmd {0} {1} {2} {3} {4} {5} {6} {7}", actionid.ToString(), p1, p2, p3, p4, p5, p6,
                p7);

            generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req, sysid, compid);

            if (!requireack)
            {
                giveComport = false;
                return true;
            }

            DateTime GUI = DateTime.Now;

            DateTime start = DateTime.Now;
            int retrys = 3;

            int timeout = 2000;

            // imu calib take a little while
            if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION && p5 == 1)
            {
                // this is for advanced accel offsets, and blocks execution
                giveComport = false;
                return true;
            }
            else if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION)
            {
                retrys = 1;
                timeout = 25000;
            }
            else if (actionid == MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN)
            {
                generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req, sysid, compid);
                giveComport = false;
                return true;
            }
            else if (actionid == MAV_CMD.COMPONENT_ARM_DISARM)
            {
                // 10 seconds as may need an imu calib
                timeout = 10000;
            }
            else if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION && p6 == 1)
            {
                // compassmot
                // send again just incase
                generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req, sysid, compid);
                giveComport = false;
                return true;
            }
            else if (actionid == MAV_CMD.GET_HOME_POSITION)
            {
                giveComport = false;
                return true;
            }

            while (true)
            {
                if (DateTime.Now > GUI.AddMilliseconds(100))
                {
                    GUI = DateTime.Now;

                    if (!MainV2.instance.InvokeRequired)
                    {
                        try
                        {
                            MainV2.instance.Invalidate();
                            MainV2.instance.FlightData.Invalidate();
                            MainV2.instance.Update();
                        }
                        catch { }
                    }
                }

                if (!(start.AddMilliseconds(timeout) > DateTime.Now))
                {
                    if (retrys > 0)
                    {
                        log.Info("doCommand Retry " + retrys);
                        generatePacket((byte) MAVLINK_MSG_ID.COMMAND_LONG, req, sysid, compid);
                        start = DateTime.Now;
                        retrys--;
                        continue;
                    }
                    giveComport = false;
                    throw new TimeoutException("Timeout on read - doCommand");
                }

                buffer = readPacket();
                if (buffer.Length > 5)
                {
                    if (buffer.msgid == (byte) MAVLINK_MSG_ID.COMMAND_ACK && buffer.sysid == sysid && buffer.compid == compid)
                    {
                        var ack = buffer.ToStructure<mavlink_command_ack_t>();

                        if (ack.command != req.command)
                        {
                            log.InfoFormat("doCommand cmd resp {0} - {1} - Commands dont match", (MAV_CMD)ack.command, (MAV_RESULT)ack.result);
                            continue;
                        }

                        log.InfoFormat("doCommand cmd resp {0} - {1}", (MAV_CMD)ack.command, (MAV_RESULT)ack.result);

                        if (ack.result == (byte) MAV_RESULT.ACCEPTED)
                        {
                            giveComport = false;
                            return true;
                        }
                        else
                        {
                            giveComport = false;
                            return false;
                        }
                    }
                }
            }
        }