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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020