private void mainloop() { run = true; DateTime tracklast = DateTime.Now.AddSeconds(0); List <PointLatLng> trackPoints = new List <PointLatLng>(); DateTime waypoints = DateTime.Now.AddSeconds(0); DateTime mapupdate = DateTime.Now.AddSeconds(0); while (run) { try { Thread.Sleep(50); Forms.Device.BeginInvokeOnMainThread(() => { var start = DateTime.Now; Hud.airspeed = MainV2.comPort.MAV.cs.airspeed; Hud.alt = MainV2.comPort.MAV.cs.alt; Hud.batterylevel = (float)MainV2.comPort.MAV.cs.battery_voltage; Hud.batteryremaining = MainV2.comPort.MAV.cs.battery_remaining; Hud.connected = MainV2.comPort.MAV.cs.connected; Hud.current = (float)MainV2.comPort.MAV.cs.current; Hud.datetime = MainV2.comPort.MAV.cs.datetime; Hud.disttowp = MainV2.comPort.MAV.cs.wp_dist; Hud.ekfstatus = MainV2.comPort.MAV.cs.ekfstatus; Hud.failsafe = MainV2.comPort.MAV.cs.failsafe; Hud.gpsfix = MainV2.comPort.MAV.cs.gpsstatus; Hud.gpsfix2 = MainV2.comPort.MAV.cs.gpsstatus2; Hud.gpshdop = MainV2.comPort.MAV.cs.gpshdop; Hud.gpshdop2 = MainV2.comPort.MAV.cs.gpshdop2; Hud.groundalt = (float)MainV2.comPort.MAV.cs.HomeAlt; Hud.groundcourse = MainV2.comPort.MAV.cs.groundcourse; Hud.groundspeed = MainV2.comPort.MAV.cs.groundspeed; Hud.heading = MainV2.comPort.MAV.cs.yaw; Hud.linkqualitygcs = MainV2.comPort.MAV.cs.linkqualitygcs; Hud.message = MainV2.comPort.MAV.cs.messageHigh; Hud.messagetime = MainV2.comPort.MAV.cs.messageHighTime; Hud.mode = MainV2.comPort.MAV.cs.mode; Hud.navpitch = MainV2.comPort.MAV.cs.nav_pitch; Hud.navroll = MainV2.comPort.MAV.cs.nav_roll; Hud.pitch = MainV2.comPort.MAV.cs.pitch; Hud.roll = MainV2.comPort.MAV.cs.roll; Hud.status = MainV2.comPort.MAV.cs.armed; Hud.targetalt = MainV2.comPort.MAV.cs.targetalt; Hud.targetheading = MainV2.comPort.MAV.cs.nav_bearing; Hud.targetspeed = MainV2.comPort.MAV.cs.targetairspeed; Hud.turnrate = MainV2.comPort.MAV.cs.turnrate; Hud.verticalspeed = MainV2.comPort.MAV.cs.verticalspeed; Hud.vibex = MainV2.comPort.MAV.cs.vibex; Hud.vibey = MainV2.comPort.MAV.cs.vibey; Hud.vibez = MainV2.comPort.MAV.cs.vibez; Hud.wpno = (int)MainV2.comPort.MAV.cs.wpno; Hud.xtrack_error = MainV2.comPort.MAV.cs.xtrack_error; Hud.AOA = MainV2.comPort.MAV.cs.AOA; Hud.SSA = MainV2.comPort.MAV.cs.SSA; Hud.critAOA = MainV2.comPort.MAV.cs.crit_AOA; // update map if (tracklast.AddSeconds(Settings.Instance.GetDouble("FD_MapUpdateDelay", 1.2)) < DateTime.Now) { adsb.CurrentPosition = MainV2.comPort.MAV.cs.HomeLocation; // show proximity screen if (MainV2.comPort.MAV?.Proximity != null && MainV2.comPort.MAV.Proximity.DataAvailable) { //this.BeginInvoke((MethodInvoker)delegate { new ProximityControl(MainV2.comPort.MAV).Show(); }); } if (Settings.Instance.GetBoolean("CHK_maprotation")) { // dont holdinvalidation here setMapBearing(); } if (route == null) { route = new GMapRoute(trackPoints, "track"); routes.Routes.Add(route); } PointLatLng currentloc = new PointLatLng(MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng); gMapControl1.HoldInvalidation = true; int numTrackLength = Settings.Instance.GetInt32("NUM_tracklength", 200); // maintain route history length if (route.Points.Count > numTrackLength) { route.Points.RemoveRange(0, route.Points.Count - numTrackLength); } // add new route point if (MainV2.comPort.MAV.cs.lat != 0 && MainV2.comPort.MAV.cs.lng != 0) { route.Points.Add(currentloc); } updateRoutePosition(); // update programed wp course if (waypoints.AddSeconds(5) < DateTime.Now) { //Console.WriteLine("Doing FD WP's"); updateClearMissionRouteMarkers(); var wps = MainV2.comPort.MAV.wps.Values.ToList(); if (wps.Count >= 1) { var homeplla = new PointLatLngAlt(MainV2.comPort.MAV.cs.HomeLocation.Lat, MainV2.comPort.MAV.cs.HomeLocation.Lng, MainV2.comPort.MAV.cs.HomeLocation.Alt / CurrentState.multiplieralt, "H"); var overlay = new WPOverlay(); { List <Locationwp> mission_items; mission_items = MainV2.comPort.MAV.wps.Values.Select(a => (Locationwp)a).ToList(); mission_items.RemoveAt(0); if (wps.Count == 1) { overlay.CreateOverlay((MAVLink.MAV_FRAME)wps[0].frame, homeplla, mission_items, 0 / CurrentState.multiplieralt, 0 / CurrentState.multiplieralt); } else { overlay.CreateOverlay((MAVLink.MAV_FRAME)wps[1].frame, homeplla, mission_items, 0 / CurrentState.multiplieralt, 0 / CurrentState.multiplieralt); } } var existing = gMapControl1.Overlays.Where(a => a.Id == overlay.overlay.Id).ToList(); foreach (var b in existing) { gMapControl1.Overlays.Remove(b); } gMapControl1.Overlays.Insert(1, overlay.overlay); overlay.overlay.ForceUpdate(); //distanceBar1.ClearWPDist(); var i = -1; var travdist = 0.0; var lastplla = overlay.pointlist.First(); foreach (var plla in overlay.pointlist) { i++; if (plla == null) { continue; } var dist = lastplla.GetDistance(plla); //distanceBar1.AddWPDist((float)dist); if (i <= MainV2.comPort.MAV.cs.wpno) { travdist += dist; } } travdist -= MainV2.comPort.MAV.cs.wp_dist; //if (MainV2.comPort.MAV.cs.mode.ToUpper() == "AUTO") //distanceBar1.traveleddist = (float)travdist; } RegeneratePolygon(); // update rally points rallypointoverlay.Markers.Clear(); foreach (var mark in MainV2.comPort.MAV.rallypoints.Values) { rallypointoverlay.Markers.Add(new GMapMarkerRallyPt(mark)); } // optional on Flight data if (MainV2.ShowAirports) { // airports foreach (var item in Airports.getAirports(gMapControl1.Position).ToArray()) { try { rallypointoverlay.Markers.Add(new GMapMarkerAirport(item) { ToolTipText = item.Tag, ToolTipMode = MarkerTooltipMode.OnMouseOver }); } catch (Exception e) { log.Error(e); } } } waypoints = DateTime.Now; } updateClearRoutesMarkers(); // add this after the mav icons are drawn if (MainV2.comPort.MAV.cs.MovingBase != null && MainV2.comPort.MAV.cs.MovingBase == PointLatLngAlt.Zero) { addMissionRouteMarker(new GMarkerGoogle(currentloc, GMarkerGoogleType.blue_dot) { Position = MainV2.comPort.MAV.cs.MovingBase, ToolTipText = "Moving Base", ToolTipMode = MarkerTooltipMode.OnMouseOver }); } // add gimbal point center try { if (MainV2.comPort.MAV.param.ContainsKey("MNT_STAB_TILT") && MainV2.comPort.MAV.param.ContainsKey("MNT_STAB_ROLL") && MainV2.comPort.MAV.param.ContainsKey("MNT_TYPE")) { float temp1 = (float)MainV2.comPort.MAV.param["MNT_STAB_TILT"]; float temp2 = (float)MainV2.comPort.MAV.param["MNT_STAB_ROLL"]; float temp3 = (float)MainV2.comPort.MAV.param["MNT_TYPE"]; if (MainV2.comPort.MAV.param.ContainsKey("MNT_STAB_PAN") && // (float)MainV2.comPort.MAV.param["MNT_STAB_PAN"] == 1 && ((float)MainV2.comPort.MAV.param["MNT_STAB_TILT"] == 1 && (float)MainV2.comPort.MAV.param["MNT_STAB_ROLL"] == 0) || (float)MainV2.comPort.MAV.param["MNT_TYPE"] == 4) // storm driver { /* * var marker = GimbalPoint.ProjectPoint(); * * if (marker != PointLatLngAlt.Zero) * { * MainV2.comPort.MAV.cs.GimbalPoint = marker; * * addMissionRouteMarker(new GMarkerGoogle(marker, GMarkerGoogleType.blue_dot) * { * ToolTipText = "Camera Target\n" + marker, * ToolTipMode = MarkerTooltipMode.OnMouseOver * }); * } */ } } // cleanup old - no markers where added, so remove all old if (MainV2.comPort.MAV.camerapoints.Count < photosoverlay.Markers.Count) { photosoverlay.Markers.Clear(); } var min_interval = 0.0; if (MainV2.comPort.MAV.param.ContainsKey("CAM_MIN_INTERVAL")) { min_interval = MainV2.comPort.MAV.param["CAM_MIN_INTERVAL"].Value / 1000.0; } // set fov's based on last grid calc if (Settings.Instance["camera_fovh"] != null) { GMapMarkerPhoto.hfov = Settings.Instance.GetDouble("camera_fovh"); GMapMarkerPhoto.vfov = Settings.Instance.GetDouble("camera_fovv"); } // add new - populate camera_feedback to map double oldtime = double.MinValue; foreach (var mark in MainV2.comPort.MAV.camerapoints.ToArray()) { var timesincelastshot = (mark.time_usec / 1000.0) / 1000.0 - oldtime; MainV2.comPort.MAV.cs.timesincelastshot = timesincelastshot; bool contains = photosoverlay.Markers.Any(p => p.Tag.Equals(mark.time_usec)); if (!contains) { if (timesincelastshot < min_interval) { addMissionPhotoMarker(new GMapMarkerPhoto(mark, true)); } else { addMissionPhotoMarker(new GMapMarkerPhoto(mark, false)); } } oldtime = (mark.time_usec / 1000.0) / 1000.0; } var GMapMarkerOverlapCount = new GMapMarkerOverlapCount(PointLatLng.Empty); // age current int camcount = MainV2.comPort.MAV.camerapoints.Count; int a = 0; foreach (var mark in photosoverlay.Markers) { if (mark is GMapMarkerPhoto) { if (CameraOverlap) { var marker = ((GMapMarkerPhoto)mark); // abandon roll higher than 25 degrees if (Math.Abs(marker.Roll) < 25) { GMapMarkerOverlapCount.Add( ((GMapMarkerPhoto)mark).footprintpoly); } } if (a < (camcount - 4)) { ((GMapMarkerPhoto)mark).drawfootprint = false; } } a++; } if (CameraOverlap) { if (!kmlpolygons.Markers.Contains(GMapMarkerOverlapCount) && camcount > 0) { kmlpolygons.Markers.Clear(); kmlpolygons.Markers.Add(GMapMarkerOverlapCount); } } else if (kmlpolygons.Markers.Contains(GMapMarkerOverlapCount)) { kmlpolygons.Markers.Clear(); } } catch (Exception ex) { log.Error(ex); } lock (MainV2.instance.adsblock) { foreach (adsb.PointLatLngAltHdg plla in MainV2.instance.adsbPlanes.Values) { // 30 seconds history if (((DateTime)plla.Time) > DateTime.Now.AddSeconds(-30)) { var adsbplane = new GMapMarkerADSBPlane(plla, plla.Heading) { ToolTipText = "ICAO: " + plla.Tag + "\n" + "Alt: " + plla.Alt.ToString("0") + "\n" + "Speed: " + plla.Speed.ToString("0") + "\n" + "Heading: " + plla.Heading.ToString("0") , ToolTipMode = MarkerTooltipMode.OnMouseOver, Tag = plla }; if (plla.DisplayICAO) { adsbplane.ToolTipMode = MarkerTooltipMode.Always; } switch (plla.ThreatLevel) { case MAVLink.MAV_COLLISION_THREAT_LEVEL.NONE: adsbplane.AlertLevel = GMapMarkerADSBPlane.AlertLevelOptions.Green; break; case MAVLink.MAV_COLLISION_THREAT_LEVEL.LOW: adsbplane.AlertLevel = GMapMarkerADSBPlane.AlertLevelOptions.Orange; break; case MAVLink.MAV_COLLISION_THREAT_LEVEL.HIGH: adsbplane.AlertLevel = GMapMarkerADSBPlane.AlertLevelOptions.Red; break; } addMissionRouteMarker(adsbplane); } } } if (route.Points.Count > 0) { // add primary route icon // draw guide mode point for only main mav if (MainV2.comPort.MAV.cs.mode.ToLower() == "guided" && MainV2.comPort.MAV.GuidedMode.x != 0) { addpolygonmarker("Guided Mode", MainV2.comPort.MAV.GuidedMode.y / 1e7, MainV2.comPort.MAV.GuidedMode.x / 1e7, (int)MainV2.comPort.MAV.GuidedMode.z, Color.Blue, routes); } // draw all icons for all connected mavs foreach (var port in MainV2.Comports.ToArray()) { // draw the mavs seen on this port foreach (var MAV in port.MAVlist) { var marker = Common.getMAVMarker(MAV); if (marker.Position.Lat == 0 && marker.Position.Lng == 0) { continue; } addMissionRouteMarker(marker); } } if (route.Points.Count == 0 || route.Points[route.Points.Count - 1].Lat != 0 && (mapupdate.AddSeconds(3) < DateTime.Now) && CHK_autopan.IsToggled) { updateMapPosition(currentloc); mapupdate = DateTime.Now; } if (route.Points.Count == 1 && gMapControl1.Zoom == 3) // 3 is the default load zoom { updateMapPosition(currentloc); updateMapZoom(17); } } prop.Update(MainV2.comPort.MAV.cs.HomeLocation, MainV2.comPort.MAV.cs.Location, MainV2.comPort.MAV.cs.battery_kmleft); prop.alt = MainV2.comPort.MAV.cs.alt; prop.altasl = MainV2.comPort.MAV.cs.altasl; prop.center = gMapControl1.Position; gMapControl1.HoldInvalidation = false; if (gMapControl1.Visible) { gMapControl1.Invalidate(); } tracklast = DateTime.Now; } var ts = (DateTime.Now - start); //Console.WriteLine("Hud update {0}", ts.TotalSeconds); }); } catch { } } }
private void domainUpDown1_ValueChanged(object sender, EventArgs e) { if (loading) { return; } if (CMB_camera.Text != "") { doCalc(); } // new grid system test grid = Grid.CreateGrid(list, CurrentState.fromDistDisplayUnit((double)NUM_altitude.Value), (double)NUM_Distance, (double)NUM_spacing, (double)NUM_angle.Value, (double)NUM_overshoot, (double)NUM_overshoot2, (Grid.StartPosition)Enum.Parse(typeof(Grid.StartPosition), CMB_startfrom.Text), false, (float)NUM_Lane_Dist, (float)NUM_leadin); map.HoldInvalidation = true; routesOverlay.Routes.Clear(); routesOverlay.Polygons.Clear(); routesOverlay.Markers.Clear(); GMapMarkerOverlap.Clear(); if (grid.Count == 0) { return; } if (chk_crossgrid.Checked) { // add crossover Grid.StartPointLatLngAlt = grid[grid.Count - 1]; grid.AddRange(Grid.CreateGrid(list, CurrentState.fromDistDisplayUnit((double)NUM_altitude.Value), (double)NUM_Distance, (double)NUM_spacing, (double)NUM_angle.Value + 90.0, (double)NUM_overshoot, (double)NUM_overshoot2, Grid.StartPosition.Point, false, (float)NUM_Lane_Dist, (float)NUM_leadin)); } //if (CHK_boundary.Checked) AddDrawPolygon(); int strips = 0; int images = 0; int a = 1; PointLatLngAlt prevpoint = grid[0]; float routetotal = 0; List <PointLatLng> segment = new List <PointLatLng>(); double maxgroundelevation = double.MinValue; double mingroundelevation = double.MaxValue; double startalt = plugin.Host.cs.HomeAlt; foreach (var item in grid) { double currentalt = srtm.getAltitude(item.Lat, item.Lng).alt; mingroundelevation = Math.Min(mingroundelevation, currentalt); maxgroundelevation = Math.Max(maxgroundelevation, currentalt); if (item.Tag == "M") { images++; //if (CHK_internals.Checked) MP中未直接打勾的就先暂时认为是不用选中 //{ // routesOverlay.Markers.Add(new GMarkerGoogle(item, GMarkerGoogleType.green) { ToolTipText = a.ToString(), ToolTipMode = MarkerTooltipMode.OnMouseOver }); // a++; // segment.Add(prevpoint); // segment.Add(item); // prevpoint = item; //} try { if (TXT_fovH != "") { if (CHK_footprints.Checked) { double fovh = double.Parse(TXT_fovH); double fovv = double.Parse(TXT_fovV); getFOV(item.Alt + startalt - currentalt, ref fovh, ref fovv); double startangle = 0; //if (!CHK_camdirection.Checked) MP中直接打勾的视为必选项 //{ startangle = 90; //} double angle1 = startangle - (Math.Sin((fovh / 2.0) / (fovv / 2.0)) * rad2deg); double dist1 = Math.Sqrt(Math.Pow(fovh / 2.0, 2) + Math.Pow(fovv / 2.0, 2)); double bearing = (double)NUM_angle.Value; //if (CHK_copter_headinghold.Checked) //{ // bearing = Convert.ToInt32(TXT_headinghold); //} double fovha = 0; double fovva = 0; getFOVangle(ref fovha, ref fovva); var itemcopy = new PointLatLngAlt(item); itemcopy.Alt += startalt; var temp = ImageProjection.calc(itemcopy, 0, 0, bearing + startangle, fovha, fovva); List <PointLatLng> footprint = new List <PointLatLng>(); footprint.Add(temp[0]); footprint.Add(temp[1]); footprint.Add(temp[2]); footprint.Add(temp[3]); GMapPolygon poly = new GMapPolygon(footprint, a.ToString()); poly.Stroke = new Pen(Color.FromArgb(250 - ((a * 5) % 240), 250 - ((a * 3) % 240), 250 - ((a * 9) % 240)), 1); poly.Fill = new SolidBrush(Color.Transparent); GMapMarkerOverlap.Add(poly); routesOverlay.Polygons.Add(poly); a++; } } } catch { } } else { if (item.Tag != "SM" && item.Tag != "ME") { strips++; } if (CHK_markers.Checked) { var marker = new GMapMarkerWP(item, a.ToString()) { ToolTipText = a.ToString(), ToolTipMode = MarkerTooltipMode.OnMouseOver }; routesOverlay.Markers.Add(marker); } segment.Add(prevpoint); segment.Add(item); prevpoint = item; a++; } GMapRoute seg = new GMapRoute(segment, "segment" + a.ToString()); seg.Stroke = new Pen(Color.Yellow, 4); seg.Stroke.DashStyle = System.Drawing.Drawing2D.DashStyle.Custom; seg.IsHitTestVisible = true; routetotal = routetotal + (float)seg.Distance; //if (CHK_grid) //{ routesOverlay.Routes.Add(seg); //} //else //{ // seg.Dispose(); //} segment.Clear(); } if (CHK_footprints.Checked) { routesOverlay.Markers.Add(GMapMarkerOverlap); } /* Old way of drawing route, incase something breaks using segments * GMapRoute wproute = new GMapRoute(list2, "GridRoute"); * wproute.Stroke = new Pen(Color.Yellow, 4); * if (chk_grid.Checked) * routesOverlay.Routes.Add(wproute); */ // turn radrad = tas^2 / (tan(angle) * G) // float v_sq = (float)(((float)NUM_UpDownFlySpeed.Value / CurrentState.multiplierspeed) * ((float)NUM_UpDownFlySpeed.Value / CurrentState.multiplierspeed)); // float turnrad = (float)(v_sq / (float)(9.808f * Math.Tan(35 * deg2rad))); // Update Stats if (DistUnits == "Feet") { // Area float area = (float)calcpolygonarea(list) * 10.7639f; // Calculate the area in square feet lbl_area.Text = area.ToString("#") + " ft^2"; if (area < 21780f) { lbl_area.Text = area.ToString("#") + " ft^2"; } else { area = area / 43560f; if (area < 640f) { lbl_area.Text = area.ToString("0.##") + " acres"; } else { area = area / 640f; lbl_area.Text = area.ToString("0.##") + " miles^2"; } } // Distance float distance = routetotal * 3280.84f; // Calculate the distance in feet if (distance < 5280f) { lbl_distance.Text = distance.ToString("#") + " ft"; } else { distance = distance / 5280f; lbl_distance.Text = distance.ToString("0.##") + " miles"; } lbl_spacing.Text = (NUM_spacing * 3.2808399m).ToString("#") + " ft"; lbl_grndres.Text = inchpixel; lbl_distbetweenlines.Text = (NUM_Distance * 3.2808399m).ToString("0.##") + " ft"; lbl_footprint.Text = feet_fovH + " x " + feet_fovV + " ft"; //lbl_turnrad.Text = (turnrad * 2 * 3.2808399).ToString("0") + " ft"; lbl_gndelev.Text = (mingroundelevation * 3.2808399).ToString("0") + "-" + (maxgroundelevation * 3.2808399).ToString("0") + " ft"; } else { // Meters lbl_area.Text = calcpolygonarea(list).ToString("#") + " m^2"; lbl_distance.Text = routetotal.ToString("0.##") + " km"; lbl_spacing.Text = NUM_spacing.ToString("#") + " m"; lbl_grndres.Text = TXT_cmpixel; lbl_distbetweenlines.Text = NUM_Distance.ToString("0.##") + " m"; lbl_footprint.Text = TXT_fovH + " x " + TXT_fovV + " m"; //lbl_turnrad.Text = (turnrad * 2).ToString("0") + " m"; lbl_gndelev.Text = mingroundelevation.ToString("0") + "-" + maxgroundelevation.ToString("0") + " m"; } try { // speed m/s var speed = ((float)NUM_UpDownFlySpeed / CurrentState.multiplierspeed); // cmpix cm/pixel var cmpix = float.Parse(TXT_cmpixel.TrimEnd(new[] { 'c', 'm', ' ' })); // m pix = m/pixel var mpix = cmpix * 0.01; // gsd / 2.0 var minmpix = mpix / 2.0; // min sutter speed var minshutter = speed / minmpix; //lbl_minshutter.Text = "1/" + (minshutter - minshutter % 1).ToString(); } catch { } double flyspeedms = CurrentState.fromSpeedDisplayUnit((double)NUM_UpDownFlySpeed); lbl_pictures.Text = images.ToString(); lbl_strips.Text = ((int)(strips / 2)).ToString(); double seconds = ((routetotal * 1000.0) / ((flyspeedms) * 0.8)); // reduce flying speed by 20 % lbl_flighttime.Text = secondsToNice(seconds); seconds = ((routetotal * 1000.0) / (flyspeedms)); lbl_photoevery.Text = secondsToNice(((double)NUM_spacing / flyspeedms)); map.HoldInvalidation = false; if (!isMouseDown && sender != NUM_angle) { map.ZoomAndCenterMarkers("routes"); } CalcHeadingHold(); map.Invalidate(); }