public void ReadFromMissionPlanner() { WebServiceConsumer consumer = WebServiceConsumer.GetInstance(); DateTime currentTime; while (IsOpen) { string result = consumer.GetWebServiceData("http://" + hostIP + ":" + port + "/network.kml\n"); currentTime = DateTime.Now; // Debug.WriteLine(result); if (result != null && result.Length > 0) { XmlDocument xmlDoc = new XmlDocument(); xmlDoc.LoadXml(result); XmlNodeList location = xmlDoc.GetElementsByTagName("Location"); Coordinate3D coord = new Coordinate3D(); foreach (XmlNode node in location[0].ChildNodes) { double val = 0.0; if (node.Name == "longitude") { if (Double.TryParse(node.InnerText, out val)) { coord.Lon = val; } } else if (node.Name == "latitude") { if (Double.TryParse(node.InnerText, out val)) { coord.Lat = val; } } else if (node.Name == "altitude") { if (Double.TryParse(node.InnerText, out val)) { coord.Alt = val; } } } LastMessage = coord; } Duration = currentTime - missionStart; if (callback != null) { callback.MissionPointUpdate(LastMessage, Position, MaximumPosition, currentTime, Duration); } Thread.Sleep(500); } }
private bool ReadNextMAVPacket() { bool readok = true; bool hasLat = false; bool hasWind = false; LastMessage = new Coordinate3D(); lock (this) { while (IsOpen && readok && !(hasLat && hasWind)) { readok = false; if (reader.BaseStream.Position < reader.BaseStream.Length) { byte[] packet = readPacket(); if (packet[5] == MAVLinkTypes.MAVLINK_MSG_ID_GLOBAL_POSITION_INT) { var gps = packet.ByteArrayToStructure<MAVLinkTypes.mavlink_global_position_int_t>(6); ((Coordinate3D)LastMessage).Lat = gps.lat * 1.0e-7f; ((Coordinate3D)LastMessage).Lon = gps.lon * 1.0e-7f; ((Coordinate3D)LastMessage).Alt = gps.alt * 1.0e-3f; hasLat = true; } if (packet[5] == MAVLinkTypes.MAVLINK_MSG_ID_WIND) { var vfr = packet.ByteArrayToStructure<MAVLinkTypes.mavlink_wind_t>(6); ((Coordinate3D)LastMessage).Wind.WindSpeed = vfr.speed; ((Coordinate3D)LastMessage).Wind.WindBearing = vfr.direction; hasWind = true; } readok = true; } } } return readok; }
public MissionPoint AddMissionPoint(Coordinate3D coord, TargetType type, string annotation) { MissionPoint mp = null; if (mission != null) { index++; mp = new MissionPoint(mission.ID, index, coord.Lat, coord.Lon, coord.Alt, type, CurrentMissionTime, annotation); mission.AddMissionPoint(mp); if (index > 1) { mission.DistanceFlown += CalcDistance(lastCoord, coord); } lastCoord = new Coordinate3D(coord.Lat, coord.Lon, coord.Alt); } return mp; }
private double CalcDistance(Coordinate3D coord1, Coordinate3D coord2) { const double RADIUS = 6378.16; double dlon = Radians(coord2.Lon - coord1.Lon); double dlat = Radians(coord2.Lat - coord1.Lat); double a = (Math.Sin(dlat / 2.0) * Math.Sin(dlat / 2.0)) + Math.Cos(Radians(coord1.Lat)) * Math.Cos(Radians(coord2.Lat)) * (Math.Sin(dlon / 2.0) * Math.Sin(dlon / 2.0)); double angle = 2.0 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1.0 - a)); return angle * RADIUS; }
public void MissionPointUpdate(LogInfo info, long currPos, long maxPos, DateTime loggedAt, TimeSpan span) { if (this.InvokeRequired) { MissionPointUpdateDelegate UpdateMissionVar = new MissionPointUpdateDelegate(MissionPointUpdate); Object[] args = new Object[5]; args[0] = info; args[1] = currPos; args[2] = maxPos; args[3] = loggedAt; args[4] = span; Invoke(UpdateMissionVar, args); } else { if (currPos == maxPos) { StopMission(); } else { if (missionControl != null && info != null) { if (info.GetType == LogInfo.InfoType.COORD) { Coordinate3D coord = info as Coordinate3D; if (Math.Abs(coord.Lat) > 0 && Math.Abs(coord.Lon) > 0) { mpMissionMap.HoldInvalidation = true; // if (bReplayingSavedMission) { currentPosition = new Coordinate3D(coord.Lat, coord.Lon, coord.Alt); if (currPos >= furthestMavlinkPos) { furthestMavlinkPos = currPos; route.Points.Add(currentPosition.GMapPoint); if (missionControl.IsTimeToPoll((long)span.TotalMilliseconds)) { MissionPoint mp = missionControl.AddMissionPoint(currentPosition, navTarget, ""); mp.WindBearing = coord.Wind.WindBearing; mp.WindSpeed = coord.Wind.WindSpeed; sampledPoints.Markers.Add(new GMapMarkerWind(currentPosition.GMapPoint, mpMissionMap, (float)coord.Wind.WindBearing, (float)coord.Wind.WindSpeed) { ToolTipText = "Wind Speed " + (coord.Wind.WindSpeed * 3.6).ToString("0.0") + "km/hr", ToolTipMode = MarkerTooltipMode.OnMouseOver }); } } if (!mouseDown && (int)currPos < tbMavLink.Maximum) { tbMavLink.Value = (int)currPos; } secsPerMavlinkPos = ((float)span.TotalSeconds) / ((float)(currPos-missionControl.MissionStartPosition)); lblMavLinkPosition.Text = GetTimeFromSeconds((int)span.TotalSeconds); routes.Markers.Clear(); routes.Markers.Add(new GMapMarkerPlane(currentPosition.GMapPoint, mpMissionMap) { ToolTipText = "current location", ToolTipMode = MarkerTooltipMode.OnMouseOver }); } if (!missionControl.IsLive && (imageStream != null && chkSynchroniseVideo.Checked)) { imageStream.SyncFrames(loggedAt); } if (route.Points.Count == 1) { mpMissionMap.Position = currentPosition.GMapPoint; } mpMissionMap.UpdateRouteLocalPosition(route); mpMissionMap.HoldInvalidation = false; mpMissionMap.Invalidate(); } } } } } }