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); }
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; }
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!"); } }
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); }
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!"); } }