source: pacpussensors/trunk/Sick/SickLDMRSSensor.cpp@ 39

/*********************************************************************
//  created:    2014/02/02 - 12:08
//  filename:   SickLDMRSSensor.cpp
//
//  author:     Cyril Fougeray
//              Copyright Heudiasyc UMR UTC/CNRS 6599
// 
//  version:    $Id: $
//
//  purpose:    The acquisition class of the SickLDMRS sensor
//
*********************************************************************/

#include "SickLDMRSSensor.h"

#include "SickSocket.h"
// #include "Pacpus/kernel/ComponentFactory.h"
#include "Pacpus/kernel/DbiteException.h"
#include "Pacpus/kernel/DbiteFileTypes.h"
#include "Pacpus/kernel/Log.h"
#include "Pacpus/PacpusTools/ShMem.h"

#include <iostream>
#include <QTcpSocket>
#include <string>

using namespace std;

// #define SICKLDMRS_SH_MEM

namespace pacpus {

DECLARE_STATIC_LOGGER("pacpus.base.SickLDMRSSensor");


static const string kSickMemoryName = "SickLDMRS";


static const long MagicWord = 0xAFFEC0C2; // Sick LDMRS


static const int SICKLDMRS_SCANDATA_TYPE    = 0x2202;
static const int SICKLDMRS_OBJECTDATA_TYPE  = 0x2221;

//////////////////////////////////////////////////////////////////////////
/// Constructor.
SickLDMRSSensor::SickLDMRSSensor(QObject *parent)
    : ipaddr_("192.168.0.1"),
      port_(12002),
      name_("SickLDMRSDefault")
{
    LOG_TRACE("constructor ("<< this->name_ << "), default settings");

    S_socket = new SickSocket(this);

    connect(S_socket, SIGNAL(configuration()), this, SLOT(configure()) );

    pendingBytes.time = 0;
    pendingBytes.previousData = false;

    this->kSickDbtFileName = name_.toStdString()+".dbt";
    this->kSickUtcFileName = name_.toStdString()+"_data.utc";
}

SickLDMRSSensor::SickLDMRSSensor(QObject *parent, QString name, QString ip, int port, int recording)
    : ipaddr_(ip),
      port_(port),
      name_(name),
      recording_((bool)recording)
{   

    LOG_TRACE("constructor ("<< this->name_ << "), configuration : " << this->ipaddr_<<":"<<this->port_);

    S_socket = new SickSocket(this);

    connect(S_socket, SIGNAL(configuration()), this, SLOT(configure()) );

    pendingBytes.time = 0;
    pendingBytes.previousData = false;

    this->kSickDbtFileName = name_.toStdString()+".dbt";
    this->kSickUtcFileName = name_.toStdString()+"_data.utc";
}

//////////////////////////////////////////////////////////////////////////
/// Destructor.
SickLDMRSSensor::~SickLDMRSSensor()
{
    LOG_TRACE("destructor ("<< this->name_ << ")");
    delete S_socket;
}

//////////////////////////////////////////////////////////////////////////
/// Called by the ComponentManager to start the component
void SickLDMRSSensor::startActivity()
{

    LOG_TRACE("Start Activity ("<< this->name_ <<")");

    S_socket->connectToServer(ipaddr_, port_);

    if (recording_) {
        LOG_TRACE("(Recording) Starting recording...");

        try {
            dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK, sizeof(SickLDMRS_dbt));
        } catch (DbiteException & e) {
            cerr << "error opening dbt file: "<< kSickDbtFileName << ", " << e.what() << endl;
            return;
        }

        // FIXME: use ofstream
        // open the file with C function to be sure that it will exist
        FILE * stream;
        if (NULL == (stream = fopen(kSickUtcFileName.c_str(), "a+"))) {
            LOG_FATAL("cannot open file '" << kSickUtcFileName.c_str() << "'");
            ::exit(-1);
        } else {
            fclose(stream);
        }

        dataFile_.open(kSickUtcFileName.c_str(), ios_base::out|ios_base::binary|ios_base::app);
        if (!dataFile_) {
            LOG_FATAL("cannot open file '" << kSickUtcFileName.c_str() << "'");
            ::exit(-1);
        }
    }
    else
        LOG_TRACE("(Recording) Not recording...");


#ifdef SICKLDMRS_SH_MEM
    shmem_ = new ShMem(kSickMemoryName.c_str(), sizeof(ScanSickData));
#endif

}

