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