Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/STA24_Console_UBX_MB_frev00.cs Secret

Created March 10, 2024 08:43
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save dj1711572002/4a2faa7eb5bab4e08c987f8f76b4fbbb to your computer and use it in GitHub Desktop.
Save dj1711572002/4a2faa7eb5bab4e08c987f8f76b4fbbb to your computer and use it in GitHub Desktop.
Download ZIP
C# Console RTK UBX file Translation an BNO085 RVC mode Binary file translation toCSVFile
Raw
STA24_Console_UBX_MB_frev00.cs
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