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;
        }
Пример #3
0
        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;
        }
Пример #4
0
        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;
        }
Пример #5
0
        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();
                            }
                        }
                    }
                }
            }
        }