Exemplo n.º 1
0
        private TrackPoint ECEF2LonLat(DataLogFixFull local)
        {
            TrackPoint result = null;

            double x = (double)local.X;
            double y = (double)local.Y;
            double z = (double)local.Z;

            double lat = 0;
            double lon = 0;
            double alt = 0;

            // ECEF_to_LLA(x, y, z, out lat, out lon, out alt);

            long time = 0;
            //time = gpstime_to_timet(local.WN, (int)local.TOW) - 315964800;
            DateTime dt = new DateTime(1980, 1, 6, 0, 0, 0);

            dt = dt.AddSeconds(time);

            result = new TrackPoint((decimal)lon, (decimal)lat, dt);

            result.Speed     = (local.V * 1000);
            result.Speed    /= 3600;
            result.Elevation = (decimal)alt;

            return(result);
        }
Exemplo n.º 2
0
        private TrackPoint ECEF2LonLat(DataLogFixFull local)
        {
            TrackPoint result = null;

            double x = (double)local.X;
            double y = (double)local.Y;
            double z = (double)local.Z;

            double lat = 0;
            double lon = 0;
            double alt = 0;

            // ECEF_to_LLA(x, y, z, out lat, out lon, out alt);

            long time = 0;
            //time = gpstime_to_timet(local.WN, (int)local.TOW) - 315964800;
            DateTime dt = new DateTime(1980, 1, 6, 0, 0, 0);
            dt = dt.AddSeconds(time);

            result = new TrackPoint((decimal)lon, (decimal)lat, dt);

            result.Speed = (local.V * 1000);
            result.Speed /= 3600;
            result.Elevation = (decimal)alt;

            return result;
        }
Exemplo n.º 3
0
        private DataLogFixFull ReadLocation(BinaryReader br, DataLogFixFull current)
        {
            UInt16 pos1 = (byte)(0x00FF & br.ReadByte());
            pos1 <<= 8;
            pos1 |= (byte)(0x00FF & br.ReadByte());

            UInt16 velocity = pos1;
            velocity &= (UInt16)0x03ff;

            switch ((pos1 & 0xE000))
            {
                // empty
                case 0xE000:
                    return null;
                    break;

                // FIX FULL POI
                case 0x6000:
                // FIX FULL
                case 0x4000:
                    {
                        DataLogFixFull data = new DataLogFixFull();
                        data.V = velocity;

                        byte b = (byte)(0x00FF & br.ReadByte());
                        data.TOW = (byte)(0x000f & (b >> 4));
                        data.WN = (UInt16)(0x0030 & b);
                        data.WN <<= 4;
                        data.WN |= (UInt16)(0x00FF & br.ReadByte());
                        UInt32 un = (UInt32)(0x00FF & br.ReadByte());
                        un <<= 8;
                        un |= (UInt32)(0x00FF & br.ReadByte());
                        un <<= 4;
                        data.TOW |= un;

                        {
                            data.X = (Int32)(0x00FF & br.ReadByte());
                            data.X <<= 8;
                            data.X |= (Int32)(0x00FF & br.ReadByte());

                            un = (UInt32)(0x00FF & br.ReadByte());
                            un <<= 8;
                            un |= (UInt32)(0x00FF & br.ReadByte());

                            un <<= 16;
                            un &= 0xffff0000;
                            data.X |= (Int32)un;
                        }


                        {
                            data.Y = (Int32)(0x00FF & br.ReadByte());
                            data.Y <<= 8;
                            data.Y |= (Int32)(0x00FF & br.ReadByte());

                            un = (UInt32)(0x00FF & br.ReadByte());
                            un <<= 8;
                            un |= (UInt32)(0x00FF & br.ReadByte());

                            un <<= 16;
                            un &= 0xffff0000;
                            data.Y |= (Int32)un;

                        }

                        {
                            data.Z = (Int32)(0x00FF & br.ReadByte());
                            data.Z <<= 8;
                            data.Z |= (Int32)(0x00FF & br.ReadByte());

                            un = (UInt32)(0x00FF & br.ReadByte());
                            un <<= 8;
                            un |= (UInt32)(0x00FF & br.ReadByte());

                            un <<= 16;
                            un &= 0xffff0000;
                            data.Z |= (Int32)un;

                        }
                        return data;
                    }
                    break;

                // FIX COMPACT
                case 0x8000:
                    {
                        UInt16 diffTOW = (UInt16)(0x00FF & br.ReadByte());
                        diffTOW <<= 8;
                        diffTOW |= (UInt16)(0x00FF & br.ReadByte());

                        Int16 diffX = (Int16)(0x00FF & br.ReadByte());
                        diffX <<= 2;
                        UInt16 un = (UInt16)(0x00FF & br.ReadByte());
                        diffX = (Int16)(0x0003 & (un >> 6));

                        if (0 != (diffX & 0x0200))
                        {
                            UInt16 unWork = 0xfC00;
                            diffX |= (Int16)unWork;   // 1111 1100 0000 0000
                        }

                        Int16 diffY = (Int16)(un & 0x003f);
                        un = (UInt16)(0x00FF & br.ReadByte());
                        diffY |= (Int16)(0x03C0 & (un << 6));  // 11 1100 0000

                        if (0 != (diffY & 0x0200))
                        {
                            UInt16 unWork = 0xfC00;
                            diffY |= (Int16)unWork;   // 1111 1100 0000 0000
                        }


                        Int16 diffZ = (Int16)(0x0003 & un);
                        diffZ <<= 8;
                        diffZ |= (Int16)(0x00FF & br.ReadByte());

                        if (0 != (diffZ & 0x0200))
                        {
                            UInt16 unWork = 0xfC00;
                            diffZ |= (Int16)unWork;   // 1111 1100 0000 0000
                        }

                        if (null == current)
                        {
                            return null;
                        }

                        DataLogFixFull result = new DataLogFixFull();

                        result.V = velocity;

                        result.WN = current.WN;
                        result.TOW = current.TOW + diffTOW;

                        result.X = current.X;
                        result.X += diffX;

                        result.Y = current.Y;
                        result.Y += diffY;

                        result.Z = current.Z;
                        result.Z += diffZ;

                        return result;
                    }
                    break;

                default:
                    throw new Exception("type error!");
            }
        }
