public poll_msg ( ICommsSerial port, byte clas, byte subclass ) : void | ||
port | ICommsSerial | |
clas | byte | |
subclass | byte | |
return | void |
private static void ProcessUBXMessage() { try { // survey in if (ubx_m8p.@class == 0x1 && ubx_m8p.subclass == 0x3b) { var svin = ubx_m8p.packet.ByteArrayToStructure <Utilities.ubx_m8p.ubx_nav_svin>(6); ubxsvin = svin; updateSVINLabel((svin.valid == 1), (svin.active == 1), svin.dur, svin.obs, svin.meanAcc / 10000.0); var pos = svin.getECEF(); double[] baseposllh = new double[3]; Utilities.rtcm3.ecef2pos(pos, ref baseposllh); if (svin.valid == 1) { //MainV2.comPort.MAV.cs.MovingBase = new Utilities.PointLatLngAlt(baseposllh[0]*Utilities.rtcm3.R2D, //baseposllh[1]*Utilities.rtcm3.R2D, baseposllh[2]); } //if (svin.valid == 1) //ubx_m8p.turnon_off(comPort, 0x1, 0x3b, 0); } else if (ubx_m8p.@class == 0x1 && ubx_m8p.subclass == 0x7) { var pvt = ubx_m8p.packet.ByteArrayToStructure <Utilities.ubx_m8p.ubx_nav_pvt>(6); //MainV2.comPort.MAV.cs.MovingBase = new Utilities.PointLatLngAlt(pvt.lat / 1e7, pvt.lon / 1e7, pvt.height / 1000.0); } else if (ubx_m8p.@class == 0x5 && ubx_m8p.subclass == 0x1) { log.InfoFormat("ubx ack {0} {1}", ubx_m8p.packet[6], ubx_m8p.packet[7]); } else if (ubx_m8p.@class == 0x5 && ubx_m8p.subclass == 0x0) { log.InfoFormat("ubx Nack {0} {1}", ubx_m8p.packet[6], ubx_m8p.packet[7]); } else if (ubx_m8p.@class == 0xa && ubx_m8p.subclass == 0x4) { var ver = ubx_m8p.packet.ByteArrayToStructure <Utilities.ubx_m8p.ubx_mon_ver>(6);//, ubx_m8p.length - 8); Console.WriteLine("ubx mon-ver {0} {1}", ASCIIEncoding.ASCII.GetString(ver.hwVersion), ASCIIEncoding.ASCII.GetString(ver.swVersion)); for (int a = 40 + 6; a < ubx_m8p.length - 2; a += 30) { var extension = ASCIIEncoding.ASCII.GetString(ubx_m8p.buffer, a, 30); Console.WriteLine("ubx mon-ver {0}", extension); } } else if (ubx_m8p.@class == 0xa && ubx_m8p.subclass == 0x9) { var hw = ubx_m8p.packet.ByteArrayToStructure <Utilities.ubx_m8p.ubx_mon_hw>(6); Console.WriteLine("ubx mon-hw noise {0} agc% {1} jam% {2} jamstate {3}", hw.noisePerMS, (hw.agcCnt / 8191.0) * 100.0, (hw.jamInd / 256.0) * 100, hw.flags & 0xc); } else if (ubx_m8p.@class == 0xf5) { // rtcm } else if (ubx_m8p.@class == 0x02) { // rxm-raw } else if (ubx_m8p.@class == 0x06 && ubx_m8p.subclass == 0x71) { // TMODE3 var tmode = ubx_m8p.packet.ByteArrayToStructure <Utilities.ubx_m8p.ubx_cfg_tmode3>(6); ubxmode = tmode; log.InfoFormat("ubx TMODE3 {0} {1}", (ubx_m8p.ubx_cfg_tmode3.modeflags)tmode.flags, ""); } else { ubx_m8p.turnon_off(comPort, ubx_m8p.@class, ubx_m8p.subclass, 0); } if (pollTMODE < DateTime.Now) { ubx_m8p.poll_msg(comPort, 0x06, 0x71); pollTMODE = DateTime.Now.AddSeconds(60); ubx_m8p.poll_msg(comPort, 0x0a, 0x4); } } catch (Exception ex) { log.Error(ex); } }
private static void ProcessUBXMessage() { try { // survey in if (ubx_m8p.@class == 0x1 && ubx_m8p.subclass == 0x3b) { var svin = ubx_m8p.packet.ByteArrayToStructure <Utilities.ubx_m8p.ubx_nav_svin>(6); ubxsvin = svin; updateSVINLabel((svin.valid == 1), (svin.active == 1), svin.dur, svin.obs, svin.meanAcc / 10000.0); var pos = svin.getECEF(); double[] baseposllh = new double[3]; Utilities.rtcm3.ecef2pos(pos, ref baseposllh); //MainV2.comPort.MAV.cs.MovingBase = new Utilities.PointLatLngAlt(baseposllh[0]*Utilities.rtcm3.R2D, //baseposllh[1]*Utilities.rtcm3.R2D, baseposllh[2]); //if (svin.valid == 1) //ubx_m8p.turnon_off(comPort, 0x1, 0x3b, 0); } else if (ubx_m8p.@class == 0x1 && ubx_m8p.subclass == 0x7) { var pvt = ubx_m8p.packet.ByteArrayToStructure <Utilities.ubx_m8p.ubx_nav_pvt>(6); //MainV2.comPort.MAV.cs.MovingBase = new Utilities.PointLatLngAlt(pvt.lat / 1e7, pvt.lon / 1e7, pvt.height / 1000.0); } else if (ubx_m8p.@class == 0x5 && ubx_m8p.subclass == 0x1) { log.InfoFormat("ubx ack {0} {1}", ubx_m8p.packet[6], ubx_m8p.packet[7]); } else if (ubx_m8p.@class == 0x5 && ubx_m8p.subclass == 0x0) { log.InfoFormat("ubx Nack {0} {1}", ubx_m8p.packet[6], ubx_m8p.packet[7]); } else if (ubx_m8p.@class == 0xa && ubx_m8p.subclass == 0x4) { log.InfoFormat("ubx mon-ver {0} {1}", ubx_m8p.packet[6], ubx_m8p.packet[7]); } else if (ubx_m8p.@class == 0xf5) { // rtcm } else if (ubx_m8p.@class == 0x02) { // rxm-raw } else if (ubx_m8p.@class == 0x06 && ubx_m8p.subclass == 0x71) { // TMODE3 var tmode = ubx_m8p.packet.ByteArrayToStructure <Utilities.ubx_m8p.ubx_cfg_tmode3>(6); ubxmode = tmode; log.InfoFormat("ubx TMODE3 {0} {1}", (ubx_m8p.ubx_cfg_tmode3.modeflags)tmode.flags, ""); } else { ubx_m8p.turnon_off(comPort, ubx_m8p.@class, ubx_m8p.subclass, 0); } if (pollTMODE < DateTime.Now) { ubx_m8p.poll_msg(comPort, 0x06, 0x71); pollTMODE = DateTime.Now.AddSeconds(60); } } catch (Exception ex) { log.Error(ex); } }