C# PointLatLngAlt.newpos方法代码示例

        public static PointLatLngAlt ProjectPoint()
        {
            PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);

            double yawangle = MainV2.comPort.MAV.cs.campointc;
            double rollangle = MainV2.comPort.MAV.cs.campointb;
            double pitchangle = MainV2.comPort.MAV.cs.campointa;

            if ((float) MainV2.comPort.MAV.param["MNT_TYPE"] == 4)
            {
                yawangle = MainV2.comPort.MAVlist[71].cs.yaw;
                rollangle = MainV2.comPort.MAVlist[71].cs.roll;
                pitchangle = MainV2.comPort.MAVlist[71].cs.pitch;
            }

            int distout = 10;
            PointLatLngAlt newpos = PointLatLngAlt.Zero;

            //dist = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.alt);

            while (distout < 1000)
            {
                // get a projected point to test intersection against - not using slope distance
                PointLatLngAlt newposdist = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout);
                newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;

                // get another point 50 infront
                PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout + 50);
                newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;

                // get the flat terrain distance out - at 0 alt
                double distflat = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.altasl);

                // x is dist from plane, y is alt
                var newpoint = FindLineIntersection(new PointF(0,MainV2.comPort.MAV.cs.altasl), new PointF((float)distflat, 0),
                    new PointF((float)distout, (float)newposdist.Alt), new PointF((float)distout + 50, (float)newposdist2.Alt));

                if (newpoint.X != 0)
                {
                    newpos = newposdist2;
                    break;
                }

                distout += 50;
            }

            //Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));

            //PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist);

            //Console.WriteLine(newpos);
            return newpos;
        }

        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;
        }

        public static PointLatLngAlt ProjectPoint()
        {
            MainV2.comPort.GetMountStatus();

            // this should be looking at rc_channel function
            yawchannel =  (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_PAN"];

            pitchchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_TILT"];

            rollchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_ROLL"];

            //if (!MainV2.comPort.BaseStream.IsOpen)
              //  return PointLatLngAlt.Zero;

            PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);

            double yawangle = MainV2.comPort.MAV.cs.campointc;
            double rollangle = MainV2.comPort.MAV.cs.campointb;
            double pitchangle = MainV2.comPort.MAV.cs.campointa;

            //
            if ((float) MainV2.comPort.MAV.param["MNT_TYPE"] == 4)
            {
                yawangle = MainV2.comPort.MAVlist[71].cs.yaw;
                rollangle = MainV2.comPort.MAVlist[71].cs.roll;
                pitchangle = MainV2.comPort.MAVlist[71].cs.pitch;
            }

            if (Math.Abs(rollangle) > 180 || yawangle == 0 && pitchangle == 0)
            {
                yawangle = ConvertPwmtoAngle(axis.yaw);
                //rollangle = ConvertPwmtoAngle(axis.roll);
                pitchangle = ConvertPwmtoAngle(axis.pitch) + MainV2.comPort.MAV.cs.pitch;

                pitchangle -= Math.Sin(yawangle * deg2rad) * MainV2.comPort.MAV.cs.roll;
            }

            // tan (o/a)
            // todo account for ground elevation change.

            int distout = 10;
            PointLatLngAlt newpos = PointLatLngAlt.Zero;

            //dist = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.alt);

            while (distout < 1000)
            {
                // get a projected point to test intersection against - not using slope distance
                PointLatLngAlt newposdist = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout);
                newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;

                // get another point 50 infront
                PointLatLngAlt newposdist2 = currentlocation.newpos(yawangle + MainV2.comPort.MAV.cs.yaw, distout + 50);
                newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;

                // get the flat terrain distance out - at 0 alt
                double distflat = Math.Tan((90 + pitchangle) * deg2rad) * (MainV2.comPort.MAV.cs.altasl);

                // x is dist from plane, y is alt
                var newpoint = FindLineIntersection(new PointF(0,MainV2.comPort.MAV.cs.altasl), new PointF((float)distflat, 0),
                    new PointF((float)distout, (float)newposdist.Alt), new PointF((float)distout + 50, (float)newposdist2.Alt));

                if (newpoint.X != 0)
                {
                    newpos = newposdist2;
                    break;
                }

                distout += 50;
            }

            //Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));

            //PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist);

            //Console.WriteLine(newpos);
            return newpos;
        }

        //http://www.chrobotics.com/library/understanding-euler-angles

        // x = north / roll
        // y = pitch / east
        // z = yaw / down

        public static List<PointLatLngAlt> calc(PointLatLngAlt plla, double R, double P, double Y, double hfov, double vfov)
        {
            // alt should be asl
            // P pitch where the center os pointing ie -80
            // R roll 

            GMapPolygon poly = new GMapPolygon(new List<PointLatLng>(), "rect");

            double frontangle = (P*0) + vfov/2;
            double backangle = (P*0) - vfov/2;
            double leftangle = (R*0) + hfov/2;
            double rightangle = (R*0) - hfov/2;

            var fovh = Math.Tan(hfov/2.0 * deg2rad)*2.0;
            var fovv = Math.Tan(vfov/2.0 * deg2rad)*2.0;

            //DoDebug();

            addtomap(plla, "plane");

            Matrix3 dcm = new Matrix3();

            dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
            dcm.normalize();

            Vector3 center1 = new Vector3(0, 0, 10000);

            var test = dcm * center1;

            var bearing2 = Math.Atan2(test.y, test.x) * rad2deg;

            var newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));

            newpos2.Alt -= test.z;

            var cen = calcIntersection(plla, newpos2);

            var dist = plla.GetDistance(cen);

            addtomap(cen, "center "+ dist.ToString("0"));

            //
            dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
            dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0));
            dcm.normalize();
            dcm.rotate(new Vector3(0, frontangle * deg2rad, 0));
            dcm.normalize();

            test = dcm * center1;

            bearing2 = (Math.Atan2(test.y, test.x) * rad2deg);

            newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));

            newpos2.Alt -= test.z;

            //addtomap(newpos2, "tr2");

            var groundpointtr = calcIntersection(plla, newpos2);

            poly.Points.Add(groundpointtr);

            addtomap(groundpointtr, "tr");

            //
            dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
            dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0));
            dcm.normalize();
            dcm.rotate(new Vector3(0, frontangle * deg2rad, 0));
            dcm.normalize();

            test = dcm * center1;

            bearing2 = Math.Atan2(test.y, test.x)*rad2deg;

            newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));

            newpos2.Alt -= test.z;

            var groundpointtl = calcIntersection(plla, newpos2);

            poly.Points.Add(groundpointtl);

            addtomap(groundpointtl, "tl");

            //
            dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
            dcm.rotate(new Vector3(leftangle * deg2rad, 0, 0));
            dcm.normalize();
            dcm.rotate(new Vector3(0, backangle * deg2rad, 0));
            dcm.normalize();

            test = dcm * center1;

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

        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;
        }

        //http://www.chrobotics.com/library/understanding-euler-angles

        // x = north / roll
        // y = pitch / east
        // z = yaw / down

        public static List<PointLatLngAlt> calc(PointLatLngAlt plla, double R, double P, double Y, double hfov, double vfov)
        {
            // alt should be asl
            // P pitch where the center os pointing ie -80
            // R roll 

            // if roll and pitch is 0, use the quick method.
            if (R == 0 && P == 0)
            {
                // calc fov in m on the ground at 0 alt
                var fovh = Math.Tan(hfov / 2.0 * deg2rad) * plla.Alt;
                var fovv = Math.Tan(vfov / 2.0 * deg2rad) * plla.Alt;
                var fovd = Math.Sqrt(fovh * fovh + fovv * fovv);

                // where we put our footprint
                var ans2 = new List<PointLatLngAlt>();

                // calc bearing from center to corner
                var bearing1 = Math.Atan2(fovh, fovv) * rad2deg;

                // calc first corner point
                var newpos1 = plla.newpos(bearing1 + Y, Math.Sqrt(fovh * fovh + fovv * fovv));
                // set alt to 0, as we used the hypot for distance and fov is based on 0 alt
                newpos1.Alt = 0;
                // calc intersection from center to new point and return ground hit point
                var cen1 = calcIntersection(plla, newpos1);
                // add to our footprint poly
                ans2.Add(cen1);
                addtomap(cen1, "cen1");

                //repeat

                newpos1 = plla.newpos(Y - bearing1, Math.Sqrt(fovh * fovh + fovv * fovv));
                newpos1.Alt = 0;
                cen1 = calcIntersection(plla, newpos1);
                ans2.Add(cen1);
                addtomap(cen1, "cen2");

                newpos1 = plla.newpos(bearing1 + Y - 180, Math.Sqrt(fovh * fovh + fovv * fovv));
                newpos1.Alt = 0;
                cen1 = calcIntersection(plla, newpos1);
                ans2.Add(cen1);
                addtomap(cen1, "cen3");

                newpos1 = plla.newpos(Y - bearing1 - 180, Math.Sqrt(fovh * fovh + fovv * fovv));
                newpos1.Alt = 0;
                cen1 = calcIntersection(plla, newpos1);
                ans2.Add(cen1);
                addtomap(cen1, "cen4");


                addtomap(plla, "plane");

                return ans2;
            }

            
            GMapPolygon poly = new GMapPolygon(new List<PointLatLng>(), "rect");

            double frontangle = (P*0) + vfov/2;
            double backangle = (P*0) - vfov/2;
            double leftangle = (R*0) + hfov/2;
            double rightangle = (R*0) - hfov/2;


            addtomap(plla, "plane");

            Matrix3 dcm = new Matrix3();

            dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
            dcm.normalize();

            Vector3 center1 = new Vector3(0, 0, 10000);

            var test = dcm * center1;

            var bearing2 = Math.Atan2(test.y, test.x) * rad2deg;

            var newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));

            newpos2.Alt -= test.z;

            var cen = calcIntersection(plla, newpos2);

            var dist = plla.GetDistance(cen);

            addtomap(cen, "center "+ dist.ToString("0"));

            //
            dcm.from_euler(R * deg2rad, P * deg2rad, Y * deg2rad);
            dcm.rotate(new Vector3(rightangle * deg2rad, 0, 0));
            dcm.normalize();
            dcm.rotate(new Vector3(0, frontangle * deg2rad, 0));
            dcm.normalize();