Exemplo n.º 4
0
        public List <TrackPoint> ReadLatLonData()
        {
            try
            {
                List <TrackPoint> items = null;

                UInt16 totalSectors;
                UInt16 freeSectors;
                bool   dataLogEnable;
                // ボーレートの設定をする
                setBaudRate(BaudRate.BaudRate_115200);

                // セクタ数を見る
                RequestBufferStatus(out totalSectors, out freeSectors, out dataLogEnable);
                System.Diagnostics.Debug.Print("freeSectors/totalSectors = {0}/{1}", freeSectors, totalSectors);

                // データが無効なら終わる
                //if (!dataLogEnable) return null;

                UInt16 sectors = totalSectors;
                sectors -= freeSectors;
                if (0 < sectors)
                {
                    byte[] readLog     = new byte[sectors * SECTOR_SIZE];
                    int    retryCount  = 0;
                    int    readSectors = 0;
                    for (int index = 0; index < sectors;)
                    {
                        readSectors = (2 <= (sectors - index)) ? 2 : 1;

                        // 読み出しを指示
                        sendReadBuffer(index, readSectors);

                        // データを取得する
                        ReadLogBuffer(readLog, index * SECTOR_SIZE, readSectors * SECTOR_SIZE);

                        string s = string.Format(@"C:\Users\Tomo\Documents\GEOTagInjector\SkyTraqSerial\hoge_{0}.bin", index);
                        using (System.IO.BinaryWriter bw = new System.IO.BinaryWriter(File.OpenWrite(s)))
                        {
                            bw.Write(readLog, index * SECTOR_SIZE, readSectors * SECTOR_SIZE);
                        }

                        // CS取得
                        byte resultCS = ReadLogBufferCS();

                        byte calcCS = 0;
                        using (MemoryStream ms = new MemoryStream(readLog, index * SECTOR_SIZE, readSectors * SECTOR_SIZE))
                        {
                            using (System.IO.BinaryReader br = new System.IO.BinaryReader(ms))
                            {
                                while (true)
                                {
                                    try
                                    {
                                        calcCS ^= br.ReadByte();
                                    }
                                    catch (System.IO.EndOfStreamException)
                                    {
                                        break;
                                    }
                                }
                            }
                        }

                        if (calcCS != resultCS)
                        {
                            if (3 <= retryCount)
                            {
                                throw new Exception("データ取得のリトライが失敗した");
                            }
                            ++retryCount;
                        }
                        else
                        {
                            retryCount = 0;
                            // 次を読み込みます。
                            index += readSectors;
                        }
                        System.Threading.Thread.Sleep(100);
                    }

                    items = new List <TrackPoint>();
                    using (BinaryReader br = new BinaryReader(new MemoryStream(readLog)))
                    {
                        DataLogFixFull local = null;
                        while (true)
                        {
                            try
                            {
                                // ECEFに変換する
                                local = ReadLocation(br, local);
                                if (null != local)
                                {
                                    // longitude/latitudeに変換する
                                    items.Add(ECEF2LonLat(local));
                                }
                            }
                            catch (System.IO.EndOfStreamException)
                            {
                                break;
                            }
                        }
                    }
                }

                // Restartして終了
                sendRestart();

                // longitude/latitudeの配列を返す
                return(items);
            }
            catch (TimeoutException)
            {
                try
                {
                    // Restartして終了
                    sendRestart();
                }
                catch (TimeoutException)
                {
                    // 処理なし
                    recovery();
                }
            }

            // 値は返せない
            return(null);
        }
