// Return List with all GPS Messages splitted in string arrays
Dictionary<long, VehicleLocation> readGPSMsgInLog(string fn)
{
Dictionary<long, VehicleLocation> vehiclePositionList = new Dictionary<long,VehicleLocation>();
// Telemetry Log
if (fn.ToLower().EndsWith("tlog"))
{
MAVLinkInterface mine = new MAVLinkInterface();
mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.MAV.packets.Initialize(); // clear
CurrentState cs = new CurrentState();
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
VehicleLocation location = new VehicleLocation();
location.Time = cs.datetime;
location.Lat = cs.lat;
location.Lon = cs.lng;
location.RelAlt = cs.alt;
location.AltAMSL = cs.altasl;
location.Roll = cs.roll;
location.Pitch = cs.pitch;
location.Yaw = cs.yaw;
vehiclePositionList[ToMilliseconds(location.Time)] = location;
// 4 5 7
Console.Write((mine.logplaybackfile.BaseStream.Position * 100 / mine.logplaybackfile.BaseStream.Length) + " \r");
}
mine.logplaybackfile.Close();
}
// DataFlash Log
else
{
StreamReader sr = new StreamReader(fn);
// Will hold the last seen Attitude information in order to incorporate them into the GPS Info
float currentYaw = 0f;
float currentRoll = 0f;
float currentPitch = 0f;
while (!sr.EndOfStream)
{
string line = sr.ReadLine();
// Look for GPS Messages. However GPS Messages do not have Roll, Pitch and Yaw
// So we have to look for one ATT message after having read a GPS one
if (line.ToLower().StartsWith("gps"))
{
VehicleLocation location = new VehicleLocation();
string[] gpsLineValues = line.Split(new char[] { ',', ':' });
try
{
location.Time = GetTimeFromGps(int.Parse(getValueFromStringArray(gpsLineValues, gpsweekpos), CultureInfo.InvariantCulture), int.Parse(getValueFromStringArray(gpsLineValues, timepos), CultureInfo.InvariantCulture));
location.Lat = double.Parse(getValueFromStringArray(gpsLineValues, latpos), CultureInfo.InvariantCulture);
location.Lon = double.Parse(getValueFromStringArray(gpsLineValues, lngpos), CultureInfo.InvariantCulture);
location.RelAlt = double.Parse(getValueFromStringArray(gpsLineValues, altpos), CultureInfo.InvariantCulture);
location.AltAMSL = double.Parse(getValueFromStringArray(gpsLineValues, altAMSLpos), CultureInfo.InvariantCulture);
location.Roll = currentRoll;
location.Pitch = currentPitch;
location.Yaw = currentYaw;
long millis = ToMilliseconds(location.Time);
//System.Diagnostics.Debug.WriteLine("GPS MSG - UTCMillis = " + millis + " GPS Week = " + getValueFromStringArray(gpsLineValues, gpsweekpos) + " TimeMS = " + getValueFromStringArray(gpsLineValues, timepos));
if (!vehiclePositionList.ContainsKey(millis))
vehiclePositionList[millis] = location;
}
catch { Console.WriteLine("Bad GPS Line"); }
}
else if (line.ToLower().StartsWith("att"))
{
string[] attLineValues = line.Split(new char[] { ',', ':' });
currentRoll = float.Parse(getValueFromStringArray(attLineValues, rollATT), CultureInfo.InvariantCulture);
currentPitch = float.Parse(getValueFromStringArray(attLineValues, pitchATT), CultureInfo.InvariantCulture);
currentYaw = float.Parse(getValueFromStringArray(attLineValues, yawATT), CultureInfo.InvariantCulture);
}
}
sr.Close();
}
return vehiclePositionList;
}