private void ParsePVTGeodetic(byte idRev, byte[] msg, double ts)
        {
            GPSPositionData posData = new GPSPositionData();
            BinaryReader    br      = new BinaryReader(new MemoryStream(msg));

            br.ReadBytes(8);                            //knock off the first 8 bytes
            double TOW        = br.ReadUInt32() * .001; //seconds
            double WNc        = br.ReadUInt16();        //week number
            byte   mode       = br.ReadByte();
            byte   error      = br.ReadByte();
            double latitude   = br.ReadDouble();           //in radians!
            double longitude  = br.ReadDouble();
            double height     = br.ReadDouble();
            double undulation = br.ReadDouble();
            double velN       = br.ReadDouble();
            double velE       = br.ReadDouble();
            double velU       = br.ReadDouble();

            posData.position.alt = height;
            posData.position.lat = latitude * 180.0 / Math.PI;
            posData.position.lon = longitude * 180.0 / Math.PI;
            posData.timeOfFix    = TOW;
            posData.timestamp    = ts;
            if (PositionMeasurementReceived != null)
            {
                PositionMeasurementReceived(this, new TimestampedEventArgs <GPSPositionData> (ts, posData));
            }
        }
Beispiel #2
0
        //Position (single precision)
        void ProcessTSIP4A(BinaryReader br, double ts)
        {
            GPSPositionData posData   = new GPSPositionData();
            float           lat       = BitConverter.ToSingle(br.ReadBytes(4).Reverse().ToArray(), 0);
            float           lon       = BitConverter.ToSingle(br.ReadBytes(4).Reverse().ToArray(), 0);
            float           alt       = BitConverter.ToSingle(br.ReadBytes(4).Reverse().ToArray(), 0);
            float           clockBias = BitConverter.ToSingle(br.ReadBytes(4).Reverse().ToArray(), 0);
            float           TimeOfFix = BitConverter.ToSingle(br.ReadBytes(4).Reverse().ToArray(), 0);
            double          latDeg    = 180.0 / Math.PI * lat;
            double          lonDeg    = 180.0 / Math.PI * lon;

            posData.position.lat = latDeg;
            posData.position.lon = lonDeg;
            posData.position.alt = alt;
            posData.timestamp    = ts;
            posData.timeOfFix    = TimeOfFix;
            if (PositionMeasurementReceived != null)
            {
                PositionMeasurementReceived(this, new TimestampedEventArgs <GPSPositionData>(ts, posData));
            }
            if (showDebugMessages)
            {
                Console.WriteLine("Position LLA sing: " + latDeg.ToString("F8") + " " + lonDeg.ToString("F8") + " " + alt.ToString("F8") + " TOF:" + TimeOfFix);
            }
        }
            public void SetMotionData(GPSPositionData previousPos)
            {
                double elapsed = (double)(Timestamp - previousPos.Timestamp) / 1000.0;

                if (elapsed <= 0)
                {
                    throw new Exception("Bad timestamp diference " + elapsed + "ms");
                }

                //cacculate speed
                double distance = Measurement.GetDistance(Latitude, Longitude, previousPos.Latitude, previousPos.Longitude);

                Speed   = distance / elapsed;
                Bearing = Measurement.GetFinalBearing(previousPos.Latitude, previousPos.Longitude, Latitude, Longitude);
            }
        private void OnPositionReceived(double latitude, double longitude)
        {
            long now = DateTime.Now.Ticks / TimeSpan.TicksPerMillisecond;

            try
            {
                positionsToAverage[nmea.LastSentenceType] = new GPSPositionData(latitude, longitude, currentHDOP, currentVDOP, currentPDOP, nmea.LastSentenceType);

                long elapsed = now - positionLastProcessed;
                if (elapsed < 1000)
                {
                    return;
                }

                GPSPositionData newPos = new GPSPositionData();
                foreach (GPSPositionData pos in positionsToAverage.Values)
                {
                    newPos.Latitude  += pos.Latitude;
                    newPos.Longitude += pos.Longitude;
                    newPos.HDOP      += pos.HDOP;
                    newPos.VDOP      += pos.VDOP;
                    newPos.PDOP      += pos.PDOP;
                    newPos.Timestamp += pos.Timestamp;
                }
                newPos.Latitude  /= positionsToAverage.Count;
                newPos.Longitude /= positionsToAverage.Count;
                newPos.HDOP      /= positionsToAverage.Count;
                newPos.VDOP      /= positionsToAverage.Count;
                newPos.PDOP      /= positionsToAverage.Count;
                newPos.Timestamp /= positionsToAverage.Count;
                if (currentPosition != null) //use previous values, if we have moved enough then these will be updated below
                {
                    newPos.Speed   = currentPosition.Speed;
                    newPos.Bearing = currentPosition.Bearing;
                    newPos.DBID    = currentPosition.DBID;
                }
                positionsToAverage.Clear();

                //set new poition as the current position
                currentPosition       = newPos;
                positionLastProcessed = now;

                //now determine previous position based on distance to set motion data
                if (previousPosition == null)
                {
                    previousPosition = currentPosition;
                    CurrentState     = State.RECORDING;
                    Tracing?.TraceEvent(TraceEventType.Information, 0, "Recording starting at " + currentPosition.ToString());
                    db?.SaveStatus("recording");
                }
                else
                {
                    double distance = Measurement.GetDistance(previousPosition.Latitude, previousPosition.Longitude, currentPosition.Latitude, currentPosition.Longitude);
                    if ((PDOPThreshold * distance / System.Math.Max(currentPDOP, PDOPThreshold)) > MinDistance)
                    {
                        currentPosition.SetMotionData(previousPosition);
                        previousPosition = currentPosition;

                        Console.WriteLine("Distance {0} and PDOP {1} exceed {2} so we update motion data: {3}", distance, currentPDOP, MinDistance, currentPosition.ToString());
                    }
                }
            } catch (Exception e)
            {
                Tracing?.TraceEvent(TraceEventType.Error, 0, "OnPositionReceived: {0}", e.Message);
                return;
            }

            //here we log to database
            if (db != null)
            {
                try
                {
                    if (now - positionLastLogged > LogPositionWait)
                    {
                        //add this as a new value
                        currentPosition.DBID = 0;
                        db.WritePosition(currentPosition);
                        positionLastLogged = now;
                    }
                    else
                    {
                        //just update the same value in the database
                        db.WritePosition(currentPosition);
                    }
                }
                catch (Exception e)
                {
                    Tracing?.TraceEvent(TraceEventType.Error, 0, "OnPositionReceived: loggin to db gives {0}", e.Message);
                }
            } //end log to db
        }