Skip to content

Instantly share code, notes, and snippets.

@dj1711572002

dj1711572002/STA24_Console_BinDataToCSV_rev00.cs Secret

Created February 5, 2024 12: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/e1ca476eabfc566c670e611e5327f81a to your computer and use it in GitHub Desktop.
Save dj1711572002/e1ca476eabfc566c670e611e5327f81a to your computer and use it in GitHub Desktop.
Download ZIP
RTK UBX data Translate Pgm
Raw
STA24_Console_BinDataToCSV_rev00.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;
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 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);
}
}
//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 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++;//172byte単位でカウントアップ
}
int bunkatsu0 = (int)(k / 3);
System.IO.StreamWriter sw = new System.IO.StreamWriter(@fc, true);
for (int i = 0; i < k - 1; i++)//172byte単位で読む
{
Console.Write(i.ToString() + ",");
//TextBox1.Textの内容を書き込む
//PVT取り出し
int startn = i * 172;
byte[] pvtb = new byte[100];
byte[] relpb = new byte[72];
if (buf[0 + startn] == 0xB5 && buf[1 +startn] == 0x62 && buf[2 + startn] == 0x01 && buf[3 + startn] == 0x07)
{
for(int j=0;j<100;j++)//bufからpvtbへ格納
{
pvtb[j] =buf[j + startn];
}
string ubx0 = PVTcnv(pvtb, ref pvtd);
string[] ubxdata = ubx0.Split(',');
Console.WriteLine(ubx0);
sw.WriteLine(ubx0);
//csv all lines でcsv書き込み
// File.AppendAllLines(@fc, ubxdata);
}//B5620107 end
bbbb
//}
//Console.WriteLine();
}
sw.Close();
Console.WriteLine("File:" + f + "file size=" + fileSize.ToString() + "k=" + (k - 1).ToString());
fs.Dispose();
}
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()
{
//var files = Directory.GetFiles(@"C:\\RTK_LOG\", "*.bin");
//files.ToList().ForEach(name => Console.WriteLine(name));
//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("---キーを押してください 'r':READ Binfile(BinAscii)--- 'u':READ BINfile(UBX+BNO CSV)");
//ConsoleKeyInfo input = Console.ReadKey();
var inp = Console.ReadKey(true);//キーインしても表示しない
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.WriteLine("rcvBytes=" + rcvBytes.Length.ToString());
}
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=='m')
{
key = 'm';
}
int n = 0;
FileStream fs = new FileStream(fname, FileMode.CreateNew);
BinaryWriter w = new BinaryWriter(fs);
}
//-------------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 = (long)i5;
}
else
{
su = (long)b1 + (long)b2 * 256 + (long)b3 * 65536 + (long)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;
}
//=================================================================================
//+++++++++++++++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