private void mainloop() { threadrun = true; while (threadrun) { try { // limit to 110 byte packets byte[] buffer = new byte[110]; while (comPort.BytesToRead > 0) { int read = comPort.Read(buffer, 0, Math.Min(buffer.Length, comPort.BytesToRead)); MainV2.comPort.InjectGpsData(buffer, (byte)read); } System.Threading.Thread.Sleep(10); } catch (Exception ex) { log.Error(ex); } } }
public override int Read(byte[] buffer, int offset, int count) { var read = CommsSerial.Read(buffer, offset, count); Position += read; return(read); }
public byte[] __recv(int count = 1) { // this will auto timeout // Console.WriteLine("recv "+count); byte[] c = new byte[count]; int pos = 0; while (pos < count) { pos += port.Read(c, pos, count - pos); } return(c); }
public bool doConnect(ICommsSerial comPort) { try { Console.WriteLine("doConnect"); // setup a known enviroment comPort.Write("ATO\r\n"); // wait Sleep(1100, comPort); comPort.DiscardInBuffer(); // send config string comPort.Write("+++"); Sleep(1100, comPort); // check for config response "OK" log.Info("Connect btr " + comPort.BytesToRead + " baud " + comPort.BaudRate); // allow time for data/response byte[] buffer = new byte[20]; int len = comPort.Read(buffer, 0, buffer.Length); string conn = ASCIIEncoding.ASCII.GetString(buffer, 0, len); log.Info("Connect first response " + conn.Replace('\0', ' ') + " " + conn.Length); if (conn.Contains("OK")) { //return true; } else { // cleanup incase we are already in cmd mode comPort.Write("\r\n"); } doCommand(comPort, "AT&T"); string version = doCommand(comPort, "ATI"); log.Info("Connect Version: " + version.Trim() + "\n"); Regex regex = new Regex(@"SiK\s+(.*)\s+on\s+(.*)"); if (regex.IsMatch(version)) { return(true); } return(false); } catch { return(false); } }
public byte[] __recvbl(int count = 1) { // this will auto timeout // Console.WriteLine("recv "+count); byte[] c = new byte[count]; int pos = 0; while (pos < count) { pos += comPortosdbl.Read(c, pos, count - pos); } this.fileLog.WriteLine("reading " + count + " bytes: -" + string.Join(",", c) + "-"); this.fileLog.Flush(); return(c); }
public byte[] __recv(int count = 1) { // this will auto timeout // Console.WriteLine("recv "+count); byte[] c = new byte[count]; int pos = 0; while (pos < count) { pos += port.Read(c, pos, count - pos); } //foreach (var i in c) // Console.WriteLine("__recv " + i.ToString("X")); return(c); }
public bool doConnect(ICommsSerial comPort) { try { Console.WriteLine("doConnect"); // setup a known enviroment comPort.Write("ATO\r\n"); // wait Sleep(1100, comPort); comPort.DiscardInBuffer(); // send config string comPort.Write("+++"); Sleep(1100,comPort); // check for config response "OK" log.Info("Connect btr " + comPort.BytesToRead + " baud " + comPort.BaudRate); // allow time for data/response byte[] buffer = new byte[20]; int len = comPort.Read(buffer, 0, buffer.Length); string conn = ASCIIEncoding.ASCII.GetString(buffer, 0, len); log.Info("Connect first response " + conn.Replace('\0', ' ') + " " + conn.Length); if (conn.Contains("OK")) { //return true; } else { // cleanup incase we are already in cmd mode comPort.Write("\r\n"); } doCommand(comPort, "AT&T"); string version = doCommand(comPort, "ATI"); log.Info("Connect Version: " + version.Trim() + "\n"); Regex regex = new Regex(@"SiK\s+(.*)\s+on\s+(.*)"); if (regex.IsMatch(version)) { return true; } return false; } catch { return false; } }
private static void mainloop() { DateTime lastrecv = DateTime.MinValue; threadrun = true; bool isrtcm = false; bool issbp = false; int reconnecttimeout = 10; while (threadrun) { try { // reconnect logic - 10 seconds with no data, or comport is closed try { if ((DateTime.Now - lastrecv).TotalSeconds > reconnecttimeout || !comPort.IsOpen) { if (comPort is CommsNTRIP || comPort is UdpSerialConnect || comPort is UdpSerial) { } else { log.Warn("Reconnecting"); // close existing comPort.Close(); // reopen comPort.Open(); } // reset timer lastrecv = DateTime.Now; } } catch { log.Error("Failed to reconnect"); // sleep for 10 seconds on error System.Threading.Thread.Sleep(10000); } // limit to 110 byte packets byte[] buffer = new byte[110]; // limit to 180 byte packet if using new packet if (rtcm_msg) { buffer = new byte[180]; } while (comPort.BytesToRead > 0) { int read = comPort.Read(buffer, 0, Math.Min(buffer.Length, comPort.BytesToRead)); if (read > 0) { lastrecv = DateTime.Now; } bytes += read; bps += read; try { if (basedata != null) { basedata.Write(buffer, 0, read); } } catch { } // if this is raw data transport of unknown packet types if (!(isrtcm || issbp)) { sendData(buffer, (byte)read); } // check for valid rtcm/sbp/ubx packets for (int a = 0; a < read; a++) { int seenmsg = -1; // rtcm if ((seenmsg = rtcm3.Read(buffer[a])) > 0) { sbp.resetParser(); ubx_m8p.resetParser(); nmea.resetParser(); isrtcm = true; sendData(rtcm3.packet, (byte)rtcm3.length); bpsusefull += rtcm3.length; string msgname = "Rtcm" + seenmsg; if (!msgseen.ContainsKey(msgname)) { msgseen[msgname] = 0; } msgseen[msgname] = (int)msgseen[msgname] + 1; ExtractBasePos(seenmsg); seenRTCM(seenmsg); } // sbp if ((seenmsg = sbp.read(buffer[a])) > 0) { rtcm3.resetParser(); ubx_m8p.resetParser(); nmea.resetParser(); issbp = true; sendData(sbp.packet, (byte)sbp.length); bpsusefull += sbp.length; string msgname = "Sbp" + seenmsg.ToString("X4"); if (!msgseen.ContainsKey(msgname)) { msgseen[msgname] = 0; } msgseen[msgname] = (int)msgseen[msgname] + 1; } // ubx if ((seenmsg = ubx_m8p.Read(buffer[a])) > 0) { rtcm3.resetParser(); sbp.resetParser(); nmea.resetParser(); ProcessUBXMessage(); string msgname = "Ubx" + seenmsg.ToString("X4"); if (!msgseen.ContainsKey(msgname)) { msgseen[msgname] = 0; } msgseen[msgname] = (int)msgseen[msgname] + 1; } // nmea if ((seenmsg = nmea.Read(buffer[a])) > 0) { rtcm3.resetParser(); sbp.resetParser(); ubx_m8p.resetParser(); string msgname = "NMEA"; if (!msgseen.ContainsKey(msgname)) { msgseen[msgname] = 0; } msgseen[msgname] = (int)msgseen[msgname] + 1; } } } System.Threading.Thread.Sleep(10); } catch (Exception ex) { log.Error(ex); } } }
public MAVLinkMessage ReadPacket() { byte[] buffer = new byte[GlobalAppData.MAX_PACKET_LEN + 25]; int count = 0; int length = 0; int readcount = 0; MAVLinkMessage message = null; DateTime start = DateTime.Now; lock (readlock) { while ((BaseStream != null && BaseStream.IsOpen) || logreadmode) { try { if (readcount > 300) { break; } readcount++; if (logreadmode) { message = ReadLog(); buffer = message.buffer; if (buffer == null || buffer.Length == 0) { return(MAVLinkMessage.Invalid); } } else { if (BaseStream.ReadTimeout != 1200) { BaseStream.ReadTimeout = 1200; // 1200 ms between chars - the gps detection requires this. } DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout); while (BaseStream.IsOpen && BaseStream.BytesToRead <= 0) { if (DateTime.Now > to) { throw new TimeoutException("Timeout"); } Task.Delay(1); } if (BaseStream.IsOpen) { BaseStream.Read(buffer, count, 1); if (rawlogfile != null && rawlogfile.CanWrite) { rawlogfile.WriteByte(buffer[count]); } } } } catch (Exception ex) { break; } // check if looks like a mavlink packet and check for exclusions and write to console if (buffer[0] != 0xfe && buffer[0] != 'U' && buffer[0] != 0xfd) { if (buffer[0] >= 0x20 && buffer[0] <= 127 || buffer[0] == '\n' || buffer[0] == '\r') { // check for line termination if (buffer[0] == '\r' || buffer[0] == '\n') { // check new line is valid if (buildplaintxtline.Length > 3) { plaintxtline = buildplaintxtline; } // reset for next line buildplaintxtline = ""; } TCPConsole.Write(buffer[0]); buildplaintxtline += (char)buffer[0]; } //_bytesReceivedSubj.OnNext(1); count = 0; buffer[1] = 0; continue; } // reset count on valid packet readcount = 0; // check for a header if (buffer[0] == 0xfe || buffer[0] == 0xfd || buffer[0] == 'U') { var mavlinkv2 = buffer[0] == GlobalAppData.MAVLINK_STX ? true : false; int headerlength = mavlinkv2 ? GlobalAppData.MAVLINK_CORE_HEADER_LEN : GlobalAppData.MAVLINK_CORE_HEADER_MAVLINK1_LEN; int headerlengthstx = headerlength + 1; // if we have the header, and no other chars, get the length and packet identifiers if (count == 0 && !logreadmode) { DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout); while (BaseStream.IsOpen && BaseStream.BytesToRead < headerlength) { if (DateTime.Now > to) { throw new TimeoutException("Timeout"); } Task.Delay(1); } int read = BaseStream.Read(buffer, 1, headerlength); count = read; if (rawlogfile != null && rawlogfile.CanWrite) { rawlogfile.Write(buffer, 1, read); } } // packet length if (buffer[0] == GlobalAppData.MAVLINK_STX) { length = buffer[1] + headerlengthstx + GlobalAppData.MAVLINK_NUM_CHECKSUM_BYTES; // data + header + checksum - magic - length if ((buffer[2] & GlobalAppData.MAVLINK_IFLAG_SIGNED) > 0) { length += GlobalAppData.MAVLINK_SIGNATURE_BLOCK_LEN; } } else { length = buffer[1] + headerlengthstx + GlobalAppData.MAVLINK_NUM_CHECKSUM_BYTES; // data + header + checksum - U - length } if (count >= headerlength || logreadmode) { try { if (logreadmode) { } else { DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout); while (BaseStream.IsOpen && BaseStream.BytesToRead < (length - (headerlengthstx))) { if (DateTime.Now > to) { break; } Task.Delay(1); } if (BaseStream.IsOpen) { int read = BaseStream.Read(buffer, headerlengthstx, length - (headerlengthstx)); if (rawlogfile != null && rawlogfile.CanWrite) { // write only what we read, temp is the whole packet, so 6-end rawlogfile.Write(buffer, headerlengthstx, read); } } } count = length; } catch { break; } break; } } count++; if (count == 299) { break; } } } // end readlock // resize the packet to the correct length Array.Resize <byte>(ref buffer, count); // add byte count //_bytesReceivedSubj.OnNext(buffer.Length); // update bps statistics if (_bpstime.Second != DateTime.Now.Second) { long btr = 0; if (BaseStream != null && BaseStream.IsOpen) { btr = BaseStream.BytesToRead; } else if (logreadmode) { btr = logplaybackfile.BaseStream.Length - logplaybackfile.BaseStream.Position; } _bps2 = _bps1; // prev sec _bps1 = 0; // current sec _bpstime = DateTime.Now; _mavlink1count = 0; _mavlink2count = 0; _mavlink2signed = 0; } _bps1 += buffer.Length; if (buffer.Length == 0) { return(MAVLinkMessage.Invalid); } if (message == null) { message = new MAVLinkMessage(buffer, DateTime.UtcNow); } uint msgid = message.msgid; GlobalAppData.message_info msginfo = GlobalAppData.GetMessageInfo(msgid); // calc crc var sigsize = (message.sig != null) ? GlobalAppData.MAVLINK_SIGNATURE_BLOCK_LEN : 0; ushort crc = MavlinkCRC.crc_calculate(buffer, message.Length - sigsize - GlobalAppData.MAVLINK_NUM_CHECKSUM_BYTES); // calc extra bit of crc for mavlink 1.0/2.0 if (message.header == 0xfe || message.header == 0xfd) { crc = MavlinkCRC.crc_accumulate(msginfo.crc, crc); } // check message length size vs table (mavlink1 explicit size check | mavlink2 allow all, undersize 0 trimmed, and oversize unknown extension) if (!message.ismavlink2 && message.payloadlength != msginfo.minlength) { if (msginfo.length == 0) // pass for unknown packets { } else { return(MAVLinkMessage.Invalid); } } // check crc if ((message.crc16 >> 8) != (crc >> 8) || (message.crc16 & 0xff) != (crc & 0xff)) { return(MAVLinkMessage.Invalid); } byte sysid = message.sysid; byte compid = message.compid; byte packetSeqNo = message.seq; // stat count if (message.buffer[0] == GlobalAppData.MAVLINK_STX) { _mavlink2count++; } else if (message.buffer[0] == GlobalAppData.MAVLINK_STX_MAVLINK1) { _mavlink1count++; } // if its a gcs packet - dont process further if (buffer.Length >= 5 && (sysid == 255 || sysid == 253) && logreadmode) // gcs packet { return(message); } return(message); }
private static void mainloop() { DateTime lastrecv = DateTime.Now; threadrun = true; bool isrtcm = false; bool issbp = false; bool iscan = false; // feed the rtcm data into the rtcm parser if we get a can message can.MessageReceived += (frame, msg, id) => { string msgname = "Can" + frame.MsgTypeID; if (!msgseen.ContainsKey(msgname)) { msgseen[msgname] = 0; } msgseen[msgname] = (int)msgseen[msgname] + 1; if (frame.MsgTypeID == (ushort)uavcan.UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_DT_ID) { var rtcm = (uavcan.uavcan_equipment_gnss_RTCMStream)msg; for (int a = 0; a < rtcm.data_len; a++) { int seenmsg = -1; if ((seenmsg = rtcm3.Read(rtcm.data[a])) > 0) { sbp.resetParser(); ubx_m8p.resetParser(); nmea.resetParser(); iscan = true; sendData(rtcm3.packet, (ushort)rtcm3.length); bpsusefull += rtcm3.length; msgname = "Rtcm" + seenmsg; if (!msgseen.ContainsKey(msgname)) { msgseen[msgname] = 0; } msgseen[msgname] = (int)msgseen[msgname] + 1; ExtractBasePos(seenmsg); seenRTCM(seenmsg); } } } }; int reconnecttimeout = 10; while (threadrun) { try { // reconnect logic - 10 seconds with no data, or comport is closed try { if ((DateTime.Now - lastrecv).TotalSeconds > reconnecttimeout || !comPort.IsOpen) { if (comPort is CommsNTRIP || comPort is UdpSerialConnect || comPort is UdpSerial) { } else { log.Warn("Reconnecting"); // close existing comPort.Close(); // reopen comPort.Open(); } // reset timer lastrecv = DateTime.Now; } } catch { log.Error("Failed to reconnect"); // sleep for 10 seconds on error System.Threading.Thread.Sleep(10000); } // limit to 110 byte packets byte[] buffer = new byte[110]; // limit to 180 byte packet if using new packet if (rtcm_msg) { buffer = new byte[180]; } while (comPort.BytesToRead > 0) { int read = comPort.Read(buffer, 0, Math.Min(buffer.Length, comPort.BytesToRead)); if (read > 0) { lastrecv = DateTime.Now; } bytes += read; bps += read; try { if (basedata != null) { basedata.Write(buffer, 0, read); } } catch { } // if this is raw data transport of unknown packet types if (!(isrtcm || issbp || iscan)) { sendData(buffer, (ushort)read); } // check for valid rtcm/sbp/ubx packets for (int a = 0; a < read; a++) { int seenmsg = -1; // rtcm and not can if (!iscan && (seenmsg = rtcm3.Read(buffer[a])) > 0) { sbp.resetParser(); ubx_m8p.resetParser(); nmea.resetParser(); isrtcm = true; sendData(rtcm3.packet, (ushort)rtcm3.length); bpsusefull += rtcm3.length; string msgname = "Rtcm" + seenmsg; if (!msgseen.ContainsKey(msgname)) { msgseen[msgname] = 0; } msgseen[msgname] = (int)msgseen[msgname] + 1; ExtractBasePos(seenmsg); seenRTCM(seenmsg); } // sbp if ((seenmsg = sbp.read(buffer[a])) > 0) { rtcm3.resetParser(); ubx_m8p.resetParser(); nmea.resetParser(); issbp = true; sendData(sbp.packet, (ushort)sbp.length); bpsusefull += sbp.length; string msgname = "Sbp" + seenmsg.ToString("X4"); if (!msgseen.ContainsKey(msgname)) { msgseen[msgname] = 0; } msgseen[msgname] = (int)msgseen[msgname] + 1; } // ubx if ((seenmsg = ubx_m8p.Read(buffer[a])) > 0) { rtcm3.resetParser(); sbp.resetParser(); nmea.resetParser(); ProcessUBXMessage(); string msgname = "Ubx" + seenmsg.ToString("X4"); if (!msgseen.ContainsKey(msgname)) { msgseen[msgname] = 0; } msgseen[msgname] = (int)msgseen[msgname] + 1; } // nmea if ((seenmsg = nmea.Read(buffer[a])) > 0) { rtcm3.resetParser(); sbp.resetParser(); ubx_m8p.resetParser(); string msgname = "NMEA"; if (!msgseen.ContainsKey(msgname)) { msgseen[msgname] = 0; } msgseen[msgname] = (int)msgseen[msgname] + 1; } // can_rtcm if ((seenmsg = can.Read(buffer[a])) > 0) { sbp.resetParser(); ubx_m8p.resetParser(); nmea.resetParser(); string msgname = "CAN"; if (!msgseen.ContainsKey(msgname)) { msgseen[msgname] = 0; } msgseen[msgname] = (int)msgseen[msgname] + 1; } } } System.Threading.Thread.Sleep(10); } catch (Exception ex) { log.Error(ex); } } }
private void mainloop() { DateTime lastrecv = DateTime.MinValue; threadrun = true; bool isrtcm = false; bool issbp = false; while (threadrun) { try { // reconnect logic - 10 seconds with no data, or comport is closed try { if ((DateTime.Now - lastrecv).TotalSeconds > 10 || !comPort.IsOpen) { log.Info("Reconnecting"); // close existing comPort.Close(); // reopen comPort.Open(); // reset timer lastrecv = DateTime.Now; } } catch { log.Error("Failed to reconnect"); // sleep for 10 seconds on error System.Threading.Thread.Sleep(10000); } // limit to 110 byte packets byte[] buffer = new byte[110]; while (comPort.BytesToRead > 0) { int read = comPort.Read(buffer, 0, Math.Min(buffer.Length, comPort.BytesToRead)); if (read > 0) { lastrecv = DateTime.Now; } bytes += read; bps += read; if (!(isrtcm || issbp)) { sendData(buffer, (byte)read); } // check for valid rtcm packets for (int a = 0; a < read; a++) { int seen = -1; // rtcm if ((seen = rtcm3.Read(buffer[a])) > 0) { isrtcm = true; sendData(rtcm3.packet, (byte)rtcm3.length); if (!msgseen.ContainsKey(seen)) { msgseen[seen] = 0; } msgseen[seen] = (int)msgseen[seen] + 1; } // sbp if ((seen = sbp.read(buffer[a])) > 0) { issbp = true; sendData(sbp.packet, (byte)sbp.length); if (!msgseen.ContainsKey(seen)) { msgseen[seen] = 0; } msgseen[seen] = (int)msgseen[seen] + 1; } } } System.Threading.Thread.Sleep(10); } catch (Exception ex) { log.Error(ex); } } }
private void mainloop() { DateTime lastrecv = DateTime.MinValue; threadrun = true; bool isrtcm = false; bool issbp = false; while (threadrun) { try { // reconnect logic - 10 seconds with no data, or comport is closed try { if ((DateTime.Now - lastrecv).TotalSeconds > 10 || !comPort.IsOpen) { this.LogInfo("Reconnecting"); // close existing comPort.Close(); // reopen comPort.Open(); // reset timer lastrecv = DateTime.Now; } } catch { this.LogError("Failed to reconnect"); // sleep for 10 seconds on error System.Threading.Thread.Sleep(10000); } // limit to 110 byte packets byte[] buffer = new byte[110]; // limit to 180 byte packet if using new packet if (rtcm_msg) { buffer = new byte[180]; } while (comPort.BytesToRead > 0) { int read = comPort.Read(buffer, 0, Math.Min(buffer.Length, comPort.BytesToRead)); if (read > 0) { lastrecv = DateTime.Now; } bytes += read; bps += read; try { if (basedata != null) { basedata.Write(buffer, 0, read); } } catch { } if (!(isrtcm || issbp)) { sendData(buffer, (byte)read); } // check for valid rtcm packets for (int a = 0; a < read; a++) { int seenmsg = -1; // rtcm if ((seenmsg = rtcm3.Read(buffer[a])) > 0) { isrtcm = true; sendData(rtcm3.packet, (byte)rtcm3.length); string msgname = "Rtcm" + seenmsg; if (!msgseen.ContainsKey(msgname)) { msgseen[msgname] = 0; } msgseen[msgname] = (int)msgseen[msgname] + 1; ExtractBasePos(seenmsg); } // sbp if ((seenmsg = sbp.read(buffer[a])) > 0) { issbp = true; sendData(sbp.packet, (byte)sbp.length); string msgname = "Sbp" + seenmsg.ToString("X4"); if (!msgseen.ContainsKey(msgname)) { msgseen[msgname] = 0; } msgseen[msgname] = (int)msgseen[msgname] + 1; } // ubx if ((seenmsg = ubx_m8p.Read(buffer[a])) > 0) { ProcessUBXMessage(); string msgname = "Ubx" + seenmsg.ToString("X4"); if (!msgseen.ContainsKey(msgname)) { msgseen[msgname] = 0; } msgseen[msgname] = (int)msgseen[msgname] + 1; } } } System.Threading.Thread.Sleep(10); } catch (Exception ex) { this.LogError(ex); } } }
public bool doConnect(ICommsSerial comPort) { char[] ends = { '\r', '\n' }; try { comLog("Connecting", 2); int trys = 1; // setup a known enviroment comPort.Write("ATO\r\n"); retry: // wait Sleep(1500, comPort); comPort.DiscardInBuffer(); // send config string comPort.Write("+++"); Sleep(1500, comPort); // check for config response "OK" comLog("Connect btr " + comPort.BytesToRead + " baud " + comPort.BaudRate, 2); // allow time for data/response if (comPort.BytesToRead == 0 && trys <= 3) { trys++; comLog("doConnect retry", 2); goto retry; } byte[] buffer = new byte[20]; int len = comPort.Read(buffer, 0, buffer.Length); string conn = ASCIIEncoding.ASCII.GetString(buffer, 0, len); comLog("Connect first response " + conn.TrimEnd(ends) + " " + conn.Length); if (conn.Contains("OK")) { //return true; } else { // cleanup incase we are already in cmd mode comPort.Write("\r\n"); } doCommand(comPort, "AT&T"); string version = doCommand(comPort, "ATI"); Regex regex = new Regex(@"SiK\s+(.*)\s+on\s+(.*)"); if (regex.IsMatch(version)) { comLog("Connected: " + version.TrimEnd(ends), 2); return(true); } comLog("Connect Version: " + version.TrimEnd(ends)); return(false); } catch { return(false); } }