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()
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.
#define min(a, b)
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
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.
double DEG2RAD(const double x)
Degrees to radians.
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:239
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
unsigned char uint8_t
Definition: rptypes.h:44
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
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)
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...
internal_msg_test_proxy< gnss::NMEA_RMC > has_RMC_datum
Evaluates as a bool; true if the corresponding field exists in messages.
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.
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
internal_msg_test_proxy< gnss::NMEA_GGA > has_GGA_datum
Evaluates as a bool; true if the corresponding field exists in messages.
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.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#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:256
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: c26d46ba6 Thu Jul 18 13:03:42 2019 +0200 at jue jul 18 13:10:17 CEST 2019