//////////////////////////////////////////////////////////////////////////
/// Called by the ComponentManager to stop the component
void SickLDMRSSensor::stopActivity()
{
    LOG_TRACE("destructor (" << this->name_ << ")");

    S_socket->sendToServer(QString((u_int32_t)0x0021));

    S_socket->closeSocket();

    if (recording_) {
        LOG_TRACE("(Recording) Recording stopped");
        dbtFile_.close();
        dataFile_.close();
    }

#ifdef SICKLDMRS_SH_MEM
    delete shmem_;
#endif
    // delete generator;

}



//////////////////////////////////////////////////////////////////////////
/// look for the position of the magic word in the packet.
/// Return it if found else return -1
u_int32_t SickLDMRSSensor::findMagicWord(const char * message, const unsigned length)
{
    if (length < 4) {
        return -1;
    }

    unsigned long i = 0;
    while(*((u_int32_t*)(message+i)) != 0xC2C0FEAF){ // BigE
        if (i == length) {
            return -1;
        }
        ++i;
    }
    return i;

}

//////////////////////////////////////////////////////////////////////////
/// Return the size of the message contained in the packet
/// You have to provide the position of the magic word in the packet
u_int32_t SickLDMRSSensor::getMessageSize(const char * message, const unsigned length, const long magicWordIndex)
{

    // we need at least 12 bytes
    if (length < 12) {
        return 0;
    }
    return ((*(message+magicWordIndex+11))&0x000000FF)
            + ((*(message+magicWordIndex+10)<<8)&0x0000FF00)
            + ((*(message+magicWordIndex+9)<<16)&0x00FF0000)
            + ((*(message+magicWordIndex+8)<<24)&0xFF000000)
            + 24 ;
}

//////////////////////////////////////////////////////////////////////////
/// @param size     the total size of the message including the 2 headers and the body
/// @param length   the number of available bytes in the packet
bool SickLDMRSSensor::isMessageComplete(const unsigned length, const long size)
{
    if (size <= length) {
        return true;
    }
    return false;
}


void SickLDMRSSensor::fillScanHeader( MessageLDMRS &msg )
{
    // address = base + Data header size (24-byte long) + offset
    msg.hScan.scanNumber = *((u_int16_t*)(msg.body+24));
    msg.hScan.scannerStatus = *((u_int16_t*)(msg.body+24+2));
    msg.hScan.phaseOffset = *((u_int16_t*)(msg.body+24+4));
    msg.hScan.startNtpTime = *((u_int64_t*)(msg.body+24+6));
    msg.hScan.endNtpTime = *((u_int64_t*)(msg.body+24+14));
    msg.hScan.ticksPerRot= *((u_int16_t*)(msg.body+24+22)); // needed to compute angle (°)
    msg.hScan.startAngle = *((int16_t*)(msg.body+24+24));
    msg.hScan.endAngle = *((int16_t*)(msg.body+24+26));
    msg.hScan.numPoints = *((u_int16_t*)(msg.body+24+28));

//    msg.hScan.mountingYawAngle = *((int16_t*)(msg.body+24+30));
//    msg.hScan.mountingPitchAngle = *((int16_t*)(msg.body+24+32));
//    msg.hScan.mountingRollAngle = *((int16_t*)(msg.body+24+34));
//    msg.hScan.mountingX = *((int16_t*)(msg.body+24+36));
//    msg.hScan.mountingY = *((int16_t*)(msg.body+24+38));
//    msg.hScan.mountingZ = *((int16_t*)(msg.body+24+40));


    LOG_TRACE("(hScan) Scan header data parsed, scan number " << msg.hScan.scanNumber);
    LOG_TRACE("(hScan) Number of scanned points : " << msg.hScan.numPoints);
}


