CCANBusReader.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

// This file contains portions of code from sicklms200.cc from the Player/Stage
// project.

#include "hwdrivers-precomp.h"  // Precompiled headers

#include <mrpt/utils/crc.h>
#include <mrpt/utils/CTicTac.h>
#include <mrpt/system/os.h>
#include <cstdio>  // printf
#include <mrpt/hwdrivers/CCANBusReader.h>

#include <thread>

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::utils;
using namespace mrpt::obs;
using namespace mrpt::hwdrivers;

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::utils::COutputLogger("CCANBusReader"),
          m_com_port(),
          m_mySerialPort(nullptr),
          m_com_baudRate(57600),
          m_nTries_connect(1),
          m_nTries_current(0),
          m_canbus_speed(250000),
          m_canreader_timestamp(false),
          m_CANBusChannel_isOpen(false)
{
        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::make_aligned_shared<mrpt::obs::CObservationCANBusJ1939>();
        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<uint8_t> out_data;
        vector<char> 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::system::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 (uint8_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 (uint8_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::utils::CConfigFileBase& configSource,
        const std::string& iniSection)
{
        //      m_sensorPose = CPose3D(
        //              configSource.read_float(iniSection,"pose_x",0),
        //              configSource.read_float(iniSection,"pose_y",0),
        //              configSource.read_float(iniSection,"pose_z",0),
        //              DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
        //              DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
        //              DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
        //              );  // irrelevant

        m_canbus_speed =
                configSource.read_int(iniSection, "CANBusSpeed", m_canbus_speed);
        m_canreader_timestamp = configSource.read_bool(
                iniSection, "useCANReaderTimestamp", m_canreader_timestamp);

#ifdef MRPT_OS_WINDOWS
        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 (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<uint8_t>& out_data, vector<char>& out_raw_frame)
{
        size_t nRead, nBytesToRead;
        size_t nFrameBytes = 0;
        size_t lengthField;
        unsigned char buf[40];

        // clear buffer
        for (uint8_t k = 0; k < 40; ++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 (uint8_t k = 0; k < 40; ++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 (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 (uint8_t k = 0; k < 40; ++k) buf[k] = 0;
                }
        }  // end while

        // Process frame
        // convert ASCII text into integer
        vector<uint8_t> 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 << 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(format("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<nBytes;i++)
        printf("%02X ",m_received_frame_buffer[i]);
    printf("\n");
#endif

        // OK
        return true;
}

bool CCANBusReader::sendCommandToCANReader(
        const uint8_t* cmd, const uint16_t cmd_len, bool wait)
{
        MRPT_UNUSED_PARAM(wait);
        uint8_t cmd_full[1024];
        ASSERT_(sizeof(cmd_full) > 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;k<NTRIES;k++)
        //    {
        if (toWrite != m_mySerialPort->Write(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;
}