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 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); } } }
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); } } }