//.........这里部分代码省略.........

        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;

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

            public string SUBMITHOST;// Submitter Host “1.2.3.4” or “enduser5.faa.gov”

            public List<List<PointLatLng>> GetPaths()
            {
                //RLN27.576944W97.108611LN27.468056W96.961111LN27.322222W97.050000LN27.345833W97.088889LN27.439167W97.186944RLN27.672778W97.212222LN27.576944W97.108611LN27.533333W97.133333LN27.638333W97.237222RCN27.686333W97.294667R007.00

                List<List<PointLatLng>> list = new List<List<PointLatLng>>();

                List<PointLatLng> pointlist = new List<PointLatLng>();

                var matches = all.Matches(BOUND);

                Console.WriteLine(BOUND);

                bool isarcterminate = false;
                bool iscircleterminate = false;
                int arcdir = 0;
                PointLatLngAlt pointcent = null;
                PointLatLngAlt pointstart = null;

                foreach (Match item in matches)
                {
                    try
                    {
                        if (item.Groups[2].Value == "L")
                        {
                            var point = new PointLatLngAlt(double.Parse(item.Groups[4].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[6].Value, CultureInfo.InvariantCulture));

                            if (item.Groups[3].Value == "S")
                                point.Lat *= -1;

                            if (item.Groups[5].Value == "W")
                                point.Lng *= -1;

                            if (isarcterminate)
                            {
                                double radius = pointcent.GetDistance(pointstart);

                                double startbearing = pointcent.GetBearing(pointstart);

                                double endbearing = pointcent.GetBearing(point);

                                if (arcdir > 0 && endbearing < startbearing)
                                    endbearing += 360;

                                if (arcdir < 0)
                                {
                                    for (double a = startbearing; a > endbearing; a += (10 * arcdir))
                                    {
                                        pointlist.Add(pointcent.newpos(a, radius));
                                    }
                                }
                                else
                                {
                                    for (double a = startbearing; a < endbearing; a += (10 * arcdir))
                                    {
                                        pointlist.Add(pointcent.newpos(a, radius));
                                    }
                                }

                                pointlist.Add(point);

                                list.Add(pointlist);
                                pointlist = new List<PointLatLng>();

                                isarcterminate = false;
                                iscircleterminate = false;

                                continue;
                            }

                            if (iscircleterminate)
                            {
                                iscircleterminate = false;
                                continue;
                            }

                            pointlist.Add(point);

                            continue;
                        }
                        else if (item.Groups[7].Value == "A")
                        {
                            pointcent = new PointLatLngAlt(double.Parse(item.Groups[10].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[12].Value, CultureInfo.InvariantCulture));

                            if (item.Groups[9].Value == "S")
                                pointcent.Lat *= -1;

                            if (item.Groups[11].Value == "W")
                                pointcent.Lng *= -1;

                            pointstart = new PointLatLngAlt(double.Parse(item.Groups[14].Value, CultureInfo.InvariantCulture), double.Parse(item.Groups[16].Value, CultureInfo.InvariantCulture));

                            if (item.Groups[13].Value == "S")
                                pointstart.Lat *= -1;

                            if (item.Groups[15].Value == "W")
                                pointstart.Lng *= -1;

                            arcdir = item.Groups[8].Value == "+" ? 1 : -1;
//.........这里部分代码省略.........

        public static PointLatLngAlt ProjectPoint()
        {
            MainV2.comPort.GetMountStatus();

            // this should be looking at rc_channel function
            yawchannel =  (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_PAN"];

            pitchchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_TILT"];

            rollchannel = (int)(float)MainV2.comPort.MAV.param["MNT_RC_IN_ROLL"];

            if (!MainV2.comPort.BaseStream.IsOpen)
                return PointLatLngAlt.Zero;

            PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);

            double yawangle = MainV2.comPort.MAV.cs.campointc;
            double rollangle = MainV2.comPort.MAV.cs.campointb;
            double pitchangle = MainV2.comPort.MAV.cs.campointa;


            if (Math.Abs(rollangle) > 180 || yawangle == 0 && pitchangle == 0)
            {
                yawangle = ConvertPwmtoAngle(axis.yaw);
                //rollangle = ConvertPwmtoAngle(axis.roll);
                pitchangle = ConvertPwmtoAngle(axis.pitch) + MainV2.comPort.MAV.cs.pitch;

                pitchangle -= Math.Sin(yawangle * deg2rad) * MainV2.comPort.MAV.cs.roll;
            }

            // tan (o/a)
            // todo account for ground elevation change.
            double dist = Math.Tan((90 +pitchangle)* deg2rad) * (MainV2.comPort.MAV.cs.alt);

            if (dist > 9999)
                return PointLatLngAlt.Zero;

            //Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));

            PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist);

            //Console.WriteLine(newpos);
            return newpos;
        }

        public static PointLatLngAlt ProjectPoint()
        {
            //MainV2.comPort.GetMountStatus();

            int fixme;

            if (!MainV2.comPort.BaseStream.IsOpen)
                return PointLatLngAlt.Zero;

            PointLatLngAlt currentlocation = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng);

            double yawangle = MainV2.comPort.MAV.cs.campointc * 0.01f;
            double rollangle = MainV2.comPort.MAV.cs.campointb * 0.01f;
            double pitchangle = MainV2.comPort.MAV.cs.campointa * 0.01f;

            yawangle = ConvertPwmtoAngle(axis.yaw);
            //rollangle = ConvertPwmtoAngle(axis.roll);
            pitchangle = ConvertPwmtoAngle(axis.pitch) + MainV2.comPort.MAV.cs.pitch;

            pitchangle -= Math.Sin(yawangle * deg2rad) * MainV2.comPort.MAV.cs.roll;

            // tan (o/a)
            double dist = Math.Tan((90 +pitchangle)* deg2rad) * (MainV2.comPort.MAV.cs.alt);

            if (dist > 9999)
                return PointLatLngAlt.Zero;

            //Console.WriteLine("pitch " + pitchangle.ToString("0.000") + " yaw " + yawangle.ToString("0.000") + " dist" + dist.ToString("0.000"));

            PointLatLngAlt newpos = currentlocation.newpos( yawangle + MainV2.comPort.MAV.cs.yaw, dist);

            //Console.WriteLine(newpos);
            return newpos;
        }