Exemplo n.º 5
0
        private DataLogFixFull ReadLocation(BinaryReader br, DataLogFixFull current)
        {
            UInt16 pos1 = (byte)(0x00FF & br.ReadByte());

            pos1 <<= 8;
            pos1  |= (byte)(0x00FF & br.ReadByte());

            UInt16 velocity = pos1;

            velocity &= (UInt16)0x03ff;

            switch ((pos1 & 0xE000))
            {
            // empty
            case 0xE000:
                return(null);

                break;

            // FIX FULL POI
            case 0x6000:
            // FIX FULL
            case 0x4000:
            {
                DataLogFixFull data = new DataLogFixFull();
                data.V = velocity;

                byte b = (byte)(0x00FF & br.ReadByte());
                data.TOW  = (byte)(0x000f & (b >> 4));
                data.WN   = (UInt16)(0x0030 & b);
                data.WN <<= 4;
                data.WN  |= (UInt16)(0x00FF & br.ReadByte());
                UInt32 un = (UInt32)(0x00FF & br.ReadByte());
                un      <<= 8;
                un       |= (UInt32)(0x00FF & br.ReadByte());
                un      <<= 4;
                data.TOW |= un;

                {
                    data.X   = (Int32)(0x00FF & br.ReadByte());
                    data.X <<= 8;
                    data.X  |= (Int32)(0x00FF & br.ReadByte());

                    un   = (UInt32)(0x00FF & br.ReadByte());
                    un <<= 8;
                    un  |= (UInt32)(0x00FF & br.ReadByte());

                    un    <<= 16;
                    un     &= 0xffff0000;
                    data.X |= (Int32)un;
                }


                {
                    data.Y   = (Int32)(0x00FF & br.ReadByte());
                    data.Y <<= 8;
                    data.Y  |= (Int32)(0x00FF & br.ReadByte());

                    un   = (UInt32)(0x00FF & br.ReadByte());
                    un <<= 8;
                    un  |= (UInt32)(0x00FF & br.ReadByte());

                    un    <<= 16;
                    un     &= 0xffff0000;
                    data.Y |= (Int32)un;
                }

                {
                    data.Z   = (Int32)(0x00FF & br.ReadByte());
                    data.Z <<= 8;
                    data.Z  |= (Int32)(0x00FF & br.ReadByte());

                    un   = (UInt32)(0x00FF & br.ReadByte());
                    un <<= 8;
                    un  |= (UInt32)(0x00FF & br.ReadByte());

                    un    <<= 16;
                    un     &= 0xffff0000;
                    data.Z |= (Int32)un;
                }
                return(data);
            }
            break;

            // FIX COMPACT
            case 0x8000:
            {
                UInt16 diffTOW = (UInt16)(0x00FF & br.ReadByte());
                diffTOW <<= 8;
                diffTOW  |= (UInt16)(0x00FF & br.ReadByte());

                Int16 diffX = (Int16)(0x00FF & br.ReadByte());
                diffX <<= 2;
                UInt16 un = (UInt16)(0x00FF & br.ReadByte());
                diffX = (Int16)(0x0003 & (un >> 6));

                if (0 != (diffX & 0x0200))
                {
                    UInt16 unWork = 0xfC00;
                    diffX |= (Int16)unWork;           // 1111 1100 0000 0000
                }

                Int16 diffY = (Int16)(un & 0x003f);
                un     = (UInt16)(0x00FF & br.ReadByte());
                diffY |= (Int16)(0x03C0 & (un << 6));          // 11 1100 0000

                if (0 != (diffY & 0x0200))
                {
                    UInt16 unWork = 0xfC00;
                    diffY |= (Int16)unWork;           // 1111 1100 0000 0000
                }


                Int16 diffZ = (Int16)(0x0003 & un);
                diffZ <<= 8;
                diffZ  |= (Int16)(0x00FF & br.ReadByte());

                if (0 != (diffZ & 0x0200))
                {
                    UInt16 unWork = 0xfC00;
                    diffZ |= (Int16)unWork;           // 1111 1100 0000 0000
                }

                if (null == current)
                {
                    return(null);
                }

                DataLogFixFull result = new DataLogFixFull();

                result.V = velocity;

                result.WN  = current.WN;
                result.TOW = current.TOW + diffTOW;

                result.X  = current.X;
                result.X += diffX;

                result.Y  = current.Y;
                result.Y += diffY;

                result.Z  = current.Z;
                result.Z += diffZ;

                return(result);
            }
            break;

            default:
                throw new Exception("type error!");
            }
        }