-
-
Save dj1711572002/4a2faa7eb5bab4e08c987f8f76b4fbbb to your computer and use it in GitHub Desktop.
C# Console RTK UBX file Translation an BNO085 RVC mode Binary file translation toCSVFile
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
using System; | |
using static System.Net.Mime.MediaTypeNames; | |
using System.IO; | |
using System.Net; | |
using System.Threading; | |
using System.IO.Ports; | |
using System.Diagnostics; | |
using System.Globalization; | |
using System.Text; | |
using System.Runtime.Intrinsics.Arm; | |
using System.Runtime.CompilerServices; | |
using System.Text.RegularExpressions; | |
public class UBX_MB | |
{ | |
//%%%%%%%%%struct pvt%%%%%%%%%% | |
struct pvt | |
{ | |
public uint itow; | |
public ushort year; | |
public byte month; | |
public byte day; | |
public byte hour; | |
public byte min; | |
public byte sec; | |
public byte valid; | |
public uint tacc; | |
public int nano; | |
public byte fixtype; | |
public byte flags; | |
public byte flags2; | |
public byte numsv; | |
public int lon; | |
public int lat; | |
public int height; | |
public int hmsl; | |
public uint hacc; | |
public uint vacc; | |
public int veln; | |
public int vele; | |
public int veld; | |
public int gspeed; | |
public int headmot; | |
public uint sacc; | |
public uint headacc; | |
public ushort pdop; | |
public byte flags3; | |
public byte resrv0; | |
public byte resrv1; | |
public byte resrv2; | |
public byte resrv3; | |
public byte resrv4; | |
public int headveh; | |
public short magdec; | |
public ushort magacc; | |
};//struct pvt end | |
static pvt pvtd; | |
static pvt pvtdr; | |
//%%%%%%%%struct relp%%%%%%%%%%%%%%%%%%%%% | |
struct relp | |
{ | |
public byte version; | |
public byte reserved1; | |
public ushort refstationid; | |
public uint itow; | |
public int relposn; | |
public int relpose; | |
public int relposd; | |
public int relposlength; | |
public int relposheading; | |
public int reserved2; | |
public sbyte relposhpn; | |
public sbyte relposhpe; | |
public sbyte relposhpd; | |
public sbyte relposhplength; | |
public uint accn; | |
public uint acce; | |
public uint accd; | |
public uint accheading; | |
public uint acclength; | |
public uint reserved3;//4byte | |
public uint flags; | |
};//struct relp end | |
static relp relpd; | |
static relp relpdr; | |
static string espip; | |
static byte[] rcvBytes; | |
static string fname; | |
static string fnameb; | |
static char key; | |
static byte[] pvtdata; | |
static byte[] relpdata; | |
static int f131; | |
static int f673 ; | |
static int itow_1 ; | |
static int itowloss ; | |
static double itowper ; | |
static double fixper ; | |
/* | |
static void beeping() | |
{ | |
Console.Beep(2000, 10000); | |
}*/ | |
static void info(byte[] buf) | |
{//itow,flags,pdop,numsv,miss_itow,n,tim | |
pvtdata = new byte[100]; | |
relpdata = new byte[72]; | |
for (int i = 0; i < buf.Length; i++) | |
{ | |
for (int j = 0; j < 100; j++) | |
{ | |
pvtdata[j] = rcvBytes[j]; | |
} | |
string result = PVTcnv(pvtdata, ref pvtd); | |
Console.WriteLine(result); | |
for (int j = 0; j < 72; j++) | |
{ | |
relpdata[j] = rcvBytes[j + 72]; | |
} | |
string result2 = RELPOScnv(relpdata, ref relpd); | |
//Console.WriteLine(relpd.relposlength); | |
} | |
} | |
// myip file read | |
static string myipread() | |
{ | |
FileStream fsi = new FileStream(@"C:\RTK_LOG\myip.txt", FileMode.Open, FileAccess.Read); | |
var reader = new StreamReader(fsi, Encoding.UTF8); | |
var line = reader.ReadLine(); | |
Console.WriteLine("read myip.txt=" + line); | |
return (string)line; | |
} | |
//ubxtocsv(n,f)==================STA24 MovingBase binFileをUBX ASCII変換してCSVファイル保管============================ | |
static void ubxtocsv(int n, string f) | |
{ | |
FileStream fs = new FileStream(@f, FileMode.Open, FileAccess.Read); | |
string fc = f + "-MBUX.csv"; | |
int fileSize = (int)fs.Length; // ファイルのサイズ | |
byte[] buf; | |
buf = new byte[fileSize]; // データ格納用配列 | |
int index0 = 0; | |
int index_1 = 0; | |
int index_error = 0; | |
int indexok = 0; | |
long[] itowarry; | |
long itow1 = 0; | |
double lat_lon = 0; | |
double lat_lon_1 = 0; | |
int straightin = 0; | |
int straightn = 0; | |
int straightin_1 = 0; | |
int itowErr, totaln; | |
//itow1 = 0; | |
int headstart = 0; | |
int headend = 0; | |
int headmot1 = 0; | |
itowErr = 0; | |
totaln = 0; | |
//fix error itowerror | |
f131 = 0; | |
f673 = 0; | |
itow_1 = 0; | |
itowloss = 0; | |
itowper = 0; | |
fixper = 0; | |
int readSize = 0; // Readメソッドで読み込んだバイト数 | |
int remain = fileSize; // 読み込むべき残りのバイト数 | |
int bufPos = 0; // データ格納用配列内の追加位置 | |
int k = 0; | |
while (remain > 0) | |
{ | |
// 344Bytesずつbuf[]に最後データまで読み込む | |
readSize = fs.Read(buf, bufPos, Math.Min(344, remain)); | |
bufPos += readSize; | |
remain -= readSize; | |
k++;//344byte単位でカウントアップ | |
} | |
itowarry = new long[k]; | |
System.IO.StreamWriter sw = new System.IO.StreamWriter(@fc, false);//CSV OPEN | |
for (int i = 0; i < k - 1; i++)//344byte単位でk回変換 | |
{ | |
//Console.Write(i.ToString() + "," + string.Format("{0:X2}", buf[i*172])+",:,"); | |
//PVT取り出し | |
int startn = i * 344;//buf[]頭だし | |
byte[] pvtb = new byte[100]; | |
byte[] relpb = new byte[72]; | |
byte[] pvtr = new byte[100]; | |
byte[] relpr = new byte[72]; | |
//byte[] bnob = new byte[172]; | |
//PVT RELPOSNED trans | |
if (buf[0 + startn] == 0xB5 && buf[1 + startn] == 0x62 && buf[2 + startn] == 0x01 && buf[3 + startn] == 0x07) | |
{ | |
//===========UBX Base translate================================ | |
for (int j = 0; j < 100; j++)//bufからpvtbへ格納 | |
{ | |
pvtb[j] = buf[j + startn]; | |
} | |
for (int j = 0; j < 72; j++)//bufからrelpbへ格納 | |
{ | |
relpb[j] = buf[j + startn + 100]; | |
} | |
itow1 = pvtd.itow;//1個前のitow記録 | |
headmot1 = pvtd.headmot; | |
string ubx0 = PVTcnv(pvtb, ref pvtd); | |
string ubx1 = RELPOScnv(relpb, ref relpd); | |
sw.WriteLine("B," + ubx0 + "," + ubx1); | |
Console.WriteLine("B"+ubx0); | |
Console.WriteLine(ubx1); | |
//===========UBX Rover translate================================ | |
for (int j = 0; j < 100; j++)//bufからpvtbへ格納 | |
{ | |
pvtr[j] = buf[j + startn+172]; | |
} | |
for (int j = 0; j < 72; j++)//bufからrelpbへ格納 | |
{ | |
relpr[j] = buf[j + startn +172+ 100]; | |
} | |
//itow1 = pvtd.itow;//1個前のitow記録 | |
// headmot1 = pvtd.headmot; | |
string ubx2 = PVTcnv(pvtr, ref pvtdr); | |
string ubx3 = RELPOScnv(relpr, ref relpdr); | |
sw.WriteLine("R," + ubx2 + "," + ubx3); | |
Console.WriteLine("R"+ubx2); | |
Console.WriteLine(ubx3); | |
//----------itowエラーカウント------------------ | |
itowarry[i] = pvtd.itow; | |
if (i > 0) | |
{ | |
if (itowarry[i] - itow1 != 120)//itow loss検出 mb120msec | |
{ | |
itowErr++; | |
Console.WriteLine(">>>>itowErr=" + itowErr + "[" + itowarry[i] + "-" + itow1); | |
} | |
} | |
//Fix率 | |
if (pvtd.flags == 131) | |
{ | |
f131++; | |
} | |
else | |
{ | |
f673++; | |
} | |
totaln++; | |
} | |
} | |
float fixpercent =( (float)f131 / totaln)*100; | |
float itowErrper = ((float)itowErr / totaln) * 100; | |
float BNOErrper= ((float)index_error / (indexok+index_error)) * 100; | |
//string results = "*******itowErr/totaln=," + itowErr.ToString() + "/" + totaln.ToString() + ",[" + itowErrper.ToString("F2") + "%]" +"Fixper["+fixpercent.ToString("F2")+ " %],BNO IndexError=," + index_error.ToString()+",[" + BNOErrper.ToString("F2")+"%] ,File:" + f + "file size=" + fileSize.ToString() + "k=" + (k - 1).ToString(); | |
string results = "*******itowErr/totaln=," + itowErr.ToString() + "/" + totaln.ToString() + ",[" + itowErrper.ToString("F2") + "%]" + "Fixper[" + fixpercent.ToString("F2") + " %]"+"File:" + f + "file size=" + fileSize.ToString() + "k=" + (k - 1).ToString(); | |
sw.WriteLine(results); | |
Console.WriteLine(results); | |
sw.Close(); | |
fs.Dispose(); | |
Console.ReadKey(); | |
} | |
static void bnotocsv(int n, string f) | |
{ | |
FileStream fs = new FileStream(@f, FileMode.Open, FileAccess.Read); | |
string fc = f + "-MBNO.csv"; | |
int fileSize = (int)fs.Length; // ファイルのサイズ | |
byte[] buf; | |
buf = new byte[fileSize]; // データ格納用配列 | |
int index0 = 0; | |
int index_1 = 0; | |
int index_error = 0; | |
int indexok = 0; | |
int readSize = 0; // Readメソッドで読み込んだバイト数 | |
int remain = fileSize; // 読み込むべき残りのバイト数 | |
int bufPos = 0; // データ格納用配列内の追加位置 | |
int k = 0; | |
while (remain > 0) | |
{ | |
// 1024Bytesずつ読み込む | |
readSize = fs.Read(buf, bufPos, Math.Min(1024, remain)); | |
bufPos += readSize; | |
remain -= readSize; | |
} | |
Console.WriteLine("filesize="+fileSize.ToString()+"1024*k="+k.ToString()); | |
int i=0; | |
int index; | |
float yaw, pitch, roll, ax, ay, az; | |
long itowbno; | |
System.IO.StreamWriter sw = new System.IO.StreamWriter(@fc, false);//CSV OPEN | |
while (i < fileSize)//bno085 translate to value | |
{ | |
k++; | |
//Console.WriteLine("filesize="+fileSize.ToString()+":i="+i.ToString() + ","); | |
//AA:buf[i * 23],AA:buf[1+i * 23],INDEX:buf[2+i * 23] | |
if (buf[i] == 0xAA && buf[i + 1] == 0xAA) | |
{ | |
Console.Write("k="+k.ToString()+":i="+i.ToString() + ":"); | |
//for(int ii=i;ii<i+23;ii++) | |
// { | |
// Console.Write(buf[ii].ToString("X")+","); | |
//} | |
//Console.WriteLine(); | |
index_1 = index0; | |
index0 = buf[i+2 ]; | |
int yawi = BitConverter.ToInt16(buf,i+3);// ((float)buf[ i+3] + (float)buf[ i+4] * 256) / 100; | |
yaw = (float)yawi / 100; | |
int pitchi = BitConverter.ToInt16(buf, i + 5); | |
pitch = (float)pitchi / 100; | |
int rolli = BitConverter.ToInt16(buf, i + 7); | |
roll = (float)rolli / 100; | |
int axi = BitConverter.ToInt16(buf, i + 9); | |
ax = (float)axi/ 100; | |
int ayi = BitConverter.ToInt16(buf, i + 11); | |
ay = (float)ayi / 100; | |
int azi = BitConverter.ToInt16(buf, i + 13); | |
az = (float)azi / 100; | |
if (buf[i + 19] == 0XAA) | |
{ | |
i = i + 19; | |
itowbno = 0; | |
} | |
else | |
{ | |
itowbno = buf[22 + i ] + buf[21 + i ] *256+ buf[20 + i ]*256*256 + buf[19 + i ]*256*256*256; | |
i = i + 23; | |
} | |
if (index0 - index_1 == 1 || index0 - index_1 == -255) | |
{ | |
indexok++; | |
} | |
else | |
{ | |
index_error++; | |
} | |
string bnval= itowbno.ToString() + "," + yaw.ToString()+","+pitch.ToString() + "," + roll.ToString() + "," + ax.ToString() + "," + ay.ToString() + "," + az.ToString(); | |
Console.WriteLine(i.ToString()+","+bnval); | |
//Debug.Print("i="+i.ToString() + "," + itowbno.ToString() + "," + pitch.ToString() + "," + roll.ToString() + "," + ax.ToString() + "," + ay.ToString() + "," + az.ToString()); | |
sw.WriteLine(bnval); | |
k++; | |
} | |
else | |
{ | |
i++; | |
} | |
} | |
float BNOErrper = ((float)index_error / (indexok + index_error)) * 100; | |
string resultbn = "index_Error=," + index_error.ToString() + ",BNOErrper=," + BNOErrper.ToString()+ ",File:," + f + ",file size=," + fileSize.ToString() + ",k=," + (k - 1).ToString(); | |
Console.WriteLine(resultbn); | |
sw.WriteLine(resultbn); | |
//Console.WriteLine("File:" + f + "file size=" + fileSize.ToString() + "k=" + (k - 1).ToString()); | |
fs.Dispose(); | |
sw.Close(); | |
} | |
static byte[] readbnofile(int n, string f) | |
{ | |
// FileStream fs = new FileStream(fname, FileMode.CreateNew); | |
// BinaryWriter w = new BinaryWriter(fs); | |
string fc = ""; | |
System.IO.StreamWriter sw = new System.IO.StreamWriter(@fc, false); | |
FileStream fs = new FileStream(@f, FileMode.Open, FileAccess.Read); | |
int fileSize = (int)fs.Length; // ファイルのサイズ | |
byte[] buf; | |
buf = new byte[fileSize]; // データ格納用配列 | |
int readSize = 0; // Readメソッドで読み込んだバイト数 | |
int remain = fileSize; // 読み込むべき残りのバイト数 | |
int bufPos = 0; // データ格納用配列内の追加位置 | |
int k = 0; | |
while (remain > 0) | |
{ | |
// 172Bytesずつ読み込む | |
readSize = fs.Read(buf, bufPos, Math.Min(172, remain)); | |
bufPos += readSize; | |
remain -= readSize; | |
k++; | |
} | |
for (int i = 0; i < k - 1; i++) | |
{ | |
Console.Write(i.ToString() + ","); | |
for (int j = 0; j < 172; j++) | |
{ | |
Console.Write(buf[j + i * 172].ToString("X")); | |
Console.Write(","); | |
} | |
Console.WriteLine(); | |
} | |
Console.WriteLine("File:" + f + "file size=" + fileSize.ToString() + "k=" + (k - 1).ToString()); | |
fs.Dispose(); | |
return buf; | |
//============BNO data trans========================== | |
// else if (buf[0 + startn] == 0Xaa && buf[1 + startn] == 0Xaa) | |
//{ | |
byte[] b0 = new byte[2]; | |
int index0; | |
int index_1 = 0; | |
int index_error = 0; | |
int indexok = 0; | |
string bnostr = ""; | |
for (int m = 0; m < 10; m++) | |
{ | |
string hex1 = string.Format("{0:X2}", buf[m * 15 ]); | |
string hex2 = string.Format("{0:X2}", buf[m * 15 + 1 ]); | |
//BitConverter.ToInt16( bytes, index ); | |
int yawi = BitConverter.ToInt16(buf, m * 15 + 3 );// Convert.ToSingle(Toint16(buf[m * 15 + 3 + startn ], buf[m * 15 + 4 + startn ]) * 0.01); | |
int pitchi = BitConverter.ToInt16(buf, m * 15 + 5 );// Convert.ToSingle(Toint16(buf[m * 15 + 5 + startn ], buf[m * 15 + 6 + startn ]) * 0.01); | |
int rolli = BitConverter.ToInt16(buf, m * 15 + 7 );// Convert.ToSingle(Toint16(buf[m * 15 + 7 + startn ], buf[m * 15 + 8 + startn ]) * 0.01); | |
float yaw = (float)(yawi * 0.01); | |
float pitch = (float)(pitchi * 0.01); | |
float roll = (float)(rolli * 0.01); | |
bnostr = bnostr + "," + index0.ToString("X") + "," + yaw.ToString() + "," + pitch.ToString() + "," + roll.ToString(); | |
Console.WriteLine("[" + hex1 + "," + hex2 + "]," + index0 + "," + yaw + "," + pitch + "," + roll); | |
} | |
int bnoitow = (int)buf[150 ] + buf[151] * 256 + buf[152 ] * 256 * 256 + buf[153 ] * 256 * 256 * 256; | |
int itownow = (int)buf[168 ] + buf[169 ] * 256 + buf[170 ] * 256 * 256 + buf[171 ] * 256 * 256 * 256; | |
Console.WriteLine("index0=," + index0.ToString() + ",index_error=," + index_error.ToString() + ",/indexOK=," + indexok.ToString() + ",itownow=," + itownow.ToString() + ",bnotiow=," + bnoitow.ToString()); | |
sw.WriteLine("B," + itownow.ToString() + "," + bnoitow.ToString() + "," + index_error.ToString() + "," + bnostr); | |
//} | |
//else | |
//{ | |
Console.WriteLine("****************Error In BNO data:buf[" + "]=" + string.Format("{0:X2}", buf[0 + 172])); | |
var inp = Console.ReadKey(true);//キーインしても表示しない | |
//} | |
} | |
static void Main() | |
{ | |
pvtdata = new byte[100]; | |
relpdata = new byte[72]; | |
string amyip = "";//現在のip | |
f131 = 0; | |
f673 = 0; | |
itow_1 = 0; | |
itowloss = 0; | |
itowper = 0; | |
fixper = 0; | |
//var files = Directory.GetFiles(@"C:\\RTK_LOG\", "*.bin"); | |
//files.ToList().ForEach(name => Console.WriteLine(name)); | |
//バインドするローカルIPとポート番号 | |
string localIpString = myipread();// "192.168.0.109"; | |
string cdir = System.Environment.CurrentDirectory; | |
Console.WriteLine(cdir); | |
// string[] files = System.IO.Directory.GetFiles(@"C:\RTK_LOG", "*.*", System.IO.SearchOption.AllDirectories); | |
string[] files = System.IO.Directory.GetFiles(@cdir, "*.*", System.IO.SearchOption.AllDirectories); | |
int[] fsize; | |
fsize = new int[files.Length]; | |
for (int i = 0; i < files.Length; i++) | |
{ | |
FileInfo filei = new FileInfo(files[i]); | |
fsize[i] = (int)filei.Length; | |
Console.WriteLine(i.ToString() + " , " + files[i] + " , " + fsize[i].ToString() + ",byte"); | |
} | |
//バインドするローカルIPとポート番号 | |
// | |
// 自分のホスト名を取得する | |
string hostname = Dns.GetHostName(); | |
// ホスト名からIPアドレスを取得する | |
IPAddress[] adrList = Dns.GetHostAddresses(hostname); | |
string myip = ""; | |
Console.WriteLine("\n\r"); | |
int j = 0; | |
foreach (IPAddress address in adrList) | |
{ | |
// Console.WriteLine("adrList["+j.ToString()+"]="+address.ToString()); | |
myip = address.ToString(); | |
j++; | |
} | |
if (myip != localIpString) | |
{ | |
Console.WriteLine("CHECK WiFi Connection:::Change Current myip=" + myip + " To= " + localIpString + "Then hit Any key"); | |
} | |
//Console.WriteLine(myip+"--To Start Hit any key--"); | |
DateTime dt = DateTime.Now; | |
string nowtime = dt.ToString("MM-dd-hh-mm-ss"); | |
//Console.WriteLine(nowtime); | |
fname = "C:\\RTK_LOG\\" + nowtime + ".bin"; | |
/* | |
//UDP Setting | |
if (amyip == myip)//ip接続できてたらバインド | |
{ | |
System.Net.IPAddress localAddress = System.Net.IPAddress.Parse(localIpString); | |
int localPort = 10000; | |
//UdpClientを作成し、ローカルエンドポイントにバインドする | |
System.Net.IPEndPoint localEP = new System.Net.IPEndPoint(localAddress, localPort); | |
System.Net.Sockets.UdpClient udp = new System.Net.Sockets.UdpClient(localEP); | |
Console.WriteLine(); | |
//System.Net.IPEndPoint remoteEP = null; | |
//Console.WriteLine(n.ToString() + ":送信元アドレス:{0}/ポート番号:{1}", remoteEP.Address, remoteEP.Port); | |
//espip = remoteEP.Address.ToString(); | |
Console.WriteLine("FileName=" + fname + ",MyIp=" + myip); | |
} | |
//string[] files = System.IO.Directory.GetFiles(@"C:\test,"*.", System.IO.SearchOption.AllDirectories); | |
//string[] files = System.IO.Directory.GetFiles(@"C:\RTK_LOG", "*.bin", System.IO.SearchOption.AllDirectories); | |
//int[] fsize; | |
fsize = new int[files.Length]; | |
for (int i = 0; i < files.Length; i++) | |
{ | |
FileInfo filei = new FileInfo(files[i]); | |
fsize[i] = (int)filei.Length; | |
Console.WriteLine(i.ToString() + " , " + files[i] + " , " + fsize[i].ToString() + ",byte"); | |
} | |
*/ | |
//DateTime dt = DateTime.Now; | |
//string nowtime = dt.ToString("MM-dd-hh-mm-ss"); | |
//Console.WriteLine(nowtime); | |
fname = "C:\\RTK_LOG\\" + nowtime + ".bin"; | |
Console.WriteLine("---キーを押してください ' b':BNO bin READ to CSV --- 'u':ubx READ to CSV"); | |
//ConsoleKeyInfo input = Console.ReadKey(); | |
//var inp = Console.ReadKey(true);//キーインしても表示しない | |
var inp = Console.ReadKey();//キーインしても表示しない | |
key = inp.KeyChar; | |
string[] filesubx; | |
string[] filesbno; | |
int[] fsizeubx; | |
int[] fsizebno; | |
if (inp.KeyChar == 'u') | |
{ | |
filesubx = System.IO.Directory.GetFiles(@cdir, "*.*", System.IO.SearchOption.AllDirectories); | |
fsize = new int[filesubx.Length]; | |
for (int i = 0; i < filesubx.Length; i++) | |
{ | |
FileInfo filei = new FileInfo(filesubx[i]); | |
fsize[i] = (int)filei.Length; | |
Console.WriteLine(i.ToString() + " , " + files[i] + " , " + fsize[i].ToString() + ",byte"); | |
} | |
//Console.WriteLine("---Binary data Converting to UBX Messages"); | |
key = 'u'; | |
Console.WriteLine("---FILE番号をキーインしてENTERしてください"); | |
string fns = Console.ReadLine(); | |
int fn = Convert.ToInt32(fns); | |
ubxtocsv(2, files[fn]); | |
//Console.ReadLine(); | |
Console.WriteLine("rcvBytes=" + rcvBytes.Length.ToString()); | |
Thread.Sleep(10000); | |
} | |
else if (inp.KeyChar == 'n')//bno file | |
{ | |
filesbno = System.IO.Directory.GetFiles(@cdir, "*.*", System.IO.SearchOption.AllDirectories); | |
fsize = new int[filesbno.Length]; | |
for (int i = 0; i < filesbno.Length; i++) | |
{ | |
FileInfo filei = new FileInfo(filesbno[i]); | |
fsize[i] = (int)filei.Length; | |
Console.WriteLine(i.ToString() + " , " + files[i] + " , " + fsize[i].ToString() + ",byte"); | |
} | |
//Console.WriteLine("---Binary data Converting to bno value"); | |
key = 'n'; | |
Console.WriteLine("---FILE番号をキーインしてENTERしてください"); | |
string fns = Console.ReadLine(); | |
int fn = Convert.ToInt32(fns); | |
bnotocsv(2, files[fn]); | |
//Console.ReadLine(); | |
// Console.WriteLine("rcvBytes=" + rcvBytes.Length.ToString()); | |
Thread.Sleep(10000); | |
} | |
else if (inp.KeyChar == 'b') | |
{ | |
Console.WriteLine("---FILE番号をキーインしてENTERしてください"); | |
string fns = Console.ReadLine(); | |
int fn = Convert.ToInt32(fns); | |
rcvBytes = readbnofile(2, files[fn]); | |
Console.WriteLine("rcvBytes=" + rcvBytes.Length.ToString()); | |
} | |
else if (inp.KeyChar == 'a' || inp.KeyChar == 'm' || inp.KeyChar == 'b') | |
{ | |
// 自分のホスト名を取得する | |
string ahostname = Dns.GetHostName(); | |
// ホスト名からIPアドレスを取得する | |
IPAddress[] aadrList = Dns.GetHostAddresses(ahostname); | |
System.Net.IPAddress localAddress = System.Net.IPAddress.Parse(localIpString); | |
int localPort = 10000; | |
//UdpClientを作成し、ローカルエンドポイントにバインドする | |
System.Net.IPEndPoint localEP = new System.Net.IPEndPoint(localAddress, localPort); | |
System.Net.Sockets.UdpClient udp = new System.Net.Sockets.UdpClient(localEP); | |
Console.WriteLine("\n\r"); | |
foreach (IPAddress aaddress in aadrList) | |
{ | |
// Console.WriteLine("adrList["+j.ToString()+"]="+address.ToString()); | |
amyip = aaddress.ToString(); | |
} | |
Console.WriteLine("Changed=" + amyip); | |
int n = 0; | |
int m = 0; | |
key = inp.KeyChar; | |
FileStream fs = new FileStream(fname, FileMode.CreateNew); | |
BinaryWriter w = new BinaryWriter(fs); | |
//========================LOOP START=========================================================== | |
for (; ; ) | |
{ | |
//データを受信する | |
System.Net.IPEndPoint remoteEP = null; | |
rcvBytes = udp.Receive(ref remoteEP); | |
for (int i = 0; i < 172; i++) | |
{ | |
w.Write(rcvBytes[i]); | |
} | |
n++; | |
//for (int i = 0; i < rcvBytes.Length; i++) | |
if (key == 'b') | |
{ | |
for (int i = 0; i < 172; i++) | |
{ | |
string hexdata = rcvBytes[i].ToString("X"); | |
Console.Write(hexdata); | |
Console.Write(','); | |
if (i == 99) | |
{ | |
Console.WriteLine(); | |
} | |
} | |
Console.WriteLine("\n\r"); | |
//File Save | |
} | |
else if (key == 'a' && rcvBytes[0] == 0xb5) | |
{ | |
for (j = 0; j < 100; j++) | |
{ | |
pvtdata[j] = rcvBytes[j]; | |
} | |
string result = PVTcnv(pvtdata, ref pvtd); | |
Console.WriteLine(result); | |
//Console.WriteLine(pvtd.itow+":"+pvtd.flags+","+pvtd.hacc+","+pvtd.pdop); | |
for (j = 0; j < 72; j++) | |
{ | |
relpdata[j] = rcvBytes[j + 72]; | |
} | |
string result2 = RELPOScnv(relpdata, ref relpd); | |
//Console.WriteLine(relpd.relposlength); | |
//Console.Beep(2200,1); | |
} | |
else if (key == 'm' && rcvBytes[0] == 0xb5) | |
{ | |
m++; | |
for (j = 0; j < 100; j++) | |
{ | |
pvtdata[j] = rcvBytes[j]; | |
} | |
itow_1 = (int)pvtd.itow; | |
string result = PVTcnv(pvtdata, ref pvtd); | |
for (j = 0; j < 72; j++) | |
{ | |
relpdata[j] = rcvBytes[j + 100]; | |
} | |
string result2 = RELPOScnv(relpdata, ref relpd); | |
//flags計算 | |
if (relpd.flags == 131) | |
{ | |
f131++; | |
} | |
else | |
{ | |
f673++; | |
} | |
fixper = (double)((double)f131 / m) * 100; | |
Debug.Print(f131.ToString() + ":" + f673.ToString() + "/" + m.ToString() + ":" + fixper.ToString()); | |
//itow loss 計算 | |
Debug.Print(itow_1.ToString() + ":" + pvtd.itow.ToString()); | |
if (pvtd.itow - itow_1 != 100) | |
{ | |
itowloss++; | |
} | |
itowper = (double)((double)(m - itowloss) / m) * 100; | |
Console.WriteLine(pvtd.itow + ":" + pvtd.flags + "," + pvtd.hacc + "," + pvtd.pdop + ",Fix[" + fixper.ToString("F2") + "%]-" + f673.ToString() + ",Loss[" + itowper.ToString("F2") + "%]-" + itowloss.ToString() + "," + relpd.relposlength.ToString() + "," + m.ToString()); | |
} | |
} | |
udp.Close(); | |
Console.WriteLine("終了しました。"); | |
} | |
} | |
//-------------Main END-------------------------------------------------------------------------------------- | |
//----------------------------------------------------------------------------------------------------------- | |
//+++PVTcnv++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
static string PVTcnv(byte[] d, ref pvt p) | |
{ | |
string pvts = ""; | |
//PVT header[0-6] | |
//0:itow[6-9] | |
p.itow = (uint)Toint32(d[9], d[8], d[7], d[6]); | |
pvts = p.itow.ToString(); | |
//Serial.printf("PVTcnv:itow=%d\n\r",pvt[0]); | |
//1:year[10-12] | |
p.year = (ushort)(d[10] + d[11] * 256); | |
pvts = pvts + "," + p.year.ToString(); | |
//2:month[12] | |
p.month = d[12]; | |
pvts = pvts + "," + p.month.ToString(); | |
//3:day[13] | |
p.day = d[13]; | |
pvts = pvts + "," + p.day.ToString(); | |
//4:hour[14] | |
p.hour = d[14]; | |
pvts = pvts + "," + p.hour.ToString(); | |
//Serial.printf("PVTcnv:hour=%d\n\r",pvt[4]); | |
//5:min[15] | |
p.min = d[15]; | |
pvts = pvts + "," + p.min.ToString(); | |
//Serial.printf("PVTcnv:min=%d\n\r",pvt[5]); | |
//6:sec[16] | |
p.sec = d[16]; | |
pvts = pvts + "," + p.sec.ToString(); | |
//Serial.printf("PVTcnv:sec=%d\n\r",pvt[6]); | |
//7:valid[17] | |
p.valid = d[17]; | |
pvts = pvts + "," + p.valid.ToString(); | |
//8:tAcc[18-21] | |
p.tacc = (uint)Toint32(d[21], d[20], d[19], d[18]); | |
pvts = pvts + "," + p.tacc.ToString(); | |
//9:nano[22-25] | |
p.nano = Toint32(d[25], d[24], d[23], d[22]); | |
pvts = pvts + "," + p.nano.ToString(); | |
//9:nano[22-25] | |
//10:fixType[26] | |
p.fixtype = d[26]; | |
pvts = pvts + "," + p.fixtype.ToString(); | |
//11:flags[27] This is Fix status 131 | |
p.flags = d[27]; | |
pvts = pvts + "," + p.flags.ToString(); | |
//12:flags2[28] | |
p.flags2 = d[28]; | |
pvts = pvts + "," + p.flags2.ToString(); | |
//13:numSV[29] | |
p.numsv = d[29]; | |
pvts = pvts + "," + p.numsv.ToString(); | |
//14:lon[30-33] | |
p.lon = Toint32(d[33], d[32], d[31], d[30]); | |
pvts = pvts + "," + p.lon.ToString(); | |
//15:lat[34-37] | |
p.lat = Toint32(d[37], d[36], d[35], d[34]); | |
pvts = pvts + "," + p.lat.ToString(); | |
//16:height[38-41] | |
p.height = Toint32(d[41], d[40], d[39], d[38]); | |
pvts = pvts + "," + p.height.ToString(); | |
//17:hMSL[42-45] | |
p.hmsl = Toint32(d[45], d[44], d[43], d[42]); | |
pvts = pvts + "," + p.hmsl.ToString(); | |
//18:hAcc[46-49] | |
p.hacc = (uint)Toint32(d[49], d[48], d[47], d[46]); | |
if (p.hacc > 1000) | |
{ | |
p.hacc = 0; | |
} | |
pvts = pvts + "," + p.hacc.ToString(); | |
//19:vAcc[50-53] | |
p.vacc = (uint)Toint32(d[53], d[52], d[51], d[50]); | |
pvts = pvts + "," + p.vacc.ToString(); | |
//20:velN[54-57] | |
p.veln = Toint32(d[57], d[56], d[55], d[54]); | |
pvts = pvts + "," + p.veln.ToString(); | |
//21:velE[58-61] | |
p.vele = Toint32(d[61], d[60], d[59], d[58]); | |
pvts = pvts + "," + p.vele.ToString(); | |
//22:velD[62-65] | |
p.veld = Toint32(d[65], d[64], d[63], d[62]); | |
pvts = pvts + "," + p.veld.ToString(); | |
//23:gSpeed[66-69] | |
p.gspeed = Toint32(d[69], d[68], d[67], d[66]); | |
pvts = pvts + "," + p.gspeed.ToString(); | |
//24:headMot[70-73] | |
p.headmot = Toint32(d[73], d[72], d[71], d[70]); | |
pvts = pvts + "," + p.headmot.ToString(); | |
//25:sAcc[74-77] | |
p.sacc = (uint)Toint32(d[77], d[76], d[75], d[74]); | |
pvts = pvts + "," + p.sacc.ToString(); | |
//26:headAcc[78-81] | |
p.headacc = (uint)Toint32(d[81], d[80], d[79], d[78]); | |
pvts = pvts + "," + p.headacc.ToString(); | |
//27:pDOP[82-83] | |
p.pdop = (ushort)(d[82] + d[83] * 256); | |
pvts = pvts + "," + p.pdop.ToString(); | |
//28:flags3[84] | |
p.flags3 = d[84]; | |
pvts = pvts + "," + p.flags3.ToString(); | |
//29:reserved1[85] | |
p.resrv0 = d[85]; | |
pvts = pvts + "," + p.resrv0.ToString(); | |
//30:headVeh[86-89] | |
p.headveh = Toint32(d[89], d[88], d[87], d[86]); | |
pvts = pvts + "," + p.headveh.ToString(); | |
//31:magDec[90-91] | |
p.magdec = (short)(d[90] + d[91] * 256); | |
pvts = pvts + "," + p.magdec.ToString(); | |
//32:magAcc[92-93] | |
p.magacc = (ushort)(d[92] + d[93] * 256); | |
pvts = pvts + "," + p.magacc.ToString(); | |
return pvts; | |
}//PVTcnv() end | |
//--RELPOScnv++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ | |
static string RELPOScnv(byte[] d, ref relp r) | |
{ | |
//RELPOS header[0-5] | |
string relps = ""; | |
//Debug.Print("{RELPOScnv]:d[0][1][2][3]" + d[0].ToString("X") + d[1].ToString("X") + d[2].ToString("X") + d[3].ToString("X")); | |
int s = 0; | |
//0:ver[6] | |
r.version = d[s + 6]; | |
relps = relps + "," + r.version.ToString(); | |
//1:reserved1[7] | |
r.reserved1 = d[s + 7]; | |
relps = relps + "," + r.reserved1.ToString(); | |
//2:refStationId[8-9] | |
r.refstationid = d[s + 8]; | |
relps = relps + "," + r.refstationid.ToString(); | |
//3:itow[10-13] | |
//Debug.Print("{RELPOScnv ITOW]:d[10][11][12][13]" + d[10].ToString("X") + d[11].ToString("X") + d[12].ToString("X") + d[13].ToString("X")); | |
r.itow = (uint)Toint32(d[s + 13], d[s + 12], d[s + 11], d[s + 10]); | |
relps = relps + "," + r.itow.ToString(); | |
//Serial.printf("RELPOScnv:itow=%d\n\r",relpos[3]); | |
//4:relposN[14-17] | |
r.relposn = Toint32(d[s + 17], d[s + 16], d[s + 15], d[s + 14]); | |
relps = relps + "," + r.relposn.ToString(); | |
//5:relposE[18-21] | |
r.relpose = Toint32(d[s + 21], d[s + 20], d[s + 19], d[s + 18]); | |
relps = relps + "," + r.relpose.ToString(); | |
//6:relposD[22-25] | |
r.relposd = Toint32(d[s + 25], d[s + 24], d[s + 23], d[s + 22]); | |
relps = relps + "," + r.relposd.ToString(); | |
//7:relposLength[26-29] | |
r.relposlength = Toint32(d[s + 29], d[s + 28], d[s + 27], d[s + 26]); | |
relps = relps + "," + r.relposlength.ToString(); | |
//Serial.printf("RELPOScnv:Lenghth=relpos[7]=%d\n\r",relpos[7]); | |
//8:relposHeading[30-33] | |
r.relposheading = Toint32(d[s + 33], d[s + 32], d[s + 31], d[s + 30]); | |
relps = relps + "," + r.relposheading.ToString(); | |
//Serial.printf("relposHeading[8]=%d,[33]%x,[32]%x,[31]%x,[30]%x,\n\r",relpos[8],d[33],d[32],d[31],d[30]); | |
//9:reserved2[34] | |
r.reserved2 = Toint32(d[s + 37], d[s + 36], d[s + 35], d[s + 34]); | |
relps = relps + "," + r.reserved2.ToString(); | |
//10:relposHPN[35] | |
r.relposhpn = (sbyte)((d[s + 38] & 127) - (d[s + 38] & 128)); | |
relps = relps + "," + r.relposhpn.ToString(); | |
//Serial.printf("HPN=%d,d[38]=%x,d[39]=%x,d[40]=%x,d[41]=%x,&127=%dx,&128=%d\n\r",relpos[10],d[38],d[39],d[40],d[41],d[38] && 127,d[38] && 128); | |
//11:relposHPE[36] | |
r.relposhpe = (sbyte)((d[s + 39] & 127) - (d[s + 39] & 128)); | |
relps = relps + "," + r.relposhpe.ToString(); | |
//12:relposHPD[37] | |
r.relposhpd = (sbyte)((d[s + 40] & 127) - (d[s + 40] & 128)); | |
relps = relps + "," + r.relposhpd.ToString(); | |
//13:relposHPLength[38] | |
r.relposhplength = (sbyte)((d[s + 41] & 127) - (d[s + 41] & 128)); | |
relps = relps + "," + r.relposhplength.ToString(); | |
//14:accN[38-41] | |
r.accn = (uint)Toint32(d[s + 41], d[s + 40], d[s + 39], d[s + 38]); | |
relps = relps + "," + r.accn.ToString(); | |
//15:accE[42-45] | |
r.acce = (uint)Toint32(d[s + 45], d[s + 44], d[s + 43], d[s + 42]); | |
relps = relps + "," + r.acce.ToString(); | |
//16:accD[46-49] | |
r.accd = (uint)Toint32(d[s + 49], d[s + 48], d[s + 47], d[s + 46]); | |
relps = relps + "," + r.accd.ToString(); | |
//17:accLength[50-53] | |
r.acclength = (uint)Toint32(d[s + 53], d[s + 52], d[s + 51], d[s + 50]); | |
relps = relps + "," + r.acclength.ToString(); | |
//18:accHeading[54-57] | |
r.accheading = (uint)Toint32(d[s + 57], d[s + 56], d[s + 55], d[s + 54]); | |
relps = relps + "," + r.accheading.ToString(); | |
//19:reserved[57-60] | |
r.reserved3 = (uint)Toint32(d[s + 61], d[s + 60], d[s + 59], d[s + 58]); | |
relps = relps + "," + r.reserved3.ToString(); | |
//20:flags[60-63] | |
r.flags = (uint)Toint32(d[s + 65], d[s + 64], d[s + 63], d[s + 62]); | |
relps = relps + "," + r.flags.ToString(); | |
return relps; | |
} | |
//+++++++++++++4byte Binary to Long ++++++++++++++++++++++++++++++++++++++++++++++ | |
static int Toint32(byte b4, byte b3, byte b2, byte b1) | |
{ | |
//pc.printf("ToInt32 IN=%s,%x,%x,%x,%x,b4&0x80=%d\n\r",sen,b4,b3,b2,b1,b4 &0x80); | |
//pc.printf("ToInt32 IN=b4&0x80=%d\n\r",b4 & 0x80); | |
long su; | |
if ((b4 & 0b10000000) == 0b10000000) | |
{ //最上位ビットたっていればマイナス | |
//su = -(256 - (long)b1) + (255 - (long)b2) * 256 + (255 - (long)b3) * 65536 + (255 - (long)b4) * 256 * 256 * 256; | |
//pc.printf("ToInt32-:sen=%s,%d,%d,%d,%d,%d\n\r",sen,b4,b3,b2,b1,su); | |
uint i1 = b1; | |
uint i2 = (uint)b2 << 8; | |
uint i3 = (uint)b3 << 16; | |
uint i4 = (uint)b4 << 24; | |
uint i5 = i1 | i2 | i3 | i4; | |
su = (int)i5; | |
} | |
else | |
{ | |
su = (int)b1 + (int)b2 * 256 + (int)b3 * 65536 + (int)b4 * 256 * 256 * 256; | |
//pc.printf("ToInt32+:sen=%s,%d,%d,%d,%d,%d,%d\n\r",sen,b4,b3,b2,b1,su); | |
} | |
return (int)su; | |
} | |
// 2byte を符号付整数へ | |
static int Toint16(byte b2, byte b1)//b2=L,b1=H | |
{ | |
//pc.printf("ToInt32 IN=%s,%x,%x,%x,%x,b4&0x80=%d\n\r",sen,b4,b3,b2,b1,b4 &0x80); | |
//pc.printf("ToInt32 IN=b4&0x80=%d\n\r",b4 & 0x80); | |
long su; | |
if ((b1 & 0b10000000) == 0b10000000) | |
{ //最上位ビットたっていればマイナス | |
//su = -(256 - (long)b1) + (255 - (long)b2) * 256 + (255 - (long)b3) * 65536 + (255 - (long)b4) * 256 * 256 * 256; | |
//pc.printf("ToInt32-:sen=%s,%d,%d,%d,%d,%d\n\r",sen,b4,b3,b2,b1,su); | |
byte i1 = (byte)(b1 & 0b01111111); | |
byte i2 = (byte)(b2 << 8); | |
uint i5 = (uint)i1 | i2; | |
su = -i5; | |
} | |
else | |
{ | |
su = (long)b1 + (long)b2 * 256; | |
//pc.printf("ToInt32+:sen=%s,%d,%d,%d,%d,%d,%d\n\r",sen,b4,b3,b2,b1,su); | |
} | |
return (int)su; | |
} | |
//================================================================================= | |
//+++++++++++++++i_to_char++++++++++++++++++++++++++++++++++++ | |
// i=IntegerValueData,*d=Array pointer, n=Array start No | |
static void i_to_char(int i, byte[] d, int n) | |
{ | |
d = BitConverter.GetBytes(i); | |
/* | |
d[n] = i & 0x000000ff; | |
d[n + 1] = (i & 0x0000ff00) >> 8; | |
d[n + 2] = (i & 0x00ff0000) >> 16; | |
d[n + 3] = (i & 0xff000000) >> 24; | |
*/ | |
// Serial.printf("itochar:i=%d,d[0]=%x,d[1]=%x,d[2]=%x,d[3]=%x\n\r",i,d[0],d[1],d[2],d[3]); | |
} | |
//++++++++++++++++String to CharArray Trans++++++++++++++++++++ | |
static void str2char(char[] c, String dataS) | |
{ | |
//String dataS; | |
//dataS="HELLO dataS"; | |
int dataS_len = dataS.Length + 1; | |
// char char_array[dataS_len]; | |
c = dataS.ToCharArray(); | |
} | |
//itow=> HH:MM:SS ================================================== | |
static String itowToHMS(int itow) | |
{ | |
String HMS; | |
int DAY, HOUR, MIN, SEC, JHOUR; | |
int DAY_MOD, HOUR_MOD, MIN_MOD;//, SEC_MOD; | |
//DAY = int(itow / 86400000); | |
DAY_MOD = itow % 86400000; | |
HOUR = (int)(DAY_MOD / 3600000); | |
HOUR_MOD = DAY_MOD % 3600000; | |
MIN = (int)(HOUR_MOD / 60000); | |
MIN_MOD = HOUR_MOD % 60000; | |
SEC = (int)(MIN_MOD / 1000); | |
//--UTC=>JST | |
if (HOUR > 15) | |
{ | |
JHOUR = HOUR + 9 - 24; | |
} | |
else | |
{ | |
JHOUR = HOUR + 9; | |
} | |
//=====18sec 進んでいる?補正======= | |
if (SEC < 18) | |
{ | |
SEC = 60 - (18 - SEC); | |
} | |
else | |
{ | |
SEC = SEC - 18; | |
} | |
//------------- | |
//Serial.printf("itowToHMS:JHOUR=%d,MIN=%d,SEC=%d\n\r",JHOUR,MIN,SEC); | |
HMS = JHOUR.ToString() + ":" + MIN.ToString() + ":" + SEC.ToString(); | |
return HMS; | |
}//itowToHMS end===================================================== | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment