public List <PointLatLngAlt> mineLocations() { List <PointLatLngAlt> lRet = new List <PointLatLngAlt>(); double bearingFromOriginToOtherRef = new PointLatLngAlt(OriginMarker.Position).GetBearing(OtherReferencePoint.Position); double internalAngleOfTriangle = (getInternalAngleOfTriangle()) * FlightData.rad2deg; if (double.IsNaN(bearingFromOriginToOtherRef) || double.IsNaN(internalAngleOfTriangle)) { MessageBox.Show("invalid bearing "); } else { PointLatLngAlt lNewPoint = OriginMarker.Position; lNewPoint = lNewPoint.newpos(bearingFromOriginToOtherRef - internalAngleOfTriangle, DistanceFromOriginToMarker * 1000); lRet.Add(lNewPoint); lNewPoint.groundTruth = true; // lNewPoint.color = System.Drawing.Color.Yellow; PointLatLngAlt lNewPoint2 = OriginMarker.Position; lNewPoint2 = lNewPoint2.newpos(bearingFromOriginToOtherRef + internalAngleOfTriangle, DistanceFromOriginToMarker * 1000); lRet.Add(lNewPoint2); lNewPoint2.groundTruth = true; // lNewPoint2.color = System.Drawing.Color.Yellow; } return(lRet); }
public static List <PointLatLngAlt> GeneratePath(MAVState MAV) { List <PointLatLngAlt> result = new List <PointLatLngAlt>(); MAVLink.mavlink_mission_item_int_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_int_t?(new MAVLink.mavlink_mission_item_int_t() { x = wp.x, y = wp.y, z = 0 }); continue; } if (wp.command != (ushort)MAVLink.MAV_CMD.WAYPOINT && wp.command != (ushort)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()); return(plla); }
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; // spline control drone.SendPositionVelocity(newpositions[a], drone.TargetVelocity / 2); drone.MavState.GuidedMode.x = (float)newpositions[a].Lat; drone.MavState.GuidedMode.y = (float)newpositions[a].Lng; drone.MavState.GuidedMode.z = (float)newpositions[a].Alt; // vel only //drone.SendVelocity(drone.TargetVelocity); a++; } }
protected override void OnPaint(System.Windows.Forms.PaintEventArgs e) { if (this.DesignMode) { return; } if (area.LocationMiddle.Lat == 0 && area.LocationMiddle.Lng == 0) { return; } try { base.OnPaint(e); } catch { return; } double heightscale = (step / 90.0) * 1; float yawradians = (float)(Math.PI * (rpy.Z * 1) / 180.0f); //radians = 0; float mouseY = (float)step / 10f; cameraX = center.Lng; // -Math.Sin(yawradians) * mouseY; // multiplying by mouseY makes the cameraY = center.Lat; // -Math.Cos(yawradians) * mouseY; // camera get closer/farther away with mouseY cameraZ = (center.Alt < srtm.getAltitude(center.Lat, center.Lng).alt) ? (srtm.getAltitude(center.Lat, center.Lng).alt + 1) * heightscale : center.Alt * heightscale; // (srtm.getAltitude(lookZ, lookX, 20) + 100) * heighscale; lookX = center.Lng + Math.Sin(yawradians) * mouseY; lookY = center.Lat + Math.Cos(yawradians) * mouseY; lookZ = cameraZ; // cameraZ += 0.04; GMapProvider type = GMap.NET.MapProviders.GoogleSatelliteMapProvider.Instance; PureProjection prj = type.Projection; int size = (int)(cameraZ * 150000); // in front PointLatLngAlt leftf = center.newpos(rpy.Z, size); // behind PointLatLngAlt rightf = center.newpos(rpy.Z, 50); // left : 90 allows for 180 degree viewing angle PointLatLngAlt left = center.newpos(rpy.Z - 90, size); // right PointLatLngAlt right = center.newpos(rpy.Z + 90, size); double maxlat = Math.Max(left.Lat, Math.Max(right.Lat, Math.Max(leftf.Lat, rightf.Lat))); double minlat = Math.Min(left.Lat, Math.Min(right.Lat, Math.Min(leftf.Lat, rightf.Lat))); double maxlng = Math.Max(left.Lng, Math.Max(right.Lng, Math.Max(leftf.Lng, rightf.Lng))); double minlng = Math.Min(left.Lng, Math.Min(right.Lng, Math.Min(leftf.Lng, rightf.Lng))); // if (Math.Abs(area.Lat - maxlat) < 0.001) { } // else { area = RectLatLng.FromLTRB(minlng, maxlat, maxlng, minlat); } GPoint topLeftPx = prj.FromLatLngToPixel(area.LocationTopLeft, zoom); GPoint rightButtomPx = prj.FromLatLngToPixel(area.Bottom, area.Right, zoom); GPoint pxDelta = new GPoint(rightButtomPx.X - topLeftPx.X, rightButtomPx.Y - topLeftPx.Y); zoom = 21; pxDelta.X = 9999; int otherzoomlevel = 12; // zoom based on pixel density while (pxDelta.X > this.Width) { zoom--; // current area topLeftPx = prj.FromLatLngToPixel(area.LocationTopLeft, zoom); rightButtomPx = prj.FromLatLngToPixel(area.Bottom, area.Right, zoom); pxDelta = new GPoint(rightButtomPx.X - topLeftPx.X, rightButtomPx.Y - topLeftPx.Y); } otherzoomlevel = zoom - 4; Console.WriteLine("zoom {0}", zoom); // update once per seconds - we only read from disk, so need to let cahce settle if (lastrefresh.AddSeconds(0.5) < DateTime.Now) { // get tiles - bg core.Provider = type; core.Position = LocationCenter; // get zoom 10 core.Zoom = otherzoomlevel; core.OnMapSizeChanged(this.Width, this.Height); // get actual current zoom core.Zoom = zoom; core.OnMapSizeChanged(this.Width, this.Height); lastrefresh = DateTime.Now; } else { //return; } float screenscale = this.Width / (float)this.Height; MakeCurrent(); GL.MatrixMode(MatrixMode.Projection); OpenTK.Matrix4 projection = OpenTK.Matrix4.CreatePerspectiveFieldOfView(120 * deg2rad, screenscale, 0.00001f, (float)step * 20000); GL.LoadMatrix(ref projection); Matrix4 modelview = Matrix4.LookAt((float)cameraX, (float)cameraY, (float)cameraZ, (float)lookX, (float)lookY, (float)lookZ, 0, 0, 1); GL.MatrixMode(MatrixMode.Modelview); // roll modelview = Matrix4.Mult(modelview, Matrix4.CreateRotationZ(rpy.X * deg2rad)); // pitch modelview = Matrix4.Mult(modelview, Matrix4.CreateRotationX((rpy.Y - 15) * -deg2rad)); GL.LoadMatrix(ref modelview); GL.ClearColor(Color.CornflowerBlue); GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit | ClearBufferMask.AccumBufferBit); GL.LightModel(LightModelParameter.LightModelAmbient, new float[] { 1f, 1f, 1f, 1f }); // GL.Disable(EnableCap.Fog); GL.Enable(EnableCap.Fog); //GL.Enable(EnableCap.Lighting); //GL.Enable(EnableCap.Light0); GL.Fog(FogParameter.FogColor, new float[] { 100 / 255.0f, 149 / 255.0f, 237 / 255.0f, 1f }); //GL.Fog(FogParameter.FogDensity,0.1f); GL.Fog(FogParameter.FogMode, (int)FogMode.Linear); GL.Fog(FogParameter.FogStart, (float)step * 40); GL.Fog(FogParameter.FogEnd, (float)(step * 50)); // GL.Enable(EnableCap.DepthTest); GL.DepthFunc(DepthFunction.Always); /* * GL.Begin(BeginMode.LineStrip); * * GL.Color3(Color.White); * GL.Vertex3(0, 0, 0); * * //GL.Color3(Color.Red); * GL.Vertex3(area.Bottom, 0, area.Left); * * //GL.Color3(Color.Yellow); * GL.Vertex3(lookX, lookY, lookZ); * * //GL.Color3(Color.Green); * GL.Vertex3(cameraX, cameraY, cameraZ); * * GL.End(); */ /* * GL.PointSize(10); * GL.Color4(Color.Yellow); * GL.LineWidth(5); * * * GL.Begin(PrimitiveType.LineStrip); * * //GL.Vertex3(new Vector3((float)center.Lng,(float)center.Lat,(float)(center.Alt * heightscale))); * //GL.Vertex3(new Vector3(0, 0, 0)); * //GL.Vertex3(new Vector3((float)cameraX, (float)cameraY, (float)cameraZ)); * //GL.Color3(Color.Green); * //GL.Vertex3(new Vector3((float)lookX, (float)lookY, (float)lookZ)); * * GL.Vertex3(area.LocationTopLeft.Lng, area.LocationTopLeft.Lat, (float)cameraZ); * GL.Vertex3(area.LocationTopLeft.Lng, area.LocationRightBottom.Lat, (float)cameraZ); * GL.Vertex3(area.LocationRightBottom.Lng, area.LocationRightBottom.Lat, (float)cameraZ); * GL.Vertex3(area.LocationRightBottom.Lng, area.LocationTopLeft.Lat, (float)cameraZ); * GL.Vertex3(area.LocationTopLeft.Lng, area.LocationTopLeft.Lat, (float)cameraZ); * * GL.End(); */ GL.Finish(); GL.PointSize((float)(step * 1)); GL.Color3(Color.Blue); GL.Begin(PrimitiveType.Points); GL.Vertex3(new Vector3((float)center.Lng, (float)center.Lat, (float)cameraZ)); GL.End(); //GL.ClampColor(ClampColorTarget.ClampReadColor, ClampColorMode.True); /* * GL.Enable(EnableCap.Blend); * GL.DepthMask(false); * GL.BlendFunc(BlendingFactorSrc.Zero, BlendingFactorDest.Src1Color); * GL.DepthMask(true); * GL.Disable(EnableCap.Blend); */ // textureid.Clear(); // get level 10 tiles List <GPoint> tileArea1 = prj.GetAreaTileList(area, otherzoomlevel, 1); // get type list at new zoom level List <GPoint> tileArea2 = prj.GetAreaTileList(area, zoom, 2); List <GPoint> tileArea = new List <GPoint>(); tileArea.AddRange(tileArea1); tileArea.AddRange(tileArea2); // get tiles & combine into one foreach (var p in tileArea) { int localzoom = zoom; core.tileDrawingListLock.AcquireReaderLock(); core.Matrix.EnterReadLock(); try { if (tileArea1.Contains(p)) { localzoom = otherzoomlevel; } topLeftPx = prj.FromLatLngToPixel(area.LocationTopLeft, localzoom); GMap.NET.Internals.Tile t = core.Matrix.GetTileWithNoLock(localzoom, p); if (t.NotEmpty) { foreach (GMapImage img in t.Overlays) { if (!textureid.ContainsKey(p)) { generateTexture(p, (Bitmap)img.Img); } } } else { } } finally { core.Matrix.LeaveReadLock(); core.tileDrawingListLock.ReleaseReaderLock(); } //GMapImage tile = ((PureImageCache)Maps.MyImageCache.Instance).GetImageFromCache(type.DbId, p, zoom) as GMapImage; //if (tile != null && !textureid.ContainsKey(p)) { // generateTexture(p, (Bitmap)tile.Img); } if (textureid.ContainsKey(p)) { int texture = textureid[p]; GL.Enable(EnableCap.Texture2D); GL.BindTexture(TextureTarget.Texture2D, texture); } else { //Console.WriteLine("Missing tile"); continue; } long x = p.X * prj.TileSize.Width - topLeftPx.X; long y = p.Y * prj.TileSize.Width - topLeftPx.Y; long xr = p.X * prj.TileSize.Width; long yr = p.Y * prj.TileSize.Width; long x2 = (p.X + 1) * prj.TileSize.Width; long y2 = (p.Y + 1) * prj.TileSize.Width; GL.LineWidth(0); GL.Color3(Color.White); // generate terrain GL.Begin(PrimitiveType.TriangleStrip); var latlng = prj.FromPixelToLatLng(xr, yr, localzoom); double heightl = srtm.getAltitude(latlng.Lat, latlng.Lng).alt; if (localzoom == 10) { heightl = 0; } //xr - topLeftPx.X, yr - topLeftPx.Y GL.TexCoord2(0, 0); GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale); // next down latlng = prj.FromPixelToLatLng(xr, y2, localzoom); heightl = srtm.getAltitude(latlng.Lat, latlng.Lng).alt; if (localzoom == 10) { heightl = 0; } GL.TexCoord2(0, 1); GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale); // next right latlng = prj.FromPixelToLatLng(x2, yr, localzoom); heightl = srtm.getAltitude(latlng.Lat, latlng.Lng).alt; if (localzoom == 10) { heightl = 0; } GL.TexCoord2(1, 0); GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale); // next right down latlng = prj.FromPixelToLatLng(x2, y2, localzoom); heightl = srtm.getAltitude(latlng.Lat, latlng.Lng).alt; if (localzoom == 10) { heightl = 0; } GL.TexCoord2(1, 1); GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale); GL.End(); } GL.Flush(); try { this.SwapBuffers(); Context.MakeCurrent(null); } catch { } //this.Invalidate(); return; }
private void map_MouseMove(object sender, MouseEventArgs e) { var mousecurrent = map.FromLocalToLatLng(e.X, e.Y); if (mousedown) { mousedragging = true; if (currentmode == mode.panmode) { if (e.Button == MouseButtons.Left) { double latdif = mousestart.Lat - mousecurrent.Lat; double lngdif = mousestart.Lng - mousecurrent.Lng; try { map.Position = new PointLatLng(map.Position.Lat + latdif, map.Position.Lng + lngdif); } catch { } } } else if (currentmode == mode.drawbox) { if (e.Button == MouseButtons.Left) { var rect = RectangleF.FromLTRB((float)mousestart.Lng, (float)mousestart.Lat, (float)mousecurrent.Lng, (float)mousecurrent.Lat); list.Clear(); // tl list.Add(mousestart); // tr list.Add(new PointLatLng(rect.Top, rect.Right)); // br list.Add(mousecurrent); // bl list.Add(new PointLatLng(rect.Bottom, rect.Left)); if (boxpoly != null) { layerpolygons.Polygons.Remove(boxpoly); } boxpoly = null; boxpoly = new GMapPolygon(list, "boxpoly"); boxpoly.IsHitTestVisible = true; boxpoly.Stroke = new Pen(Color.Red, 2); boxpoly.Fill = Brushes.Transparent; layerpolygons.Polygons.Add(boxpoly); map.Invalidate(); } } else if (currentmode == mode.movebox) { //if (mouseinsidepoly) { double latdif = mousestart.Lat - mousecurrent.Lat; double lngdif = mousestart.Lng - mousecurrent.Lng; for (int a = 0; a < boxpoly.Points.Count; a++) { boxpoly.Points[a] = new PointLatLng(boxpoly.Points[a].Lat - latdif, boxpoly.Points[a].Lng - lngdif); } UpdateListFromBox(); map.UpdatePolygonLocalPosition(boxpoly); map.Invalidate(); mousestart = mousecurrent; } } else if (currentmode == mode.editbox) { double latdif = mousestart.Lat - mousecurrent.Lat; double lngdif = mousestart.Lng - mousecurrent.Lng; // 2 point the create the lowest crosstrack distance // extend at 90 degrees to the bearing of the points based on mouse position PointLatLngAlt p0; PointLatLngAlt p1; PointLatLngAlt bestp0 = PointLatLngAlt.Zero; PointLatLngAlt bestp1 = PointLatLngAlt.Zero; double bestcrosstrack = 9999999; double R = 6371000; for (int a = 0; a < boxpoly.Points.Count; a++) { p0 = boxpoly.Points[a]; p1 = boxpoly.Points[(a + 1) % (boxpoly.Points.Count)]; var distp0p1 = p0.GetDistance(mousecurrent); var bearingp0curr = p0.GetBearing(mousecurrent); var bearringp0p1 = p0.GetBearing(p1); var ct = Math.Asin(Math.Sin(distp0p1 / R) * Math.Sin((bearingp0curr - bearringp0p1) * deg2rad)) * R; if (Math.Abs(ct) < Math.Abs(bestcrosstrack)) { bestp0 = p0; bestp1 = p1; bestcrosstrack = ct; } } var bearing = bestp0.GetBearing(bestp1); layerpolygons.Markers.Clear(); layerpolygons.Markers.Add(new GMarkerGoogle(bestp0, GMarkerGoogleType.blue)); layerpolygons.Markers.Add(new GMarkerGoogle(bestp1, GMarkerGoogleType.blue)); bearing = ((PointLatLngAlt)mousestart).GetBearing(mousecurrent); var newposp0 = bestp0.newpos(bearing, Math.Abs(bestcrosstrack)); var newposp1 = bestp1.newpos(bearing, Math.Abs(bestcrosstrack)); boxpoly.Points[boxpoly.Points.IndexOf(bestp0)] = newposp0; boxpoly.Points[boxpoly.Points.IndexOf(bestp1)] = newposp1; UpdateListFromBox(); map.UpdatePolygonLocalPosition(boxpoly); map.Invalidate(); mousestart = mousecurrent; } } mousedragging = false; }
public override void SendCommand() { if (masterpos.Lat == 0 || masterpos.Lng == 0) { return; } //Console.WriteLine(DateTime.Now); //Console.WriteLine("Leader {0} {1} {2}", masterpos.Lat, masterpos.Lng, masterpos.Alt); int a = 0; foreach (var port in MainV2.Comports.ToArray()) { foreach (var mav in port.MAVlist) { if (mav == Leader) { continue; } PointLatLngAlt target = new PointLatLngAlt(masterpos); try { int utmzone = (int)((masterpos.Lng - -186.0) / 6.0); IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone, masterpos.Lat < 0 ? false : true); ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm); double[] pll1 = { target.Lng, target.Lat }; double[] p1 = trans.MathTransform.Transform(pll1); double heading = -Leader.cs.yaw; double length = offsets[mav].length(); var x = ((Vector3)offsets[mav]).x; var y = ((Vector3)offsets[mav]).y; // add offsets to utm p1[0] += x * Math.Cos(heading * MathHelper.deg2rad) - y * Math.Sin(heading * MathHelper.deg2rad); p1[1] += x * Math.Sin(heading * MathHelper.deg2rad) + y * Math.Cos(heading * MathHelper.deg2rad); // convert back to wgs84 IMathTransform inversedTransform = trans.MathTransform.Inverse(); double[] point = inversedTransform.Transform(p1); target.Lat = point[1]; target.Lng = point[0]; target.Alt += ((Vector3)offsets[mav]).z; if (mav.cs.firmware == Firmwares.ArduPlane) { // get distance from target position var dist = target.GetDistance(mav.cs.Location); // get bearing to target var targyaw = mav.cs.Location.GetBearing(target); var targettrailer = target.newpos(Leader.cs.yaw, Math.Abs(dist) * -0.25); var targetleader = target.newpos(Leader.cs.yaw, 10 + dist); var yawerror = wrap_180(targyaw - mav.cs.yaw); var mavleadererror = wrap_180(Leader.cs.yaw - mav.cs.yaw); if (dist < 100) { targyaw = mav.cs.Location.GetBearing(targetleader); yawerror = wrap_180(targyaw - mav.cs.yaw); var targBearing = mav.cs.Location.GetBearing(target); // check the bearing for the leader and target are within 45 degrees. if (Math.Abs(wrap_180(targBearing - targyaw)) > 45) { dist *= -1; } } else { targyaw = mav.cs.Location.GetBearing(targettrailer); yawerror = wrap_180(targyaw - mav.cs.yaw); } // display update mav.GuidedMode.x = (float)target.Lat; mav.GuidedMode.y = (float)target.Lng; mav.GuidedMode.z = (float)target.Alt; MAVLink.mavlink_set_attitude_target_t att_target = new MAVLink.mavlink_set_attitude_target_t(); att_target.target_system = mav.sysid; att_target.target_component = mav.compid; att_target.type_mask = 0xff; Tuple <PID, PID, PID, PID> pid; if (pids.ContainsKey(mav)) { pid = pids[mav]; } else { pid = new Tuple <PID, PID, PID, PID>( new PID(1f, .03f, 0.02f, 10, 20, 0.1f, 0), new PID(1f, .03f, 0.02f, 10, 20, 0.1f, 0), new PID(1, 0, 0.00f, 15, 20, 0.1f, 0), new PID(0.01f, 0.001f, 0, 0.5f, 20, 0.1f, 0)); pids.Add(mav, pid); } var rollp = pid.Item1; var pitchp = pid.Item2; var yawp = pid.Item3; var thrustp = pid.Item4; var newroll = 0d; var newpitch = 0d; if (true) { var altdelta = target.Alt - mav.cs.alt; newpitch = altdelta; att_target.type_mask -= 0b00000010; pitchp.set_input_filter_all((float)altdelta); newpitch = pitchp.get_pid(); } if (true) { var leaderturnrad = Leader.cs.radius; var mavturnradius = leaderturnrad - x; { var distToTarget = mav.cs.Location.GetDistance(target); var bearingToTarget = mav.cs.Location.GetBearing(target); // bearing stability if (distToTarget < 30) { bearingToTarget = mav.cs.Location.GetBearing(targetleader); } // fly in from behind if (distToTarget > 100) { bearingToTarget = mav.cs.Location.GetBearing(targettrailer); } var bearingDelta = wrap_180(bearingToTarget - mav.cs.yaw); var tangent90 = bearingDelta > 0 ? 90 : -90; newroll = 0; // if the delta is > 90 then we are facing the wrong direction if (Math.Abs(bearingDelta) < 85) { var insideAngle = Math.Abs(tangent90 - bearingDelta); var angleCenter = 180 - insideAngle * 2; // sine rule var sine1 = Math.Max(distToTarget, 40) / Math.Sin(angleCenter * MathHelper.deg2rad); var radius = sine1 * Math.Sin(insideAngle * MathHelper.deg2rad); // average calced + leader offset turnradius - acts as a FF radius = (Math.Abs(radius) + Math.Abs(mavturnradius)) / 2; var angleBank = ((mav.cs.groundspeed * mav.cs.groundspeed) / radius) / 9.8; angleBank *= MathHelper.rad2deg; if (bearingDelta > 0) { newroll = Math.Abs(angleBank); } else { newroll = -Math.Abs(angleBank); } } newroll += MathHelper.constrain(bearingDelta, -20, 20); } // tr = gs2 / (9.8 * x) // (9.8 * x) * tr = gs2 // 9.8 * x = gs2 / tr // (gs2/tr)/9.8 = x var angle = ((mav.cs.groundspeed * mav.cs.groundspeed) / mavturnradius) / 9.8; //newroll = angle * MathHelper.rad2deg; // 1 degree of roll for ever 1 degree of yaw error //newroll += MathHelper.constrain(yawerror, -20, 20); //rollp.set_input_filter_all((float)yawdelta); } // do speed if (true) { //att_target.thrust = (float) MathHelper.mapConstrained(dist, 0, 40, 0, 1); att_target.type_mask -= 0b01000000; // in m out 0-1 thrustp.set_input_filter_all((float)dist); // prevent buildup prior to being close if (dist > 40) { thrustp.reset_I(); } // 0.1 demand + pid results att_target.thrust = (float)MathHelper.constrain(thrustp.get_pid(), 0.1, 1); } Quaternion q = new Quaternion(); q.from_vector312(newroll * MathHelper.deg2rad, newpitch * MathHelper.deg2rad, yawerror * MathHelper.deg2rad); att_target.q = new float[4]; att_target.q[0] = (float)q.q1; att_target.q[1] = (float)q.q2; att_target.q[2] = (float)q.q3; att_target.q[3] = (float)q.q4; //0b0= rpy att_target.type_mask -= 0b10000101; //att_target.type_mask -= 0b10000100; Console.WriteLine("sysid {0} - {1} dist {2} r {3} p {4} y {5}", mav.sysid, att_target.thrust, dist, newroll, newpitch, (targyaw - mav.cs.yaw)); /* Console.WriteLine("rpyt {0} {1} {2} {3} I {4} {5} {6} {7}", * rollp.get_pid(), pitchp.get_pid(), yawp.get_pid(), thrustp.get_pid(), * rollp.get_i(), pitchp.get_i(), yawp.get_i(), thrustp.get_i()); */ port.sendPacket(att_target, mav.sysid, mav.compid); } else { Vector3 vel = new Vector3(Leader.cs.vx, Leader.cs.vy, Leader.cs.vz); // do pos/vel port.setPositionTargetGlobalInt(mav.sysid, mav.compid, true, true, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT, target.Lat, target.Lng, target.Alt, vel.x, vel.y, vel.z, 0, 0); // do yaw if (!gimbal) { // within 3 degrees dont send if (Math.Abs(mav.cs.yaw - Leader.cs.yaw) > 3) { port.doCommand(mav.sysid, mav.compid, MAVLink.MAV_CMD.CONDITION_YAW, Leader.cs.yaw, 100.0f, 0, 0, 0, 0, 0, false); } } else { // gimbal direction if (Math.Abs(mav.cs.yaw - Leader.cs.yaw) > 3) { port.setMountControl(mav.sysid, mav.compid, 45, 0, Leader.cs.yaw, false); } } } //Console.WriteLine("{0} {1} {2} {3}", port.ToString(), target.Lat, target.Lng, target.Alt); } catch (Exception ex) { Console.WriteLine("Failed to send command " + mav.ToString() + "\n" + ex.ToString()); } a++; } } }
//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 * MathHelper.deg2rad) * plla.Alt; var fovv = Math.Tan(vfov / 2.0 * MathHelper.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) * MathHelper.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 * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.normalize(); Vector3 center1 = new Vector3(0, 0, 10000); var test = dcm * center1; var bearing2 = Math.Atan2(test.y, test.x) * MathHelper.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 * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(rightangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * MathHelper.deg2rad, 0)); dcm.normalize(); /* * var mx = new Matrix3(); * var my = new Matrix3(); * var mz = new Matrix3(); * * mx.from_euler((rightangle + R) * MathHelper.deg2rad, 0, 0); * my.from_euler(0, (frontangle + P) * MathHelper.deg2rad, 0); * mz.from_euler(0, 0, Y * MathHelper.deg2rad); * * printdcm(mx); * printdcm(my); * printdcm(mz); * printdcm(my * mx); * printdcm(mz * my * mx); * * test = mz * my * mx * center1; */ test = dcm * center1; bearing2 = (Math.Atan2(test.y, test.x) * MathHelper.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 * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(leftangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, frontangle * MathHelper.deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * MathHelper.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 * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(leftangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * MathHelper.deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbl = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbl); addtomap(groundpointbl, "bl"); // dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad); dcm.rotate(new Vector3(rightangle * MathHelper.deg2rad, 0, 0)); dcm.normalize(); dcm.rotate(new Vector3(0, backangle * MathHelper.deg2rad, 0)); dcm.normalize(); test = dcm * center1; bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg; newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y)); newpos2.Alt -= test.z; var groundpointbr = calcIntersection(plla, newpos2); poly.Points.Add(groundpointbr); addtomap(groundpointbr, "br"); // polygons.Polygons.Clear(); polygons.Polygons.Add(poly); var ans = new List <PointLatLngAlt>(); ans.Add(groundpointtl); ans.Add(groundpointtr); ans.Add(groundpointbr); ans.Add(groundpointbl); return(ans); }
protected override void OnPaint(System.Windows.Forms.PaintEventArgs e) { if (this.DesignMode) { return; } if (area.LocationMiddle.Lat == 0 && area.LocationMiddle.Lng == 0) { return; } try { base.OnPaint(e); } catch { return; } double heightscale = (step / 90.0) * 3; float yawradians = (float)(Math.PI * (rpy.Z * 1) / 180.0f); //radians = 0; float mouseY = (float)(0.0025); cameraX = area.LocationMiddle.Lng; // -Math.Sin(yawradians) * mouseY; // multiplying by mouseY makes the cameraY = area.LocationMiddle.Lat; // -Math.Cos(yawradians) * mouseY; // camera get closer/farther away with mouseY cameraZ = (LocationCenter.Alt < srtm.getAltitude(center.Lat, center.Lng)) ? (srtm.getAltitude(center.Lat, center.Lng) + 1) * heightscale : LocationCenter.Alt * heightscale; // (srtm.getAltitude(lookZ, lookX, 20) + 100) * heighscale; lookX = area.LocationMiddle.Lng + Math.Sin(yawradians) * mouseY; lookY = area.LocationMiddle.Lat + Math.Cos(yawradians) * mouseY; lookZ = cameraZ; // cameraZ += 0.04; GMapProvider type = GMap.NET.MapProviders.GoogleSatelliteMapProvider.Instance; PureProjection prj = type.Projection; PointLatLngAlt leftf = center.newpos(rpy.Z, 500); PointLatLngAlt rightf = center.newpos(rpy.Z, 10); PointLatLngAlt left = center.newpos(rpy.Z - 90, 500); PointLatLngAlt right = center.newpos(rpy.Z + 90, 500); double maxlat = Math.Max(left.Lat, Math.Max(right.Lat, Math.Max(leftf.Lat, rightf.Lat))); double minlat = Math.Min(left.Lat, Math.Min(right.Lat, Math.Min(leftf.Lat, rightf.Lat))); double maxlng = Math.Max(left.Lng, Math.Max(right.Lng, Math.Max(leftf.Lng, rightf.Lng))); double minlng = Math.Min(left.Lng, Math.Min(right.Lng, Math.Min(leftf.Lng, rightf.Lng))); area = RectLatLng.FromLTRB(minlng, maxlat, maxlng, minlat); GPoint topLeftPx = prj.FromLatLngToPixel(area.LocationTopLeft, zoom); GPoint rightButtomPx = prj.FromLatLngToPixel(area.Bottom, area.Right, zoom); GPoint pxDelta = new GPoint(rightButtomPx.X - topLeftPx.X, rightButtomPx.Y - topLeftPx.Y); zoom = 17; // zoom based on pixel density while (pxDelta.X > 2000) { zoom--; // current area topLeftPx = prj.FromLatLngToPixel(area.LocationTopLeft, zoom); rightButtomPx = prj.FromLatLngToPixel(area.Bottom, area.Right, zoom); pxDelta = new GPoint(rightButtomPx.X - topLeftPx.X, rightButtomPx.Y - topLeftPx.Y); } // update once per seconds - we only read from disk, so need to let cahce settle if (lastrefresh.AddSeconds(.6) < DateTime.Now) { // get tiles - bg core.Provider = type; core.Position = LocationCenter; core.Zoom = zoom; core.OnMapSizeChanged(this.Width, this.Height); lastrefresh = DateTime.Now; } else { //return; } MakeCurrent(); GL.MatrixMode(MatrixMode.Projection); OpenTK.Matrix4 projection = OpenTK.Matrix4.CreatePerspectiveFieldOfView(130 * deg2rad, 1f, 0.00001f, (float)step * 100); GL.LoadMatrix(ref projection); Matrix4 modelview = Matrix4.LookAt((float)cameraX, (float)cameraY, (float)cameraZ, (float)lookX, (float)lookY, (float)lookZ, 0, 0, 1); GL.MatrixMode(MatrixMode.Modelview); // roll modelview = Matrix4.Mult(modelview, Matrix4.CreateRotationZ(rpy.X * deg2rad)); // pitch modelview = Matrix4.Mult(modelview, Matrix4.CreateRotationX((rpy.Y - 10) * -deg2rad)); GL.LoadMatrix(ref modelview); GL.ClearColor(Color.LightBlue); GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit); GL.LightModel(LightModelParameter.LightModelAmbient, new float[] { 1f, 1f, 1f, 1f }); GL.Enable(EnableCap.Fog); // GL.Enable(EnableCap.Lighting); // GL.Enable(EnableCap.Light0); GL.Fog(FogParameter.FogColor, new float[] { 0.5f, 0.5f, 0.5f, 1f }); //GL.Fog(FogParameter.FogDensity,0.1f); GL.Fog(FogParameter.FogMode, (int)FogMode.Linear); GL.Fog(FogParameter.FogStart, (float)step * 1); GL.Fog(FogParameter.FogEnd, (float)(step * 7)); /* * GL.Begin(BeginMode.LineStrip); * * GL.Color3(Color.White); * GL.Vertex3(0, 0, 0); * * GL.Vertex3(area.Bottom, 0, area.Left); * * GL.Vertex3(lookX, lookY, lookZ); * * //GL.Vertex3(cameraX, cameraY, cameraZ); * * GL.End(); */ GL.Begin(PrimitiveType.LineStrip); GL.PointSize(200); GL.Color3(Color.Red); //GL.Vertex3(new Vector3((float)center.Lng,(float)center.Lat,(float)(center.Alt * heightscale))); //GL.Vertex3(new Vector3(0, 0, 0)); //GL.Vertex3(new Vector3((float)cameraX, (float)cameraY, (float)cameraZ)); //GL.Color3(Color.Green); //GL.Vertex3(new Vector3((float)lookX, (float)lookY, (float)lookZ)); GL.Vertex3(area.LocationTopLeft.Lng, area.LocationTopLeft.Lat, 0); GL.Vertex3(area.LocationTopLeft.Lng, area.LocationRightBottom.Lat, 0); GL.Vertex3(area.LocationRightBottom.Lng, area.LocationRightBottom.Lat, 0); GL.Vertex3(area.LocationRightBottom.Lng, area.LocationTopLeft.Lat, 0); GL.Vertex3(area.LocationTopLeft.Lng, area.LocationTopLeft.Lat, 0); GL.End(); GL.Begin(PrimitiveType.LineStrip); GL.PointSize(200); GL.Color3(Color.Red); GL.Vertex3(new Vector3((float)center.Lng, (float)center.Lat, 0)); GL.Vertex3(new Vector3((float)leftf.Lng, (float)leftf.Lat, 0)); GL.End(); GL.Begin(PrimitiveType.Points); GL.PointSize(100); GL.Color3(Color.Blue); GL.Vertex3(new Vector3((float)center.Lng, (float)center.Lat, 0)); GL.End(); //textureid.Clear(); // get type list at new zoom level List <GPoint> tileArea = prj.GetAreaTileList(area, zoom, 0); // get tiles & combine into one foreach (var p in tileArea) { core.tileDrawingListLock.AcquireReaderLock(); core.Matrix.EnterReadLock(); try { GMap.NET.Internals.Tile t = core.Matrix.GetTileWithNoLock(core.Zoom, p); if (t.NotEmpty) { foreach (GMapImage img in t.Overlays) { if (!textureid.ContainsKey(p)) { generateTexture(p, (Bitmap)img.Img); } } } else { } } finally { core.Matrix.LeaveReadLock(); core.tileDrawingListLock.ReleaseReaderLock(); } //GMapImage tile = ((PureImageCache)Maps.MyImageCache.Instance).GetImageFromCache(type.DbId, p, zoom) as GMapImage; //if (tile != null && !textureid.ContainsKey(p)) { // generateTexture(p, (Bitmap)tile.Img); } if (textureid.ContainsKey(p)) { int texture = textureid[p]; GL.Enable(EnableCap.Texture2D); GL.BindTexture(TextureTarget.Texture2D, texture); } else { GL.Disable(EnableCap.Texture2D); } long x = p.X * prj.TileSize.Width - topLeftPx.X; long y = p.Y * prj.TileSize.Width - topLeftPx.Y; long xr = p.X * prj.TileSize.Width; long yr = p.Y * prj.TileSize.Width; long x2 = (p.X + 1) * prj.TileSize.Width; long y2 = (p.Y + 1) * prj.TileSize.Width; // generate terrain GL.Begin(PrimitiveType.TriangleStrip); var latlng = prj.FromPixelToLatLng(xr, yr, zoom); double heightl = srtm.getAltitude(latlng.Lat, latlng.Lng); GL.Color3(Color.White); //xr - topLeftPx.X, yr - topLeftPx.Y GL.TexCoord2(0, 0); GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale); // next down latlng = prj.FromPixelToLatLng(xr, y2, zoom); heightl = srtm.getAltitude(latlng.Lat, latlng.Lng); GL.TexCoord2(0, .99); GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale); // next right latlng = prj.FromPixelToLatLng(x2, yr, zoom); heightl = srtm.getAltitude(latlng.Lat, latlng.Lng); GL.TexCoord2(.99, 0); GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale); // next right down latlng = prj.FromPixelToLatLng(x2, y2, zoom); heightl = srtm.getAltitude(latlng.Lat, latlng.Lng); GL.TexCoord2(.99, .99); GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale); GL.End(); } GL.Enable(EnableCap.Blend); GL.DepthMask(false); GL.BlendFunc(BlendingFactorSrc.SrcAlpha, BlendingFactorDest.One); GL.DepthMask(true); GL.Disable(EnableCap.Blend); GL.Flush(); try { this.SwapBuffers(); Context.MakeCurrent(null); } catch { } return; }