static object Convert(Locationwp cmd, bool isint = false)
{
if (isint)
{
var temp = new MAVLink.mavlink_mission_item_int_t()
{
command = cmd.id,
param1 = cmd.p1,
param2 = cmd.p2,
param3 = cmd.p3,
param4 = cmd.p4,
x = (int)(cmd.lat * 1.0e7),
y = (int)(cmd.lng * 1.0e7),
z = (float)cmd.alt
};
return(temp);
}
else
{
var temp = new MAVLink.mavlink_mission_item_t()
{
command = cmd.id,
param1 = cmd.p1,
param2 = cmd.p2,
param3 = cmd.p3,
param4 = cmd.p4,
x = (float)cmd.lat,
y = (float)cmd.lng,
z = (float)cmd.alt
};
return(temp);
}
}
private static void SetMissionItem(int number)
{
MAVLink.mavlink_mission_item_t item = new MAVLink.mavlink_mission_item_t();
item.target_system = sysID;
item.target_component = compID;
item.seq = (ushort)number;
item.command = (ushort)missionItems[number].command;
item.x = (float)missionItems[number].lat;
item.y = (float)missionItems[number].lng;
item.param1 = missionItems[number].p1;
item.param2 = missionItems[number].p2;
item.param3 = missionItems[number].p3;
item.param4 = missionItems[number].p4;
item.z = (float)missionItems[number].alt;
byte[] bytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.MISSION_ITEM, item);
if (SendPacket(bytes))
{
// Notify everyone
if (OnMissionWritten != null)
{
// Convert from zero based to 1-based
OnMissionWritten(number + 1, missionItems.Count());
}
}
}
private MAVLink.MAV_MISSION_RESULT setWP(MAVLink.mavlink_mission_item_t req)
{
ushort index = req.seq;
DateTime start = DateTime.Now;
generatePacket((byte)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM, req);
return(MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED);
}
public static void ConditionYaw(double targetAngle, double angularSpeed, bool CW, bool relative)
{
MAVLink.mavlink_mission_item_t msg = new MAVLink.mavlink_mission_item_t();
msg.seq = 0;
msg.current = 2; // TODO use guided mode enum
msg.frame = (byte)MAVLink.MAV_FRAME.GLOBAL;
msg.command = (ushort)MAVLink.MAV_CMD.CONDITION_YAW;
msg.param1 = (float)targetAngle; // 0-360. 0 is North
msg.param2 = (float)angularSpeed; // Deg per second
msg.param3 = (float)(CW ? 1.0 : -1.0); // 1 - CW, -1 CCW
msg.param4 = (float)(relative ? 1 : 0); // 1 - Relative, 0 - Absolute
msg.autocontinue = 1; // TODO use correct parameter
msg.target_system = sysID;
msg.target_component = compID;
byte[] commandBytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.MISSION_ITEM, msg);
SendPacket(commandBytes);
}
private MAVLink.MAV_MISSION_RESULT setWP(Locationwp loc, ushort index, MAVLink.MAV_FRAME frame, byte current = 0, byte autocontinue = 1)
{
MAVLink.mavlink_mission_item_t req = new MAVLink.mavlink_mission_item_t();
req.target_system = TARGET_SYSTEM_ID;
req.target_component = TARGET_SYS_COMPID; // MSG_NAMES.MISSION_ITEM
req.command = loc.id;
req.current = current;
req.autocontinue = autocontinue;
req.frame = (byte)frame;
req.y = (float)(loc.lng);
req.x = (float)(loc.lat);
req.z = (float)(loc.alt);
req.param1 = loc.p1;
req.param2 = loc.p2;
req.param3 = loc.p3;
req.param4 = loc.p4;
req.seq = index;
return(setWP(req));
}
public static void GoToWaypoint(double latitude, double longitude, double altInMeters)
{
MAVLink.mavlink_mission_item_t msg = new MAVLink.mavlink_mission_item_t();
msg.seq = 0;
msg.current = 2; // TODO use guided mode enum
msg.frame = (byte)MAVLink.MAV_FRAME.GLOBAL;
msg.command = (ushort)MAVLink.MAV_CMD.WAYPOINT; //
msg.param1 = 0; // TODO use correct parameter
msg.param2 = 0; // TODO use correct parameter
msg.param3 = 0; // TODO use correct parameter
msg.param4 = 0; // desired yaw
msg.x = (float)latitude;
msg.y = (float)longitude;
msg.z = (float)altInMeters;
msg.autocontinue = 1; // TODO use correct parameter
msg.target_system = sysID;
msg.target_component = compID;
byte[] commandBytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.MISSION_ITEM, msg);
SendPacket(commandBytes);
}
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;
//comPort.stopall(true);
while (threadrun)
{
if (MainV2.comPort.giveComport)
{
Thread.Sleep(50);
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
{
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 = 0;
float.TryParse(MainV2.getConfig("speechbatteryvolt"), out warnvolt);
float warnpercent = 0;
float.TryParse(MainV2.getConfig("speechbatterypercent"), out warnpercent);
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)
{
if (MainV2.config["CHK_maprotation"] != null &&
MainV2.config["CHK_maprotation"].ToString() == "True")
{
// 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 cnt = 0;
while (gMapControl1.inOnPaint)
{
Thread.Sleep(1);
cnt++;
}
// maintain route history length
if (route.Points.Count > int.Parse(MainV2.config["NUM_tracklength"].ToString()))
{
// trackPoints.RemoveRange(0, trackPoints.Count - int.Parse(MainV2.config["NUM_tracklength"].ToString()));
route.Points.RemoveRange(0,
route.Points.Count - int.Parse(MainV2.config["NUM_tracklength"].ToString()));
}
// add new route point
if (MainV2.comPort.MAV.cs.lat != 0)
{
// trackPoints.Add(currentloc);
route.Points.Add(currentloc);
}
while (gMapControl1.inOnPaint)
{
Thread.Sleep(1);
cnt++;
}
//route = new GMapRoute(route.Points, "track");
//track.Stroke = Pens.Red;
//route.Stroke = new Pen(Color.FromArgb(144, Color.Red));
//route.Stroke.Width = 5;
//route.Tag = "track";
//updateClearRoutes();
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 == (byte) 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;
}
//routes.Polygons.Add(poly);
if (route.Points.Count > 0)
{
// add primary route icon
updateClearRouteMarker(currentloc);
// 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.GetMAVStates())
{
PointLatLng portlocation = new PointLatLng(MAV.cs.lat, MAV.cs.lng);
if (MAV.cs.firmware == MainV2.Firmwares.ArduPlane ||
MAV.cs.firmware == MainV2.Firmwares.Ateryx)
{
routes.Markers.Add(new GMapMarkerPlane(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing)
{
ToolTipText = MAV.cs.alt.ToString("0"),
ToolTipMode = MarkerTooltipMode.Always
});
}
else if (MAV.cs.firmware == MainV2.Firmwares.ArduRover)
{
routes.Markers.Add(new GMapMarkerRover(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.cs.target_bearing));
}
else if (MAV.aptype == MAVLink.MAV_TYPE.HELICOPTER)
{
routes.Markers.Add(new GMapMarkerHeli(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing));
}
else if (MAV.cs.firmware == MainV2.Firmwares.ArduTracker)
{
routes.Markers.Add(new GMapMarkerAntennaTracker(portlocation, MAV.cs.yaw,
MAV.cs.target_bearing));
}
else if (MAV.cs.firmware == MainV2.Firmwares.ArduCopter2)
{
routes.Markers.Add(new GMapMarkerQuad(portlocation, MAV.cs.yaw,
MAV.cs.groundcourse, MAV.cs.nav_bearing, MAV.sysid));
}
else
{
// unknown type
routes.Markers.Add(new GMarkerGoogle(portlocation, GMarkerGoogleType.green_dot));
}
}
}
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.ZoomAndCenterMarkers("routes");// ZoomAndCenterRoutes("routes");
}
}
// add this after the mav icons are drawn
if (MainV2.comPort.MAV.cs.MovingBase != null)
{
routes.Markers.Add(new GMarkerGoogle(currentloc, GMarkerGoogleType.blue_dot)
{
Position = MainV2.comPort.MAV.cs.MovingBase,
ToolTipText = "Moving Base",
ToolTipMode = MarkerTooltipMode.OnMouseOver
});
}
// for testing
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;
routes.Markers.Add(new GMarkerGoogle(marker, GMarkerGoogleType.blue_dot)
{
ToolTipText = "Camera Target\n" + marker,
ToolTipMode = MarkerTooltipMode.OnMouseOver
});
}
}
}
}
catch
{
}
lock (MainV2.instance.adsblock)
{
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
if (((DateTime) MainV2.instance.adsbPlaneAge[plla.Tag]) > DateTime.Now.AddSeconds(-30))
routes.Markers.Add(new GMapMarkerADSBPlane(plla, plla.Heading)
{
ToolTipText = "ICAO: " + plla.Tag + " " + plla.Alt.ToString("0"),
ToolTipMode = MarkerTooltipMode.OnMouseOver
});
}
}
// routes.Markers.Clear();
gMapControl1.HoldInvalidation = false;
if (gMapControl1.Visible)
{
gMapControl1.Invalidate();
}
tracklast = DateTime.Now;
}
}
catch (Exception ex)
{
log.Error(ex);
Console.WriteLine("FD Main loop exception " + ex);
}
}
Console.WriteLine("FD Main loop exit");
}
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");
}
static object Convert(Locationwp cmd, bool isint = false)
{
if (isint)
{
var temp = new MAVLink.mavlink_mission_item_int_t()
{
command = cmd.id,
param1 = cmd.p1,
param2 = cmd.p2,
param3 = cmd.p3,
param4 = cmd.p4,
x = (int)(cmd.lat * 1.0e7),
y = (int)(cmd.lng * 1.0e7),
z = (float) cmd.alt,
seq = cmd._seq,
frame = cmd._frame
};
return temp;
}
else
{
var temp = new MAVLink.mavlink_mission_item_t()
{
command = cmd.id,
param1 = cmd.p1,
param2 = cmd.p2,
param3 = cmd.p3,
param4 = cmd.p4,
x = (float) cmd.lat,
y = (float) cmd.lng,
z = (float) cmd.alt,
seq = cmd._seq,
frame = cmd._frame
};
return temp;
}
}
private void LogPacket(object packet, bool ingoing, int sysId, int compId)
{
if (threadDone)
{
return;
}
if (listViewMessages.InvokeRequired)
{
try { listViewMessages.Invoke(new Action <object, bool, int, int>(LogPacket), packet, ingoing, sysId, compId); }
catch { };
return;
}
List <string> fields = new List <string>();
fields.Add(sysId.ToString());
fields.Add(compId.ToString());
if ((ingoing && !checkBoxIngoing.Checked) || (!ingoing && !checkBoxOutgoing.Checked))
{
return;
}
string messageName = packet.ToString().Replace("MAVLink+mavlink_", "");
if (IsMessageFilteredOut(messageName))
{
return;
}
if (listViewMessages.IsDisposed)
{
return;
}
if (packet.GetType() == typeof(MAVLink.mavlink_gps_raw_int_t))
{
MAVLink.mavlink_gps_raw_int_t gps = (MAVLink.mavlink_gps_raw_int_t)packet;
fields.Add("GPS Raw Int");
fields.Add(((double)gps.lat / 10000000).ToString());
fields.Add(((double)gps.lon / 10000000).ToString());
fields.Add(gps.alt.ToString());
fields.Add(gps.vel.ToString());
fields.Add(gps.satellites_visible.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_global_position_int_t))
{
MAVLink.mavlink_global_position_int_t gps = (MAVLink.mavlink_global_position_int_t)packet;
fields.Add("GPS Global Position Int");
fields.Add(((double)gps.lat / 10000000).ToString());
fields.Add(((double)gps.lon / 10000000).ToString());
fields.Add(gps.alt.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_attitude_t))
{
MAVLink.mavlink_attitude_t att = (MAVLink.mavlink_attitude_t)packet;
fields.Add("Attitude");
fields.Add((att.pitch * 180.0 / Math.PI).ToString());
fields.Add((att.roll * 180.0 / Math.PI).ToString());
fields.Add((att.yaw * 180.0 / Math.PI).ToString());
fields.Add((att.pitchspeed * 180.0 / Math.PI).ToString());
fields.Add((att.rollspeed * 180.0 / Math.PI).ToString());
fields.Add((att.yawspeed * 180.0 / Math.PI).ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu_t))
{
MAVLink.mavlink_scaled_imu_t imu = (MAVLink.mavlink_scaled_imu_t)packet;
fields.Add("Scaled IMU");
fields.Add(imu.xmag.ToString());
fields.Add(imu.ymag.ToString());
fields.Add(imu.zmag.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu3_t))
{
MAVLink.mavlink_scaled_imu3_t imu = (MAVLink.mavlink_scaled_imu3_t)packet;
fields.Add("Scaled IMU3");
fields.Add(imu.xmag.ToString());
fields.Add(imu.ymag.ToString());
fields.Add(imu.zmag.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu2_t))
{
MAVLink.mavlink_scaled_imu2_t imu = (MAVLink.mavlink_scaled_imu2_t)packet;
fields.Add("Scaled IMU2");
fields.Add(imu.xmag.ToString());
fields.Add(imu.ymag.ToString());
fields.Add(imu.zmag.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_sys_status_t))
{
MAVLink.mavlink_sys_status_t status = (MAVLink.mavlink_sys_status_t)packet;
fields.Add("System Status");
fields.Add(status.voltage_battery.ToString());
}
// else if (packet.GetType() == typeof(MAVLink.mavlink_autopilot_version_t))
// {
// MAVLink.mavlink_autopilot_version_t ver = (MAVLink.mavlink_autopilot_version_t)packet;
// listViewMessages.Items.Add(new ListViewItem(new string[] {
// "Autopilot Version",
// ver.version.ToString(),
// ver.custom_version.ToString(),
// ver.capabilities.ToString()}));
// }
else if (packet.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)packet;
fields.Add("Heartbeat");
fields.Add(hb.autopilot.ToString());
fields.Add(hb.system_status.ToString());
fields.Add(hb.mavlink_version.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_statustext_t))
{
MAVLink.mavlink_statustext_t status = (MAVLink.mavlink_statustext_t)packet;
fields.Add("Status Text");
fields.Add(ASCIIEncoding.ASCII.GetString(status.text));
fields.Add(status.severity.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_value_t))
{
MAVLink.mavlink_param_value_t paramValue = (MAVLink.mavlink_param_value_t)packet;
fields.Add("Param Value");
fields.Add(ASCIIEncoding.ASCII.GetString(paramValue.param_id));
fields.Add(paramValue.param_value.ToString());
fields.Add(paramValue.param_count.ToString());
fields.Add(paramValue.param_index.ToString());
fields.Add(paramValue.param_type.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_request_read_t))
{
MAVLink.mavlink_param_request_read_t paramReq = (MAVLink.mavlink_param_request_read_t)packet;
fields.Add("Param Request Read");
fields.Add(ASCIIEncoding.ASCII.GetString(paramReq.param_id));
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_set_t))
{
MAVLink.mavlink_param_set_t paramSet = (MAVLink.mavlink_param_set_t)packet;
fields.Add("Param Set");
fields.Add(ASCIIEncoding.ASCII.GetString(paramSet.param_id));
fields.Add(paramSet.param_value.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_count_t))
{
MAVLink.mavlink_mission_count_t paramValue = (MAVLink.mavlink_mission_count_t)packet;
fields.Add("Mission Count");
fields.Add(paramValue.count.ToString());
fields.Add(paramValue.target_component.ToString());
fields.Add(paramValue.target_system.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_item_t))
{
MAVLink.mavlink_mission_item_t item = (MAVLink.mavlink_mission_item_t)packet;
fields.Add("Mission Item");
fields.Add(item.seq.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_request_t))
{
MAVLink.mavlink_mission_request_t item = (MAVLink.mavlink_mission_request_t)packet;
fields.Add("Mission Request Item");
fields.Add(item.seq.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_command_ack_t))
{
MAVLink.mavlink_command_ack_t paramValue = (MAVLink.mavlink_command_ack_t)packet;
fields.Add("Ack");
fields.Add(((MAVLink.MAV_CMD)paramValue.command).ToString());
fields.Add(((MAVLink.MAV_RESULT)paramValue.result).ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_ack_t))
{
MAVLink.mavlink_mission_ack_t paramValue = (MAVLink.mavlink_mission_ack_t)packet;
fields.Add("Mission Ack");
fields.Add(paramValue.type.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_radio_status_t))
{
MAVLink.mavlink_radio_status_t radio = (MAVLink.mavlink_radio_status_t)packet;
fields.Add("Radio Status");
fields.Add(radio.rssi.ToString());
fields.Add(radio.remrssi.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_ekf_status_report_t))
{
MAVLink.mavlink_ekf_status_report_t ekf = (MAVLink.mavlink_ekf_status_report_t)packet;
fields.Add("EKF Status");
fields.Add(ekf.flags.ToString());
fields.Add(ekf.velocity_variance.ToString());
fields.Add(ekf.pos_horiz_variance.ToString());
fields.Add(ekf.pos_vert_variance.ToString());
fields.Add(ekf.compass_variance.ToString());
fields.Add(ekf.terrain_alt_variance.ToString());
}
else
{
fields.Add(messageName);
//Log(packet.ToString());
}
if (ingoing)
{
listViewMessages.Items.Add(INGOING(new ListViewItem(fields.ToArray())));
}
else
{
listViewMessages.Items.Add(OUTGOING(new ListViewItem(fields.ToArray())));
}
}
private Locationwp getWP(ushort index, ref bool succeed)
{
Locationwp loc = new Locationwp();
MAVLink.mavlink_mission_request_t req = new MAVLink.mavlink_mission_request_t();
req.target_system = TARGET_SYSTEM_ID;
req.target_component = TARGET_SYS_COMPID;
req.seq = index;
MAVLink.mavlink_mission_item_t wp = new MAVLink.mavlink_mission_item_t();
bool found_ret = false;
for (int i = 0; i < 3; i++)
{
generatePacket((byte)MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST, req);
Program.mav_msg_handler.SetTimeout(5);
while (found_ret == false)
{
if (Program.mav_msg_handler.Wait())
{
this.ListMessage("获取航点坐标" + index.ToString() + "超时失败[" + (i + 1).ToString() + "]");
break;
}
Application.DoEvents();
if (Program.mav_msg_handler.mission_items.ContainsKey(req.seq))
{
found_ret = true;
wp = Program.mav_msg_handler.mission_items[req.seq];
break;
}
}
if (found_ret == false)
{
succeed = false;
}
else
{
succeed = true;
loc.options = (byte)(wp.frame);
loc.id = (byte)(wp.command);
loc.p1 = (wp.param1);
loc.p2 = (wp.param2);
loc.p3 = (wp.param3);
loc.p4 = (wp.param4);
loc.alt = ((wp.z));
loc.lat = ((wp.x));
loc.lng = ((wp.y));
this.ListMessage("获取航点坐标" + index.ToString() + "成功...");
break;
}
}
return(loc);
}
private static void HandlePacket(object packet)
{
if (packet.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
MAVLink.mavlink_heartbeat_t parsed = (MAVLink.mavlink_heartbeat_t)packet;
droneState = (MAVLink.MAV_STATE)parsed.system_status;
//MAVLink.MAV_MODE
mode = (APMModes.Quadrotor)parsed.custom_mode;
}
else if (packet.GetType() == typeof(MAVLink.mavlink_global_position_int_t))
{
MAVLink.mavlink_global_position_int_t gps = (MAVLink.mavlink_global_position_int_t)packet;
// dronePosition.lat = (double)gps.lat / 10000000;
// dronePosition.lng = (double)gps.lon / 10000000;
// dronePosition.alt = (double)gps.alt / 1000;
dronePosition.altFromGround = (double)gps.relative_alt / 1000;
dronePosition.heading = (double)gps.hdg / 100;
}
else if (packet.GetType() == typeof(MAVLink.mavlink_gps_raw_int_t))
{
MAVLink.mavlink_gps_raw_int_t gps = (MAVLink.mavlink_gps_raw_int_t)packet;
GPSHdop = (double)gps.eph / 100;
dronePosition.lat = (double)gps.lat / 10000000;
dronePosition.lng = (double)gps.lon / 10000000;
dronePosition.alt = (double)gps.alt / 1000;
satCount = gps.satellites_visible;
IsDroneReady();
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_count_t))
{
MAVLink.mavlink_mission_count_t paramValue = (MAVLink.mavlink_mission_count_t)packet;
missionWaypointsCount = paramValue.count;
missionItems = new SMissionItem[missionWaypointsCount];
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_item_t))
{
MAVLink.mavlink_mission_item_t paramValue = (MAVLink.mavlink_mission_item_t)packet;
SMissionItem missionItem = new SMissionItem();
missionItem.command = (MAVLink.MAV_CMD)paramValue.command;
// if (missionItem.command == MAVLink.MAV_CMD.TAKEOFF)
// // || (missionItem.command == MAVLink.MAV_CMD.RETURN_TO_LAUNCH))
// {
// missionItem.lat = dronePosition.lat;
// missionItem.lng = dronePosition.lng;
// }
// else
{
missionItem.lat = paramValue.x;
missionItem.lng = paramValue.y;
}
missionItem.p1 = paramValue.param1;
missionItem.p2 = paramValue.param2;
missionItem.p3 = paramValue.param3;
missionItem.p4 = paramValue.param4;
missionItem.alt = paramValue.z;
// Is this the home?
missionItem.IsHome = (paramValue.seq == 0);
missionItems[paramValue.seq] = missionItem;
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_request_t))
{
MAVLink.mavlink_mission_request_t missionReq = (MAVLink.mavlink_mission_request_t)packet;
SetMissionItem(missionReq.seq);
IsDroneReady();
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_value_t))
{
MAVLink.mavlink_param_value_t paramValue = (MAVLink.mavlink_param_value_t)packet;
string name = ASCIIEncoding.ASCII.GetString(paramValue.param_id);
name = name.Trim().Replace("\0", "");
double val = paramValue.param_value;
if (OnParamUpdate != null)
{
OnParamUpdate(name, val, paramValue.param_index);
}
}
else if (packet.GetType() == typeof(MAVLink.mavlink_ekf_status_report_t))
{
MAVLink.mavlink_ekf_status_report_t ekf = (MAVLink.mavlink_ekf_status_report_t)packet;
// Check that all estimations are good
EKFStatus = (float)Math.Max(ekf.pos_vert_variance, Math.Max(ekf.compass_variance, Math.Max(ekf.pos_horiz_variance, Math.Max(ekf.pos_vert_variance, ekf.terrain_alt_variance))));
EKFFlags = ekf.flags;
IsDroneReady();
}
}