void SickLDMRSSensor::fillDataHeader(MessageLDMRS &msg)
{

    msg.hData.magicWord = ((*(msg.body+3))&0x000000FF) +
            ((*(msg.body+2)<<8)&0x0000FF00) +
            ((*(msg.body+1)<<16)&0x00FF0000)+
            ((*(msg.body)<<24)&0xFF000000);
    LOG_TRACE("(hData) Magic word read "<<msg.hData.magicWord);
    // TODO check if OK

    msg.hData.sizePreviousMessage = ((*(msg.body+7))&0x000000FF)+
            ((*(msg.body+6)<<8)&0x0000FF00)+
            ((*(msg.body+5)<<16)&0x00FF0000)+
            ((*(msg.body+4)<<24)&0xFF000000);
    LOG_TRACE("(hData) previous message size : "<<msg.hData.sizePreviousMessage);

    msg.hData.sizeCurrentMessage =((*(msg.body+11))&0x000000FF)+
            ((*(msg.body+10)<<8)&0x0000FF00)+
            ((*(msg.body+9)<<16)&0x00FF0000)+
            ((*(msg.body+8)<<24)&0xFF000000);

    LOG_TRACE("(hData) current message size : "<<msg.hData.sizeCurrentMessage);


    msg.hData.deviceId = *(msg.body+13);

    msg.hData.dataType =  ((*(msg.body+15))&0x000000FF)+((*(msg.body+14)<<8)&0x0000FF00);
    LOG_TRACE("(hData) Data type : " << msg.hData.dataType);


    msg.hData.ntpTime = ((*(msg.body+15))&0x000000FF)+
            ((*(msg.body+14)<<8)&0x0000FF00)+
            ((*(msg.body+13)<<16)&0x00FF0000)+
            ((*(msg.body+12)<<24)&0xFF000000)+
            ((*(msg.body+11))&0x000000FF)+
            ((*(msg.body+10)<<8)&0x0000FF00)+
            ((*(msg.body+9)<<16)&0x00FF0000)+
            ((*(msg.body+8)<<24)&0xFF000000);
}

//////////////////////////////////////////////////////////////////////////
/// this function is called when no complete message has been found
/// we set the flag previousData to true for the next processing stage
/// and we store the timestamp of the bytes acquisition
void SickLDMRSSensor::storePendingBytes(road_time_t time)
{
    if (!pendingBytes.previousData)
    {
        pendingBytes.time = time;
        pendingBytes.previousData = true;
    }
}


void SickLDMRSSensor:: splitPacket(const char * packet, const int length, road_time_t time)
{ 
    long index = 0;
    long msgSize = 0;
    bool msgComplete = false;

    // we are working on the previous not decoded data + the actual incoming packet
    pendingBytes.data.append(packet,length);
    LOG_TRACE("(Packet reconstitution) Pending bytes : " << pendingBytes.data.size() );


    while (pendingBytes.data.size() > 0)
    {
        // we are looking for the MagicWord
        index = findMagicWord(pendingBytes.data.c_str() , pendingBytes.data.size() );
        LOG_TRACE("(Packet reconstitution) MagicWord index : " << index );
        if (index == -1)
        {
            storePendingBytes(time);
            // exit the while loop
            break;
        }


        // we are looking for the size of the message (scan data + scan points)
        msgSize = getMessageSize(pendingBytes.data.c_str(), pendingBytes.data.size(), index);
        if (msgSize == 0)
        {
            storePendingBytes(time);
            // exit the while loop
            break;
        }

        LOG_TRACE("(Packet reconstitution) Message size (data header(24) + size of the message) : " << msgSize );

        // we are verifying if the message is complete
        msgComplete = isMessageComplete( pendingBytes.data.size() , msgSize );
        if (msgComplete == false)
        {
            storePendingBytes(time);
            // exit the while loop
            break;
        }

        // we have a complete message available that we can add to the list
         MessageLDMRS msg;

        // we copy the bytes in the body message
        char* messageData = (char*)malloc(msgSize);
        memcpy(messageData, pendingBytes.data.c_str() + index, msgSize);

        msg.body = messageData;

        // we set the timestamp of the message
        if (pendingBytes.previousData)
        {
            // the timestamp is the one of the previous packet
            msg.time = pendingBytes.time;
            pendingBytes.previousData = false;
        }
        else
        {
            // the timestamp is the one of the actual received packet
            msg.time = time;
        }

        // we add the message to the list
        msgList.push_back(msg);
        // and we suppress the processed bytes of the pending data
        pendingBytes.data.erase(0, msgSize);
    }
}


