Debian Codegraph @

libs/hwdrivers/src/CGPSInterface.cpp

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