MRPT  2.0.4
CGPSInterface.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
14 #include <mrpt/system/filesystem.h>
15 #include <mrpt/system/os.h>
16 
17 #include <iostream>
18 #include <list>
19 #include <mutex>
20 #include <thread>
21 
22 using namespace mrpt::hwdrivers;
23 using namespace mrpt::obs;
24 using namespace mrpt::system;
25 using namespace mrpt::comms;
26 using namespace std;
27 
29 
31 {
32  std::list<CGPSInterface::ptr_parser_t> all_parsers;
33 
34  static const TParsersRegistry& getInstance()
35  {
36  static TParsersRegistry reg;
37  return reg;
38  }
39 
40  private:
42  {
43  all_parsers.push_back(&CGPSInterface::implement_parser_NMEA);
44  all_parsers.push_back(&CGPSInterface::implement_parser_NOVATEL_OEM6);
45  }
46 };
47 
48 /* -----------------------------------------------------
49  Constructor
50  ----------------------------------------------------- */
52  : mrpt::system::COutputLogger("CGPSInterface"),
53  m_customInit(),
54  m_rx_buffer(0x10000),
55  m_raw_dump_file_prefix(),
56  m_COMname(),
57  m_last_timestamp(INVALID_TIMESTAMP),
58  m_JAVAD_rtk_src_port(),
59  m_JAVAD_rtk_format("cmr")
60 {
61  m_sensorLabel = "GPS";
62 }
63 
64 /* -----------------------------------------------------
65  loadConfig_sensorSpecific
66  ----------------------------------------------------- */
68  const mrpt::config::CConfigFileBase& configSource,
69  const std::string& iniSection)
70 {
72  iniSection, "parser", m_parser, false /*Allow default values*/);
73  m_raw_dump_file_prefix = configSource.read_string(
74  iniSection, "raw_dump_file_prefix", m_raw_dump_file_prefix,
75  false /*Allow default values*/);
76 
77 #ifdef _WIN32
78  m_COMname =
79  configSource.read_string(iniSection, "COM_port_WIN", m_COMname, true);
80 #else
81  m_COMname =
82  configSource.read_string(iniSection, "COM_port_LIN", m_COMname, true);
83 #endif
84 
85  m_COMbauds =
86  configSource.read_int(iniSection, "baudRate", m_COMbauds, true);
88  iniSection, "sensor_label_append_msg_type", m_sensorLabelAppendMsgType);
89 
90  // legacy custom cmds:
91  m_customInit =
92  configSource.read_string(iniSection, "customInit", m_customInit, false);
93 
94  // new custom cmds:
95  m_custom_cmds_delay = configSource.read_float(
96  iniSection, "custom_cmds_delay", m_custom_cmds_delay);
97  m_custom_cmds_append_CRLF = configSource.read_bool(
98  iniSection, "custom_cmds_append_CRLF", m_custom_cmds_append_CRLF);
99  // Load as many strings as found on the way:
100  m_setup_cmds.clear();
101  for (int i = 1; true; i++)
102  {
103  std::string sLine = configSource.read_string(
104  iniSection, mrpt::format("setup_cmd%i", i), std::string());
105  sLine = mrpt::system::trim(sLine);
106  if (sLine.empty()) break;
107  m_setup_cmds.push_back(sLine);
108  }
109 
110  m_shutdown_cmds.clear();
111  for (int i = 1; true; i++)
112  {
113  std::string sLine = configSource.read_string(
114  iniSection, mrpt::format("shutdown_cmd%i", i), std::string());
115  sLine = mrpt::system::trim(sLine);
116  if (sLine.empty()) break;
117  m_shutdown_cmds.push_back(sLine);
118  }
119 
121  configSource.read_float(iniSection, "pose_x", 0),
122  configSource.read_float(iniSection, "pose_y", 0),
123  configSource.read_float(iniSection, "pose_z", 0),
124  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
125  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
126  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
127 
128  m_JAVAD_rtk_src_port = configSource.read_string(
129  iniSection, "JAVAD_rtk_src_port", m_JAVAD_rtk_src_port);
130  m_JAVAD_rtk_src_baud = configSource.read_int(
131  iniSection, "JAVAD_rtk_src_baud", m_JAVAD_rtk_src_baud);
132  m_JAVAD_rtk_format = configSource.read_string(
133  iniSection, "JAVAD_rtk_format", m_JAVAD_rtk_format);
134 
135  m_topcon_useAIMMode = configSource.read_bool(
136  iniSection, "JAVAD_useAIMMode", m_topcon_useAIMMode);
138  1.0 / configSource.read_double(
139  iniSection, "outputRate", m_topcon_data_period);
140 }
141 
143 
145 {
146  m_parser = parser;
147 }
150  const std::shared_ptr<mrpt::io::CStream>& external_stream,
151  const std::shared_ptr<std::mutex>& csOptionalExternalStream)
152 {
154  m_data_stream = external_stream;
155  m_data_stream_cs = csOptionalExternalStream ? csOptionalExternalStream
157 }
158 void CGPSInterface::setSetupCommandsDelay(const double delay_secs)
159 {
160  m_custom_cmds_delay = delay_secs;
161 }
163 {
164  return m_custom_cmds_delay;
165 }
166 void CGPSInterface::setSetupCommands(const std::vector<std::string>& cmds)
167 {
168  m_setup_cmds = cmds;
169 }
170 const std::vector<std::string>& CGPSInterface::getSetupCommands() const
171 {
172  return m_setup_cmds;
173 }
174 void CGPSInterface::setShutdownCommands(const std::vector<std::string>& cmds)
175 {
176  m_shutdown_cmds = cmds;
177 }
178 const std::vector<std::string>& CGPSInterface::getShutdownCommands() const
179 {
180  return m_shutdown_cmds;
181 }
183 {
184  m_custom_cmds_append_CRLF = enable;
185 }
187 {
189 }
190 
191 /* -----------------------------------------------------
192  setSerialPortName
193 ----------------------------------------------------- */
194 void CGPSInterface::setSerialPortName(const std::string& COM_port)
195 {
196  // Dont allow changing the serial port if:
199  "Cannot change serial port name: an external stream has been "
200  "already bound manually.");
201 
202  if (m_data_stream)
203  {
204  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
205  auto serial =
206  dynamic_cast<mrpt::comms::CSerialPort*>(m_data_stream.get());
207  if (serial && serial->isOpen())
209  "Cannot change serial port name when it is already open");
210  }
211 
212  // OK:
213  m_COMname = COM_port;
214 }
215 
216 /* -----------------------------------------------------
217  getSerialPortName
218 ----------------------------------------------------- */
219 std::string CGPSInterface::getSerialPortName() const { return m_COMname; }
220 /* -----------------------------------------------------
221  tryToOpenTheCOM
222 ----------------------------------------------------- */
224 {
225  // If this is the first use of the COM port, create it:
226  if (!m_data_stream)
227  {
228  m_data_stream = std::make_shared<mrpt::comms::CSerialPort>();
231  }
232 
233  auto serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
234  if (serial)
235  {
236  {
237  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
238  if (serial->isOpen()) return true; // Already open
239 
240  if (m_verbose)
241  cout << "[CGPSInterface] Opening " << m_COMname << " @ "
242  << m_COMbauds << endl;
243  }
244  try
245  {
246  serial->open(m_COMname);
247  // Config:
248  serial->setConfig(m_COMbauds, 0, 8, 1);
249  serial->setTimeouts(1, 0, 1, 1, 1);
250 
251  // Do extra initialization?
253  {
254  serial->close();
255  return false;
256  }
257  return true; // All OK
258  }
259  catch (const std::exception& e)
260  {
261  std::cerr << "[CGPSInterface::tryToOpenTheCOM] Error opening or "
262  "configuring serial port:"
263  << std::endl
264  << e.what();
265  serial->close();
266  return false;
267  }
268  } // end of this is a serial port
269 
270  return true; // All OK
271 }
272 
273 /* -----------------------------------------------------
274  isGPS_connected
275 ----------------------------------------------------- */
277 /* -----------------------------------------------------
278  doProcess
279 ----------------------------------------------------- */
281 {
282  // Is the COM open?
283  if (!tryToOpenTheCOM())
284  {
285  m_state = ssError;
286  THROW_EXCEPTION("Could not open the input stream");
287  }
288  ASSERT_(m_data_stream != nullptr);
289  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
290  auto* stream_tcpip = dynamic_cast<CClientTCPSocket*>(m_data_stream.get());
291 
292  // Read as many bytes as available:
293  uint8_t buf[0x1000];
294  const size_t to_read =
295  std::min(m_rx_buffer.available() - 1, sizeof(buf) - 1);
296  try
297  {
298  size_t nRead = 0;
299  if (to_read > 0)
300  {
301  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
302  if (stream_tcpip)
303  {
304  nRead = stream_tcpip->readAsync(buf, to_read, 100, 10);
305  }
306  else if (stream_serial)
307  {
308  nRead = stream_serial->Read(buf, to_read);
309  }
310  else
311  {
312  nRead = m_data_stream->Read(buf, to_read);
313  }
314  }
315 
316  if (nRead) m_rx_buffer.push_many(buf, nRead);
317 
318  // Also dump to raw file:
319  if (!m_raw_dump_file_prefix.empty() &&
321  {
322  // 1st time open:
324  mrpt::system::timestampToParts(now(), parts, true);
325  string sFilePostfix = "_";
326  sFilePostfix += format(
327  "%04u-%02u-%02u_%02uh%02um%02us", (unsigned int)parts.year,
328  (unsigned int)parts.month, (unsigned int)parts.day,
329  (unsigned int)parts.hour, (unsigned int)parts.minute,
330  (unsigned int)parts.second);
331  const string sFileName =
334  string(".gps");
335 
336  if (m_verbose)
337  std::cout << "[CGPSInterface] Creating RAW dump file: `"
338  << sFileName << "`\n";
339  m_raw_output_file.open(sFileName);
340  }
341  if (nRead && m_raw_output_file.fileOpenCorrectly())
342  {
343  m_raw_output_file.Write(buf, nRead);
344  }
345  }
346  catch (std::exception&)
347  {
348  // ERROR:
350  "[CGPSInterface::doProcess] Error reading stream of data: Closing "
351  "communications\n");
352  if (stream_serial)
353  {
354  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
355  stream_serial->close();
356  }
357  m_GPS_comsWork = false;
358  return;
359  }
360 
361  // Try to parse incomming data as messages:
362  parseBuffer();
363 
364  // Decide whether to push out a new observation in old legacy mode.
365  if (!m_customInit.empty())
366  { // "Advanced" (RTK,mmGPS) device (kept for backwards-compatibility)
367  bool do_append_obs = false;
368  // FAMD
369  // Append observation if:
370  // 0. the timestamp seems to be correct!
371  // 1. it contains both synched GGA and RMC data
372  // 2. it contains only GGA or RMC but the next one is not synched with
373  // it
375  {
376  if (m_verbose)
377  cout << "[CGPSInterface] Initial timestamp: "
380  << endl;
381  // Check if the initial timestamp seems to be OK (not a spurio one)
382  TTimeStamp tmNow = mrpt::system::now();
383  const double tdif = mrpt::system::timeDifference(
385  if (tdif >= 0 && tdif < 7500 /*Up to two hours*/)
387  else
388  {
389  if (m_verbose)
390  cout << "[CGPSInterface] Warning: The initial timestamp "
391  "seems to be wrong! : "
392  << tdif << endl;
393  }
394  } // end-if
395  else
396  {
397  const double time_diff = mrpt::system::timeDifference(
399  if (time_diff < 0 || time_diff > 300) // Assert that the current
400  // timestamp is after the
401  // previous one and not more
402  // than 5 minutes later ->
403  // remove spurious
404  {
405  if (m_verbose)
406  cout << "[CGPSInterface ] Bad timestamp difference" << endl;
407  return;
408  }
409 
410  if (time_diff - m_topcon_data_period > 0.25 * m_topcon_data_period)
411  {
412  if (m_verbose)
413  cout << "[CGPSInterface] WARNING: According to the "
414  "timestamps, we probably skipped one frame!"
415  << endl;
416  }
417 
418  // a. These GPS data have both synched RMC and GGA data
419  // don't append observation until we have both data
420  do_append_obs =
423  } // end-else
424 
425  if (do_append_obs) flushParsedMessagesNow();
426  }
427 }
428 
429 /** Queue out now the messages in \a m_just_parsed_messages, leaving it empty */
430 
432 {
433  // Generic observation data:
438  else
440  // Add observation to the output queue:
441  CObservationGPS::Ptr newObs = std::make_shared<CObservationGPS>();
442  m_just_parsed_messages.swap(*newObs);
446 
447  // And this means the comms works:
448  m_GPS_comsWork = true;
449  m_state = ssWorking;
450 }
451 
452 /* -----------------------------------------------------
453  parseBuffer
454 ----------------------------------------------------- */
456 {
457  if (m_parser == CGPSInterface::NONE) return; // Dont try to parse data
458 
459  // Only one parser selected?
460  ptr_parser_t parser_ptr = nullptr;
461  switch (m_parser)
462  {
463  case CGPSInterface::NMEA:
465  break;
468  break;
469  case CGPSInterface::AUTO:
470  break; // Leave it as NULL
471  default:
472  throw std::runtime_error("[CGPSInterface] Unknown parser!");
473  };
474  if (parser_ptr)
475  {
476  // Use only one parser ----------
477  size_t min_bytes;
478  do
479  {
480  if (!(*this.*parser_ptr)(min_bytes))
481  {
482  if (m_rx_buffer.size() != 0)
483  m_rx_buffer.pop(); // Not the start of a frame, skip 1 byte
484  }
485  if (m_customInit.empty() /* If we are not in old legacy mode */ &&
488  } while (m_rx_buffer.size() >= min_bytes);
489  } // end one parser mode ----------
490  else
491  {
492  // AUTO mode --------
493  const std::list<CGPSInterface::ptr_parser_t>& all_parsers =
495 
496  size_t global_min_bytes_max = 0;
497  do
498  {
499  bool all_parsers_want_to_skip = true;
500  for (auto all_parser : all_parsers)
501  {
502  parser_ptr = all_parser;
503  size_t this_parser_min_bytes;
504  if ((*this.*parser_ptr)(this_parser_min_bytes))
505  all_parsers_want_to_skip = false;
506  mrpt::keep_max(global_min_bytes_max, this_parser_min_bytes);
507  }
508 
509  if (all_parsers_want_to_skip && m_rx_buffer.size() != 0)
510  m_rx_buffer.pop(); // Not the start of a frame, skip 1 byte
511 
512  if (m_customInit.empty() /* If we are not in old legacy mode */ &&
515  } while (m_rx_buffer.size() >= global_min_bytes_max);
516  } // end AUTO mode ----
517 }
518 
519 /* -----------------------------------------------------
520  JAVAD_sendMessage
521 ----------------------------------------------------- */
522 void CGPSInterface::JAVAD_sendMessage(const char* str, bool waitForAnswer)
523 {
524  if (!str) return;
525  const size_t len = strlen(str);
526  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
527  if (!stream_serial) return;
528 
529  size_t written;
530 
531  {
532  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
533  written = stream_serial->Write(str, len);
534  }
535 
536  if (m_verbose) std::cout << "[CGPSInterface] TX: " << str;
537 
538  if (written != len)
539  throw std::runtime_error(
540  format("Error sending command: '%s'", str).c_str());
541  std::this_thread::sleep_for(5ms);
542 
543  if (!waitForAnswer) return;
544 
545  std::this_thread::sleep_for(200ms);
546  char buf[200];
547  buf[0] = '\0';
548 
549  int bad_counter = 0;
550  while (bad_counter < 10)
551  {
552  size_t nRead;
553  {
554  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
555  written = stream_serial->Write(str, len);
556  nRead = stream_serial->Read(buf, sizeof(buf));
557  }
558 
559  if (m_verbose) std::cout << "[CGPSInterface] RX: " << buf << std::endl;
560 
561  if (nRead < 3)
562  throw std::runtime_error(format(
563  "ERROR: Invalid response '%s' for command '%s'", buf, str));
564 
565  if (nRead >= 3 && buf[0] == 'R' && buf[1] == 'E')
566  return; // Ok!
567  else
568  ++bad_counter;
569  }
570  throw std::runtime_error(
571  format("ERROR: Invalid response '%s' for command '%s'", buf, str));
572 }
573 
575 {
576  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
577 
578  if (stream_serial && !stream_serial->isOpen()) return false;
579 
580  // Send commands:
581  for (const auto& m_shutdown_cmd : m_shutdown_cmds)
582  {
583  if (m_verbose)
584  cout << "[CGPSInterface] TX shutdown command: `" << m_shutdown_cmd
585  << "`\n";
586 
587  std::string sTx = m_shutdown_cmd;
588  if (m_custom_cmds_append_CRLF) sTx += std::string("\r\n");
589  try
590  {
591  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
592  m_data_stream->Write(&sTx[0], sTx.size());
593  }
594  catch (...)
595  {
596  return false; // On any I/O error
597  }
598 
599  std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(
600  m_custom_cmds_delay * 1000));
601  }
602  return true;
603 }
604 
605 /* -----------------------------------------------------
606  OnConnectionEstablished
607 ----------------------------------------------------- */
609 {
610  m_last_GGA.clear(); // On comms reset, empty this cache
612 
613  // Legacy behavior:
614  if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") ||
615  !os::_strcmpi(m_customInit.c_str(), "TOPCON"))
616  {
618  }
619 
620  // Purge input:
621  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
622  if (stream_serial)
623  {
624  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
625  stream_serial->purgeBuffers();
626  }
627 
628  // New behavior: Send generic commands set-up by the user in the config
629  // file.
630 
631  // Send commands:
632  for (const auto& m_setup_cmd : m_setup_cmds)
633  {
634  if (m_verbose)
635  cout << "[CGPSInterface] TX setup command: `" << m_setup_cmd
636  << "`\n";
637 
638  std::string sTx = m_setup_cmd;
639  if (m_custom_cmds_append_CRLF) sTx += std::string("\r\n");
640 
641  try
642  {
643  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
644  m_data_stream->Write(&sTx[0], sTx.size());
645  }
646  catch (const std::exception& e)
647  {
648  std::cerr << "[CGPSInterface::OnConnectionEstablished] Error "
649  "sending setup cmds: "
650  << e.what() << std::endl;
651  return false;
652  }
653  std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(
654  m_custom_cmds_delay * 1000));
655  }
656  std::this_thread::sleep_for(
657  std::chrono::duration<double, std::milli>(m_custom_cmds_delay * 1000));
658  return true;
659 }
660 
662 {
663  MRPT_START
664  if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") ||
665  !os::_strcmpi(m_customInit.c_str(), "TOPCON"))
666  {
667  // Stop messaging:
668  JAVAD_sendMessage("%%dm\r\n", false);
669  std::this_thread::sleep_for(500ms);
670  JAVAD_sendMessage("%%dm\r\n", false);
671  std::this_thread::sleep_for(1000ms);
672 
673  // Purge input:
674  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
675  if (stream_serial)
676  {
677  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
678  stream_serial->purgeBuffers();
679  }
680 
681  JAVAD_sendMessage("%%set,/par/cur/term/imode,cmd\r\n"); // set the
682  // current port
683  // in command
684  // mode
685  return true;
686  }
687  else
688  return true;
689  MRPT_END
690 } // end-unsetJAVAD_AIM_mode
691 
693 {
694  MRPT_START
695  if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") ||
696  !os::_strcmpi(m_customInit.c_str(), "TOPCON"))
697  {
699  format("%%%%set,/par%s/imode,cmd\r\n", m_JAVAD_rtk_src_port.c_str())
700  .c_str()); // set the port in command mode
702  "%%set,/par/cur/term/jps/0,{nscmd,37,n,\"\"}\r\n"); // any command
703  // starting
704  // with % will
705  // be treated
706  // as normal
707 
708  ASSERT_(!m_JAVAD_rtk_format.empty());
709  cout << "Formato de correcciones para GR3: " << m_JAVAD_rtk_format
710  << endl;
711  if (m_JAVAD_rtk_format == "cmr")
712  {
714  format(
715  "%%%%set,/par/cur/term/jps/1,{cmr,-1,y,%s}\r\n",
716  m_JAVAD_rtk_src_port.c_str())
717  .c_str()); // set corrections type CMR or CMR+
718  JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
720  "%%%%set,/par%s/imode,cmr\r\n",
721  m_JAVAD_rtk_src_port.c_str())
722  .c_str());
723  }
724  else if (m_JAVAD_rtk_format == "rtcm")
725  {
727  format(
728  "%%%%set,/par/cur/term/jps/1,{rtcm,-1,y,%s}\r\n",
729  m_JAVAD_rtk_src_port.c_str())
730  .c_str()); // set corrections type RTCM
731  JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
733  "%%%%set,/par%s/imode,rtcm\r\n",
734  m_JAVAD_rtk_src_port.c_str())
735  .c_str());
736  }
737  else if (m_JAVAD_rtk_format == "rtcm3")
738  {
740  format(
741  "%%%%set,/par/cur/term/jps/1,{rtcm3,-1,y,%s}\r\n",
742  m_JAVAD_rtk_src_port.c_str())
743  .c_str()); // set corrections type RTCM 3.x
744  JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
746  "%%%%set,/par%s/imode,rtcm3\r\n",
747  m_JAVAD_rtk_src_port.c_str())
748  .c_str());
749  }
750  else
751  {
752  cout << "Unknown RTK corrections format. Only supported: CMR, RTCM "
753  "or RTCM3"
754  << endl;
755  return false;
756  }
757  JAVAD_sendMessage("%%set,/par/cur/term/imode,jps\r\n"); // sets current
758  // port into
759  // "JPS" mode
760 
761  return true;
762 
763  } // end-if
764  else
765  return true;
766  MRPT_END
767 } // end-setJAVAD_AIM_mode
768 
769 std::string CGPSInterface::getLastGGA(bool reset)
770 {
771  std::string ret = m_last_GGA;
772  if (reset) m_last_GGA.clear();
773  return ret;
774 }
775 
777 {
778  // Stop messaging:
779  JAVAD_sendMessage("%%dm\r\n", false);
780  std::this_thread::sleep_for(500ms);
781  JAVAD_sendMessage("%%dm\r\n", false);
782  std::this_thread::sleep_for(1000ms);
783 
784  // Purge input:
785  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
786  if (stream_serial)
787  {
788  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
789  stream_serial->purgeBuffers();
790  }
791 
792  // Configure RTK mode and source:
793  if (m_verbose) cout << "[CGPSInterface] Configure RTK options" << endl;
794 
795  if (!m_JAVAD_rtk_src_port.empty())
796  {
797  const int elevation_mask = 5; // Degs
798 
800  format("%%%%set,/par/lock/elm,%i\r\n", elevation_mask)
801  .c_str()); // Set elevation mask to track satellites
803  "%%set,/par/base/mode/,off\r\n"); // Set Base Mode off
804  JAVAD_sendMessage("%%set,/par/pos/pd/period,1.0\r\n"); // Differential
805  // Correction
806  // Interval
807  // JAVAD_sendMessage("%%set,hd/mode,off\r\n"); // fixed distance to rtk
808  // base: Off
809  // JAVAD_sendMessage("%%set,/par/pos/pd/hd/mode,off\r\n"); // fixed
810  // distance to rtk base: Off <-- Not working with TopCon GR3! (option
811  // disabled)
813  "%%set,/par/pos/pd/qcheck,off\r\n"); // Set Quality Checks Off
815  "%%set,/par/pos/mode/cur,pd\r\n"); // Pos Mode Phase Diff
817  "%%set,/par/pos/pd/textr,10\r\n"); // RTK Extrapolation Limit
819  "%%set,/par/pos/pd/inuse,-1\r\n"); // Set Rovers Reference Station
820  JAVAD_sendMessage("%%set,/par/pos/pd/nrs/mode,y\r\n"); // Enable
821  // Nearest
822  // Reference
823  // Station Mode
824  JAVAD_sendMessage("%%set,/par/pos/pd/mode,extrap\r\n"); // Enable
825  // EXTRAPOLATED
826  // mode in RTK
827  // corrections
828 
829  // Set Differential Correction Source
831  format(
832  "%%%%set,/par/pos/pd/port,%s\r\n", m_JAVAD_rtk_src_port.c_str())
833  .c_str());
834 
835  // Set port bauds:
839  "%%%%set,/par%s/rate,%u\r\n",
840  m_JAVAD_rtk_src_port.c_str(),
842  .c_str());
843 
844  // Set Input Mode: CMR,RTCM,...
845  if (!m_topcon_useAIMMode && !m_JAVAD_rtk_format.empty())
847  "%%%%set,/par%s/imode,%s\r\n",
848  m_JAVAD_rtk_src_port.c_str(),
849  m_JAVAD_rtk_format.c_str())
850  .c_str());
851  }
852 
853  // Start NMEA messaging:
854  // JAVAD_sendMessage("%%em,,/msg/nmea/GGA:0.2\r\n");
855  // JAVAD_sendMessage("%%em,,/msg/nmea/RMC:0.2\r\n");
856  // JAVAD_sendMessage("%%em,,/msg/jps/PS:0.2\r\n");
857 
859  {
860  if (m_verbose) cout << "[CGPSInterface] Using Advanced Input Mode";
862  if (m_verbose) cout << "... done" << endl;
863  }
865  format("%%%%em,,/msg/nmea/GGA:%.1f\r\n", m_topcon_data_period).c_str());
867  format("%%%%em,,/msg/nmea/RMC:%.1f\r\n", m_topcon_data_period)
868  .c_str()); // FAMD: 10 Hz
869 
871  {
872  if (m_verbose)
873  cout << "[CGPSInterface::OnConnectionEstablished] JAVAD/TopCon "
874  "commands sent successfully with AIM."
875  << endl;
876  }
877  else
878  {
879  if (m_verbose)
880  cout << "[CGPSInterface::OnConnectionEstablished] JAVAD/TopCon "
881  "commands sent successfully."
882  << endl;
883  }
884 
885  return true;
886 }
887 
888 /** Send a custom data block to the GNSS device right now. Can be used to change
889  * its behavior online as needed. */
890 bool CGPSInterface::sendCustomCommand(const void* data, const size_t datalen)
891 {
892  try
893  {
894  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
895  m_data_stream->Write(data, datalen);
896  return true;
897  }
898  catch (const std::exception& e)
899  {
900  std::cerr << "[CGPSInterface::sendCustomCommand] Error sending cmd: "
901  << e.what() << std::endl;
902  return false;
903  }
904 }
size_t available() const
The maximum number of elements that can be written ("push") without rising an overflow error...
void timestampToParts(TTimeStamp t, TTimeParts &p, bool localTime=false)
Gets the individual parts of a date/time (days, hours, minutes, seconds) - UTC time or local time...
Definition: datetime.cpp:50
static const TParsersRegistry & getInstance()
bool has_GGA_datum() const
true if the corresponding field exists in messages.
PARSERS
Read about parser selection in the documentation for CGPSInterface.
A class capable of reading GPS/GNSS/GNSS+IMU receiver data, from a serial port or from any input stre...
std::shared_ptr< std::mutex > m_data_stream_cs
#define MRPT_START
Definition: exceptions.h:241
void JAVAD_sendMessage(const char *str, bool waitForAnswer=true)
Private auxiliary method.
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
A communications serial port built as an implementation of a utils::CStream.
Definition: CSerialPort.h:41
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
bool isGPS_connected()
Returns true if communications work, i.e.
mrpt::obs::CObservationGPS m_just_parsed_messages
A private copy of the last received gps datum.
const std::vector< std::string > & getSetupCommands() const
bool OnConnectionShutdown()
Like OnConnectionEstablished() for sending optional shutdown commands.
std::string m_sensorLabel
See CGenericSensor.
void swap(CObservationGPS &o)
message_list_t messages
The main piece of data in this class: a list of GNNS messages.
bool unsetJAVAD_AIM_mode()
Unset Advanced Input Mode for the primary port and use it only as a command port. ...
bool m_topcon_AIMConfigured
Indicates if the AIM has been properly set up.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
Contains classes for various device interfaces.
bool(CGPSInterface::*)(size_t &out_minimum_rx_buf_to_decide) ptr_parser_t
bool strCmp(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case sensitive)
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
void enableSetupCommandsAppendCRLF(const bool enable)
std::string timeToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (UTC): HH:MM:SS.MMMMMM.
Definition: datetime.cpp:244
STL namespace.
void parseBuffer()
Process data in "m_buffer" to extract GPS messages, and remove them from the buffer.
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores (&#39;_&#39;) or any other user-given char. ...
Definition: filesystem.cpp:329
bool sendCustomCommand(const void *data, const size_t datalen)
Send a custom data block to the GNSS device right now.
bool tryToOpenTheCOM()
Returns true if the COM port is already open, or try to open it in other case.
unsigned int m_JAVAD_rtk_src_baud
Only used when "m_JAVAD_rtk_src_port" is not empty.
std::string m_JAVAD_rtk_src_port
If not empty, will send a cmd "set,/par/pos/pd/port,...".
std::list< CGPSInterface::ptr_parser_t > all_parsers
bool isEnabledSetupCommandsAppendCRLF() const
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
ENUMTYPE read_enum(const std::string &section, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
bool has_RMC_datum() const
true if the corresponding field exists in messages.
This class allows loading and storing values and vectors of different types from a configuration text...
T pop()
Retrieve an element from the buffer.
void doProcess() override
This method will be invoked at a minimum rate of "process_rate" (Hz)
constexpr double DEG2RAD(const double x)
Degrees to radians.
bool implement_parser_NMEA(size_t &out_minimum_rx_buf_to_decide)
Versatile class for consistent logging and management of output messages.
The parts of a date/time (it&#39;s like the standard &#39;tm&#39; but with fractions of seconds).
Definition: datetime.h:49
void setSetupCommands(const std::vector< std::string > &cmds)
uint8_t day
Month (1-12)
Definition: datetime.h:53
void setSetupCommandsDelay(const double delay_secs)
This namespace contains representation of robot actions and observations.
std::string m_JAVAD_rtk_format
Only used when "m_JAVAD_rtk_src_port" is not empty: format of RTK corrections: "cmr", "rtcm", "rtcm3", etc.
std::shared_ptr< std::mutex > m_data_stream_mine_cs
bool implement_parser_NOVATEL_OEM6(size_t &out_minimum_rx_buf_to_decide)
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
bool OnConnectionEstablished()
Implements custom messages to be sent to the GPS unit just after connection and before normal use...
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
size_t size() const
Return the number of elements available for read ("pop") in the buffer (this is NOT the maximum size ...
std::vector< std::string > m_setup_cmds
void clear()
Empties this observation, clearing the container messages.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
bool fileOpenCorrectly() const
Returns true if the file was open without errors.
bool open(const std::string &fileName, bool append=false)
Open the given file for write.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
double second
Minute (0-59)
Definition: datetime.h:56
std::string getSerialPortName() const
Get the serial port to use (COM1, ttyUSB0, etc).
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
uint8_t minute
Hour (0-23)
Definition: datetime.h:55
mrpt::io::CFileOutputStream m_raw_output_file
mrpt::poses::CPose3D sensorPose
The sensor pose on the robot/vehicle.
#define MRPT_LOG_ERROR(_STRING)
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
bool setJAVAD_AIM_mode()
Set Advanced Input Mode for the primary port.
size_t Write(const void *Buffer, size_t Count) override
Introduces a pure virtual method responsible for writing to the stream.
#define MRPT_END
Definition: exceptions.h:245
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:265
A TCP socket that can be connected to a TCP server, implementing MRPT&#39;s CStream interface for passing...
uint8_t month
The year.
Definition: datetime.h:52
std::shared_ptr< mrpt::io::CStream > m_data_stream
Typically a CSerialPort created by this class, but may be set externally.
std::string getLastGGA(bool reset=true)
Gets the latest GGA command or an empty string if no newer GGA command was received since the last ca...
std::string trim(const std::string &str)
Removes leading and trailing spaces.
uint8_t hour
Day (1-31)
Definition: datetime.h:54
void push_many(T *array_elements, size_t count)
Insert an array of elements in the buffer.
double m_topcon_data_period
The period in seconds which the data should be provided by the GPS.
mrpt::containers::circular_buffer< uint8_t > m_rx_buffer
Auxiliary buffer for readings.
double timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
Definition: datetime.h:123
Serial and networking devices and utilities.
mrpt::system::TTimeStamp m_last_timestamp
std::vector< std::string > m_shutdown_cmds
std::string m_last_GGA
Used in getLastGGA()
bool m_topcon_useAIMMode
Use this mode for receive RTK corrections from a external source through the primary port...
void flushParsedMessagesNow()
Queue out now the messages in m_just_parsed_messages, leaving it empty.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
void setSerialPortName(const std::string &COM_port)
Set the serial port to use (COM1, ttyUSB0, etc).
void setParser(PARSERS parser)
Select the parser for incomming data, among the options enumerated in CGPSInterface.
const std::vector< std::string > & getShutdownCommands() const
void bindStream(const std::shared_ptr< mrpt::io::CStream > &external_stream, const std::shared_ptr< std::mutex > &csOptionalExternalStream=std::shared_ptr< std::mutex >())
This enforces the use of a given user stream, instead of trying to open the serial port set in this c...
static struct FontData data
Definition: gltext.cpp:144
void setShutdownCommands(const std::vector< std::string > &cmds)
int _strcmpi(const char *str1, const char *str2) noexcept
An OS-independent version of strcmpi.
Definition: os.cpp:331



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020