private void mainloop()
{
threadrun = true;
EndPoint Remote = new IPEndPoint(IPAddress.Any, 0);
DateTime tracklast = DateTime.Now.AddSeconds(0);
DateTime tunning = DateTime.Now.AddSeconds(0);
DateTime mapupdate = DateTime.Now.AddSeconds(0);
DateTime vidrec = DateTime.Now.AddSeconds(0);
DateTime waypoints = DateTime.Now.AddSeconds(0);
DateTime updatescreen = DateTime.Now;
DateTime tsreal = DateTime.Now;
double taketime = 0;
double timeerror = 0;
while (!IsHandleCreated)
Thread.Sleep(100);
while (threadrun)
{
if (MainV2.comPort.giveComport)
{
Thread.Sleep(50);
updateBindingSource();
continue;
}
if (!MainV2.comPort.logreadmode)
Thread.Sleep(50); // max is only ever 10 hz but we go a little faster to empty the serial queue
try
{
if (aviwriter != null && vidrec.AddMilliseconds(100) <= DateTime.Now)
{
vidrec = DateTime.Now;
hud1.streamjpgenable = true;
//aviwriter.avi_start("test.avi");
// add a frame
aviwriter.avi_add(hud1.streamjpg.ToArray(), (uint) hud1.streamjpg.Length);
// write header - so even partial files will play
aviwriter.avi_end(hud1.Width, hud1.Height, 10);
}
}
catch
{
log.Error("Failed to write avi");
}
// log playback
if (MainV2.comPort.logreadmode && MainV2.comPort.logplaybackfile != null)
{
if (MainV2.comPort.BaseStream.IsOpen)
{
MainV2.comPort.logreadmode = false;
try
{
MainV2.comPort.logplaybackfile.Close();
}
catch
{
log.Error("Failed to close logfile");
}
MainV2.comPort.logplaybackfile = null;
}
//Console.WriteLine(DateTime.Now.Millisecond);
if (updatescreen.AddMilliseconds(300) < DateTime.Now)
{
try
{
updatePlayPauseButton(true);
updateLogPlayPosition();
}
catch
{
log.Error("Failed to update log playback pos");
}
updatescreen = DateTime.Now;
}
//Console.WriteLine(DateTime.Now.Millisecond + " done ");
DateTime logplayback = MainV2.comPort.lastlogread;
try
{
if (!MainV2.comPort.giveComport)
MainV2.comPort.readPacket();
}
catch
{
log.Error("Failed to read log packet");
}
double act = (MainV2.comPort.lastlogread - logplayback).TotalMilliseconds;
if (act > 9999 || act < 0)
act = 0;
double ts = 0;
if (LogPlayBackSpeed == 0)
LogPlayBackSpeed = 0.01;
try
{
ts = Math.Min((act/LogPlayBackSpeed), 1000);
}
catch
{
}
double timetook = (DateTime.Now - tsreal).TotalMilliseconds;
if (timetook != 0)
{
//Console.WriteLine("took: " + timetook + "=" + taketime + " " + (taketime - timetook) + " " + ts);
//Console.WriteLine(MainV2.comPort.lastlogread.Second + " " + DateTime.Now.Second + " " + (MainV2.comPort.lastlogread.Second - DateTime.Now.Second));
//if ((taketime - timetook) < 0)
{
timeerror += (taketime - timetook);
if (ts != 0)
{
ts += timeerror;
timeerror = 0;
}
}
if (Math.Abs(ts) > 1000)
ts = 1000;
}
taketime = ts;
tsreal = DateTime.Now;
if (ts > 0 && ts < 1000)
Thread.Sleep((int) ts);
tracklast = tracklast.AddMilliseconds(ts - act);
tunning = tunning.AddMilliseconds(ts - act);
if (tracklast.Month != DateTime.Now.Month)
{
tracklast = DateTime.Now;
tunning = DateTime.Now;
}
try
{
if (MainV2.comPort.logplaybackfile != null &&
MainV2.comPort.logplaybackfile.BaseStream.Position ==
MainV2.comPort.logplaybackfile.BaseStream.Length)
{
MainV2.comPort.logreadmode = false;
}
}
catch
{
MainV2.comPort.logreadmode = false;
}
}
else
{
// ensure we know to stop
if (MainV2.comPort.logreadmode)
MainV2.comPort.logreadmode = false;
updatePlayPauseButton(false);
if (!playingLog && MainV2.comPort.logplaybackfile != null)
{
continue;
}
}
try
{
CheckAndBindPreFlightData();
//Console.WriteLine(DateTime.Now.Millisecond);
//int fixme;
updateBindingSource();
// Console.WriteLine(DateTime.Now.Millisecond + " done ");
// battery warning.
float warnvolt = Settings.Instance.GetFloat("speechbatteryvolt");
float warnpercent = Settings.Instance.GetFloat("speechbatterypercent");
if (MainV2.comPort.MAV.cs.battery_voltage <= warnvolt)
{
hud1.lowvoltagealert = true;
}
else if ((MainV2.comPort.MAV.cs.battery_remaining) < warnpercent)
{
hud1.lowvoltagealert = true;
}
else
{
hud1.lowvoltagealert = false;
}
// update opengltest
if (OpenGLtest.instance != null)
{
OpenGLtest.instance.rpy = new Vector3(MainV2.comPort.MAV.cs.roll, MainV2.comPort.MAV.cs.pitch,
MainV2.comPort.MAV.cs.yaw);
OpenGLtest.instance.LocationCenter = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat,
MainV2.comPort.MAV.cs.lng, MainV2.comPort.MAV.cs.alt, "here");
}
// update opengltest2
if (OpenGLtest2.instance != null)
{
OpenGLtest2.instance.rpy = new Vector3(MainV2.comPort.MAV.cs.roll, MainV2.comPort.MAV.cs.pitch,
MainV2.comPort.MAV.cs.yaw);
OpenGLtest2.instance.LocationCenter = new PointLatLngAlt(MainV2.comPort.MAV.cs.lat,
MainV2.comPort.MAV.cs.lng, MainV2.comPort.MAV.cs.alt, "here");
}
// update vario info
Vario.SetValue(MainV2.comPort.MAV.cs.climbrate);
// udpate tunning tab
if (tunning.AddMilliseconds(50) < DateTime.Now && CB_tuning.Checked)
{
double time = (Environment.TickCount - tickStart)/1000.0;
if (list1item != null)
list1.Add(time, ConvertToDouble(list1item.GetValue(MainV2.comPort.MAV.cs, null)));
if (list2item != null)
list2.Add(time, ConvertToDouble(list2item.GetValue(MainV2.comPort.MAV.cs, null)));
if (list3item != null)
list3.Add(time, ConvertToDouble(list3item.GetValue(MainV2.comPort.MAV.cs, null)));
if (list4item != null)
list4.Add(time, ConvertToDouble(list4item.GetValue(MainV2.comPort.MAV.cs, null)));
if (list5item != null)
list5.Add(time, ConvertToDouble(list5item.GetValue(MainV2.comPort.MAV.cs, null)));
if (list6item != null)
list6.Add(time, ConvertToDouble(list6item.GetValue(MainV2.comPort.MAV.cs, null)));
if (list7item != null)
list7.Add(time, ConvertToDouble(list7item.GetValue(MainV2.comPort.MAV.cs, null)));
if (list8item != null)
list8.Add(time, ConvertToDouble(list8item.GetValue(MainV2.comPort.MAV.cs, null)));
if (list9item != null)
list9.Add(time, ConvertToDouble(list9item.GetValue(MainV2.comPort.MAV.cs, null)));
if (list10item != null)
list10.Add(time, ConvertToDouble(list10item.GetValue(MainV2.comPort.MAV.cs, null)));
}
// update map
if (tracklast.AddSeconds(1.2) < DateTime.Now)
{
// show disable joystick button
if (MainV2.joystick != null && MainV2.joystick.enabled)
{
this.Invoke((MethodInvoker) delegate {
but_disablejoystick.Visible = true;
});
}
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");
// 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);
}
gMapControl1.UpdateRouteLocalPosition(route);
// update programed wp course
if (waypoints.AddSeconds(5) < DateTime.Now)
{
//Console.WriteLine("Doing FD WP's");
updateClearMissionRouteMarkers();
float dist = 0;
float travdist = 0;
distanceBar1.ClearWPDist();
MAVLink.mavlink_mission_item_t lastplla = new MAVLink.mavlink_mission_item_t();
MAVLink.mavlink_mission_item_t home = new MAVLink.mavlink_mission_item_t();
foreach (MAVLink.mavlink_mission_item_t plla in MainV2.comPort.MAV.wps.Values)
{
if (plla.x == 0 || plla.y == 0)
continue;
if (plla.command == (ushort)MAVLink.MAV_CMD.DO_SET_ROI)
{
addpolygonmarkerred(plla.seq.ToString(), plla.y, plla.x, (int) plla.z, Color.Red,
routes);
continue;
}
string tag = plla.seq.ToString();
if (plla.seq == 0 && plla.current != 2)
{
tag = "Home";
home = plla;
}
if (plla.current == 2)
{
continue;
}
if (lastplla.command == 0)
lastplla = plla;
try
{
dist =
(float)
new PointLatLngAlt(plla.x, plla.y).GetDistance(new PointLatLngAlt(
lastplla.x, lastplla.y));
distanceBar1.AddWPDist(dist);
if (plla.seq <= MainV2.comPort.MAV.cs.wpno)
{
travdist += dist;
}
lastplla = plla;
}
catch
{
}
addpolygonmarker(tag, plla.y, plla.x, (int) plla.z, Color.White, polygons);
}
try
{
//dist = (float)new PointLatLngAlt(home.x, home.y).GetDistance(new PointLatLngAlt(lastplla.x, lastplla.y));
// distanceBar1.AddWPDist(dist);
}
catch
{
}
travdist -= MainV2.comPort.MAV.cs.wp_dist;
if (MainV2.comPort.MAV.cs.mode.ToUpper() == "AUTO")
distanceBar1.traveleddist = 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))
{
rallypointoverlay.Markers.Add(new GMapMarkerAirport(item)
{
ToolTipText = item.Tag,
ToolTipMode = MarkerTooltipMode.OnMouseOver
});
}
}
waypoints = DateTime.Now;
}
updateClearRoutesMarkers();
// add this after the mav icons are drawn
if (MainV2.comPort.MAV.cs.MovingBase != null)
{
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"))
{
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 == 0)
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;
}
// 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)
{
MainV2.comPort.MAV.GMapMarkerOverlapCount.Add(
((GMapMarkerPhoto) mark).footprintpoly);
}
}
if (a < (camcount-4))
((GMapMarkerPhoto)mark).drawfootprint = false;
}
a++;
}
if (CameraOverlap)
{
if (!kmlpolygons.Markers.Contains(MainV2.comPort.MAV.GMapMarkerOverlapCount) &&
camcount > 0)
{
kmlpolygons.Markers.Clear();
kmlpolygons.Markers.Add(MainV2.comPort.MAV.GMapMarkerOverlapCount);
}
}
else if (kmlpolygons.Markers.Contains(MainV2.comPort.MAV.GMapMarkerOverlapCount))
{
kmlpolygons.Markers.Clear();
}
}
catch
{
}
lock (MainV2.instance.adsblock)
{
// cleanup old
for (int a = (routes.Markers.Count - 1); a >= 0; a--)
{
if (routes.Markers[a].ToolTipText != null &&
routes.Markers[a].ToolTipText.Contains("ICAO"))
{
routes.Markers.RemoveAt(a);
}
}
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 + " " + plla.Alt.ToString("0"),
ToolTipMode = MarkerTooltipMode.OnMouseOver,
};
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,
MainV2.comPort.MAV.GuidedMode.x, (int)MainV2.comPort.MAV.GuidedMode.z, Color.Blue,
routes);
}
// draw all icons for all connected mavs
foreach (var port in MainV2.Comports)
{
// draw the mavs seen on this port
foreach (var MAV in port.MAVlist)
{
var marker = Common.getMAVMarker(MAV);
addMissionRouteMarker(marker);
}
}
if (route.Points[route.Points.Count - 1].Lat != 0 &&
(mapupdate.AddSeconds(3) < DateTime.Now) && CHK_autopan.Checked)
{
updateMapPosition(currentloc);
mapupdate = DateTime.Now;
}
if (route.Points.Count == 1 && gMapControl1.Zoom == 3) // 3 is the default load zoom
{
updateMapPosition(currentloc);
updateMapZoom(17);
}
}
gMapControl1.HoldInvalidation = false;
if (gMapControl1.Visible)
{
gMapControl1.Invalidate();
}
tracklast = DateTime.Now;
}
}
catch (Exception ex)
{
log.Error(ex);
Tracking.AddException(ex);
Console.WriteLine("FD Main loop exception " + ex);
}
}
Console.WriteLine("FD Main loop exit");
}
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
{
}
}
}
