Esempio n. 1
0
        // Get the GPS data from the file
        public static GPSDataList ReadGPS(string _file, GPSBox _gpsBox)
        {
            GPSDataList GPSDataList = new GPSDataList();

            try
            {
                // Open the file in Read mode
                FileStream fin = new FileStream(_file, FileMode.Open, FileAccess.Read, FileShare.ReadWrite);

                // Seek to the end of the "GPS " boc
                // Skip the first 16 'header' bits
                Streams.Seek(fin, _gpsBox.Pos + 16);
                int gps_read = 16;

                // While we are reading in the sub box
                while (gps_read < _gpsBox.Size - 8)
                {
                    // Get the GPS box position and size
                    int gps_box_pos  = (int)Streams.ReadUInt(fin, true);
                    int gps_box_size = (int)Streams.ReadUInt(fin, true);

                    // Update our position
                    gps_read = gps_read + 8;

                    // Save the current file seek position
                    long pos = fin.Position;

                    // Seek to this GPS data
                    fin.Seek(gps_box_pos, SeekOrigin.Begin);

                    // Get the GPS box, size, type and magic
                    int    gps_box_size1 = (int)Streams.ReadUInt(fin, true);
                    string gps_box_type  = Streams.ReadString(fin, 4);
                    string gps_box_magic = Streams.ReadString(fin, 4);

                    // Verify the read data is GPS data
                    if (gps_box_size != gps_box_size1 || gps_box_type != "free" || gps_box_magic != "GPS ")
                    {
                        // Log incorrect data format
                        Log.WriteLine("ERROR", "Box at position: " + gps_box_pos + " is not the expected format\r\n\t\t\t\t" +
                                      "(Expected size: " + gps_box_size + ", Actual size: " + gps_box_size1 + ", " +
                                      "Expected type: free, Actual type: " + gps_box_type + ", " +
                                      "Expected magic: GPS , Actual magic: " + gps_box_magic + ")\r\n");
                        continue;
                    }

                    // Extract GPS data
                    int version = (int)Streams.ReadUInt(fin);

                    // Maybe able to identify firmware version with this
                    // Firmware 1.1 and 2.02 = 76
                    // Firmware 2.0 = 15344
                    // Firmware 2.01 - NOT COMPATIBLE, BUG IN FIRMWARE

                    if (version == 76)
                    {
                        // Firmware 1.1 or 2.02
                        fin.Seek(32, SeekOrigin.Current);
                    }
                    else if (version == 15344)
                    {
                        // Firmware 2.0
                        // Nothing to do
                    }
                    else
                    {
                        // Unknown Firmware Version
                    }

                    int hour   = (int)Streams.ReadUInt(fin);
                    int minute = (int)Streams.ReadUInt(fin);
                    int second = (int)Streams.ReadUInt(fin);
                    int year   = (int)Streams.ReadUInt(fin);
                    int month  = (int)Streams.ReadUInt(fin);
                    int day    = (int)Streams.ReadUInt(fin);

                    string active      = Streams.ReadString(fin, 1);
                    string latitude_b  = Streams.ReadString(fin, 1);
                    string longitude_b = Streams.ReadString(fin, 1);
                    string unknown2    = Streams.ReadString(fin, 1);
                    float  latitude    = Streams.ReadFloat(fin);
                    float  longitude   = Streams.ReadFloat(fin);

                    // Check the GPS had a fix
                    if (active == "A")
                    {
                        // Correct the speed during load
                        // 1 knot = 0.514444 m/s
                        float speed = Streams.ReadFloat(fin) * (float)0.514444;

                        DateTime dateTime;

                        if (!Enumerable.Range(1, 31).Contains(day))
                        {
                            // TODO: Calculate day from somewhere
                            // Firmware 1.1 has an issue with day, used to get it from file name?
                        }

                        // Create DateTime object
                        if (year != 0 && month != 0 && day != 0)
                        {
                            dateTime = new DateTime((year + 2000), month, day, hour, minute, second);
                        }
                        else
                        {
                            // Date not valid
                            dateTime = new DateTime(1);
                            continue;
                        }

                        // Change coordinate from DDDmm.mmmm format to traditional lat/long
                        double flatitude  = GPSHelpers.FixCoordinates(latitude_b, latitude);
                        double flongitude = GPSHelpers.FixCoordinates(longitude_b, longitude);

                        // Create new GPSData object
                        GPSData gps = new GPSData();

                        gps.DateTime = dateTime;
                        gps.Speed    = speed;

                        // Kalman filter the GPS data to remove jumps
                        Kalman k = new Kalman(gps.Speed);
                        k.Process(flatitude, flongitude, 9.9f, 1000);
                        gps.Latitude  = k.get_lat();
                        gps.Longitude = k.get_lng();

                        if (lastLat != 0 && lastLong != 0)
                        {
                            gps.Distance = GPSHelpers.Distance(lastLat, lastLong, flatitude, flongitude);
                            gps.Heading  = GPSHelpers.Heading(lastLat, lastLong, flatitude, flongitude);
                        }
                        lastLat  = gps.Latitude;
                        lastLong = gps.Longitude;

                        if (!slowSpeed)
                        {
                            GPSDataList.FilteredGPSData.Add(gps);
                        }
                        GPSDataList.AllGPSData.Add(gps);

                        // Remove a few points when speed is low to stop jagged lines
                        if (speed <= 1)
                        {
                            slowSpeed = true;
                        }
                        else
                        {
                            slowSpeed = false;
                        }
                    }
                    else
                    {
                        Log.WriteVerboseLine("GPS Location is not locked. Trying next block.");
                    }

                    // Return to original file seek
                    fin.Seek(pos, SeekOrigin.Begin);
                }
            }
            catch (Exception e)
            {
                // Unable to open file
                Log.WriteLine("ERROR", "Unable to process file: " + _file);
                Log.WriteLine("ERROR", e.Message.ToString());
            }

            return(GPSDataList);
        }