unsigned long SickLDMRSSensor::processMessage(MessageLDMRS &msg)
{ 
    fillDataHeader(msg);
    if (SICKLDMRS_SCANDATA_TYPE == msg.hData.dataType) {
        LOG_TRACE("(Process Message) Scan Data Type!");
        fillScanHeader(msg);

        int index = 24 + 44; // data header + scan header
        ScanPoint* scanPoints = (ScanPoint*) malloc(sizeof(ScanPoint) * msg.hScan.numPoints);
        int position = 0;

        for (int i = 0; i < msg.hScan.numPoints; ++i) {
            ((ScanPoint*)(scanPoints + position))->layerEcho = *((uchar*)(msg.body + index));
            ((ScanPoint*)(scanPoints + position))->flags = *((uchar*)(msg.body + index + 1));
            ((ScanPoint*)(scanPoints + position))->angle = *((u_int16_t*)(msg.body + index + 2));
            ((ScanPoint*)(scanPoints + position))->distance = *((u_int16_t*)(msg.body + index + 4));
            ((ScanPoint*)(scanPoints + position))->echoPulseWidth = *((u_int16_t*)(msg.body + index + 6));
            index += 10;
            position += sizeof(ScanPoint);
        }

        // raw data (message) no longer needed, free memory and replace with structured data
        free(msg.body);
        msg.body = (char*) scanPoints;
    }
    else if (msg.hData.dataType == SICKLDMRS_OBJECTDATA_TYPE){
        LOG_TRACE("(Process Message) Object Data Type!");



        // TODO

        // raw message no longer needed, free memory
        free(msg.body);
        // msg.body = (char*) scanObjects;
    }
    else // irrelevant data type
        free(msg.body); // free raw data

    return msg.hData.dataType;
}

//////////////////////////////////////////////////////////////////////////
/// write the data on the disk:
/// - complete the dbt file with msg.hData
/// - complete the binary data file with the alascaData.point
void SickLDMRSSensor::writeData(MessageLDMRS &msg)
{
    // record the data in a dbt file. The dbt data is only the structure DataHeader
    // scan data are recorded in the sickldmrs_data.utc file

    SickLDMRS_dbt entry;
    entry.timeStartFromSensor = msg.hData.ntpTime;
    entry.hScan = msg.hScan;
    entry.dataPos = dataFile_.tellp(); // absolute position of pointer in UTC file
    entry.time = msg.time;
    entry.timerange = msg.timerange;

    // write DBT
    try {
        dbtFile_.writeRecord(msg.time, msg.timerange, (char *) &entry, sizeof(SickLDMRS_dbt));
    } catch (DbiteException & e) {
        cerr << "error writing data: " << e.what() << endl;
        return;
    }

    // record the scan data in a binary file sickldmrs_data.utc with "UTC\0" to separate the data
    for (unsigned int i = 0 ; i < msg.hScan.numPoints ; ++i) {
        dataFile_.write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
    }
    // add a magic word to delimit the block of data
    int32_t utcMagicWord = UTC_MAGIC_WORD;
    dataFile_.write(reinterpret_cast<char*>(&(utcMagicWord)), sizeof(int32_t));

}



///
void SickLDMRSSensor::configure(){
    // Start measuring
    // S_socket->sendToServer(QString((u_int32_t)0x0020));

    // LOG_TRACE(this->name_ +" configured.");
}


//////////////////////////////////////////////////////////////////////////
/// Event call by the SickSocket class when new data has been arrived
/// on the network.
void SickLDMRSSensor::customEvent(QEvent * e)
{   
    SickFrame * frame = ((SickFrameEvent*)e)->frame;

    // we try to find some messages in the current packet + the pending bytes of the previous incoming data
    splitPacket(frame->msg, frame->size, frame->time);

    // we delete the heap variable
    delete frame;

    // we test if we have some messages to decode
    while ( !msgList.empty() )
    {
        // get the first (the eldest) message and process it
        MessageLDMRS msgToProcess = msgList.front();
        unsigned long type = processMessage(msgToProcess);

        if (type == SICKLDMRS_SCANDATA_TYPE)
        {
//            setState(ComponentBase::MONITOR_OK);
            // write data on the disk
            if (recording_)
                writeData(msgList.front());

#ifdef SICKLDMRS_SH_MEM
            /// push data in shared memory
            // First the scan info
            SickLDMRS_shMem toWrite;
            toWrite.time = msgToProcess.time;
            toWrite.timerange = msgToProcess.timerange;
            toWrite.scanInfo = msgToProcess.hScan;
            shmem_->write(toWrite, sizeof(SickLDMRS_shMem));

            // Then, the points
            for (unsigned int i = 0 ; i < msgToProcess.hScan.numPoints ; ++i) {
                shmem_->write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
            }
#endif

        }
        else if (type == SICKLDMRS_OBJECTDATA_TYPE)
        {
            // Handled via CAN bus ?
        }

        // (malloced memory) ScanPoints[] or ScanObjects[] no longer needed
        free(msgToProcess.body);

        // removes the processed item of the list
        msgList.pop_front();
    }
}

} // namespace pacpus