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