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