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