void ProcessTSIP84(BinaryReader br, double ts) { GPSPositionData posData = new GPSPositionData(); double lat = BitConverter.ToDouble(br.ReadBytes(8).Reverse().ToArray(), 0); double lon = BitConverter.ToDouble(br.ReadBytes(8).Reverse().ToArray(), 0); double alt = BitConverter.ToDouble(br.ReadBytes(8).Reverse().ToArray(), 0); double clockBias = BitConverter.ToDouble(br.ReadBytes(8).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.timeOfFix = TimeOfFix; posData.timestamp = ts; if (PositionMeasurementReceived != null) PositionMeasurementReceived(this, new TimestampedEventArgs<GPSPositionData>(ts, posData)); if (showDebugMessages) Console.WriteLine("Position LLA dbl: " + latDeg.ToString("F8") + " " + lonDeg.ToString("F8") + " " + alt.ToString("F8") + " TOF:" + TimeOfFix); }
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 )); }