BinParser

BinParser.cs

using uint8_t = System.Byte;

class BinParser
    {
          public struct GPS_LINE
          {
              public DateTime datetime;
              public string type;
              public float lat;
              public float lon;
              public float alt;
      
              public float roll;
              public float pitch;
              public float yaw;
          }

        const byte HEAD_BYTE1 = 0xA3;    // Decimal 163  
        const byte HEAD_BYTE2 = 0x95;    // Decimal 149  

        [StructLayout(LayoutKind.Sequential, Pack = 1)]
        public struct log_Format
        {
            public uint8_t type;
            public uint8_t length;
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
            public byte[] name;
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = 16)]
            public byte[] format;
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = 64)]
            public byte[] labels;
        }

        static Dictionary<string, log_Format> logformat = new Dictionary<string, log_Format>();



        List<GPS_LINE> lines = new List<GPS_LINE>();

        public BinParser(String filename)
        {


            using (BinaryReader br = new BinaryReader(File.OpenRead(filename)))
            {
                int log_step = 0;

                while (br.BaseStream.Position < br.BaseStream.Length)
                {
                    byte data = br.ReadByte();

                    switch (log_step)
                    {
                        case 0:
                            if (data == HEAD_BYTE1)
                            {
                                log_step++;
                            }
                            break;

                        case 1:
                            if (data == HEAD_BYTE2)
                            {
                                log_step++;
                            }
                            else
                            {
                                log_step = 0;
                            }
                            break;

                        case 2:
                            log_step = 0;
                            try
                            {
                                string line = logEntry(data, br);
                                var processed_line = processLine(line);
                                if (processed_line.type == "GPS" || processed_line.type == "ATT")
                                {
                                    lines.Add(processed_line);
                                }

                            }
                            catch { Console.WriteLine("Bad Binary log line {0}", data); }
                            break;
                    }
                }
            }

        }

        public List<GPS_LINE> Lines()
        {
            return lines;
        }

        private DateTime DateTimeFromGPSTimeandGPSWeek(String gpstime, String gpsweek)
        {
            DateTime gpsweekstart = DateTime.Parse("06 January 1980");
            int days = int.Parse(gpsweek) * 7;
            //converts to localtime from UTC
            DateTime convertedDate = gpsweekstart.AddDays(days).AddSeconds(double.Parse(gpstime) / 1000);
            return TimeZone.CurrentTimeZone.ToLocalTime(convertedDate);



        }




        static string logEntry(byte packettype, BinaryReader br)
        {

            switch (packettype)
            {

                case 0x80:  // FMT

                    log_Format logfmt = new log_Format();

                    object obj = logfmt;

                    int len = Marshal.SizeOf(obj);

                    byte[] bytearray = br.ReadBytes(len);

                    IntPtr i = Marshal.AllocHGlobal(len);

                    // create structure from ptr
                    obj = Marshal.PtrToStructure(i, obj.GetType());

                    // copy byte array to ptr
                    Marshal.Copy(bytearray, 0, i, len);

                    obj = Marshal.PtrToStructure(i, obj.GetType());

                    Marshal.FreeHGlobal(i);

                    logfmt = (log_Format)obj;

                    string lgname = ASCIIEncoding.ASCII.GetString(logfmt.name).Trim(new char[] { '\0' });
                    string lgformat = ASCIIEncoding.ASCII.GetString(logfmt.format).Trim(new char[] { '\0' });
                    string lglabels = ASCIIEncoding.ASCII.GetString(logfmt.labels).Trim(new char[] { '\0' });

                    logformat[lgname] = logfmt;

                    string line = String.Format("FMT, {0}, {1}, {2}, {3}, {4}\r\n", logfmt.type, logfmt.length, lgname, lgformat, lglabels);

                    return line;

                default:
                    string format = "";
                    string name = "";
                    int size = 0;

                    foreach (log_Format fmt in logformat.Values)
                    {
                        if (fmt.type == packettype)
                        {
                            name = ASCIIEncoding.ASCII.GetString(fmt.name).Trim(new char[] { '\0' });
                            format = ASCIIEncoding.ASCII.GetString(fmt.format).Trim(new char[] { '\0' });
                            size = fmt.length;
                            break;
                        }
                    }

                    // didnt find a match, return unknown packet type
                    if (size == 0)
                        return "UNKW, " + packettype;

                    return ProcessMessage(br.ReadBytes(size - 3), name, format); // size - 3 = message - messagetype - (header *2)
            }
        }


        static string ProcessMessage(byte[] message, string name, string format)
        {
            char[] form = format.ToCharArray();

            int offset = 0;

            StringBuilder line = new StringBuilder(name);

            foreach (char ch in form)
            {
                switch (ch)
                {
                    case 'b':
                        line.Append(", " + (sbyte)message[offset]);
                        offset++;
                        break;
                    case 'B':
                        line.Append(", " + message[offset]);
                        offset++;
                        break;
                    case 'h':
                        line.Append(", " + BitConverter.ToInt16(message, offset).ToString(System.Globalization.CultureInfo.InvariantCulture));
                        offset += 2;
                        break;
                    case 'H':
                        line.Append(", " + BitConverter.ToUInt16(message, offset).ToString(System.Globalization.CultureInfo.InvariantCulture));
                        offset += 2;
                        break;
                    case 'i':
                        line.Append(", " + BitConverter.ToInt32(message, offset).ToString(System.Globalization.CultureInfo.InvariantCulture));
                        offset += 4;
                        break;
                    case 'I':
                        line.Append(", " + BitConverter.ToUInt32(message, offset).ToString(System.Globalization.CultureInfo.InvariantCulture));
                        offset += 4;
                        break;
                    case 'f':
                        line.Append(", " + BitConverter.ToSingle(message, offset).ToString(System.Globalization.CultureInfo.InvariantCulture));
                        offset += 4;
                        break;
                    case 'c':
                        line.Append(", " + (BitConverter.ToInt16(message, offset) / 100.0).ToString("0.00", System.Globalization.CultureInfo.InvariantCulture));
                        offset += 2;
                        break;
                    case 'C':
                        line.Append(", " + (BitConverter.ToUInt16(message, offset) / 100.0).ToString("0.00", System.Globalization.CultureInfo.InvariantCulture));
                        offset += 2;
                        break;
                    case 'e':
                        line.Append(", " + (BitConverter.ToInt32(message, offset) / 100.0).ToString("0.00", System.Globalization.CultureInfo.InvariantCulture));
                        offset += 4;
                        break;
                    case 'E':
                        line.Append(", " + (BitConverter.ToUInt32(message, offset) / 100.0).ToString("0.00", System.Globalization.CultureInfo.InvariantCulture));
                        offset += 4;
                        break;
                    case 'L':
                        line.Append(", " + ((double)BitConverter.ToInt32(message, offset) / 10000000.0).ToString(System.Globalization.CultureInfo.InvariantCulture));
                        offset += 4;
                        break;
                    case 'n':
                        line.Append(", " + ASCIIEncoding.ASCII.GetString(message, offset, 4).Trim(new char[] { '\0' }));
                        offset += 4;
                        break;
                    case 'N':
                        line.Append(", " + ASCIIEncoding.ASCII.GetString(message, offset, 16).Trim(new char[] { '\0' }));
                        offset += 16;
                        break;
                    case 'M':
                        break;
                    case 'Z':
                        line.Append(", " + ASCIIEncoding.ASCII.GetString(message, offset, 64).Trim(new char[] { '\0' }));
                        offset += 64;
                        break;
                    default:

                        break;
                }
            }

            line.Append("\r\n");
            return line.ToString();
        }


        private GPS_LINE processLine(String line)
        {

            GPS_LINE gps_line = new GPS_LINE();
            string[] items = line.Split(',', ':');
            if (items[0].Contains("GPS"))
            {
                var gps_time = items[2];
                var gps_week = items[3];
                var lat = items[6];
                var lon = items[7];
                var alt = items[9];

                gps_line.datetime = DateTimeFromGPSTimeandGPSWeek(gps_time, gps_week);
                gps_line.lat = float.Parse(lat);
                gps_line.lon = float.Parse(lon);
                gps_line.alt = float.Parse(alt);
                gps_line.type = "GPS";


                //Console.WriteLine("time: {0} {1} {2} {3}",DateTimeFromGPSTimeandGPSWeek(gps_time, gps_week), lat, lon, alt );
                // foreach (var item in items)
                //     Console.Write(item);
            }
            else if (items[0].Contains("ATT"))
            {
                var roll = items[3];
                var pitch = items[5];
                var yaw = items[7];
                gps_line.type = "ATT";

                gps_line.roll = float.Parse(roll);
                gps_line.pitch = float.Parse(pitch);
                gps_line.yaw = float.Parse(yaw);

                // Console.WriteLine("{0} {1} {2}", roll, pitch, yaw);

                // foreach (var item in items)
                //      Console.Write(item);
            }

            return gps_line;

        }


    } //end BIN Parser Class