MissionPlanner.Georefimage.readGPSMsgInLog C# (CSharp) Method

        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)/100000;
                            location.Lon = double.Parse(getValueFromStringArray(gpsLineValues, lngpos), CultureInfo.InvariantCulture)/100000;
                            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;
        }