C# Utilities.Locationwp类代码示例

        static object Convert(Locationwp cmd, bool isint = false)
        {
            if (isint)
            {
                var temp = new MAVLink.mavlink_mission_item_int_t()
                {
                    command = cmd.id,
                    param1 = cmd.p1,
                    param2 = cmd.p2,
                    param3 = cmd.p3,
                    param4 = cmd.p4,
                    x = (int)(cmd.lat * 1.0e7),
                    y = (int)(cmd.lng * 1.0e7),
                    z = (float) cmd.alt
                };

                return temp;
            }
            else
            {
                var temp = new MAVLink.mavlink_mission_item_t()
                {
                    command = cmd.id,
                    param1 = cmd.p1,
                    param2 = cmd.p2,
                    param3 = cmd.p3,
                    param4 = cmd.p4,
                    x = (float) cmd.lat,
                    y = (float) cmd.lng,
                    z = (float) cmd.alt
                };

                return temp;
            }
        }

        /*
        calculate lookahead rise in terrain. This returns extra altitude
        needed to clear upcoming terrain in meters
        */

        float lookahead(Locationwp loc, float bearing, float distance, float climb_ratio)
        {
            if (!enable || grid_spacing <= 0)
            {
                return 0;
            }

            //Locationwp loc;
            //if (!ahrs.get_position(loc))
            {
                // we don't know where we are
                //return 0;
            }
            float base_height = 0;
            if (!height_amsl(loc, ref base_height))
            {
                // we don't know our current terrain height
                return 0;
            }

            float climb = 0;
            float lookahead_estimate = 0;

            // check for terrain at grid spacing intervals
            while (distance > 0)
            {
                location_update(loc, bearing, grid_spacing);
                climb += climb_ratio*grid_spacing;
                distance -= grid_spacing;
                float height = 0;
                if (height_amsl(loc, ref height))
                {
                    float rise = (height - base_height) - climb;
                    if (rise > lookahead_estimate)
                    {
                        lookahead_estimate = rise;
                    }
                }
            }

            return lookahead_estimate;
        }

        private void updateLocationLabel(Locationwp plla)
        {
            if (!Instance.IsDisposed)
            {
                Instance.BeginInvoke((MethodInvoker)delegate
                {
                    Instance.LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt + " " + gotolocation.Tag;
                }
           );
            }

        }

 private void updateLocationLabel(Locationwp plla)
 {
     this.BeginInvoke((MethodInvoker)delegate
      {
          LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt +" "+ gotolocation.Tag;
     }
     );
 }

        public void setGuidedModeWP(Locationwp gotohere)
        {
            if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0)
                return;

            giveComport = true;

            try
            {
                gotohere.id = (byte) MAV_CMD.WAYPOINT;

                // fix for followme change
                if (MAV.cs.mode.ToUpper() != "GUIDED")
                    setMode("GUIDED");

                MAV_MISSION_RESULT ans = setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte) 2);

                if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
                    throw new Exception("Guided Mode Failed");
            }
            catch (Exception ex)
            {
                log.Error(ex);
            }

            giveComport = false;
        }

        /// <summary>
        /// Gets specfied WP
        /// </summary>
        /// <param name="index"></param>
        /// <returns>WP</returns>
        public Locationwp getWP(ushort index)
        {
            while (giveComport)
                System.Threading.Thread.Sleep(100);

            bool use_int = (MAV.cs.capabilities & MAV_PROTOCOL_CAPABILITY.MISSION_INT) > 0;

            object req;

            if (use_int)
            {
                mavlink_mission_request_int_t reqi = new mavlink_mission_request_int_t();

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

                reqi.seq = index;
                
                // request
                generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST_INT, reqi);

                req = reqi;
            }
            else
            {
                mavlink_mission_request_t reqf = new mavlink_mission_request_t();

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

                reqf.seq = index;

                // request
                generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, reqf);

                req = reqf;
            }

            giveComport = true;
            Locationwp loc = new Locationwp();

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

            while (true)
            {
                if (!(start.AddMilliseconds(3500) > DateTime.Now)) // apm times out after 5000ms
                {
                    if (retrys > 0)
                    {
                        log.Info("getWP Retry " + retrys);
                        if (use_int)
                            generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST_INT, req);
                        else
                            generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, req);
                        start = DateTime.Now;
                        retrys--;
                        continue;
                    }
                    giveComport = false;
                    throw new TimeoutException("Timeout on read - getWP");
                }
                //Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
                MAVLinkMessage buffer = readPacket();
                //Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
                if (buffer.Length > 5)
                {
                    if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_ITEM)
                    {
                        //Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);

                        var wp = buffer.ToStructure<mavlink_mission_item_t>();

                        // received a packet, but not what we requested
                        if (index != wp.seq)
                        {
                            generatePacket((byte) MAVLINK_MSG_ID.MISSION_REQUEST, req);
                            continue;
                        }

                        loc.options = (byte) (wp.frame);
                        loc.id = (byte) (wp.command);
                        loc.p1 = (wp.param1);
                        loc.p2 = (wp.param2);
                        loc.p3 = (wp.param3);
                        loc.p4 = (wp.param4);

                        loc.alt = ((wp.z));
                        loc.lat = ((wp.x));
                        loc.lng = ((wp.y));

                        log.InfoFormat("getWP {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng,
                            loc.options);

                        break;
//.........这里部分代码省略.........

        private void goHereToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (!MainV2.comPort.BaseStream.IsOpen)
            {
                CustomMessageBox.Show(Strings.PleaseConnect, Strings.ERROR);
                return;
            }

            if (MainV2.comPort.MAV.GuidedMode.z == 0)
            {
                flyToHereAltToolStripMenuItem_Click(null, null);

                if (MainV2.comPort.MAV.GuidedMode.z == 0)
                    return;
            }

            if (MouseDownStart.Lat == 0 || MouseDownStart.Lng == 0)
            {
                CustomMessageBox.Show(Strings.BadCoords, Strings.ERROR);
                return;
            }

            Locationwp gotohere = new Locationwp();

            gotohere.id = (byte) MAVLink.MAV_CMD.WAYPOINT;
            gotohere.alt = MainV2.comPort.MAV.GuidedMode.z; // back to m
            gotohere.lat = (MouseDownStart.Lat);
            gotohere.lng = (MouseDownStart.Lng);

            try
            {
                MainV2.comPort.setGuidedModeWP(gotohere);
            }
            catch (Exception ex)
            {
                MainV2.comPort.giveComport = false;
                CustomMessageBox.Show(Strings.CommandFailed + ex.Message, Strings.ERROR);
            }
        }

        private void goHereToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (!MainV2.comPort.BaseStream.IsOpen)
            {
                CustomMessageBox.Show("Please Connect First");
                return;
            }

            if (MainV2.comPort.MAV.GuidedMode.z == 0)
            {
                flyToHereAltToolStripMenuItem_Click(null, null);

                if (MainV2.comPort.MAV.GuidedMode.z == 0)
                    return;
            }

            if (gotolocation.Lat == 0 || gotolocation.Lng == 0)
            {
                CustomMessageBox.Show("Bad Lat/Long");
                return;
            }

            Locationwp gotohere = new Locationwp();

            gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
            gotohere.alt = (float)(MainV2.comPort.MAV.GuidedMode.z); // back to m
            gotohere.lat = (gotolocation.Lat);
            gotohere.lng = (gotolocation.Lng);

            try
            {
                MainV2.comPort.setGuidedModeWP(gotohere);
            }
            catch (Exception ex) { MainV2.comPort.giveComport = false; CustomMessageBox.Show("Error sending command : " + ex.Message); }
        }

        /// <summary>
        /// Gets specfied WP
        /// </summary>
        /// <param name="index"></param>
        /// <returns>WP</returns>
        public Locationwp getWP(ushort index)
        {
            while (giveComport)
                System.Threading.Thread.Sleep(100);

            giveComport = true;
            Locationwp loc = new Locationwp();
            mavlink_mission_request_t req = new mavlink_mission_request_t();

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

            req.seq = index;

            //Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);

            // request
            generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, req);

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

            while (true)
            {
                if (!(start.AddMilliseconds(800) > DateTime.Now)) // apm times out after 1000ms
                {
                    if (retrys > 0)
                    {
                        log.Info("getWP Retry " + retrys);
                        generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, req);
                        start = DateTime.Now;
                        retrys--;
                        continue;
                    }
                    giveComport = false;
                    throw new Exception("Timeout on read - getWP");
                }
                //Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
                byte[] buffer = readPacket();
                //Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
                if (buffer.Length > 5)
                {
                    if (buffer[5] == (byte)MAVLINK_MSG_ID.MISSION_ITEM)
                    {
                        //Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);

                        var wp = buffer.ByteArrayToStructure<mavlink_mission_item_t>(6);

                        // received a packet, but not what we requested
                        if (req.seq != wp.seq)
                        {
                            generatePacket((byte)MAVLINK_MSG_ID.MISSION_REQUEST, req);
                            continue;
                        }

                        loc.options = (byte)(wp.frame);
                        loc.id = (byte)(wp.command);
                        loc.p1 = (wp.param1);
                        loc.p2 = (wp.param2);
                        loc.p3 = (wp.param3);
                        loc.p4 = (wp.param4);

                        loc.alt = ((wp.z));
                        loc.lat = ((wp.x));
                        loc.lng = ((wp.y));
                       
                        log.InfoFormat("getWP {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng, loc.options);

                        break;
                    }
                    else
                    {
                        log.Info(DateTime.Now + " PC getwp " + buffer[5]);
                    }
                }
            }
            giveComport = false;
            return loc;
        }

        void mainloop()
        {
            DateTime nextsend = DateTime.Now;

            StreamWriter sw = new StreamWriter(File.OpenWrite("followmeraw.txt"));

            threadrun = true;
            while (threadrun)
            {
                try
                {
                    string line = comPort.ReadLine();

                    sw.WriteLine(line);

                    //string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", "");
                    if (line.StartsWith("$GPGGA")) // 
                    {
                        string[] items = line.Trim().Split(',', '*');

                        if (items[15] != GetChecksum(line.Trim()))
                        {
                            Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim()));
                            continue;
                        }

                        if (items[6] == "0")
                        {
                            Console.WriteLine("No Fix");
                            continue;
                        }

                        gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture)/100.0;

                        gotolocation.Lat = (int) gotolocation.Lat + ((gotolocation.Lat - (int) gotolocation.Lat)/0.60);

                        if (items[3] == "S")
                            gotolocation.Lat *= -1;

                        gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture)/100.0;

                        gotolocation.Lng = (int) gotolocation.Lng + ((gotolocation.Lng - (int) gotolocation.Lng)/0.60);

                        if (items[5] == "W")
                            gotolocation.Lng *= -1;

                        gotolocation.Alt = intalt;

                        gotolocation.Tag = "Sats " + items[7] + " hdop " + items[8];
                    }


                    if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0 &&
                        gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation && 
                    {
                        nextsend = DateTime.Now.AddMilliseconds(1000/updaterate);
                        Console.WriteLine("Sending follow wp " + DateTime.Now.ToString("h:MM:ss") + " " +
                                          gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt);
                        lastgotolocation = new PointLatLngAlt(gotolocation);

                        Locationwp gotohere = new Locationwp();

                        gotohere.id = (byte) MAVLink.MAV_CMD.WAYPOINT;
                        gotohere.alt = (float) (gotolocation.Alt);
                        gotohere.lat = (gotolocation.Lat);
                        gotohere.lng = (gotolocation.Lng);

                        try
                        {
                            updateLocationLabel(gotohere);
                        }
                        catch
                        {
                        }

                        if (MainV2.comPort.BaseStream.IsOpen && MainV2.comPort.giveComport == false)
                        {
                            try
                            {
                                MainV2.comPort.giveComport = true;

                                MainV2.comPort.setGuidedModeWP(gotohere);

                                MainV2.comPort.giveComport = false;
                            }
                            catch
                            {
                                MainV2.comPort.giveComport = false;
                            }
                        }
                    }
                }
                catch
                {
                    System.Threading.Thread.Sleep((int) (1000/updaterate));
                }
            }

            sw.Close();
        }

        /*
        *  extrapolate latitude/longitude given distances north and east
        *  This function costs about 80 usec on an AVR2560
        */

        void location_offset(Locationwp loc, float ofs_north, float ofs_east)
        {
            if (!is_zero(ofs_north) || !is_zero(ofs_east))
            {
                int32_t dlat = (int32_t) (ofs_north*LOCATION_SCALING_FACTOR_INV);
                int32_t dlng = (int32_t) ((ofs_east*LOCATION_SCALING_FACTOR_INV)/longitude_scale(loc));
                loc.lat += dlat;
                loc.lng += dlng;
            }
        }

        /*
        *  extrapolate latitude/longitude given bearing and distance
        * Note that this function is accurate to about 1mm at a distance of 
        * 100m. This function has the advantage that it works in relative
        * positions, so it keeps the accuracy even when dealing with small
        * distances and floating point numbers
        */

        void location_update(Locationwp loc, float bearing, float distance)
        {
            float ofs_north = cosf(radians(bearing))*distance;
            float ofs_east = sinf(radians(bearing))*distance;
            location_offset(loc, ofs_north, ofs_east);
        }

        /*
        return terrain height in meters above average sea level (WGS84) for
        a given position

        This is the base function that other height calculations are derived
        from. The functions below are more convenient for most uses

        This function costs about 20 microseconds on Pixhawk
        */

        bool height_amsl(Locationwp loc, ref float height)
        {
            height = (float) srtm.getAltitude(loc.lat, loc.lng).alt;
            return true;
        }

 float longitude_scale(Locationwp loc)
 {
     int32_t last_lat = -999999999;
     float scale = 1.0f;
     if (labs(last_lat - loc.lat) < 100000)
     {
         // we are within 0.01 degrees (about 1km) of the
         // same latitude. We can avoid the cos() and return
         // the same scale factor.
         return scale;
     }
     scale = cosf(loc.lat*1.0e-7f*DEG_TO_RAD);
     scale = constrain_float(scale, 0.01f, 1.0f);
     last_lat = (int32_t) loc.lat;
     return scale;
 }

        void mainloop()
        {
            DateTime nextsend = DateTime.Now;

            DateTime nextrallypntupdate = DateTime.Now;

            StreamWriter sw = new StreamWriter(File.OpenWrite("MovingBase.txt"));

            threadrun = true;
            while (threadrun)
            {
                try
                {
                    string line = comPort.ReadLine();

                    sw.WriteLine(line);

                    //string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", "");
                    if (line.StartsWith("$GPGGA")) // 
                    {
                        string[] items = line.Trim().Split(',','*');

                        if (items[15] != GetChecksum(line.Trim()))
                        {
                            Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim()));
                            continue;
                        }

                        if (items[6] == "0")
                        {
                            Console.WriteLine("No Fix");
                            continue;
                        }

                        gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture) / 100.0;

                        gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60);

                        if (items[3] == "S")
                            gotolocation.Lat *= -1;

                        gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture) / 100.0;

                        gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60);

                        if (items[5] == "W")
                            gotolocation.Lng *= -1;

                        gotolocation.Alt = double.Parse(items[9], CultureInfo.InvariantCulture);

                        gotolocation.Tag = "Sats "+ items[7] + " hdop " + items[8] ;

                    }


                    if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0 && gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation && 
                    {
                        nextsend = DateTime.Now.AddMilliseconds(1000/updaterate);
                        Console.WriteLine("new home wp " +DateTime.Now.ToString("h:MM:ss")+" "+ gotolocation.Lat + " " + gotolocation.Lng + " " +gotolocation.Alt);
                        lastgotolocation = new PointLatLngAlt(gotolocation);

                        Locationwp gotohere = new Locationwp();

                        gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
                        gotohere.alt = (float)(gotolocation.Alt);
                        gotohere.lat = (gotolocation.Lat);
                        gotohere.lng = (gotolocation.Lng);

                        try
                        {
                            updateLocationLabel(gotohere);
                        }
                        catch { }

                        MainV2.comPort.MAV.cs.MovingBase = gotolocation;

                        // plane only
                        if (updaterallypnt && DateTime.Now > nextrallypntupdate)
                        {
                            nextrallypntupdate = DateTime.Now.AddSeconds(5);
                            try
                            {
                                    MainV2.comPort.setParam("RALLY_TOTAL", 1);

                                    MainV2.comPort.setRallyPoint(0, new PointLatLngAlt(gotolocation) { Alt = gotolocation.Alt + double.Parse(MainV2.config["TXT_DefaultAlt"].ToString()) }, 0, 0, 0, (byte)(float)MainV2.comPort.MAV.param["RALLY_TOTAL"]);

                                    MainV2.comPort.setParam("RALLY_TOTAL", 1);
                            }
                            catch (Exception ex) { Console.WriteLine(ex.ToString()); }
                        }
                    }
                }
                catch { System.Threading.Thread.Sleep((int)(1000 / updaterate)); }
            }

            sw.Close();
        }