Created
March 10, 2024 08:39
-
-
Save dj1711572002/85da6b69827c45f178cf4ad42a1df54e to your computer and use it in GitHub Desktop.
C# UDP Reciving Logging pgm for RTK SimpleRTK2B +IMU BNO085
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.Drawing; | |
public class pvtc | |
{ | |
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; | |
} | |
public class bindatactocsv | |
{ | |
//%%%%%%%%%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; | |
//%%%%%%%%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; | |
//BNO struct | |
// KIJUN struct | |
struct kijun | |
{ | |
public double rNaveN; | |
public double rEaveN; | |
public double rDaveN; | |
public double rLaveN; | |
public double rNstd; | |
public double rEstd; | |
public double rDstd; | |
public double rLstd; | |
public double rLmaveN; | |
public double rLmstd; | |
public double mHead; | |
public int pday; | |
public int phour; | |
public int pmin; | |
public int psec; | |
}; | |
kijun kave;//基準点 | |
struct bno | |
{ | |
//Euler Angle from Quarternion | |
public float[] eX; | |
public float[] eY; | |
public float[] eZ; | |
//Quzrternion | |
public float[] qW; | |
public float[] qX; | |
public float[] qY; | |
public float[] qZ; | |
//Linear Acc | |
public float[] laX; | |
public float[] laY; | |
public float[] laZ; | |
//Gravity acc | |
public float[] grX; | |
public float[] grY; | |
public float[] grZ; | |
//time stamp | |
public int[] tst;// | |
} | |
static string espip; | |
static byte[] rcvBytes; | |
static string fname; | |
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; | |
} | |
//bintocsv(n,f)==================STA24 UDP_LogfileをUBX ASCII変換してCSVファイル保管============================ | |
static void bintocsv(int n, string f) | |
{ | |
FileStream fs = new FileStream(@f, FileMode.Open, FileAccess.Read); | |
string fc = f + ".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; | |
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; | |
while (remain > 0) | |
{ | |
// 172Bytesずつbuf[]に読み込む | |
readSize = fs.Read(buf, bufPos, Math.Min(172, remain)); | |
bufPos += readSize; | |
remain -= readSize; | |
k++;//172byte単位でカウントアップ | |
} | |
itowarry = new long[k]; | |
System.IO.StreamWriter sw = new System.IO.StreamWriter(@fc, false); | |
for (int i = 0; i < k - 1; i++)//172byte単位で読む | |
{ | |
//Console.Write(i.ToString() + "," + string.Format("{0:X2}", buf[i*172])+",:,"); | |
//TextBox1.Textの内容を書き込む | |
//PVT取り出し | |
int startn = i * 172; | |
byte[] pvtb = new byte[100]; | |
byte[] relpb = 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 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("U," + ubx0 + "," + ubx1); | |
Console.WriteLine(ubx0); | |
Console.WriteLine(ubx1); | |
//----------itowエラーカウント------------------ | |
itowarry[i] = pvtd.itow; | |
if (i > 0) | |
{ | |
if (itowarry[i] - itow1 != 100)//itow loss検出 | |
{ | |
itowErr++; | |
Console.WriteLine(">>>>itowErr=" + itowErr + "[" + itowarry[i] + "-" + itow1); | |
} | |
} | |
//Fix率 | |
if (pvtd.flags == 131) | |
{ | |
f131++; | |
} | |
else | |
{ | |
f673++; | |
} | |
totaln++; | |
} | |
//============BNO data trans========================== | |
else if (buf[0 + startn] == 0Xaa && buf[1 + startn] == 0Xaa) | |
{ | |
byte[] b0 = new byte[2]; | |
string bnostr = ""; | |
for (int m = 0; m < 10; m++) | |
{ | |
string hex1 = string.Format("{0:X2}", buf[m * 15 + startn]); | |
string hex2 = string.Format("{0:X2}", buf[m * 15 + 1 + startn]); | |
index_1 = index0; | |
index0 = (int)buf[m * 15 + 2 + startn]; | |
if (index0 - index_1 == 1 || index0 - index_1 == -255) | |
{ | |
indexok++; | |
} | |
else | |
{ | |
index_error++; | |
} | |
//BitConverter.ToInt16( bytes, index ); | |
int yawi = BitConverter.ToInt16(buf, m * 15 + 3 + startn);// Convert.ToSingle(Toint16(buf[m * 15 + 3 + startn ], buf[m * 15 + 4 + startn ]) * 0.01); | |
int pitchi = BitConverter.ToInt16(buf, m * 15 + 5 + startn);// Convert.ToSingle(Toint16(buf[m * 15 + 5 + startn ], buf[m * 15 + 6 + startn ]) * 0.01); | |
int rolli = BitConverter.ToInt16(buf, m * 15 + 7 + startn);// 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 + startn] + buf[151 + startn] * 256 + buf[152 + startn] * 256 * 256 + buf[153 + startn] * 256 * 256 * 256; | |
int itownow = (int)buf[168 + startn] + buf[169 + startn] * 256 + buf[170 + startn] * 256 * 256 + buf[171 + startn] * 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[" + (startn + 172) + "]=" + string.Format("{0:X2}", buf[0 + startn + 172])); | |
var inp = Console.ReadKey(true);//キーインしても表示しない | |
} | |
} | |
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(); | |
sw.WriteLine(results); | |
Console.WriteLine(results); | |
sw.Close(); | |
fs.Dispose(); | |
Console.ReadKey(); | |
} | |
static byte[] readbinfile(int n, string f) | |
{ | |
// FileStream fs = new FileStream(fname, FileMode.CreateNew); | |
// BinaryWriter w = new BinaryWriter(fs); | |
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) | |
{ | |
// 1024Bytesずつ読み込む | |
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; | |
} | |
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[] 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"); | |
} | |
//バインドするローカル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("---キーを押してください 'a':Recieve 'm':Monitor(Fix,itow)--- 'u':READ BINfile(UBX+BNO CSV)"); | |
//ConsoleKeyInfo input = Console.ReadKey(); | |
//var inp = Console.ReadKey(true);//キーインしても表示しない | |
var inp = Console.ReadKey();//キーインしても表示しない | |
key = inp.KeyChar; | |
if (inp.KeyChar == 'u') | |
{ | |
//Console.WriteLine("---Binary data Converting to UBX Messages"); | |
key = 'u'; | |
Console.WriteLine("---FILE番号をキーインしてENTERしてください"); | |
string fns = Console.ReadLine(); | |
int fn = Convert.ToInt32(fns); | |
bintocsv(2, files[fn]); | |
//Console.ReadLine(); | |
Console.WriteLine("rcvBytes=" + rcvBytes.Length.ToString()); | |
Thread.Sleep(10000); | |
} | |
else if (inp.KeyChar == 'r') | |
{ | |
Console.WriteLine("---FILE番号をキーインしてENTERしてください"); | |
string fns = Console.ReadLine(); | |
int fn = Convert.ToInt32(fns); | |
rcvBytes = readbinfile(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 + ":flags=," + pvtd.flags + ",hacc=," + pvtd.hacc + ",pdop=," + pvtd.pdop + ",Fix[" + fixper.ToString("F2") + "%]" + ",itowLoss[" + itowper.ToString("F2") + "%]," ); | |
} | |
} | |
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