private void SectorsData2LatLon(byte[] readLog, List <bykIFv1.Point> items) { using (BinaryReader br = new BinaryReader(new MemoryStream(readLog))) { // 変換開始を通知します OnRead(new ReadProgressEvent(ReadProgressEvent.READ_PHASE.CONVERT, 0, (int)br.BaseStream.Length)); DataLogFixFull local = null; while (true) { try { // 変換状況を通知します OnRead(new ReadProgressEvent(ReadProgressEvent.READ_PHASE.CONVERT, (int)br.BaseStream.Position, (int)br.BaseStream.Length)); // ECEFに変換する local = ReadLocation(br, local); if (null != local) { // longitude/latitudeに変換する items.Add(ECEF2LonLat(local)); } } catch (System.IO.EndOfStreamException) { break; } } // 変換終了を通知します OnRead(new ReadProgressEvent(ReadProgressEvent.READ_PHASE.CONVERT, (int)br.BaseStream.Length, (int)br.BaseStream.Length)); } }
private bykIFv1.Point ECEF2LonLat(DataLogFixFull local) { bykIFv1.Point 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; ECEF2LLA(x, y, z, out lat, out lon, out alt); DateTime dt = GPSTIME2diffUTC(local.WN, local.TOW); double spd = (local.V * 1000); spd /= 3600; result = new bykIFv1.Point(dt, lat, lon, alt, spd, (DataLogFixFull.TYPE.FULL_POI == local.type)); return(result); }
private DataLogFixFull ReadLocation(BinaryReader br, DataLogFixFull current) { UInt16 pos1 = (UInt16)(0x00FF & br.ReadByte()); pos1 <<= 8; pos1 |= (UInt16)(0x00FF & br.ReadByte()); UInt16 velocity = pos1; velocity &= (UInt16)0x03ff; switch ((pos1 & 0xE000)) { // empty case 0xE000: return(null); // FIX FULL POI case 0x6000: // FIX FULL case 0x4000: { DataLogFixFull data = new DataLogFixFull(); data.type = ((pos1 & 0xE000) == 0x4000) ? DataLogFixFull.TYPE.FULL : DataLogFixFull.TYPE.FULL_POI; data.V = velocity; UInt16[] unWork = new UInt16[8]; for (int index = 0; index < unWork.Length; ++index) { unWork[index] = (UInt16)(0x00FF & br.ReadByte()); unWork[index] <<= 8; unWork[index] |= (UInt16)(0x00FF & br.ReadByte()); } data.WN = (UInt16)(0x03ff & unWork[0]); data.TOW = unWork[1]; data.TOW <<= 4; data.TOW |= (UInt32)(0x000f & (unWork[0] >> 12)); data.X = unWork[3]; data.X <<= 16; data.X |= unWork[2]; data.Y = unWork[5]; data.Y <<= 16; data.Y |= unWork[4]; data.Z = unWork[7]; data.Z <<= 16; data.Z |= unWork[6]; return(data); } break; // FIX COMPACT case 0x8000: { UInt16[] unWork = new UInt16[3]; for (int index = 0; index < unWork.Length; ++index) { unWork[index] = (UInt16)(0x00FF & br.ReadByte()); unWork[index] <<= 8; unWork[index] |= (UInt16)(0x00FF & br.ReadByte()); } UInt16 diffTOW = unWork[0]; Int16 diffX = (Int16)(0x03ff & (unWork[1] >> 6)); Int16 diffY = (Int16)(0x03C0 & (unWork[2] >> 6)); diffY |= (Int16)(0x003f & unWork[1]); Int16 diffZ = (Int16)(0x03ff & unWork[2]); const Int16 flag = 511; if (flag < diffX) { diffX = (Int16)(flag - diffX); } if (flag < diffY) { diffY = (Int16)(flag - diffY); } if (flag < diffZ) { diffZ = (Int16)(flag - diffZ); } DataLogFixFull result = new DataLogFixFull(); result.type = DataLogFixFull.TYPE.COMPACT; 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!"); } }