C# Console UDP Reciver Test

UDP_F9P_Console_00.cs

using System;
using static System.Net.Mime.MediaTypeNames;
using System.IO;
using System.Net;
using System.Reflection.PortableExecutable;

public class UdpReceiver
{
    //%%%%%%%%%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
      //%%%%%%%%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
      //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;//
        /*
        public bno(float[] eX, float[] eY, float[] eZ, float[] qW, float[] qX, float[] qY, float[] qZ, float[] laX, float[] laY, float[] laZ, float[] grX, float[] grY, float[] grZ, int[] tst)
        {
            this.eX = eX;
            this.eY = eY;
            this.eZ = eZ;
            this.qW = qW;
            this.qX = qX;
            this.qY = qY;
            this.qZ = qZ;
            this.laX = laX;
            this.laY = laY;
            this.laZ = laZ;
            this.grX = grX;
            this.grY = grY;
            this.grZ = grZ;
            this.tst = tst;
        }
        */

    }
    static string espip;
    static byte[] rcvBytes;
    static string fname;
    static int chcksum,missn;
    static void Main()
    {
        //バインドするローカルIPとポート番号
        string localIpString = "192.168.0.109";
        //
        // 自分のホスト名を取得する
        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++;
        }
        Console.WriteLine(myip+"--To Start Hit any key--");
        DateTime dt = DateTime.Now;
        string nowtime = dt.ToString("dd-hh-mm-ss");
        //Console.WriteLine(nowtime);
        fname = "C:\\RTK_LOG\\" + nowtime + ".bin";
        
        
        //UDP Setting
        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);
        //WAIT KEY INPUT
        if (System.Diagnostics.Debugger.IsAttached)
        {
            Console.WriteLine("LOG 開始するには何かキーを押してください . . .");
            Console.ReadKey();
        }
        int n = 0;
        FileStream fs = new FileStream(fname, FileMode.CreateNew);
        BinaryWriter w = new BinaryWriter(fs);
        FileStream fstx = new FileStream(fname + 't', FileMode.CreateNew);
        StreamWriter sw = new StreamWriter(fstx);

        missn = 0;
        for (; ; )
        {

                //データを受信する
                System.Net.IPEndPoint remoteEP = null;
                rcvBytes = udp.Receive(ref remoteEP);
            //for (int i = 0; i < rcvBytes.Length; i++)
            
            　 chcksum = 0;
                for (int i = 0; i < 172; i++)
                {
                if (rcvBytes[i] != i)
                {
                    chcksum++;
                    missn++;
                }

            }
            DateTime now = DateTime.Now;
            int msect = (int)now.Minute * 60000 + (int)now.Second * 1000 + (int)now.Millisecond;
            //Console.Write(msect);
            string result = chcksum.ToString() + "," + missn.ToString() + "," + msect.ToString();
            sw.WriteLine(result);
            Console.WriteLine(result);
            
                
                for (int i = 0; i < 172; i++)
                {
                       w.Write(rcvBytes[i]);
                }
                

                
                n++;
               
                
        }
        udp.Close();
        Console.WriteLine("終了しました。");
          Console.ReadLine();
    
    }
    //+++PVTcnv++++++++++++++++++++++++++++++++++++++++++++++++++++++
    int PVTcnv(byte[] d, ref pvt p)
    {
        //PVT header[0-6]
        //0:itow[6-9]
        p.itow = (uint)Toint32(d[9], d[8], d[7], d[6]);
        //Serial.printf("PVTcnv:itow=%d\n\r",pvt[0]);
        //1:year[10-12]
        p.year = (ushort)(d[10] + d[11] * 256);
        //2:month[12]
        p.month = d[12];
        //3:day[13]
        p.day = d[13];
        //4:hour[14]
        p.hour = d[14];
        //Serial.printf("PVTcnv:hour=%d\n\r",pvt[4]);
        //5:min[15]
        p.min = d[15];
        //Serial.printf("PVTcnv:min=%d\n\r",pvt[5]);
        //6:sec[16]
        p.sec = d[16];
        //Serial.printf("PVTcnv:sec=%d\n\r",pvt[6]);
        //7:valid[17]
        p.valid = d[17];
        //8:tAcc[18-21]
        p.tacc = (uint)Toint32(d[21], d[20], d[19], d[18]);
        //9:nano[22-25]
        p.nano = Toint32(d[25], d[24], d[23], d[22]);
        //10:fixType[26]
        p.fixtype = d[26];
        //11:flags[27] This is Fix status 131
        p.flags = d[27];
        //12:flags2[28]
        p.flags2 = d[28];
        //13:numSV[29]
        p.numsv = d[29];
        //14:lon[30-33]
        p.lon = Toint32(d[33], d[32], d[31], d[30]);
        //15:lat[34-37]
        p.lat = Toint32(d[37], d[36], d[35], d[34]);
        //16:height[38-41]
        p.height = Toint32(d[41], d[40], d[39], d[38]);
        //17:hMSL[42-45]
        p.hmsl = Toint32(d[45], d[44], d[43], d[42]);
        //18:hAcc[46-49]
        p.hacc = (uint)Toint32(d[49], d[48], d[47], d[46]);
        if (p.hacc > 1000)
        {
            p.hacc = 0;
        }
        //19:vAcc[50-53]
        p.vacc = (uint)Toint32(d[53], d[52], d[51], d[50]);
        //20:velN[54-57]
        p.veln = Toint32(d[57], d[56], d[55], d[54]);
        //21:velE[58-61]
        p.vele = Toint32(d[61], d[60], d[59], d[58]);
        //22:velD[62-65]
        p.veld = Toint32(d[65], d[64], d[63], d[62]);
        //23:gSpeed[66-69]
        p.gspeed = Toint32(d[69], d[68], d[67], d[66]);
        //24:headMot[70-73]
        p.headmot = Toint32(d[73], d[72], d[71], d[70]);

        //25:sAcc[74-77]
        p.sacc = (uint)Toint32(d[77], d[76], d[75], d[74]);
        //26:headAcc[78-81]
        p.headacc = (uint)Toint32(d[81], d[80], d[79], d[78]);
        //27:pDOP[82-83]
        p.pdop = (ushort)(d[82] + d[83] * 256);
        //28:flags3[84]
        p.flags3 = d[84];
        //29:reserved1[85]
        p.resrv0 = d[85];
        //30:headVeh[86-89]
        p.headveh = Toint32(d[89], d[88], d[87], d[86]);
        //31:magDec[90-91]
        p.magdec = (short)(d[90] + d[91] * 256);
        //32:magAcc[92-93]
        p.magacc = (ushort)(d[92] + d[93] * 256);

        return (int)p.itow;
    }//PVTcnv() end
     //--RELPOScnv++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    int RELPOScnv(byte[] d, ref relp r)
    {
        //RELPOS header[0-5]

        //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];
        //1:reserved1[7]
        r.reserved1 = d[s + 7];
        //2:refStationId[8-9]
        r.refstationid = d[s + 8];
        //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]);
        //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]);
        //5:relposE[18-21]
        r.relpose = Toint32(d[s + 21], d[s + 20], d[s + 19], d[s + 18]);
        //6:relposD[22-25]
        r.relposd = Toint32(d[s + 25], d[s + 24], d[s + 23], d[s + 22]);
        //7:relposLength[26-29]
        r.relposlength = Toint32(d[s + 29], d[s + 28], d[s + 27], d[s + 26]);
        //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]);
        //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]);
        //10:relposHPN[35]
        r.relposhpn = (sbyte)((d[s + 38] & 127) - (d[s + 38] & 128));
        //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));
        //12:relposHPD[37]
        r.relposhpd = (sbyte)((d[s + 40] & 127) - (d[s + 40] & 128));
        //13:relposHPLength[38]
        r.relposhplength = (sbyte)((d[s + 41] & 127) - (d[s + 41] & 128));
        //14:accN[38-41]
        r.accn = (uint)Toint32(d[s + 41], d[s + 40], d[s + 39], d[s + 38]);
        //15:accE[42-45]
        r.acce = (uint)Toint32(d[s + 45], d[s + 44], d[s + 43], d[s + 42]);
        //16:accD[46-49]
        r.accd = (uint)Toint32(d[s + 49], d[s + 48], d[s + 47], d[s + 46]);
        //17:accLength[50-53]
        r.acclength = (uint)Toint32(d[s + 53], d[s + 52], d[s + 51], d[s + 50]);
        //18:accHeading[54-57]
        r.accheading = (uint)Toint32(d[s + 57], d[s + 56], d[s + 55], d[s + 54]);
        //19:reserved[57-60]
        r.reserved3 = (uint)Toint32(d[s + 61], d[s + 60], d[s + 59], d[s + 58]);
        //20:flags[60-63]
        r.flags = (uint)Toint32(d[s + 65], d[s + 64], d[s + 63], d[s + 62]);

        return (int)r.itow;
    }


    //+++++++++++++4byte Binary to Long ++++++++++++++++++++++++++++++++++++++++++++++
    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
    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++++++++++++++++++++
    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 ==================================================
    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=====================================================
}