Debian Codegraph @

libs/hwdrivers/src/CCANBusReader.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 // This file contains portions of code from sicklms200.cc from the Player/Stage
13 // project.
14
15 #include <mrpt/hwdrivers/CCANBusReader.h>
16 #include <mrpt/system/CTicTac.h>
17 #include <mrpt/system/crc.h>
18 #include <mrpt/system/os.h>
19
20 #include <chrono>
21 #include <cstdio> // printf
22 #include <cstring> // memset
23 #include <iostream>
24 #include <thread>
25
26 IMPLEMENTS_GENERIC_SENSOR(CCANBusReader, mrpt::hwdrivers)
27
28 #define RET_ERROR(msg) \
29 { \
30 cout << "[" << __CURRENT_FUNCTION_NAME__ << "] " << msg << endl; \
31 return false; \
32 }
33
34 using namespace std;
35 using namespace mrpt;
36 using namespace mrpt::obs;
37 using namespace mrpt::hwdrivers;
38 using namespace mrpt::system;
39
40 char hexCharToInt(char n)
41 {
42 if (n >= '0' && n <= '9')
43 return (n - '0');
44 else
45
46 if (n >= 'A' && n <= 'F')
47 return (n - 'A' + 10);
48 else
49 return 0;
50 }
51
52 /*-------------------------------------------------------------
53 CCANBusReader
54 -------------------------------------------------------------*/
55 CCANBusReader::CCANBusReader() : mrpt::system::COutputLogger("CCANBusReader"), m_com_port()
56
57 {
58 m_sensorLabel = "CANBusReader";
59 memset(m_received_frame_buffer, 0, sizeof(m_received_frame_buffer));
60 }
61
62 /*-------------------------------------------------------------
63 ~CCANBusReader
64 -------------------------------------------------------------*/
65 CCANBusReader::~CCANBusReader()
66 {
67 if (m_CANBusChannel_isOpen)
68 {
69 try
70 {
71 CANBusCloseChannel();
72 }
73 catch (...)
74 {
75 }
76 }
77
78 if (m_mySerialPort)
79 {
80 delete m_mySerialPort;
81 m_mySerialPort = nullptr;
82 }
83 }
84
85 void CCANBusReader::doProcess()
86 {
87 mrpt::obs::CObservationCANBusJ1939::Ptr obs = mrpt::obs::CObservationCANBusJ1939::Create();
88 bool thereIsObservation;
89 bool hardwareError;
90
91 doProcessSimple(thereIsObservation, *obs, hardwareError);
92 if (thereIsObservation)
93 appendObservation(obs);
94 else
95 cout << "No frame received" << endl;
96 }
97
98 /*-------------------------------------------------------------
99 doProcess
100 -------------------------------------------------------------*/
101 void CCANBusReader::doProcessSimple(
102 bool& outThereIsObservation,
103 mrpt::obs::CObservationCANBusJ1939& outObservation,
104 bool& hardwareError)
105 {
106 outThereIsObservation = false;
107 hardwareError = false;
108
109 if (!tryToOpenComms())
110 {
111 hardwareError = true;
112 return;
113 }
114
115 m_state = ssWorking;
116
117 // Wait for a scan:
118 uint8_t out_prio, out_pdu_format, out_pdu_spec, out_src_address, out_data_length;
119 uint16_t out_pgn;
120 vector<uint8_t> out_data;
121 vector<char> out_raw_frame;
122 if (!waitContinuousSampleFrame(
123 out_prio, out_pdu_format, out_pdu_spec, out_src_address, out_data_length, out_pgn,
124 out_data, out_raw_frame))
125 return;
126
127 // Yes, we have a new scan:
128 // cout << "we've got a frame" << endl;
129 // -----------------------------------------------
130 // Extract the observation:
131 // -----------------------------------------------
132 outObservation.timestamp = mrpt::Clock::now();
133 outObservation.sensorLabel = m_sensorLabel; // Set label
134
135 // And the scan ranges:
136 outObservation.m_priority = out_prio;
137 outObservation.m_pdu_spec = out_pdu_spec;
138 outObservation.m_pdu_format = out_pdu_format;
139 outObservation.m_src_address = out_src_address;
140 outObservation.m_pgn = out_pgn;
141 outObservation.m_data_length = out_data_length;
142 outObservation.m_data.resize(out_data.size());
143 for (size_t k = 0; k < out_data.size(); ++k) outObservation.m_data[k] = out_data[k];
144 outObservation.m_raw_frame.resize(out_raw_frame.size());
145 for (size_t k = 0; k < out_raw_frame.size(); ++k)
146 outObservation.m_raw_frame[k] = out_raw_frame[k];
147
148 // we've got a new observation
149 outThereIsObservation = true;
150 }
151
152 /*-------------------------------------------------------------
153 loadConfig_sensorSpecific
154 -------------------------------------------------------------*/
155 void CCANBusReader::loadConfig_sensorSpecific(
156 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
157 {
158 m_canbus_speed = configSource.read_int(iniSection, "CANBusSpeed", m_canbus_speed);
159 m_canreader_timestamp =
160 configSource.read_bool(iniSection, "useCANReaderTimestamp", m_canreader_timestamp);
161
162 #ifdef _WIN32
163 m_com_port = configSource.read_string(iniSection, "COM_port_WIN", m_com_port, true);
164 #else
165 m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port, true);
166 #endif
167
168 m_com_baudRate = configSource.read_int(iniSection, "COM_baudRate", m_com_baudRate);
169 m_nTries_connect = configSource.read_int(iniSection, "nTries_connect", m_nTries_connect);
170 }
171
172 /*-------------------------------------------------------------
173 Tries to open the com port and setup
174 all the LMS protocol. Returns true if OK or already open.
175 -------------------------------------------------------------*/
176 bool CCANBusReader::tryToOpenComms(std::string* err_msg)
177 {
178 if (err_msg) *err_msg = "";
179 try
180 {
181 if (!m_mySerialPort)
182 {
183 // There is no COMMS port open yet...
184 if (!m_com_port.empty())
185 {
186 // cout << "Creating port" << endl;
187 m_mySerialPort = new mrpt::comms::CSerialPort(); // Create the port myself:
188 }
189 else
190 throw std::logic_error(
191 "ERROR: No serial port attached with bindIO, neither it "
192 "set with 'setSerialPort'");
193 }
194
195 // We assure now we have a stream... try to open it, if it's not done
196 // yet.
197 bool just_open = false;
198 if (m_mySerialPort != nullptr)
199 {
200 if (!m_mySerialPort->isOpen())
201 {
202 // Try to open it now:
203 m_mySerialPort->setSerialPortName(m_com_port);
204 m_mySerialPort->open(); // will raise an exception on error.
205
206 // Set basic params:
207 m_mySerialPort->setConfig(9600);
208 m_mySerialPort->setTimeouts(100, 0, 10, 0, 50);
209
210 just_open = true;
211 }
212 }
213
214 // It seems the port was open and working so we are done here.
215 if (!just_open) return true;
216
217 // ==================================================================
218 // Otherwise, it was just opened now, we must send the
219 // ** initialization commands **
220 // and put the CAN Converter in recording mode:
221 // ==================================================================
222 cout << "Setting up serial comms in port " << m_com_port;
223 if (!setupSerialComms()) RET_ERROR("error");
224 cout << " ... done" << endl;
225
226 // initialize
227 // set CAN Bus speed
228 /**/
229 bool res;
230 cout << "Setting up CAN BUS Speed at: " << m_canbus_speed << endl;
231 for (int nTry = 0; nTry < 250000 /*4*/; nTry++)
232 if (true == (res = sendCANBusReaderSpeed())) break;
233 if (!res) return false;
234 cout << " ... done" << endl;
235
236 // open the CAN channel. If true, at this point, frames should be poping
237 // out the CAN Bus
238 cout << "Opening CAN BUS and starting to receive." << endl;
239 for (int nTry = 0; nTry < 250000 /*4*/; nTry++)
240 if (true == (res = CANBusOpenChannel())) break;
241 if (!res) return false;
242 cout << " ... done" << endl;
243
244 // cout << "Autopoll" << endl;
245 // for (int nTry=0;nTry<250000/*4*/;nTry++)
246 // if (true==(res=CANBusAutoPoll()))
247 // break;
248 // if(!res) return false;
249 // cout << " ... done" << endl;
250
251 return res;
252 /**/
253 }
254 catch (const std::exception& e)
255 {
256 std::string s = "[CCANBusReader] Error trying to open CANBusReader at port ";
257 s += e.what();
258 if (err_msg) *err_msg = s;
259 MRPT_LOG_ERROR_STREAM(s);
260 return false;
261 }
262 }
263
264 /*-------------------------------------------------------------
265 Tries to send the command to set up the speed of the
266 CAN Bus reader -> tractor link
267 -------------------------------------------------------------*/
268 bool CCANBusReader::sendCANBusReaderSpeed()
269 {
270 // command: S0 --> S8 according to the selected speed
271 unsigned char cmd[2];
272
273 cmd[0] = 'S';
274 switch (m_canbus_speed)
275 {
276 case 10000:
277 cmd[1] = '0';
278 break;
279 case 20000:
280 cmd[1] = '1';
281 break;
282 case 50000:
283 cmd[1] = '2';
284 break;
285 case 100000:
286 cmd[1] = '3';
287 break;
288 case 125000:
289 cmd[1] = '4';
290 break;
291 case 250000:
292 cmd[1] = '5';
293 break;
294 case 500000:
295 cmd[1] = '6';
296 break;
297 case 800000:
298 cmd[1] = '7';
299 break;
300 case 1000000:
301 cmd[1] = '8';
302 break;
303 default:
304 RET_ERROR("Incorrect CAN Bus speed");
305 break;
306 }
307 sendCommandToCANReader(cmd, 2);
308 return waitACK(50);
309 }
310
311 bool CCANBusReader::CANBusOpenChannel()
312 {
313 unsigned char cmd[1];
314 cmd[0] = 'O';
315 sendCommandToCANReader(cmd, 1);
316 m_CANBusChannel_isOpen = waitACK(50);
317 return m_CANBusChannel_isOpen;
318 }
319
320 bool CCANBusReader::CANBusCloseChannel()
321 {
322 unsigned char cmd[1];
323 cmd[0] = 'C';
324 sendCommandToCANReader(cmd, 1);
325 // m_CANBusChannel_isOpen = !waitACK(50);
326 m_CANBusChannel_isOpen = false;
327 // return !m_CANBusChannel_isOpen;
328 return true;
329 }
330
331 bool CCANBusReader::CANBusAutoPoll()
332 {
333 unsigned char cmd[1];
334 cmd[0] = 'A';
335 sendCommandToCANReader(cmd, 1);
336 return waitACK(50);
337 }
338
339 bool CCANBusReader::CANBusX1()
340 {
341 unsigned char cmd[2];
342 cmd[0] = 'X';
343 cmd[1] = '1';
344 sendCommandToCANReader(cmd, 2);
345 return waitACK(50);
346 }
347
348 bool CCANBusReader::CANBusPoll()
349 {
350 unsigned char cmd[1];
351 cmd[0] = 'P';
352 sendCommandToCANReader(cmd, 1);
353 return waitACK(50);
354 }
355
356 /*-------------------------------------------------------------
357 waitContinuousSampleFrame
358 -------------------------------------------------------------*/
359 bool CCANBusReader::waitContinuousSampleFrame(
360 uint8_t& out_prio,
361 uint8_t& out_pdu_format,
362 uint8_t& out_pdu_spec,
363 uint8_t& out_src_address,
364 uint8_t& out_data_length,
365 uint16_t& out_pgn,
366 vector<uint8_t>& out_data,
367 vector<char>& out_raw_frame)
368 {
369 size_t nRead, nBytesToRead;
370 size_t nFrameBytes = 0;
371 size_t lengthField;
372 unsigned char buf[40];
373
374 // clear buffer
375 for (unsigned char& k : buf) k = 0;
376
377 uint8_t dlc = 0;
378 while (nFrameBytes < (lengthField = (10U + dlc + 1U)))
379 {
380 // cout << "trying to receive" << endl;
381 if (lengthField > 30)
382 {
383 cout << "#" << int(dlc) << " ";
384 nFrameBytes = 0; // No es cabecera de trama correcta
385 for (unsigned char& k : buf) k = 0;
386 dlc = 0;
387 }
388
389 if (nFrameBytes < 10)
390 nBytesToRead = 1;
391 else
392 {
393 dlc = 2 * uint8_t(hexCharToInt(buf[9]));
394 // cout << "dlc: " << int(dlc) << endl;
395 nBytesToRead = (lengthField)-nFrameBytes;
396 }
397
398 try
399 {
400 nRead = m_mySerialPort->Read(buf + nFrameBytes, nBytesToRead);
401 // cout << "to read: " << nBytesToRead << " received: " <<
402 // nRead << " -> ";
403 // for( uint8_t k = 0; k < nRead; ++k )
404 // cout << int(buf[k+nFrameBytes]);
405 // cout << endl;
406 }
407 catch (const std::exception& e)
408 {
409 // Disconnected?
410 MRPT_LOG_ERROR_STREAM(
411 "[waitContinuousSampleFrame] Disconnecting due to comms error: " << e.what());
412 return false;
413 }
414
415 if (!nRead) return false;
416
417 if (nRead < nBytesToRead) std::this_thread::sleep_for(30ms);
418
419 // Reading OK:
420 // Was it the first one?
421 if (nFrameBytes > 0 || (nFrameBytes == 0 && buf[0] == 0x54 /*T*/))
422 nFrameBytes += nRead;
423 else
424 {
425 nFrameBytes = 0; // No es cabecera de trama correcta
426 for (unsigned char& k : buf) k = 0;
427 }
428 } // end while
429
430 // Process frame
431 // convert ASCII text into integer
432 vector<uint8_t> aux;
433 out_raw_frame.resize(nFrameBytes);
434 for (uint8_t k = 0; k < nFrameBytes; ++k)
435 {
436 aux.push_back(hexCharToInt(buf[k]));
437 out_raw_frame[k] = buf[k];
438 }
439
440 out_prio = (aux[1] << 2) | (aux[2] >> 2);
441 out_pdu_format = (aux[3] << 4) | aux[4];
442 out_pdu_spec = (aux[5] << 4) | aux[6];
443 out_src_address = (aux[7] << 4) | aux[8];
444 out_data_length = aux[9];
445 out_pgn = uint16_t(out_pdu_format) << 8 | uint16_t(out_pdu_spec);
446 out_data.resize(out_data_length);
447 for (uint8_t k = 0, k2 = 0; k < 2 * out_data_length; k += 2, k2++)
448 out_data[k2] = (aux[10 + k] << 4) | aux[11 + k];
449
450 if (buf[nFrameBytes - 1] != 0x0D)
451 {
452 cout << mrpt::format(
453 "[CCANBusReader::waitContinuousSampleFrame] expected 0x0D "
454 "ending flag, 0x%X found instead",
455 buf[nFrameBytes])
456 << endl;
457 return false; // Bad ending flag
458 }
459
460 // All OK
461 return true;
462 }
463
464 /*-------------------------------------------------------------
465 initialize
466 -------------------------------------------------------------*/
467 void CCANBusReader::initialize()
468 {
469 string err_str;
470 memset(m_received_frame_buffer, 0, sizeof(m_received_frame_buffer));
471 if (!tryToOpenComms(&err_str))
472 {
473 cerr << err_str << endl;
474 throw std::logic_error(err_str);
475 }
476 }
477
478 /*-----------------------------------------------------------------
479 Assures laser is connected and operating at 38400, in
480 its case returns true.
481 -----------------------------------------------------------------*/
482 bool CCANBusReader::setupSerialComms()
483 {
484 ASSERT_(
485 m_com_baudRate == 9600 || m_com_baudRate == 38400 || m_com_baudRate == 57600 ||
486 m_com_baudRate == 500000);
487
488 if (m_mySerialPort == nullptr) return true;
489
490 int detected_rate = 0;
491 for (size_t reps = 0; !detected_rate && reps < m_nTries_connect; reps++)
492 {
493 m_nTries_current = reps;
494
495 int rates[] = {0, 9600, 38400, 57600, 500000};
496
497 // Try first the desired rate to speed up the process, just in case
498 // the converter is already setup from a previous run:
499 rates[0] = m_com_baudRate;
500
501 detected_rate = 0;
502
503 for (size_t i = 0; !detected_rate && i < sizeof(rates) / sizeof(rates[0]); i++)
504 {
505 // Are we already receiving at 500k?
506 // ------------------------------------------------
507 m_mySerialPort->setConfig(rates[i]);
508 std::this_thread::sleep_for(100ms);
509 m_mySerialPort->purgeBuffers();
510
511 // close the connection
512 /**/
513 cout << endl << "Closing CAN Channel " << endl;
514 for (int nTry = 0; nTry < 250000 /*4*/; nTry++)
515 if (true == CANBusCloseChannel()) break;
516 cout << " ... done" << endl;
517 /**/
518
519 std::this_thread::sleep_for(100ms);
520 m_mySerialPort->purgeBuffers();
521
522 for (int nTry = 0; nTry < 250000 /*4*/ && !detected_rate; nTry++)
523 {
524 m_mySerialPort->purgeBuffers();
525
526 // Ask for the laser version at the current rate:
527 if (queryVersion(true))
528 {
529 detected_rate = rates[i];
530 break;
531 }
532 std::this_thread::sleep_for(20ms);
533 } // for tries
534 // There is no link, or the baudrate is wrong...
535 }
536
537 // Try again in a while:
538 if (!detected_rate && reps != (m_nTries_connect - 1)) std::this_thread::sleep_for(5000ms);
539 }
540
541 // Switch "this" serial port to the detected baudrate
542 setBaudRate(detected_rate);
543
544 m_mySerialPort->setConfig(m_com_baudRate);
545 m_mySerialPort->purgeBuffers();
546
547 // Wait...
548 std::this_thread::sleep_for(500ms);
549
550 // And check comms at the new baud rate:
551 return true;
552 }
553
554 /*-----------------------------------------------------------------
555 Query to LMS a status query.
556 Returns true if response is read ok.
557 -----------------------------------------------------------------*/
558 bool CCANBusReader::queryVersion(bool printOutVersion)
559 {
560 ASSERT_(m_mySerialPort);
561
562 uint8_t cmd[1];
563 cmd[0] = 'V';
564 uint16_t cmd_len = 1;
565
566 if (!sendCommandToCANReader(cmd, cmd_len, false)) return false;
567 return waitForVersion(500, printOutVersion);
568 }
569
570 // Returns false if timeout
571 bool CCANBusReader::waitACK(uint16_t timeout_ms)
572 {
573 uint8_t b = 0;
574 CTicTac tictac;
575 tictac.Tic();
576
577 do
578 {
579 if (m_mySerialPort->Read(&b, 1))
580 {
581 // Byte rx:
582 if (b == 0x0D /*0x30*/)
583 {
584 cout << int(b) << endl;
585 return true; // [CR]
586 }
587 }
588 } while (tictac.Tac() < timeout_ms * 1e-3);
589
590 if (b == 0x07) // [BELL]
591 RET_ERROR("ERROR received.")
592 else if (b != 0)
593 RET_ERROR(format("Unexpected code received: 0x%02X", b))
594 else
595 return false; // RET_ERROR("Timeout")
596 }
597
598 bool CCANBusReader::waitForVersion(uint16_t timeout, bool printOutVersion)
599 {
600 uint8_t b;
601 unsigned int nBytes = 0;
602
603 CTicTac tictac;
604 tictac.Tic();
605 const double maxTime = timeout * 1e-3;
606
607 while (nBytes < 6)
608 {
609 if (m_mySerialPort->Read(&b, 1))
610 {
611 // cout << "received " << nBytes << " bytes: " << char(b)
612 //<<
613 // endl;
614 // First byte must be STX:
615 if (nBytes > 0 || (nBytes == 0 && b == 'V'))
616 {
617 // Store in frame:
618 m_received_frame_buffer[nBytes] = b;
619 nBytes++;
620 }
621 }
622 if (tictac.Tac() >= maxTime)
623 {
624 cout << "Version timeout" << endl;
625 return false; // Timeout
626 }
627 }
628
629 // Check len:
630 if (m_received_frame_buffer[nBytes - 1] != 0x0D)
631 {
632 printf(
633 "[CCANBusReader::waitForVersion] Error: expected 0x0D final byte, "
634 "received %x\n",
635 m_received_frame_buffer[nBytes - 1]);
636 return false;
637 }
638
639 if (printOutVersion)
640 {
641 cout << "Version: ";
642 for (uint8_t k = 0; k < nBytes; ++k) cout << char(m_received_frame_buffer[k]);
643 cout << endl;
644 }
645 return true;
646 }
647 // Returns false if timeout
648 bool CCANBusReader::waitIncomingFrame(uint16_t timeout)
649 {
650 uint8_t b;
651 unsigned int nBytes = 0;
652
653 CTicTac tictac;
654 tictac.Tic();
655 const double maxTime = timeout * 1e-3;
656 uint8_t dlc = 0;
657 while (nBytes < 10 || (nBytes < (10U + dlc + 1U /*CR*/)))
658 {
659 if (m_mySerialPort->Read(&b, 1))
660 {
661 // First byte must be STX:
662 if (nBytes > 1 || (!nBytes && b == 0x54 /*'T'*/))
663 {
664 // Store in frame:
665 m_received_frame_buffer[nBytes] = b;
666 nBytes++;
667 }
668 if (nBytes == 10)
669 dlc = 2 * uint8_t(hexCharToInt(m_received_frame_buffer[9])); // here is the
670 // number of
671 // BYTES of
672 // data -> 2
673 // hex values
674 // for byte
675 }
676 if (tictac.Tac() >= maxTime) return false; // Timeout
677 }
678 // Check final flag
679 if (m_received_frame_buffer[10U + dlc] != 0x0D)
680 {
681 printf(
682 "[CCANBusReader::waitIncomingFrame] Error: expected 0x0D as final "
683 "flag, received %x\n",
684 m_received_frame_buffer[10U + dlc]);
685 return false;
686 }
687
688 #if 0
689 printf("RX: ");
690 for (unsigned int i=0;i<nBytes;i++)
691 printf("%02X ",m_received_frame_buffer[i]);
692 printf("\n");
693 #endif
694
695 // OK
696 return true;
697 }
698
699 bool CCANBusReader::sendCommandToCANReader(
700 const uint8_t* cmd, const uint16_t cmd_len, [[maybe_unused]] bool wait)
701 {
702 uint8_t cmd_full[1024];
703 ASSERT_(sizeof(cmd_full) > cmd_len);
704
705 // command is just plain text so no frame header nor CRC is needed
706 memcpy(cmd_full, cmd, cmd_len);
707 cmd_full[cmd_len] = 0x0D; // [CR] at the end
708
709 const size_t toWrite = cmd_len + 1;
710
711 #if 1
712 printf("TX: ");
713 for (unsigned int i = 0; i < toWrite; i++) printf("%02X ", cmd_full[i]);
714 printf("\n");
715 #endif
716
717 // const int NTRIES = 3;
718
719 // for (int k=0;k<NTRIES;k++)
720 // {
721 if (toWrite != m_mySerialPort->Write(cmd_full, toWrite))
722 {
723 cout << "[CCANBusReader::SendCommandToCANReader] Error writing data to "
724 "serial port."
725 << endl;
726 return false;
727 }
728 return true;
729 // std::this_thread::sleep_for(15ms);
730 // if(wait)
731 // {
732 // if(waitACK(5000))
733 // return true;
734 // std::this_thread::sleep_for(10ms);
735 // }
736 // else
737 // return true; // perform special wait outside this method
738 // }
739
740 return false;
741 }