Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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>
14 #include <mrpt/system/filesystem.h>
17 #include <list>
18 
19 using namespace mrpt::hwdrivers;
20 using namespace mrpt::obs;
21 using namespace mrpt::system;
22 using namespace mrpt::synch;
23 using namespace mrpt::utils;
24 using namespace std;
25 
27 
29 {
30  std::list<CGPSInterface::ptr_parser_t> all_parsers;
31 
32  static const TParsersRegistry & getInstance()
33  {
34  static TParsersRegistry reg;
35  return reg;
36  }
37 
38 private:
40  {
41  all_parsers.push_back( &CGPSInterface::implement_parser_NMEA );
42  all_parsers.push_back( &CGPSInterface::implement_parser_NOVATEL_OEM6 );
43  }
44 };
45 
46 /* -----------------------------------------------------
47  Constructor
48  ----------------------------------------------------- */
50  mrpt::utils::COutputLogger("CGPSInterface"),
51  m_data_stream(NULL), // Typically a CSerialPort created by this class, but may be set externally.
52  m_data_stream_cs(NULL),
53  m_data_stream_is_external(false),
54  m_customInit (),
55  m_rx_buffer (0x10000),
56  m_parser (CGPSInterface::AUTO),
57  m_raw_dump_file_prefix(),
58  m_COMname (),
59  m_COMbauds (4800),
60  m_sensorLabelAppendMsgType (true),
61  m_GPS_comsWork (false),
62  m_last_timestamp ( INVALID_TIMESTAMP ),
63  m_custom_cmds_delay (0.1),
64  m_custom_cmds_append_CRLF(true),
65 
66  m_JAVAD_rtk_src_port (),
67  m_JAVAD_rtk_src_baud (0),
68  m_JAVAD_rtk_format ("cmr"),
69  m_topcon_useAIMMode ( false ),
70  m_topcon_AIMConfigured ( false ),
71  m_topcon_data_period ( 0.2 ) // 20 Hz
72 {
73  m_sensorLabel = "GPS";
74 }
75 
76 /* -----------------------------------------------------
77  loadConfig_sensorSpecific
78  ----------------------------------------------------- */
80  const mrpt::utils::CConfigFileBase &configSource,
81  const std::string &iniSection )
82 {
83  m_parser = configSource.read_enum<CGPSInterface::PARSERS>(iniSection,"parser",m_parser,false /*Allow default values*/);
84  m_raw_dump_file_prefix = configSource.read_string(iniSection,"raw_dump_file_prefix",m_raw_dump_file_prefix,false /*Allow default values*/);
85 
86 #ifdef MRPT_OS_WINDOWS
87  m_COMname = configSource.read_string(iniSection, "COM_port_WIN", m_COMname, true );
88 #else
89  m_COMname = configSource.read_string(iniSection, "COM_port_LIN", m_COMname, true );
90 #endif
91 
92  m_COMbauds = configSource.read_int( iniSection, "baudRate",m_COMbauds, true );
93  m_sensorLabelAppendMsgType = configSource.read_bool(iniSection,"sensor_label_append_msg_type",m_sensorLabelAppendMsgType );
94 
95  // legacy custom cmds:
96  m_customInit = configSource.read_string( iniSection, "customInit", m_customInit, false );
97 
98  // new custom cmds:
99  m_custom_cmds_delay = configSource.read_float( iniSection, "custom_cmds_delay",m_custom_cmds_delay );
100  m_custom_cmds_append_CRLF = configSource.read_bool( iniSection, "custom_cmds_append_CRLF",m_custom_cmds_append_CRLF);
101  // Load as many strings as found on the way:
102  m_setup_cmds.clear();
103  for (int i=1; true; i++)
104  {
105  std::string sLine = configSource.read_string(iniSection, mrpt::format("setup_cmd%i",i),std::string() );
106  sLine = mrpt::system::trim( sLine );
107  if (sLine.empty())
108  break;
109  m_setup_cmds.push_back(sLine);
110  }
111 
112  m_shutdown_cmds.clear();
113  for (int i=1; true; i++)
114  {
115  std::string sLine = configSource.read_string(iniSection, mrpt::format("shutdown_cmd%i",i),std::string() );
116  sLine = mrpt::system::trim( sLine );
117  if (sLine.empty())
118  break;
119  m_shutdown_cmds.push_back(sLine);
120  }
121 
123  configSource.read_float(iniSection,"pose_x",0),
124  configSource.read_float(iniSection,"pose_y",0),
125  configSource.read_float(iniSection,"pose_z",0),
126  DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
127  DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
128  DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
129  );
130 
131  m_JAVAD_rtk_src_port = configSource.read_string(iniSection, "JAVAD_rtk_src_port",m_JAVAD_rtk_src_port );
132  m_JAVAD_rtk_src_baud = configSource.read_int(iniSection, "JAVAD_rtk_src_baud",m_JAVAD_rtk_src_baud );
133  m_JAVAD_rtk_format = configSource.read_string(iniSection,"JAVAD_rtk_format", m_JAVAD_rtk_format );
134 
135  m_topcon_useAIMMode = configSource.read_bool( iniSection,"JAVAD_useAIMMode", m_topcon_useAIMMode );
136  m_topcon_data_period = 1.0/configSource.read_double( iniSection,"outputRate", m_topcon_data_period );
137 }
138 
140 {
142 
144  {
145  delete m_data_stream;
146  m_data_stream = NULL;
147  }
148 }
149 
151  m_parser = parser;
152 }
154  return m_parser;
155 }
156 void CGPSInterface::bindStream(mrpt::utils::CStream * external_stream, mrpt::synch::CCriticalSection *csOptionalExternalStream)
157 {
159  delete m_data_stream;
160  m_data_stream = NULL;
161  }
162 
164  m_data_stream = external_stream;
165  m_data_stream_cs = csOptionalExternalStream;
166 }
167 void CGPSInterface::setSetupCommandsDelay(const double delay_secs) {
168  m_custom_cmds_delay = delay_secs;
169 }
171  return m_custom_cmds_delay;
172 }
173 void CGPSInterface::setSetupCommands(const std::vector<std::string> &cmds) {
174  m_setup_cmds = cmds;
175 }
176 const std::vector<std::string> & CGPSInterface::getSetupCommands() const {
177  return m_setup_cmds;
178 }
179 void CGPSInterface::setShutdownCommands(const std::vector<std::string> &cmds) {
180  m_shutdown_cmds = cmds;
181 }
182 const std::vector<std::string> & CGPSInterface::getShutdownCommands() const {
183  return m_shutdown_cmds;
184 }
186  m_custom_cmds_append_CRLF = enable;
187 }
190 }
191 
192 /* -----------------------------------------------------
193  setSerialPortName
194 ----------------------------------------------------- */
196 {
197  // Dont allow changing the serial port if:
199  THROW_EXCEPTION("Cannot change serial port name: an external stream has been already bound manually.")
200 
201  if (m_data_stream)
202  {
204  CSerialPort *serial = dynamic_cast<CSerialPort*>(m_data_stream);
205  if (serial && serial->isOpen())
206  THROW_EXCEPTION("Cannot change serial port name when it is already open")
207  }
208 
209  // OK:
210  m_COMname = COM_port;
211 }
212 
213 /* -----------------------------------------------------
214  getSerialPortName
215 ----------------------------------------------------- */
217 {
218  return m_COMname;
219 }
220 
221 /* -----------------------------------------------------
222  tryToOpenTheCOM
223 ----------------------------------------------------- */
225 {
226  // If this is the first use of the COM port, create it:
227  if (!m_data_stream)
228  {
229  m_data_stream = new CSerialPort();
231  }
232 
233  CSerialPort *serial = dynamic_cast<CSerialPort*>(m_data_stream);
234  if (serial)
235  {
237  if (serial->isOpen())
238  return true; // Already open
239 
240  if (m_verbose) cout << "[CGPSInterface] Opening " << m_COMname << " @ " << m_COMbauds << endl;
241 
242  try
243  {
244  serial->open(m_COMname);
245  // Config:
246  serial->setConfig( m_COMbauds, 0, 8, 1 );
247  serial->setTimeouts( 1, 0, 1, 1, 1 );
248 
249  // Do extra initialization?
250  if (! OnConnectionEstablished() )
251  {
252  serial->close();
253  return false;
254  }
255  return true; // All OK
256  }
257  catch (std::exception &e)
258  {
259  std::cerr << "[CGPSInterface::tryToOpenTheCOM] Error opening or configuring serial port:" << std::endl << e.what();
260  serial->close();
261  return false;
262  }
263  } // end of this is a serial port
264 
265  return true; // All OK
266 }
267 
268 /* -----------------------------------------------------
269  isGPS_connected
270 ----------------------------------------------------- */
272 {
273  return m_GPS_comsWork;
274 }
275 
276 /* -----------------------------------------------------
277  doProcess
278 ----------------------------------------------------- */
280 {
281  // Is the COM open?
282  if (!tryToOpenTheCOM()) {
283  m_state = ssError;
284  THROW_EXCEPTION("Could not open the input stream");
285  }
286  ASSERT_(m_data_stream!=NULL)
287  CSerialPort *stream_serial = dynamic_cast<CSerialPort*>(m_data_stream);
288  CClientTCPSocket *stream_tcpip = dynamic_cast<CClientTCPSocket*>(m_data_stream);
289 
290  // Read as many bytes as available:
291  uint8_t buf[0x1000];
292  const size_t to_read=std::min(m_rx_buffer.available()-1,sizeof(buf)-1);
293  try
294  {
295  size_t nRead=0;
296  if (to_read>0)
297  {
299  if (stream_tcpip) {
300  nRead = stream_tcpip->readAsync( buf, to_read, 100, 10 );
301  }
302  else if (stream_serial) {
303  nRead = stream_serial->Read(buf,to_read);
304  }
305  else{
306  nRead = m_data_stream->ReadBuffer(buf,to_read);
307  }
308  }
309 
310  if (nRead) m_rx_buffer.push_many(buf,nRead);
311 
312  // Also dump to raw file:
314  // 1st time open:
316  mrpt::system::timestampToParts(now(), parts, true);
317  string sFilePostfix = "_";
318  sFilePostfix += format("%04u-%02u-%02u_%02uh%02um%02us",(unsigned int)parts.year, (unsigned int)parts.month, (unsigned int)parts.day, (unsigned int)parts.hour, (unsigned int)parts.minute, (unsigned int)parts.second );
319  const string sFileName = m_raw_dump_file_prefix + mrpt::system::fileNameStripInvalidChars( sFilePostfix ) + string(".gps");
320 
321  if (m_verbose) std::cout << "[CGPSInterface] Creating RAW dump file: `" << sFileName << "`\n";
322  m_raw_output_file.open(sFileName);
323  }
324  if (nRead && m_raw_output_file.fileOpenCorrectly()) {
325  m_raw_output_file.WriteBuffer(buf,nRead);
326  }
327  }
328  catch (std::exception &)
329  {
330  // ERROR:
331  MRPT_LOG_ERROR("[CGPSInterface::doProcess] Error reading stream of data: Closing communications\n");
332  if(stream_serial) {
334  stream_serial->close();
335  }
336  m_GPS_comsWork = false;
337  return;
338  }
339 
340  // Try to parse incomming data as messages:
341  parseBuffer( );
342 
343  // Decide whether to push out a new observation in old legacy mode.
344  if (!m_customInit.empty())
345  { // "Advanced" (RTK,mmGPS) device (kept for backwards-compatibility)
346  bool do_append_obs = false;
347  // FAMD
348  // Append observation if:
349  // 0. the timestamp seems to be correct!
350  // 1. it contains both synched GGA and RMC data
351  // 2. it contains only GGA or RMC but the next one is not synched with it
353  {
354  if (m_verbose) cout << "[CGPSInterface] Initial timestamp: " << mrpt::system::timeToString(m_just_parsed_messages.timestamp) << endl;
355  // Check if the initial timestamp seems to be OK (not a spurio one)
356  TTimeStamp tmNow = mrpt::system::now();
357  const double tdif = mrpt::system::timeDifference( m_just_parsed_messages.timestamp, tmNow );
358  if( tdif >= 0 && tdif < 7500 /*Up to two hours*/)
360  else
361  { if (m_verbose) cout << "[CGPSInterface] Warning: The initial timestamp seems to be wrong! : " << tdif << endl;}
362  } // end-if
363  else
364  {
366  if( time_diff < 0 || time_diff > 300 ) // Assert that the current timestamp is after the previous one and not more than 5 minutes later -> remove spurious
367  { if (m_verbose) cout << "[CGPSInterface ] Bad timestamp difference" << endl; return; }
368 
369  if( time_diff-m_topcon_data_period > 0.25*m_topcon_data_period )
370  { if (m_verbose) cout << "[CGPSInterface] WARNING: According to the timestamps, we probably skipped one frame!" << endl; }
371 
372  // a. These GPS data have both synched RMC and GGA data
373  // don't append observation until we have both data
375  } // end-else
376 
377  if (do_append_obs)
379  }
380 
381 }
382 
383 //!< Queue out now the messages in \a m_just_parsed_messages, leaving it empty
385 {
386  // Generic observation data:
391  // Add observation to the output queue:
392  CObservationGPSPtr newObs = CObservationGPS::Create();
393  m_just_parsed_messages.swap(*newObs);
397 
398  // And this means the comms works:
399  m_GPS_comsWork = true;
400  m_state = ssWorking;
401 }
402 
403 /* -----------------------------------------------------
404  parseBuffer
405 ----------------------------------------------------- */
407 {
408  if (m_parser == CGPSInterface::NONE) return; // Dont try to parse data
409 
410  // Only one parser selected?
411  ptr_parser_t parser_ptr = NULL;
412  switch (m_parser)
413  {
416  case CGPSInterface::AUTO: break; // Leave it as NULL
417  default:
418  throw std::runtime_error("[CGPSInterface] Unknown parser!");
419  };
420  if (parser_ptr)
421  {
422  // Use only one parser ----------
423  size_t min_bytes;
424  do
425  {
426  if (!(*this.*parser_ptr)(min_bytes))
427  {
428  m_rx_buffer.pop(); // Not the start of a frame, skip 1 byte
429  }
430  if (m_customInit.empty() /* If we are not in old legacy mode */ && !m_just_parsed_messages.messages.empty() )
432  } while (m_rx_buffer.size()>=min_bytes);
433  } // end one parser mode ----------
434  else
435  {
436  // AUTO mode --------
437  const std::list<CGPSInterface::ptr_parser_t> &all_parsers = TParsersRegistry::getInstance().all_parsers;
438 
439  size_t global_min_bytes_max=0;
440  do
441  {
442  bool all_parsers_want_to_skip = true;
443  for (std::list<CGPSInterface::ptr_parser_t>::const_iterator it=all_parsers.begin();it!=all_parsers.end();++it)
444  {
445  parser_ptr = *it;
446  size_t this_parser_min_bytes;
447  if ((*this.*parser_ptr)(this_parser_min_bytes))
448  all_parsers_want_to_skip = false;
449  mrpt::utils::keep_max(global_min_bytes_max, this_parser_min_bytes);
450  }
451 
452  if (all_parsers_want_to_skip)
453  m_rx_buffer.pop(); // Not the start of a frame, skip 1 byte
454 
455  if (m_customInit.empty() /* If we are not in old legacy mode */ && !m_just_parsed_messages.messages.empty() )
457  } while (m_rx_buffer.size()>=global_min_bytes_max);
458  } // end AUTO mode ----
459 }
460 
461 /* -----------------------------------------------------
462  JAVAD_sendMessage
463 ----------------------------------------------------- */
464 void CGPSInterface::JAVAD_sendMessage(const char *str, bool waitForAnswer )
465 {
466  if (!str) return;
467  const size_t len = strlen(str);
468  CSerialPort *stream_serial = dynamic_cast<CSerialPort*>(m_data_stream);
469  if (!stream_serial) return;
470 
471  size_t written;
472 
473  {
475  written = stream_serial->Write(str,len);
476  }
477 
478  if (m_verbose)
479  std::cout << "[CGPSInterface] TX: " << str;
480 
481  if (written != len )
482  throw std::runtime_error(format("Error sending command: '%s'",str).c_str());
484 
485  if (!waitForAnswer) return;
486 
487  mrpt::system::sleep(200);
488  char buf[200];
489  buf[0]='\0';
490 
491  int bad_counter = 0;
492  while(bad_counter < 10)
493  {
494  size_t nRead;
495  {
497  written = stream_serial->Write(str,len);
498  nRead = stream_serial->Read(buf,sizeof(buf));
499  }
500 
501  if (m_verbose)
502  std::cout << "[CGPSInterface] RX: " << buf << std::endl;
503 
504  if (nRead<3 )
505  throw std::runtime_error(format("ERROR: Invalid response '%s' for command '%s'",buf,str));
506 
507  if (nRead>=3 && buf[0]=='R' && buf[1]=='E')
508  return; // Ok!
509  else
510  ++bad_counter;
511  }
512  throw std::runtime_error(format("ERROR: Invalid response '%s' for command '%s'",buf,str));
513 }
514 
516 {
517  CSerialPort *stream_serial = dynamic_cast<CSerialPort*>(m_data_stream);
518 
519  if (stream_serial && !stream_serial->isOpen())
520  return false;
521 
522  // Send commands:
523  for (size_t i=0;i<m_shutdown_cmds.size();i++)
524  {
525  if (m_verbose)
526  cout << "[CGPSInterface] TX shutdown command: `" << m_shutdown_cmds[i] << "`\n";
527 
528  std::string sTx = m_shutdown_cmds[i];
530  sTx+=std::string("\r\n");
531  try
532  {
534  m_data_stream->WriteBuffer(&sTx[0],sTx.size());
535  } catch (...) {
536  return false; // On any I/O error
537  }
538 
540  }
541  return true;
542 }
543 
544 /* -----------------------------------------------------
545  OnConnectionEstablished
546 ----------------------------------------------------- */
548 {
549  m_last_GGA.clear(); // On comms reset, empty this cache
551 
552  // Legacy behavior:
553  if ( !os::_strcmpi( m_customInit.c_str(), "JAVAD" ) || !os::_strcmpi( m_customInit.c_str(), "TOPCON" ) ) {
555  }
556 
557  // Purge input:
558  CSerialPort *stream_serial = dynamic_cast<CSerialPort*>(m_data_stream);
559  if (stream_serial)
560  {
562  stream_serial->purgeBuffers();
563  }
564 
565  // New behavior: Send generic commands set-up by the user in the config file.
566 
567  // Send commands:
568  for (size_t i=0;i<m_setup_cmds.size();i++)
569  {
570  if (m_verbose)
571  cout << "[CGPSInterface] TX setup command: `" << m_setup_cmds[i] << "`\n";
572 
573  std::string sTx = m_setup_cmds[i];
575  sTx+=std::string("\r\n");
576 
577  try {
579  m_data_stream->WriteBuffer(&sTx[0],sTx.size());
580  } catch (std::exception &e) {
581  std::cerr << "[CGPSInterface::OnConnectionEstablished] Error sending setup cmds: " << e.what() << std::endl;
582  return false;
583  }
585  }
587  return true;
588 }
589 
591 {
592  MRPT_START
593  if ( !os::_strcmpi( m_customInit.c_str(), "JAVAD" ) || !os::_strcmpi( m_customInit.c_str(), "TOPCON" ) )
594  {
595  // Stop messaging:
596  JAVAD_sendMessage("%%dm\r\n", false);
597  mrpt::system::sleep(500);
598  JAVAD_sendMessage("%%dm\r\n",false);
599  mrpt::system::sleep(1000);
600 
601  // Purge input:
602  CSerialPort *stream_serial = dynamic_cast<CSerialPort*>(m_data_stream);
603  if (stream_serial)
604  {
606  stream_serial->purgeBuffers();
607  }
608 
609  JAVAD_sendMessage("%%set,/par/cur/term/imode,cmd\r\n"); // set the current port in command mode
610  return true;
611  }
612  else
613  return true;
614  MRPT_END
615 } // end-unsetJAVAD_AIM_mode
616 
618 {
619  MRPT_START
620  if ( !os::_strcmpi( m_customInit.c_str(), "JAVAD" ) || !os::_strcmpi( m_customInit.c_str(), "TOPCON" ) )
621  {
622  JAVAD_sendMessage(format("%%%%set,/par%s/imode,cmd\r\n",m_JAVAD_rtk_src_port.c_str()).c_str()); // set the port in command mode
623  JAVAD_sendMessage("%%set,/par/cur/term/jps/0,{nscmd,37,n,\"\"}\r\n"); // any command starting with % will be treated as normal
624 
625  ASSERT_(!m_JAVAD_rtk_format.empty())
626  cout << "Formato de correcciones para GR3: " << m_JAVAD_rtk_format << endl;
627  if( m_JAVAD_rtk_format == "cmr" )
628  {
629  JAVAD_sendMessage(format("%%%%set,/par/cur/term/jps/1,{cmr,-1,y,%s}\r\n", m_JAVAD_rtk_src_port.c_str()).c_str()); // set corrections type CMR or CMR+
630  JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
631  JAVAD_sendMessage(format("%%%%set,/par%s/imode,cmr\r\n", m_JAVAD_rtk_src_port.c_str()).c_str());
632  }
633  else if( m_JAVAD_rtk_format == "rtcm" )
634  {
635  JAVAD_sendMessage(format("%%%%set,/par/cur/term/jps/1,{rtcm,-1,y,%s}\r\n", m_JAVAD_rtk_src_port.c_str()).c_str()); // set corrections type RTCM
636  JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
637  JAVAD_sendMessage(format("%%%%set,/par%s/imode,rtcm\r\n", m_JAVAD_rtk_src_port.c_str()).c_str());
638  }
639  else if( m_JAVAD_rtk_format == "rtcm3" )
640  {
641  JAVAD_sendMessage(format("%%%%set,/par/cur/term/jps/1,{rtcm3,-1,y,%s}\r\n", m_JAVAD_rtk_src_port.c_str()).c_str()); // set corrections type RTCM 3.x
642  JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
643  JAVAD_sendMessage(format("%%%%set,/par%s/imode,rtcm3\r\n", m_JAVAD_rtk_src_port.c_str()).c_str());
644  }
645  else
646  {
647  cout << "Unknown RTK corrections format. Only supported: CMR, RTCM or RTCM3" << endl;
648  return false;
649  }
650  JAVAD_sendMessage("%%set,/par/cur/term/imode,jps\r\n"); // sets current port into "JPS" mode
651 
652  return true;
653 
654  } // end-if
655  else
656  return true;
657  MRPT_END
658 } // end-setJAVAD_AIM_mode
659 
661 {
662  std::string ret = m_last_GGA;
663  if (reset) m_last_GGA.clear();
664  return ret;
665 }
666 
668 {
669  // Stop messaging:
670  JAVAD_sendMessage("%%dm\r\n", false);
671  mrpt::system::sleep(500);
672  JAVAD_sendMessage("%%dm\r\n",false);
673  mrpt::system::sleep(1000);
674 
675  // Purge input:
676  CSerialPort *stream_serial = dynamic_cast<CSerialPort*>(m_data_stream);
677  if (stream_serial)
678  {
680  stream_serial->purgeBuffers();
681  }
682 
683  // Configure RTK mode and source:
684  if (m_verbose)
685  cout << "[CGPSInterface] Configure RTK options" << endl;
686 
687  if (!m_JAVAD_rtk_src_port.empty())
688  {
689  const int elevation_mask = 5; // Degs
690 
691  JAVAD_sendMessage(format("%%%%set,/par/lock/elm,%i\r\n", elevation_mask).c_str()); // Set elevation mask to track satellites
692  JAVAD_sendMessage("%%set,/par/base/mode/,off\r\n"); // Set Base Mode off
693  JAVAD_sendMessage("%%set,/par/pos/pd/period,1.0\r\n"); // Differential Correction Interval
694  //JAVAD_sendMessage("%%set,hd/mode,off\r\n"); // fixed distance to rtk base: Off
695  //JAVAD_sendMessage("%%set,/par/pos/pd/hd/mode,off\r\n"); // fixed distance to rtk base: Off <-- Not working with TopCon GR3! (option disabled)
696  JAVAD_sendMessage("%%set,/par/pos/pd/qcheck,off\r\n"); // Set Quality Checks Off
697  JAVAD_sendMessage("%%set,/par/pos/mode/cur,pd\r\n"); // Pos Mode Phase Diff
698  JAVAD_sendMessage("%%set,/par/pos/pd/textr,10\r\n"); // RTK Extrapolation Limit
699  JAVAD_sendMessage("%%set,/par/pos/pd/inuse,-1\r\n"); // Set Rovers Reference Station
700  JAVAD_sendMessage("%%set,/par/pos/pd/nrs/mode,y\r\n"); // Enable Nearest Reference Station Mode
701  JAVAD_sendMessage("%%set,/par/pos/pd/mode,extrap\r\n");// Enable EXTRAPOLATED mode in RTK corrections
702 
703  // Set Differential Correction Source
704  JAVAD_sendMessage(format("%%%%set,/par/pos/pd/port,%s\r\n",m_JAVAD_rtk_src_port.c_str()).c_str());
705 
706  // Set port bauds:
708  JAVAD_sendMessage(format("%%%%set,/par%s/rate,%u\r\n",m_JAVAD_rtk_src_port.c_str(), m_JAVAD_rtk_src_baud).c_str());
709 
710  // Set Input Mode: CMR,RTCM,...
711  if( ! m_topcon_useAIMMode && !m_JAVAD_rtk_format.empty())
712  JAVAD_sendMessage(format("%%%%set,/par%s/imode,%s\r\n", m_JAVAD_rtk_src_port.c_str(), m_JAVAD_rtk_format.c_str()).c_str());
713  }
714 
715  // Start NMEA messaging:
716 // JAVAD_sendMessage("%%em,,/msg/nmea/GGA:0.2\r\n");
717 // JAVAD_sendMessage("%%em,,/msg/nmea/RMC:0.2\r\n");
718  //JAVAD_sendMessage("%%em,,/msg/jps/PS:0.2\r\n");
719 
720  if( m_topcon_useAIMMode )
721  {
722  if (m_verbose) cout << "[CGPSInterface] Using Advanced Input Mode";
724  if (m_verbose) cout << "... done" << endl;
725  }
726  JAVAD_sendMessage(format("%%%%em,,/msg/nmea/GGA:%.1f\r\n", m_topcon_data_period ).c_str());
727  JAVAD_sendMessage(format("%%%%em,,/msg/nmea/RMC:%.1f\r\n", m_topcon_data_period ).c_str()); // FAMD: 10 Hz
728 
729  if( m_topcon_useAIMMode )
730  { if (m_verbose) cout << "[CGPSInterface::OnConnectionEstablished] JAVAD/TopCon commands sent successfully with AIM." << endl;}
731  else
732  { if (m_verbose) cout << "[CGPSInterface::OnConnectionEstablished] JAVAD/TopCon commands sent successfully." << endl;}
733 
734  return true;
735 }
736 
737 /** Send a custom data block to the GNSS device right now. Can be used to change its behavior online as needed. */
738 bool CGPSInterface::sendCustomCommand(const void* data, const size_t datalen)
739 {
740  try
741  {
743  m_data_stream->WriteBuffer(data,datalen);
744  return true;
745  }
746  catch (std::exception &e)
747  {
748  std::cerr << "[CGPSInterface::sendCustomCommand] Error sending cmd: " << e.what() << std::endl;
749  return false;
750  }
751 }
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
bool fileOpenCorrectly()
Returns true if the file was open without errors.
bool(CGPSInterface::* ptr_parser_t)(size_t &out_minimum_rx_buf_to_decide)
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer On any error, or if ZERO bytes are read...
Definition: CStream.cpp:45
void BASE_IMPEXP 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:101
static const TParsersRegistry & getInstance()
This class provides simple critical sections functionality.
mrpt::utils::CStream * m_data_stream
Typically a CSerialPort created by this class, but may be set externally.
PARSERS
Read about parser selection in the documentation for CGPSInterface.
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
A class capable of reading GPS/GNSS/GNSS+IMU receiver data, from a serial port or from any input stre...
void JAVAD_sendMessage(const char *str, bool waitForAnswer=true)
Private auxiliary method. Raises exception on error.
void doProcess()
Queue out now the messages in m_just_parsed_messages, leaving it empty.
#define min(a, b)
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...
A communications serial port built as an implementation of a utils::CStream.
Definition: CSerialPort.h:43
void setTimeouts(int ReadIntervalTimeout, int ReadTotalTimeoutMultiplier, int ReadTotalTimeoutConstant, int WriteTotalTimeoutMultiplier, int WriteTotalTimeoutConstant)
Changes the timeouts of the port, in milliseconds.
bool isGPS_connected()
Returns true if communications work, i.e. if some message has been received.
mrpt::obs::CObservationGPS m_just_parsed_messages
A private copy of the last received gps datum.
bool OnConnectionShutdown()
Like OnConnectionEstablished() for sending optional shutdown commands.
std::string m_sensorLabel
See CGenericSensor.
#define THROW_EXCEPTION(msg)
void swap(CObservationGPS &o)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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. ...
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CStream.cpp:67
bool m_topcon_AIMConfigured
Indicates if the AIM has been properly set up.
bool open(const std::string &fileName, bool append=false)
Open the given file for write.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
bool BASE_IMPEXP strCmp(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case sensitive)
void enableSetupCommandsAppendCRLF(const bool enable)
size_t Write(const void *Buffer, size_t Count)
Implements the virtual method responsible for writing to the stream.
std::string BASE_IMPEXP timeToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (UTC): HH:MM:SS.MMMMMM.
Definition: datetime.cpp:347
size_t size() const
Return the number of elements available for read ("pop") in the buffer (this is NOT the maximum size ...
void parseBuffer()
Process data in "m_buffer" to extract GPS messages, and remove them from the buffer.
bool sendCustomCommand(const void *data, const size_t datalen)
Send a custom data block to the GNSS device right now.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
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,...". Example value: "/dev/ser/b".
std::list< CGPSInterface::ptr_parser_t > all_parsers
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.
This class allows loading and storing values and vectors of different types from a configuration text...
const std::vector< std::string > & getShutdownCommands() const
unsigned char uint8_t
Definition: rptypes.h:43
bool isEnabledSetupCommandsAppendCRLF() const
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
#define MRPT_END
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)
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
void setConfig(int baudRate, int parity=0, int bits=8, int nStopBits=1, bool enableFlowControl=false)
Changes the configuration of the port.
The parts of a date/time (it's like the standard 'tm' but with fractions of seconds).
Definition: datetime.h:35
GLenum GLsizei len
Definition: glew.h:3955
int const JOCTET unsigned int datalen
Definition: mrpt_jpeglib.h:947
void setSetupCommands(const std::vector< std::string > &cmds)
uint8_t day
Month (1-12)
Definition: datetime.h:39
void setSetupCommandsDelay(const double delay_secs)
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
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)
mrpt::utils::circular_buffer< uint8_t > m_rx_buffer
Auxiliary buffer for readings.
GLint GLenum GLsizei GLint GLsizei const GLvoid * data
Definition: glew.h:1284
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
bool OnConnectionEstablished()
Implements custom messages to be sent to the GPS unit just after connection and before normal use...
size_t available() const
The maximum number of elements that can be written ("push") without rising an overflow error...
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
const std::vector< std::string > & getSetupCommands() const
GLboolean reset
Definition: glew.h:2897
std::vector< std::string > m_setup_cmds
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
void close()
Close the port.
T pop()
Retrieve an element from the buffer.
void clear()
Empties this observation, clearing the container messages.
std::string getSerialPortName() const
Get the serial port to use (COM1, ttyUSB0, etc).
double second
Minute (0-59)
Definition: datetime.h:42
std::string BASE_IMPEXP fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars= '_')
Replace invalid filename chars by underscores ('_') or any other user-given char. ...
Definition: filesystem.cpp:315
#define MRPT_START
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
bool isOpen() const
Returns if port has been correctly open.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
uint8_t minute
Hour (0-23)
Definition: datetime.h:41
GLsizei const GLcharARB ** string
Definition: glew.h:3293
void purgeBuffers()
Purge tx and rx buffers.
void open()
Open the port.
Definition: CSerialPort.cpp:87
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.
void bindStream(mrpt::utils::CStream *external_stream, mrpt::synch::CCriticalSection *csOptionalExternalStream=NULL)
This enforces the use of a given user stream, instead of trying to open the serial port set in this c...
int BASE_IMPEXP _strcmpi(const char *str1, const char *str2) MRPT_NO_THROWS
An OS-independent version of strcmpi.
Definition: os.cpp:320
bool setJAVAD_AIM_mode()
Set Advanced Input Mode for the primary port.
mrpt::utils::CFileOutputStream m_raw_output_file
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:254
uint8_t month
The year.
Definition: datetime.h:38
#define ASSERT_(f)
void loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
void appendObservation(const mrpt::utils::CSerializablePtr &obj)
Like appendObservations() but for just one observation.
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 BASE_IMPEXP trim(const std::string &str)
Removes leading and trailing spaces.
uint8_t hour
Day (1-31)
Definition: datetime.h:40
A TCP socket that can be connected to a TCP server, implementing MRPT's CStream interface for passing...
#define MRPT_LOG_ERROR(_STRING)
double m_topcon_data_period
The period in seconds which the data should be provided by the GPS.
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
double BASE_IMPEXP 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.cpp:205
mrpt::system::TTimeStamp m_last_timestamp
mrpt::synch::CCriticalSection * m_data_stream_cs
std::vector< std::string > m_shutdown_cmds
void push_many(T *array_elements, size_t count)
Insert an array of elements in the buffer.
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.
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...
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.
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.
void setShutdownCommands(const std::vector< std::string > &cmds)



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018