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