C# PointLatLngAlt.GetBearing方法代码示例

        public List<PointLatLngAlt> calcspline(PointLatLngAlt currentpos, PointLatLngAlt p1, PointLatLngAlt p2)
        {
            List<PointLatLngAlt> answer = new List<PointLatLngAlt>();

            spline_t = spline_t_sq = 0.0f;

            spline_p0 = currentpos;
            spline_p1 = p1;
            spline_p2 = p2;
            spline_p0_prime = new Vector3();
            double yaw = (currentpos.GetBearing(p1) + 360) % 360;
            spline_p0_prime.x = .000001 * Math.Sin(yaw * deg2rad);
            spline_p0_prime.y = .000001 * Math.Cos(yaw * deg2rad);
            spline_p0_prime.z = spline_p1.z - spline_p0.z;

            initialize_spline_segment();

            int steps = 30;

            foreach (var step in range(steps +1))
            {
                spline_t = (1f / steps) * step;
                spline_t_sq = spline_t * spline_t;
                evaluate_spline();
                answer.Add(new PointLatLngAlt(spline_target.x, spline_target.y, spline_target.z, ""));
            }

            return answer;
        }

        public static List<PointLatLngAlt> GeneratePath(MAVState MAV)
        {
            List<PointLatLngAlt> result = new List<PointLatLngAlt>();

            MAVLink.mavlink_mission_item_t? prevwp = null;

            int a = -1;

            foreach (var wp in MAV.wps.Values)
            {
                a++;
                if (!prevwp.HasValue)
                {
                    // change firstwp/aka home to valid alt
                    prevwp = new MAVLink.mavlink_mission_item_t?(new MAVLink.mavlink_mission_item_t() { x=wp.x,y = wp.y, z = 0});
                    continue;
                }

                if (wp.command != (byte)MAVLink.MAV_CMD.WAYPOINT && wp.command != (byte)MAVLink.MAV_CMD.SPLINE_WAYPOINT)
                    continue;

                var wp1 = new PointLatLngAlt(prevwp.Value);
                var wp2 = new PointLatLngAlt(wp);
                var bearing = wp1.GetBearing(wp2);
                var distwps = wp1.GetDistance(wp2);
                var startalt = wp1.Alt;
                var endalt = wp2.Alt;

                if (startalt == 0)
                {
                    startalt = endalt;
                }

                if(distwps > 5000)
                    continue;

                for (double d = 0.1; d < distwps; d += 0.1)
                {
                    var pnt = wp1.newpos(bearing, d);
                    pnt.Alt = startalt + (d/distwps) * (endalt-startalt);
                    result.Add(pnt);
                }

                prevwp = wp;
            }

            return result;
        }

        static PointLatLngAlt calcIntersection(PointLatLngAlt plla, PointLatLngAlt dest, int step = 100)
        {
            int distout = 10;
            PointLatLngAlt newpos = PointLatLngAlt.Zero;

            var dist = plla.GetDistance(dest);
            var Y = plla.GetBearing(dest);

            // 20 km
            while (distout < (dist+100))
            {
                // get a projected point to test intersection against - not using slope distance
                PointLatLngAlt newposdist = plla.newpos(Y, distout);
                newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;

                // get another point 'step' infront
                PointLatLngAlt newposdist2 = plla.newpos(Y, distout + step);
                newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;

                // x is dist from plane, y is alt
                var newpoint = FindLineIntersection(new PointF(0, (float)plla.Alt),
                    new PointF((float)dist, (float)dest.Alt),
                    new PointF((float)distout, (float)newposdist.Alt),
                    new PointF((float)distout + step, (float)newposdist2.Alt));

                if (newpoint.X != 0)
                {
                    newpos = plla.newpos(Y, newpoint.X);
                    newpos.Alt = newpoint.Y;

                    return newpos;
                }

                distout += step;
            }

            //addtomap(newpos, distout.ToString());

            dest.Alt = 0;

            return dest;
        }

        public void UpdatePositions()
        {
            // add new point to trail
            trail.Add(new PointLatLngAlt(GroundMasterDrone.MavState.cs.lat, GroundMasterDrone.MavState.cs.lng, GroundMasterDrone.MavState.cs.alt,""));

            while (trail.Count > 1000)
                trail.RemoveAt(0);

            // get current positions and velocitys
            foreach (var drone in Drones)
            {
                if (drone.Location == null)
                    drone.Location = new PointLatLngAlt();
                drone.Location.Lat = drone.MavState.cs.lat;
                drone.Location.Lng = drone.MavState.cs.lng;
                drone.Location.Alt = drone.MavState.cs.alt;
                if (drone.Velocity == null)
                    drone.Velocity = new Vector3();
                drone.Velocity.x = Math.Cos(drone.MavState.cs.groundcourse*deg2rad)*drone.MavState.cs.groundspeed;
                drone.Velocity.y = Math.Sin(drone.MavState.cs.groundcourse*deg2rad)*drone.MavState.cs.groundspeed;
                drone.Velocity.z = drone.MavState.cs.verticalspeed;

                drone.TargetVelocity = GroundMasterDrone.Velocity;
            }

            var targetbearing = GroundMasterDrone.Heading;

            //
            if (GroundMasterDrone.MavState.cs.wp_dist < Seperation*1.5)
            {
                var headingtowp = (int) GroundMasterDrone.MavState.cs.wpno;
                var nextwp = headingtowp + 1;

                try
                {
                    PointLatLngAlt targetwp = new PointLatLngAlt(GroundMasterDrone.MavState.wps[headingtowp]);
                    //PointLatLngAlt targetwp = GroundMasterDrone.Location;
                    PointLatLngAlt nexttargetwp = new PointLatLngAlt(GroundMasterDrone.MavState.wps[nextwp]);

                    var bearing = targetwp.GetBearing(nexttargetwp);

                    // point on the wp line for target
                    var targetpnt = targetwp.newpos(bearing, Seperation);

                    targetbearing = GroundMasterDrone.Location.GetBearing(targetpnt);

                    if (Math.Abs(targetbearing - bearing) > 20)
                    {
                        //targetbearing = bearing;
                    }

                    AirMasterDrone.TargetVelocity.x = Math.Cos(targetbearing*deg2rad)*
                                                      GroundMasterDrone.MavState.cs.groundspeed;
                    AirMasterDrone.TargetVelocity.y = Math.Sin(targetbearing*deg2rad)*
                                                      GroundMasterDrone.MavState.cs.groundspeed;
                }
                catch
                {
                }
            }
            else
            {
                
            }

            // calc airmaster position
            AirMasterDrone.TargetLocation = GroundMasterDrone.Location.newpos(targetbearing, Seperation);
            AirMasterDrone.TargetLocation.Alt = Altitude;

            // send new position to airmaster
            AirMasterDrone.SendPositionVelocity(AirMasterDrone.TargetLocation, AirMasterDrone.TargetVelocity * 0.6);

            AirMasterDrone.MavState.GuidedMode.x = (float)AirMasterDrone.TargetLocation.Lat;
            AirMasterDrone.MavState.GuidedMode.y = (float)AirMasterDrone.TargetLocation.Lng;
            AirMasterDrone.MavState.GuidedMode.z = (float)AirMasterDrone.TargetLocation.Alt;

            // get the path
            List<PointLatLngAlt> newpositions = PlanMove();

            List<PointLatLngAlt> newlist = new List<PointLatLngAlt>();
            newlist.Add(GroundMasterDrone.Location);
            newlist.AddRange(newpositions);

            newpositions = newlist;

            int a = 0;
            // send position and velocity
            foreach (var drone in Drones)
            {
                if(drone.MavState == airmaster)
                    continue;

                if (drone.MavState == groundmaster)
                    continue;

                if (a > (newpositions.Count - 1))
                    break;

                newpositions[a].Alt += Altitude;

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