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
}