Пример #1
0
        public double p4;                               // param 4

        /// <summary>
        /// this constructor is used only for reading mission files
        /// </summary>
        /// <param name="lw"></param>
        internal LocationWp(Locationwp lw)
        {
            waypointState = WaypointState.None;

            number = lw.number;
            id     = (MAV_CMD)Enum.Parse(typeof(MAV_CMD), lw.id.ToString()); // command id

            isHome = lw.ishome != 0;

            coordinateFrameOption = lw.options == 1 ? CoordinateFrameOption.MAV_FRAME_GLOBAL_RELATIVE_ALT : CoordinateFrameOption.MAV_FRAME_GLOBAL;

            // alt is in meters, can be above the ground (AGL) or above mean sea level (MSL), depending on coordinateFrameOption
            geoPosition = new GeoPosition(lw.lng, lw.lat, lw.alt);

            // p1-p4 are just float point numbers that can be added to waypoints and be interpreted by behaviors. We pass them all directly.
            p1 = lw.p1;
            p2 = lw.p2;
            p3 = lw.p3;
            p4 = lw.p4;
        }
Пример #2
0
        /// <summary>
        /// this constructor is used only for reading mission files
        /// </summary>
        /// <param name="lw"></param>
        internal LocationWp(Locationwp lw)
        {
            waypointState = WaypointState.None;

            number = lw.number;
            id = (MAV_CMD)Enum.Parse(typeof(MAV_CMD), lw.id.ToString());    // command id

            isHome = lw.ishome != 0;

            coordinateFrameOption = lw.options == 1 ? CoordinateFrameOption.MAV_FRAME_GLOBAL_RELATIVE_ALT : CoordinateFrameOption.MAV_FRAME_GLOBAL;

            // alt is in meters, can be above the ground (AGL) or above mean sea level (MSL), depending on coordinateFrameOption
            geoPosition = new GeoPosition(lw.lng, lw.lat, lw.alt);

            // p1-p4 are just float point numbers that can be added to waypoints and be interpreted by behaviors. We pass them all directly.
            p1 = lw.p1;
            p2 = lw.p2;
            p3 = lw.p3;
            p4 = lw.p4;
        }
Пример #3
0
        public bool doCommand(MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
        {
            giveComport = true;
            MAVLinkMessage buffer;

            mavlink_command_long_t req = new mavlink_command_long_t();

                req.target_system = MAV.sysid;
                req.target_component = MAV.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);

            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
                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);
                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);
                giveComport = false;
                return true;
            }
            else if (actionid == MAV_CMD.GET_HOME_POSITION)
            {
                return true;
            }

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

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


                        if (ack.result == (byte) MAV_RESULT.ACCEPTED)
                        {
                            giveComport = false;
                            return true;
                        }
                        else
                        {
                            giveComport = false;
                            return false;
                        }
                    }
                }
            }
        }
Пример #4
0
        public bool doCommand(MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
        {

            MainV2.givecomport = true;
            byte[] 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;

            generatePacket(MAVLINK_MSG_ID_COMMAND_LONG, req);

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

            int timeout = 2000;

            // imu calib take a little while
            if (actionid == MAV_CMD.PREFLIGHT_CALIBRATION)
            {
                retrys = 1;
                timeout = 6000;
            }

            while (true)
            {
                if (!(start.AddMilliseconds(timeout) > DateTime.Now))
                {
                    if (retrys > 0)
                    {
                        Console.WriteLine("doAction Retry " + retrys);
                        generatePacket(MAVLINK_MSG_ID_COMMAND_LONG, req);
                        start = DateTime.Now;
                        retrys--;
                        continue;
                    }
                    MainV2.givecomport = false;
                    throw new Exception("Timeout on read - doAction");
                }

                buffer = readPacket();
                if (buffer.Length > 5)
                {
                    if (buffer[5] == MAVLINK_MSG_ID_COMMAND_ACK)
                    {
                        __mavlink_command_ack_t ack = new __mavlink_command_ack_t();

                        object temp = (object)ack;

                        ByteArrayToStructure(buffer, ref temp, 6);

                        ack = (__mavlink_command_ack_t)(temp);

                        if (ack.result == (byte)MAV_RESULT.MAV_RESULT_ACCEPTED)
                        {
                            MainV2.givecomport = false;
                            return true;
                        }
                        else
                        {
                            MainV2.givecomport = false;
                            return false;
                        }
                    }
                }
            }
#else
            MainV2.givecomport = true;
            byte[] buffer;

            __mavlink_waypoint_set_current_t req = new __mavlink_waypoint_set_current_t();

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

            generatePacket(MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, req);

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

            while (true)
            {
                if (!(start.AddMilliseconds(2000) > DateTime.Now))
                {
                    if (retrys > 0)
                    {
                        Console.WriteLine("setWPCurrent Retry " + retrys);
                        generatePacket(MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, req);
                        start = DateTime.Now;
                        retrys--;
                        continue;
                    }
                    MainV2.givecomport = false;
                    throw new Exception("Timeout on read - setWPCurrent");
                }

                buffer = readPacket();
                if (buffer.Length > 5)
                {
                    if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_CURRENT)
                    {
                        MainV2.givecomport = false;
                        return true;
                    }
                }
            }
        }
Пример #5
0
        public bool doCommand(MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
        {
            giveComport = true;
            byte[] buffer;

            mavlink_command_long_t req = new mavlink_command_long_t();

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

            if (actionid == MAV_CMD.COMPONENT_ARM_DISARM)
            {
                req.target_component = (byte)MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;
            }

            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;

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

            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
                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);
                giveComport = false;
                return true;
            }
            else if (actionid == MAV_CMD.COMPONENT_ARM_DISARM)
            {
                // 10 seconds as may need an imu calib
                timeout = 10000;
            }

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

                buffer = readPacket();
                if (buffer.Length > 5)
                {
                    if (buffer[5] == (byte)MAVLINK_MSG_ID.COMMAND_ACK)
                    {

                        var ack = buffer.ByteArrayToStructure<mavlink_command_ack_t>(6);

                        if (ack.result == (byte)MAV_RESULT.ACCEPTED)
                        {
                            giveComport = false;
                            return true;
                        }
                        else
                        {
                            giveComport = false;
                            return false;
                        }
                    }
                }
            }
        }
Пример #6
0
 public bool doCommand(MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7,
     bool requireack = true)
 {
     return doCommand(MAV.sysid, MAV.compid, actionid, p1, p2, p3, p4, p5, p6, p7, requireack);
 }