CCANBusReader.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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/system/crc.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/os.h>
#include <cstdio>  // printf
#include <cstring>  // memset
#include <mrpt/hwdrivers/CCANBusReader.h>

#include <thread>
#include <iostream>

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_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::config::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 _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 (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;
}