/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include "hwdrivers-precomp.h" // Precompiled headers // // This file contains portions of code from sicklms200.cc from the Player/Stage // project. #include #include #include #include #include #include // printf #include // memset #include #include IMPLEMENTS_GENERIC_SENSOR(CCANBusReader, mrpt::hwdrivers) #define RET_ERROR(msg) \ { \ cout << "[" << __CURRENT_FUNCTION_NAME__ << "] " << msg << endl; \ return false; \ } using namespace std; using namespace mrpt; using namespace mrpt::obs; using namespace mrpt::hwdrivers; using namespace mrpt::system; char hexCharToInt(char n) { if (n >= '0' && n <= '9') return (n - '0'); else if (n >= 'A' && n <= 'F') return (n - 'A' + 10); else return 0; } /*------------------------------------------------------------- CCANBusReader -------------------------------------------------------------*/ CCANBusReader::CCANBusReader() : mrpt::system::COutputLogger("CCANBusReader"), m_com_port() { m_sensorLabel = "CANBusReader"; memset(m_received_frame_buffer, 0, sizeof(m_received_frame_buffer)); } /*------------------------------------------------------------- ~CCANBusReader -------------------------------------------------------------*/ CCANBusReader::~CCANBusReader() { if (m_CANBusChannel_isOpen) { try { CANBusCloseChannel(); } catch (...) { } } if (m_mySerialPort) { delete m_mySerialPort; m_mySerialPort = nullptr; } } void CCANBusReader::doProcess() { mrpt::obs::CObservationCANBusJ1939::Ptr obs = mrpt::obs::CObservationCANBusJ1939::Create(); bool thereIsObservation; bool hardwareError; doProcessSimple(thereIsObservation, *obs, hardwareError); if (thereIsObservation) appendObservation(obs); else cout << "No frame received" << endl; } /*------------------------------------------------------------- doProcess -------------------------------------------------------------*/ void CCANBusReader::doProcessSimple( bool& outThereIsObservation, mrpt::obs::CObservationCANBusJ1939& outObservation, bool& hardwareError) { outThereIsObservation = false; hardwareError = false; if (!tryToOpenComms()) { hardwareError = true; return; } m_state = ssWorking; // Wait for a scan: uint8_t out_prio, out_pdu_format, out_pdu_spec, out_src_address, out_data_length; uint16_t out_pgn; vector out_data; vector out_raw_frame; if (!waitContinuousSampleFrame( out_prio, out_pdu_format, out_pdu_spec, out_src_address, out_data_length, out_pgn, out_data, out_raw_frame)) return; // Yes, we have a new scan: // cout << "we've got a frame" << endl; // ----------------------------------------------- // Extract the observation: // ----------------------------------------------- outObservation.timestamp = mrpt::Clock::now(); outObservation.sensorLabel = m_sensorLabel; // Set label // And the scan ranges: outObservation.m_priority = out_prio; outObservation.m_pdu_spec = out_pdu_spec; outObservation.m_pdu_format = out_pdu_format; outObservation.m_src_address = out_src_address; outObservation.m_pgn = out_pgn; outObservation.m_data_length = out_data_length; outObservation.m_data.resize(out_data.size()); for (size_t k = 0; k < out_data.size(); ++k) outObservation.m_data[k] = out_data[k]; outObservation.m_raw_frame.resize(out_raw_frame.size()); for (size_t k = 0; k < out_raw_frame.size(); ++k) outObservation.m_raw_frame[k] = out_raw_frame[k]; // we've got a new observation outThereIsObservation = true; } /*------------------------------------------------------------- loadConfig_sensorSpecific -------------------------------------------------------------*/ void CCANBusReader::loadConfig_sensorSpecific( const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection) { m_canbus_speed = configSource.read_int(iniSection, "CANBusSpeed", m_canbus_speed); m_canreader_timestamp = configSource.read_bool(iniSection, "useCANReaderTimestamp", m_canreader_timestamp); #ifdef _WIN32 m_com_port = configSource.read_string(iniSection, "COM_port_WIN", m_com_port, true); #else m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port, true); #endif m_com_baudRate = configSource.read_int(iniSection, "COM_baudRate", m_com_baudRate); m_nTries_connect = configSource.read_int(iniSection, "nTries_connect", m_nTries_connect); } /*------------------------------------------------------------- Tries to open the com port and setup all the LMS protocol. Returns true if OK or already open. -------------------------------------------------------------*/ bool CCANBusReader::tryToOpenComms(std::string* err_msg) { if (err_msg) *err_msg = ""; try { if (!m_mySerialPort) { // There is no COMMS port open yet... if (!m_com_port.empty()) { // cout << "Creating port" << endl; m_mySerialPort = new mrpt::comms::CSerialPort(); // Create the port myself: } else throw std::logic_error( "ERROR: No serial port attached with bindIO, neither it " "set with 'setSerialPort'"); } // We assure now we have a stream... try to open it, if it's not done // yet. bool just_open = false; if (m_mySerialPort != nullptr) { if (!m_mySerialPort->isOpen()) { // Try to open it now: m_mySerialPort->setSerialPortName(m_com_port); m_mySerialPort->open(); // will raise an exception on error. // Set basic params: m_mySerialPort->setConfig(9600); m_mySerialPort->setTimeouts(100, 0, 10, 0, 50); just_open = true; } } // It seems the port was open and working so we are done here. if (!just_open) return true; // ================================================================== // Otherwise, it was just opened now, we must send the // ** initialization commands ** // and put the CAN Converter in recording mode: // ================================================================== cout << "Setting up serial comms in port " << m_com_port; if (!setupSerialComms()) RET_ERROR("error"); cout << " ... done" << endl; // initialize // set CAN Bus speed /**/ bool res; cout << "Setting up CAN BUS Speed at: " << m_canbus_speed << endl; for (int nTry = 0; nTry < 250000 /*4*/; nTry++) if (true == (res = sendCANBusReaderSpeed())) break; if (!res) return false; cout << " ... done" << endl; // open the CAN channel. If true, at this point, frames should be poping // out the CAN Bus cout << "Opening CAN BUS and starting to receive." << endl; for (int nTry = 0; nTry < 250000 /*4*/; nTry++) if (true == (res = CANBusOpenChannel())) break; if (!res) return false; cout << " ... done" << endl; // cout << "Autopoll" << endl; // for (int nTry=0;nTry<250000/*4*/;nTry++) // if (true==(res=CANBusAutoPoll())) // break; // if(!res) return false; // cout << " ... done" << endl; return res; /**/ } catch (const std::exception& e) { std::string s = "[CCANBusReader] Error trying to open CANBusReader at port "; s += e.what(); if (err_msg) *err_msg = s; MRPT_LOG_ERROR_STREAM(s); return false; } } /*------------------------------------------------------------- Tries to send the command to set up the speed of the CAN Bus reader -> tractor link -------------------------------------------------------------*/ bool CCANBusReader::sendCANBusReaderSpeed() { // command: S0 --> S8 according to the selected speed unsigned char cmd[2]; cmd[0] = 'S'; switch (m_canbus_speed) { case 10000: cmd[1] = '0'; break; case 20000: cmd[1] = '1'; break; case 50000: cmd[1] = '2'; break; case 100000: cmd[1] = '3'; break; case 125000: cmd[1] = '4'; break; case 250000: cmd[1] = '5'; break; case 500000: cmd[1] = '6'; break; case 800000: cmd[1] = '7'; break; case 1000000: cmd[1] = '8'; break; default: RET_ERROR("Incorrect CAN Bus speed"); break; } sendCommandToCANReader(cmd, 2); return waitACK(50); } bool CCANBusReader::CANBusOpenChannel() { unsigned char cmd[1]; cmd[0] = 'O'; sendCommandToCANReader(cmd, 1); m_CANBusChannel_isOpen = waitACK(50); return m_CANBusChannel_isOpen; } bool CCANBusReader::CANBusCloseChannel() { unsigned char cmd[1]; cmd[0] = 'C'; sendCommandToCANReader(cmd, 1); // m_CANBusChannel_isOpen = !waitACK(50); m_CANBusChannel_isOpen = false; // return !m_CANBusChannel_isOpen; return true; } bool CCANBusReader::CANBusAutoPoll() { unsigned char cmd[1]; cmd[0] = 'A'; sendCommandToCANReader(cmd, 1); return waitACK(50); } bool CCANBusReader::CANBusX1() { unsigned char cmd[2]; cmd[0] = 'X'; cmd[1] = '1'; sendCommandToCANReader(cmd, 2); return waitACK(50); } bool CCANBusReader::CANBusPoll() { unsigned char cmd[1]; cmd[0] = 'P'; sendCommandToCANReader(cmd, 1); return waitACK(50); } /*------------------------------------------------------------- waitContinuousSampleFrame -------------------------------------------------------------*/ bool CCANBusReader::waitContinuousSampleFrame( uint8_t& out_prio, uint8_t& out_pdu_format, uint8_t& out_pdu_spec, uint8_t& out_src_address, uint8_t& out_data_length, uint16_t& out_pgn, vector& out_data, vector& out_raw_frame) { size_t nRead, nBytesToRead; size_t nFrameBytes = 0; size_t lengthField; unsigned char buf[40]; // clear buffer for (unsigned char& k : buf) k = 0; uint8_t dlc = 0; while (nFrameBytes < (lengthField = (10U + dlc + 1U))) { // cout << "trying to receive" << endl; if (lengthField > 30) { cout << "#" << int(dlc) << " "; nFrameBytes = 0; // No es cabecera de trama correcta for (unsigned char& k : buf) k = 0; dlc = 0; } if (nFrameBytes < 10) nBytesToRead = 1; else { dlc = 2 * uint8_t(hexCharToInt(buf[9])); // cout << "dlc: " << int(dlc) << endl; nBytesToRead = (lengthField)-nFrameBytes; } try { nRead = m_mySerialPort->Read(buf + nFrameBytes, nBytesToRead); // cout << "to read: " << nBytesToRead << " received: " << // nRead << " -> "; // for( uint8_t k = 0; k < nRead; ++k ) // cout << int(buf[k+nFrameBytes]); // cout << endl; } catch (const std::exception& e) { // Disconnected? MRPT_LOG_ERROR_STREAM( "[waitContinuousSampleFrame] Disconnecting due to comms error: " << e.what()); return false; } if (!nRead) return false; if (nRead < nBytesToRead) std::this_thread::sleep_for(30ms); // Reading OK: // Was it the first one? if (nFrameBytes > 0 || (nFrameBytes == 0 && buf[0] == 0x54 /*T*/)) nFrameBytes += nRead; else { nFrameBytes = 0; // No es cabecera de trama correcta for (unsigned char& k : buf) k = 0; } } // end while // Process frame // convert ASCII text into integer vector aux; out_raw_frame.resize(nFrameBytes); for (uint8_t k = 0; k < nFrameBytes; ++k) { aux.push_back(hexCharToInt(buf[k])); out_raw_frame[k] = buf[k]; } out_prio = (aux[1] << 2) | (aux[2] >> 2); out_pdu_format = (aux[3] << 4) | aux[4]; out_pdu_spec = (aux[5] << 4) | aux[6]; out_src_address = (aux[7] << 4) | aux[8]; out_data_length = aux[9]; out_pgn = uint16_t(out_pdu_format) << 8 | uint16_t(out_pdu_spec); out_data.resize(out_data_length); for (uint8_t k = 0, k2 = 0; k < 2 * out_data_length; k += 2, k2++) out_data[k2] = (aux[10 + k] << 4) | aux[11 + k]; if (buf[nFrameBytes - 1] != 0x0D) { cout << mrpt::format( "[CCANBusReader::waitContinuousSampleFrame] expected 0x0D " "ending flag, 0x%X found instead", buf[nFrameBytes]) << endl; return false; // Bad ending flag } // All OK return true; } /*------------------------------------------------------------- initialize -------------------------------------------------------------*/ void CCANBusReader::initialize() { string err_str; memset(m_received_frame_buffer, 0, sizeof(m_received_frame_buffer)); if (!tryToOpenComms(&err_str)) { cerr << err_str << endl; throw std::logic_error(err_str); } } /*----------------------------------------------------------------- Assures laser is connected and operating at 38400, in its case returns true. -----------------------------------------------------------------*/ bool CCANBusReader::setupSerialComms() { ASSERT_( m_com_baudRate == 9600 || m_com_baudRate == 38400 || m_com_baudRate == 57600 || m_com_baudRate == 500000); if (m_mySerialPort == nullptr) return true; int detected_rate = 0; for (size_t reps = 0; !detected_rate && reps < m_nTries_connect; reps++) { m_nTries_current = reps; int rates[] = {0, 9600, 38400, 57600, 500000}; // Try first the desired rate to speed up the process, just in case // the converter is already setup from a previous run: rates[0] = m_com_baudRate; detected_rate = 0; for (size_t i = 0; !detected_rate && i < sizeof(rates) / sizeof(rates[0]); i++) { // Are we already receiving at 500k? // ------------------------------------------------ m_mySerialPort->setConfig(rates[i]); std::this_thread::sleep_for(100ms); m_mySerialPort->purgeBuffers(); // close the connection /**/ cout << endl << "Closing CAN Channel " << endl; for (int nTry = 0; nTry < 250000 /*4*/; nTry++) if (true == CANBusCloseChannel()) break; cout << " ... done" << endl; /**/ std::this_thread::sleep_for(100ms); m_mySerialPort->purgeBuffers(); for (int nTry = 0; nTry < 250000 /*4*/ && !detected_rate; nTry++) { m_mySerialPort->purgeBuffers(); // Ask for the laser version at the current rate: if (queryVersion(true)) { detected_rate = rates[i]; break; } std::this_thread::sleep_for(20ms); } // for tries // There is no link, or the baudrate is wrong... } // Try again in a while: if (!detected_rate && reps != (m_nTries_connect - 1)) std::this_thread::sleep_for(5000ms); } // Switch "this" serial port to the detected baudrate setBaudRate(detected_rate); m_mySerialPort->setConfig(m_com_baudRate); m_mySerialPort->purgeBuffers(); // Wait... std::this_thread::sleep_for(500ms); // And check comms at the new baud rate: return true; } /*----------------------------------------------------------------- Query to LMS a status query. Returns true if response is read ok. -----------------------------------------------------------------*/ bool CCANBusReader::queryVersion(bool printOutVersion) { ASSERT_(m_mySerialPort); uint8_t cmd[1]; cmd[0] = 'V'; uint16_t cmd_len = 1; if (!sendCommandToCANReader(cmd, cmd_len, false)) return false; return waitForVersion(500, printOutVersion); } // Returns false if timeout bool CCANBusReader::waitACK(uint16_t timeout_ms) { uint8_t b = 0; CTicTac tictac; tictac.Tic(); do { if (m_mySerialPort->Read(&b, 1)) { // Byte rx: if (b == 0x0D /*0x30*/) { cout << int(b) << endl; return true; // [CR] } } } while (tictac.Tac() < timeout_ms * 1e-3); if (b == 0x07) // [BELL] RET_ERROR("ERROR received.") else if (b != 0) RET_ERROR(format("Unexpected code received: 0x%02X", b)) else return false; // RET_ERROR("Timeout") } bool CCANBusReader::waitForVersion(uint16_t timeout, bool printOutVersion) { uint8_t b; unsigned int nBytes = 0; CTicTac tictac; tictac.Tic(); const double maxTime = timeout * 1e-3; while (nBytes < 6) { if (m_mySerialPort->Read(&b, 1)) { // cout << "received " << nBytes << " bytes: " << char(b) //<< // endl; // First byte must be STX: if (nBytes > 0 || (nBytes == 0 && b == 'V')) { // Store in frame: m_received_frame_buffer[nBytes] = b; nBytes++; } } if (tictac.Tac() >= maxTime) { cout << "Version timeout" << endl; return false; // Timeout } } // Check len: if (m_received_frame_buffer[nBytes - 1] != 0x0D) { printf( "[CCANBusReader::waitForVersion] Error: expected 0x0D final byte, " "received %x\n", m_received_frame_buffer[nBytes - 1]); return false; } if (printOutVersion) { cout << "Version: "; for (uint8_t k = 0; k < nBytes; ++k) cout << char(m_received_frame_buffer[k]); cout << endl; } return true; } // Returns false if timeout bool CCANBusReader::waitIncomingFrame(uint16_t timeout) { uint8_t b; unsigned int nBytes = 0; CTicTac tictac; tictac.Tic(); const double maxTime = timeout * 1e-3; uint8_t dlc = 0; while (nBytes < 10 || (nBytes < (10U + dlc + 1U /*CR*/))) { if (m_mySerialPort->Read(&b, 1)) { // First byte must be STX: if (nBytes > 1 || (!nBytes && b == 0x54 /*'T'*/)) { // Store in frame: m_received_frame_buffer[nBytes] = b; nBytes++; } if (nBytes == 10) dlc = 2 * uint8_t(hexCharToInt(m_received_frame_buffer[9])); // here is the // number of // BYTES of // data -> 2 // hex values // for byte } if (tictac.Tac() >= maxTime) return false; // Timeout } // Check final flag if (m_received_frame_buffer[10U + dlc] != 0x0D) { printf( "[CCANBusReader::waitIncomingFrame] Error: expected 0x0D as final " "flag, received %x\n", m_received_frame_buffer[10U + dlc]); return false; } #if 0 printf("RX: "); for (unsigned int i=0;i cmd_len); // command is just plain text so no frame header nor CRC is needed memcpy(cmd_full, cmd, cmd_len); cmd_full[cmd_len] = 0x0D; // [CR] at the end const size_t toWrite = cmd_len + 1; #if 1 printf("TX: "); for (unsigned int i = 0; i < toWrite; i++) printf("%02X ", cmd_full[i]); printf("\n"); #endif // const int NTRIES = 3; // for (int k=0;kWrite(cmd_full, toWrite)) { cout << "[CCANBusReader::SendCommandToCANReader] Error writing data to " "serial port." << endl; return false; } return true; // std::this_thread::sleep_for(15ms); // if(wait) // { // if(waitACK(5000)) // return true; // std::this_thread::sleep_for(10ms); // } // else // return true; // perform special wait outside this method // } return false; }