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