MRPT  1.9.9
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-2019, 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 
54  m_customInit(),
55  m_rx_buffer(0x10000),
56 
57  m_raw_dump_file_prefix(),
58  m_COMname(),
59 
60  m_last_timestamp(INVALID_TIMESTAMP),
61 
62  m_JAVAD_rtk_src_port(),
63 
64  m_JAVAD_rtk_format("cmr")
65 {
66  m_sensorLabel = "GPS";
67 }
68 
69 /* -----------------------------------------------------
70  loadConfig_sensorSpecific
71  ----------------------------------------------------- */
73  const mrpt::config::CConfigFileBase& configSource,
74  const std::string& iniSection)
75 {
77  iniSection, "parser", m_parser, false /*Allow default values*/);
78  m_raw_dump_file_prefix = configSource.read_string(
79  iniSection, "raw_dump_file_prefix", m_raw_dump_file_prefix,
80  false /*Allow default values*/);
81 
82 #ifdef _WIN32
83  m_COMname =
84  configSource.read_string(iniSection, "COM_port_WIN", m_COMname, true);
85 #else
86  m_COMname =
87  configSource.read_string(iniSection, "COM_port_LIN", m_COMname, true);
88 #endif
89 
90  m_COMbauds =
91  configSource.read_int(iniSection, "baudRate", m_COMbauds, true);
93  iniSection, "sensor_label_append_msg_type", m_sensorLabelAppendMsgType);
94 
95  // legacy custom cmds:
96  m_customInit =
97  configSource.read_string(iniSection, "customInit", m_customInit, false);
98 
99  // new custom cmds:
100  m_custom_cmds_delay = configSource.read_float(
101  iniSection, "custom_cmds_delay", m_custom_cmds_delay);
102  m_custom_cmds_append_CRLF = configSource.read_bool(
103  iniSection, "custom_cmds_append_CRLF", m_custom_cmds_append_CRLF);
104  // Load as many strings as found on the way:
105  m_setup_cmds.clear();
106  for (int i = 1; true; i++)
107  {
108  std::string sLine = configSource.read_string(
109  iniSection, mrpt::format("setup_cmd%i", i), std::string());
110  sLine = mrpt::system::trim(sLine);
111  if (sLine.empty()) break;
112  m_setup_cmds.push_back(sLine);
113  }
114 
115  m_shutdown_cmds.clear();
116  for (int i = 1; true; i++)
117  {
118  std::string sLine = configSource.read_string(
119  iniSection, mrpt::format("shutdown_cmd%i", i), std::string());
120  sLine = mrpt::system::trim(sLine);
121  if (sLine.empty()) break;
122  m_shutdown_cmds.push_back(sLine);
123  }
124 
126  configSource.read_float(iniSection, "pose_x", 0),
127  configSource.read_float(iniSection, "pose_y", 0),
128  configSource.read_float(iniSection, "pose_z", 0),
129  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
130  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
131  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
132 
133  m_JAVAD_rtk_src_port = configSource.read_string(
134  iniSection, "JAVAD_rtk_src_port", m_JAVAD_rtk_src_port);
135  m_JAVAD_rtk_src_baud = configSource.read_int(
136  iniSection, "JAVAD_rtk_src_baud", m_JAVAD_rtk_src_baud);
137  m_JAVAD_rtk_format = configSource.read_string(
138  iniSection, "JAVAD_rtk_format", m_JAVAD_rtk_format);
139 
140  m_topcon_useAIMMode = configSource.read_bool(
141  iniSection, "JAVAD_useAIMMode", m_topcon_useAIMMode);
143  1.0 / configSource.read_double(
144  iniSection, "outputRate", m_topcon_data_period);
145 }
146 
148 {
150 
152  {
153  delete m_data_stream;
154  m_data_stream = nullptr;
155  }
156 }
157 
159 {
160  m_parser = parser;
161 }
164  mrpt::io::CStream* external_stream, std::mutex* csOptionalExternalStream)
165 {
167  {
168  delete m_data_stream;
169  m_data_stream = nullptr;
170  }
171 
173  m_data_stream = external_stream;
174  m_data_stream_cs = csOptionalExternalStream ? csOptionalExternalStream
176 }
177 void CGPSInterface::setSetupCommandsDelay(const double delay_secs)
178 {
179  m_custom_cmds_delay = delay_secs;
180 }
182 {
183  return m_custom_cmds_delay;
184 }
185 void CGPSInterface::setSetupCommands(const std::vector<std::string>& cmds)
186 {
187  m_setup_cmds = cmds;
188 }
189 const std::vector<std::string>& CGPSInterface::getSetupCommands() const
190 {
191  return m_setup_cmds;
192 }
193 void CGPSInterface::setShutdownCommands(const std::vector<std::string>& cmds)
194 {
195  m_shutdown_cmds = cmds;
196 }
197 const std::vector<std::string>& CGPSInterface::getShutdownCommands() const
198 {
199  return m_shutdown_cmds;
200 }
202 {
203  m_custom_cmds_append_CRLF = enable;
204 }
206 {
208 }
209 
210 /* -----------------------------------------------------
211  setSerialPortName
212 ----------------------------------------------------- */
214 {
215  // Dont allow changing the serial port if:
218  "Cannot change serial port name: an external stream has been "
219  "already bound manually.");
220 
221  if (m_data_stream)
222  {
223  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
224  auto serial = dynamic_cast<mrpt::comms::CSerialPort*>(m_data_stream);
225  if (serial && serial->isOpen())
227  "Cannot change serial port name when it is already open");
228  }
229 
230  // OK:
231  m_COMname = COM_port;
232 }
233 
234 /* -----------------------------------------------------
235  getSerialPortName
236 ----------------------------------------------------- */
238 /* -----------------------------------------------------
239  tryToOpenTheCOM
240 ----------------------------------------------------- */
242 {
243  // If this is the first use of the COM port, create it:
244  if (!m_data_stream)
245  {
249  }
250 
251  auto serial = dynamic_cast<CSerialPort*>(m_data_stream);
252  if (serial)
253  {
254  {
255  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
256  if (serial->isOpen()) return true; // Already open
257 
258  if (m_verbose)
259  cout << "[CGPSInterface] Opening " << m_COMname << " @ "
260  << m_COMbauds << endl;
261  }
262  try
263  {
264  serial->open(m_COMname);
265  // Config:
266  serial->setConfig(m_COMbauds, 0, 8, 1);
267  serial->setTimeouts(1, 0, 1, 1, 1);
268 
269  // Do extra initialization?
271  {
272  serial->close();
273  return false;
274  }
275  return true; // All OK
276  }
277  catch (const std::exception& e)
278  {
279  std::cerr << "[CGPSInterface::tryToOpenTheCOM] Error opening or "
280  "configuring serial port:"
281  << std::endl
282  << e.what();
283  serial->close();
284  return false;
285  }
286  } // end of this is a serial port
287 
288  return true; // All OK
289 }
290 
291 /* -----------------------------------------------------
292  isGPS_connected
293 ----------------------------------------------------- */
295 /* -----------------------------------------------------
296  doProcess
297 ----------------------------------------------------- */
299 {
300  // Is the COM open?
301  if (!tryToOpenTheCOM())
302  {
303  m_state = ssError;
304  THROW_EXCEPTION("Could not open the input stream");
305  }
306  ASSERT_(m_data_stream != nullptr);
307  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream);
308  auto* stream_tcpip = dynamic_cast<CClientTCPSocket*>(m_data_stream);
309 
310  // Read as many bytes as available:
311  uint8_t buf[0x1000];
312  const size_t to_read =
313  std::min(m_rx_buffer.available() - 1, sizeof(buf) - 1);
314  try
315  {
316  size_t nRead = 0;
317  if (to_read > 0)
318  {
319  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
320  if (stream_tcpip)
321  {
322  nRead = stream_tcpip->readAsync(buf, to_read, 100, 10);
323  }
324  else if (stream_serial)
325  {
326  nRead = stream_serial->Read(buf, to_read);
327  }
328  else
329  {
330  nRead = m_data_stream->Read(buf, to_read);
331  }
332  }
333 
334  if (nRead) m_rx_buffer.push_many(buf, nRead);
335 
336  // Also dump to raw file:
337  if (!m_raw_dump_file_prefix.empty() &&
339  {
340  // 1st time open:
342  mrpt::system::timestampToParts(now(), parts, true);
343  string sFilePostfix = "_";
344  sFilePostfix += format(
345  "%04u-%02u-%02u_%02uh%02um%02us", (unsigned int)parts.year,
346  (unsigned int)parts.month, (unsigned int)parts.day,
347  (unsigned int)parts.hour, (unsigned int)parts.minute,
348  (unsigned int)parts.second);
349  const string sFileName =
352  string(".gps");
353 
354  if (m_verbose)
355  std::cout << "[CGPSInterface] Creating RAW dump file: `"
356  << sFileName << "`\n";
357  m_raw_output_file.open(sFileName);
358  }
359  if (nRead && m_raw_output_file.fileOpenCorrectly())
360  {
361  m_raw_output_file.Write(buf, nRead);
362  }
363  }
364  catch (std::exception&)
365  {
366  // ERROR:
368  "[CGPSInterface::doProcess] Error reading stream of data: Closing "
369  "communications\n");
370  if (stream_serial)
371  {
372  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
373  stream_serial->close();
374  }
375  m_GPS_comsWork = false;
376  return;
377  }
378 
379  // Try to parse incomming data as messages:
380  parseBuffer();
381 
382  // Decide whether to push out a new observation in old legacy mode.
383  if (!m_customInit.empty())
384  { // "Advanced" (RTK,mmGPS) device (kept for backwards-compatibility)
385  bool do_append_obs = false;
386  // FAMD
387  // Append observation if:
388  // 0. the timestamp seems to be correct!
389  // 1. it contains both synched GGA and RMC data
390  // 2. it contains only GGA or RMC but the next one is not synched with
391  // it
393  {
394  if (m_verbose)
395  cout << "[CGPSInterface] Initial timestamp: "
398  << endl;
399  // Check if the initial timestamp seems to be OK (not a spurio one)
400  TTimeStamp tmNow = mrpt::system::now();
401  const double tdif = mrpt::system::timeDifference(
403  if (tdif >= 0 && tdif < 7500 /*Up to two hours*/)
405  else
406  {
407  if (m_verbose)
408  cout << "[CGPSInterface] Warning: The initial timestamp "
409  "seems to be wrong! : "
410  << tdif << endl;
411  }
412  } // end-if
413  else
414  {
415  const double time_diff = mrpt::system::timeDifference(
417  if (time_diff < 0 || time_diff > 300) // Assert that the current
418  // timestamp is after the
419  // previous one and not more
420  // than 5 minutes later ->
421  // remove spurious
422  {
423  if (m_verbose)
424  cout << "[CGPSInterface ] Bad timestamp difference" << endl;
425  return;
426  }
427 
428  if (time_diff - m_topcon_data_period > 0.25 * m_topcon_data_period)
429  {
430  if (m_verbose)
431  cout << "[CGPSInterface] WARNING: According to the "
432  "timestamps, we probably skipped one frame!"
433  << endl;
434  }
435 
436  // a. These GPS data have both synched RMC and GGA data
437  // don't append observation until we have both data
438  do_append_obs =
441  } // end-else
442 
443  if (do_append_obs) flushParsedMessagesNow();
444  }
445 }
446 
447 /** Queue out now the messages in \a m_just_parsed_messages, leaving it empty */
448 
450 {
451  // Generic observation data:
456  else
458  // Add observation to the output queue:
459  CObservationGPS::Ptr newObs = std::make_shared<CObservationGPS>();
460  m_just_parsed_messages.swap(*newObs);
464 
465  // And this means the comms works:
466  m_GPS_comsWork = true;
467  m_state = ssWorking;
468 }
469 
470 /* -----------------------------------------------------
471  parseBuffer
472 ----------------------------------------------------- */
474 {
475  if (m_parser == CGPSInterface::NONE) return; // Dont try to parse data
476 
477  // Only one parser selected?
478  ptr_parser_t parser_ptr = nullptr;
479  switch (m_parser)
480  {
481  case CGPSInterface::NMEA:
483  break;
486  break;
487  case CGPSInterface::AUTO:
488  break; // Leave it as NULL
489  default:
490  throw std::runtime_error("[CGPSInterface] Unknown parser!");
491  };
492  if (parser_ptr)
493  {
494  // Use only one parser ----------
495  size_t min_bytes;
496  do
497  {
498  if (!(*this.*parser_ptr)(min_bytes))
499  {
500  if (m_rx_buffer.size() != 0)
501  m_rx_buffer.pop(); // Not the start of a frame, skip 1 byte
502  }
503  if (m_customInit.empty() /* If we are not in old legacy mode */ &&
506  } while (m_rx_buffer.size() >= min_bytes);
507  } // end one parser mode ----------
508  else
509  {
510  // AUTO mode --------
511  const std::list<CGPSInterface::ptr_parser_t>& all_parsers =
513 
514  size_t global_min_bytes_max = 0;
515  do
516  {
517  bool all_parsers_want_to_skip = true;
518  for (auto all_parser : all_parsers)
519  {
520  parser_ptr = all_parser;
521  size_t this_parser_min_bytes;
522  if ((*this.*parser_ptr)(this_parser_min_bytes))
523  all_parsers_want_to_skip = false;
524  mrpt::keep_max(global_min_bytes_max, this_parser_min_bytes);
525  }
526 
527  if (all_parsers_want_to_skip && m_rx_buffer.size() != 0)
528  m_rx_buffer.pop(); // Not the start of a frame, skip 1 byte
529 
530  if (m_customInit.empty() /* If we are not in old legacy mode */ &&
533  } while (m_rx_buffer.size() >= global_min_bytes_max);
534  } // end AUTO mode ----
535 }
536 
537 /* -----------------------------------------------------
538  JAVAD_sendMessage
539 ----------------------------------------------------- */
540 void CGPSInterface::JAVAD_sendMessage(const char* str, bool waitForAnswer)
541 {
542  if (!str) return;
543  const size_t len = strlen(str);
544  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream);
545  if (!stream_serial) return;
546 
547  size_t written;
548 
549  {
550  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
551  written = stream_serial->Write(str, len);
552  }
553 
554  if (m_verbose) std::cout << "[CGPSInterface] TX: " << str;
555 
556  if (written != len)
557  throw std::runtime_error(
558  format("Error sending command: '%s'", str).c_str());
559  std::this_thread::sleep_for(5ms);
560 
561  if (!waitForAnswer) return;
562 
563  std::this_thread::sleep_for(200ms);
564  char buf[200];
565  buf[0] = '\0';
566 
567  int bad_counter = 0;
568  while (bad_counter < 10)
569  {
570  size_t nRead;
571  {
572  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
573  written = stream_serial->Write(str, len);
574  nRead = stream_serial->Read(buf, sizeof(buf));
575  }
576 
577  if (m_verbose) std::cout << "[CGPSInterface] RX: " << buf << std::endl;
578 
579  if (nRead < 3)
580  throw std::runtime_error(format(
581  "ERROR: Invalid response '%s' for command '%s'", buf, str));
582 
583  if (nRead >= 3 && buf[0] == 'R' && buf[1] == 'E')
584  return; // Ok!
585  else
586  ++bad_counter;
587  }
588  throw std::runtime_error(
589  format("ERROR: Invalid response '%s' for command '%s'", buf, str));
590 }
591 
593 {
594  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream);
595 
596  if (stream_serial && !stream_serial->isOpen()) return false;
597 
598  // Send commands:
599  for (const auto& m_shutdown_cmd : m_shutdown_cmds)
600  {
601  if (m_verbose)
602  cout << "[CGPSInterface] TX shutdown command: `" << m_shutdown_cmd
603  << "`\n";
604 
605  std::string sTx = m_shutdown_cmd;
606  if (m_custom_cmds_append_CRLF) sTx += std::string("\r\n");
607  try
608  {
609  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
610  m_data_stream->Write(&sTx[0], sTx.size());
611  }
612  catch (...)
613  {
614  return false; // On any I/O error
615  }
616 
617  std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(
618  m_custom_cmds_delay * 1000));
619  }
620  return true;
621 }
622 
623 /* -----------------------------------------------------
624  OnConnectionEstablished
625 ----------------------------------------------------- */
627 {
628  m_last_GGA.clear(); // On comms reset, empty this cache
630 
631  // Legacy behavior:
632  if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") ||
633  !os::_strcmpi(m_customInit.c_str(), "TOPCON"))
634  {
636  }
637 
638  // Purge input:
639  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream);
640  if (stream_serial)
641  {
642  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
643  stream_serial->purgeBuffers();
644  }
645 
646  // New behavior: Send generic commands set-up by the user in the config
647  // file.
648 
649  // Send commands:
650  for (const auto& m_setup_cmd : m_setup_cmds)
651  {
652  if (m_verbose)
653  cout << "[CGPSInterface] TX setup command: `" << m_setup_cmd
654  << "`\n";
655 
656  std::string sTx = m_setup_cmd;
657  if (m_custom_cmds_append_CRLF) sTx += std::string("\r\n");
658 
659  try
660  {
661  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
662  m_data_stream->Write(&sTx[0], sTx.size());
663  }
664  catch (const std::exception& e)
665  {
666  std::cerr << "[CGPSInterface::OnConnectionEstablished] Error "
667  "sending setup cmds: "
668  << e.what() << std::endl;
669  return false;
670  }
671  std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(
672  m_custom_cmds_delay * 1000));
673  }
674  std::this_thread::sleep_for(
675  std::chrono::duration<double, std::milli>(m_custom_cmds_delay * 1000));
676  return true;
677 }
678 
680 {
681  MRPT_START
682  if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") ||
683  !os::_strcmpi(m_customInit.c_str(), "TOPCON"))
684  {
685  // Stop messaging:
686  JAVAD_sendMessage("%%dm\r\n", false);
687  std::this_thread::sleep_for(500ms);
688  JAVAD_sendMessage("%%dm\r\n", false);
689  std::this_thread::sleep_for(1000ms);
690 
691  // Purge input:
692  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream);
693  if (stream_serial)
694  {
695  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
696  stream_serial->purgeBuffers();
697  }
698 
699  JAVAD_sendMessage("%%set,/par/cur/term/imode,cmd\r\n"); // set the
700  // current port
701  // in command
702  // mode
703  return true;
704  }
705  else
706  return true;
707  MRPT_END
708 } // end-unsetJAVAD_AIM_mode
709 
711 {
712  MRPT_START
713  if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") ||
714  !os::_strcmpi(m_customInit.c_str(), "TOPCON"))
715  {
717  format("%%%%set,/par%s/imode,cmd\r\n", m_JAVAD_rtk_src_port.c_str())
718  .c_str()); // set the port in command mode
720  "%%set,/par/cur/term/jps/0,{nscmd,37,n,\"\"}\r\n"); // any command
721  // starting
722  // with % will
723  // be treated
724  // as normal
725 
726  ASSERT_(!m_JAVAD_rtk_format.empty());
727  cout << "Formato de correcciones para GR3: " << m_JAVAD_rtk_format
728  << endl;
729  if (m_JAVAD_rtk_format == "cmr")
730  {
732  format(
733  "%%%%set,/par/cur/term/jps/1,{cmr,-1,y,%s}\r\n",
734  m_JAVAD_rtk_src_port.c_str())
735  .c_str()); // set corrections type CMR or CMR+
736  JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
738  "%%%%set,/par%s/imode,cmr\r\n",
739  m_JAVAD_rtk_src_port.c_str())
740  .c_str());
741  }
742  else if (m_JAVAD_rtk_format == "rtcm")
743  {
745  format(
746  "%%%%set,/par/cur/term/jps/1,{rtcm,-1,y,%s}\r\n",
747  m_JAVAD_rtk_src_port.c_str())
748  .c_str()); // set corrections type RTCM
749  JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
751  "%%%%set,/par%s/imode,rtcm\r\n",
752  m_JAVAD_rtk_src_port.c_str())
753  .c_str());
754  }
755  else if (m_JAVAD_rtk_format == "rtcm3")
756  {
758  format(
759  "%%%%set,/par/cur/term/jps/1,{rtcm3,-1,y,%s}\r\n",
760  m_JAVAD_rtk_src_port.c_str())
761  .c_str()); // set corrections type RTCM 3.x
762  JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
764  "%%%%set,/par%s/imode,rtcm3\r\n",
765  m_JAVAD_rtk_src_port.c_str())
766  .c_str());
767  }
768  else
769  {
770  cout << "Unknown RTK corrections format. Only supported: CMR, RTCM "
771  "or RTCM3"
772  << endl;
773  return false;
774  }
775  JAVAD_sendMessage("%%set,/par/cur/term/imode,jps\r\n"); // sets current
776  // port into
777  // "JPS" mode
778 
779  return true;
780 
781  } // end-if
782  else
783  return true;
784  MRPT_END
785 } // end-setJAVAD_AIM_mode
786 
788 {
789  std::string ret = m_last_GGA;
790  if (reset) m_last_GGA.clear();
791  return ret;
792 }
793 
795 {
796  // Stop messaging:
797  JAVAD_sendMessage("%%dm\r\n", false);
798  std::this_thread::sleep_for(500ms);
799  JAVAD_sendMessage("%%dm\r\n", false);
800  std::this_thread::sleep_for(1000ms);
801 
802  // Purge input:
803  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream);
804  if (stream_serial)
805  {
806  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
807  stream_serial->purgeBuffers();
808  }
809 
810  // Configure RTK mode and source:
811  if (m_verbose) cout << "[CGPSInterface] Configure RTK options" << endl;
812 
813  if (!m_JAVAD_rtk_src_port.empty())
814  {
815  const int elevation_mask = 5; // Degs
816 
818  format("%%%%set,/par/lock/elm,%i\r\n", elevation_mask)
819  .c_str()); // Set elevation mask to track satellites
821  "%%set,/par/base/mode/,off\r\n"); // Set Base Mode off
822  JAVAD_sendMessage("%%set,/par/pos/pd/period,1.0\r\n"); // Differential
823  // Correction
824  // Interval
825  // JAVAD_sendMessage("%%set,hd/mode,off\r\n"); // fixed distance to rtk
826  // base: Off
827  // JAVAD_sendMessage("%%set,/par/pos/pd/hd/mode,off\r\n"); // fixed
828  // distance to rtk base: Off <-- Not working with TopCon GR3! (option
829  // disabled)
831  "%%set,/par/pos/pd/qcheck,off\r\n"); // Set Quality Checks Off
833  "%%set,/par/pos/mode/cur,pd\r\n"); // Pos Mode Phase Diff
835  "%%set,/par/pos/pd/textr,10\r\n"); // RTK Extrapolation Limit
837  "%%set,/par/pos/pd/inuse,-1\r\n"); // Set Rovers Reference Station
838  JAVAD_sendMessage("%%set,/par/pos/pd/nrs/mode,y\r\n"); // Enable
839  // Nearest
840  // Reference
841  // Station Mode
842  JAVAD_sendMessage("%%set,/par/pos/pd/mode,extrap\r\n"); // Enable
843  // EXTRAPOLATED
844  // mode in RTK
845  // corrections
846 
847  // Set Differential Correction Source
849  format(
850  "%%%%set,/par/pos/pd/port,%s\r\n", m_JAVAD_rtk_src_port.c_str())
851  .c_str());
852 
853  // Set port bauds:
857  "%%%%set,/par%s/rate,%u\r\n",
858  m_JAVAD_rtk_src_port.c_str(),
860  .c_str());
861 
862  // Set Input Mode: CMR,RTCM,...
863  if (!m_topcon_useAIMMode && !m_JAVAD_rtk_format.empty())
865  "%%%%set,/par%s/imode,%s\r\n",
866  m_JAVAD_rtk_src_port.c_str(),
867  m_JAVAD_rtk_format.c_str())
868  .c_str());
869  }
870 
871  // Start NMEA messaging:
872  // JAVAD_sendMessage("%%em,,/msg/nmea/GGA:0.2\r\n");
873  // JAVAD_sendMessage("%%em,,/msg/nmea/RMC:0.2\r\n");
874  // JAVAD_sendMessage("%%em,,/msg/jps/PS:0.2\r\n");
875 
877  {
878  if (m_verbose) cout << "[CGPSInterface] Using Advanced Input Mode";
880  if (m_verbose) cout << "... done" << endl;
881  }
883  format("%%%%em,,/msg/nmea/GGA:%.1f\r\n", m_topcon_data_period).c_str());
885  format("%%%%em,,/msg/nmea/RMC:%.1f\r\n", m_topcon_data_period)
886  .c_str()); // FAMD: 10 Hz
887 
889  {
890  if (m_verbose)
891  cout << "[CGPSInterface::OnConnectionEstablished] JAVAD/TopCon "
892  "commands sent successfully with AIM."
893  << endl;
894  }
895  else
896  {
897  if (m_verbose)
898  cout << "[CGPSInterface::OnConnectionEstablished] JAVAD/TopCon "
899  "commands sent successfully."
900  << endl;
901  }
902 
903  return true;
904 }
905 
906 /** Send a custom data block to the GNSS device right now. Can be used to change
907  * its behavior online as needed. */
908 bool CGPSInterface::sendCustomCommand(const void* data, const size_t datalen)
909 {
910  try
911  {
912  std::lock_guard<std::mutex> lock(*m_data_stream_cs);
913  m_data_stream->Write(data, datalen);
914  return true;
915  }
916  catch (const std::exception& e)
917  {
918  std::cerr << "[CGPSInterface::sendCustomCommand] Error sending cmd: "
919  << e.what() << std::endl;
920  return false;
921  }
922 }
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...
#define MRPT_START
Definition: exceptions.h:241
void JAVAD_sendMessage(const char *str, bool waitForAnswer=true)
Private auxiliary method.
void bindStream(mrpt::io::CStream *external_stream, std::mutex *csOptionalExternalStream=nullptr)
This enforces the use of a given user stream, instead of trying to open the serial port set in this c...
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)
virtual size_t Read(void *Buffer, size_t Count)=0
Introduces a pure virtual method responsible for reading from the stream.
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,...".
GLenum GLsizei len
Definition: glext.h:4756
std::list< CGPSInterface::ptr_parser_t > all_parsers
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: io/CStream.h:28
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.
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
GLsizei const GLchar ** string
Definition: glext.h:4116
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)
virtual size_t Write(const void *Buffer, size_t Count)=0
Introduces a pure virtual method responsible for writing to the stream.
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
GLboolean reset
Definition: glext.h:3586
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
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
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3550
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 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:322



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: cb560b230 Wed Nov 13 08:06:48 2019 +0100 at miƩ nov 13 08:15:10 CET 2019