AbstractInterface.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "AbstractInterface.h"
#include "Response.h"
#include "Checksum.h"
#include "Types.h"
#include "TransmissionException.h"
#include <string.h>
#include <iostream>
#include <stdio.h>
#include <stdexcept>
 
 
AbstractInterface::AbstractInterface()
    : connected(false)
{
 
}
 
AbstractInterface::~AbstractInterface()
{
}
 
int AbstractInterface::read(unsigned char* buf, unsigned int len)
{
    int res = readInternal(buf, len);
 
    if (log != NULL)
    {
        log->logRead(buf, res);
    }
 
    return res;
}
 
int AbstractInterface::write(unsigned char* buf, unsigned int len)
{
    if (log != NULL)
    {
        log->logWrite(buf, len);
    }
 
    return writeInternal(buf, len);
}
 
 
Response AbstractInterface::submitCmd(unsigned char id, unsigned char* payload, unsigned int len, bool pending)
{
    fireAndForgetCmd(id, payload, len, pending);
    return receive(pending, id);
}
 
void AbstractInterface::fireAndForgetCmd(unsigned char id, unsigned char* payload, unsigned int len, bool pending)
{
 
    int res;
 
    // Check if we're connected
    if (!connected)
    {
        throw TransmissionException("Interface not connected");
    }
 
    // Send command
    res = send(id, len, payload);
 
    if (res < 0)
    {
        throw TransmissionException("Message send failed");
    }
}
 
void AbstractInterface::startLogging(std::string file)
{
    log.reset(new BinaryLogger(file));
}
 
void AbstractInterface::logText(std::string message)
{
    if (log != NULL)
    {
        log->logText(message);
    }
}
 
Response AbstractInterface::receiveWithoutChecks()
{
    int res;
    status_t status;
    msg_t msg;
 
    // Receive response data
    res = receive(&msg);
 
    if (res < 0)
    {
        throw TransmissionException("Message receive failed");
    }
 
    status = (status_t) make_short(msg.data[0], msg.data[1]);
 
    return Response(res, msg.id, status, msg.data, msg.len);
}
 
Response AbstractInterface::receive(bool pending, unsigned char expectedId)
{
    int res;
    status_t status;
    msg_t msg;
 
    // Receive response. Repeat if pending.
    do
    {
        // Receive response data
        res = receive(&msg);
 
        if (res < 0)
        {
            throw TransmissionException("Message receive failed");
        }
 
        // Check response ID
        if (msg.id != expectedId)
        {
            //std::strstream strStream;
            //strStream << "Response ID ()" << msg.id << ") does not match submitted command ID (" << id << ")";
            //throw std::runtime_error(strStream.str());
            throw TransmissionException(str(boost::format("Response ID (%02X) does not match submitted command ID (%02X)") % (int)msg.id % (int)expectedId));
        }
 
        if (pending)
        {
            if (msg.len < 2)
            {
                throw TransmissionException("No status code received");
            }
 
            status = (status_t) make_short(msg.data[0], msg.data[1]);
        }
    }
    while (pending && status == E_CMD_PENDING);
 
    status = (status_t) make_short(msg.data[0], msg.data[1]);
 
    return Response(res, msg.id, status, msg.data, msg.len);
}
 
int AbstractInterface::receive(msg_t* msg)
{
    int res;
    unsigned char header[3];            // 1 byte command, 2 bytes payload length
    unsigned short checksum = 0x50f5;   // Checksum over preamble (0xaa 0xaa 0xaa)
    unsigned int sync;
 
    logText("read preamble");
    // Syncing - necessary for compatibility with serial interface
    sync = 0;
 
    while (sync != MSG_PREAMBLE_LEN)
    {
        res = read(header, 1);
 
        if (res > 0 && header[0] == MSG_PREAMBLE_BYTE)
        {
            sync++;
        }
        else
        {
            sync = 0;
        }
    }
 
    // Read header
    res = read(header, 3);
 
    if (res < 3)
    {
        throw TransmissionException(str(boost::format("Failed to receive header data (%d bytes read)") % res));
    }
 
    // Calculate checksum over header
    checksum = Checksum::Update_crc16(header, 3, checksum);
 
    // Get message id of received
    msg->id = header[0];
 
    // Get payload size of received message
    msg->len = make_short(header[1], header[2]);
 
    // Allocate space for payload and checksum
    //msg->data.resize( msg->len + 2u );
    //if ( !msg->data ) return -1;
    //std::shared_ptr<unsigned char[]> data(new unsigned char[ msg->len + 2u ]);
 
    unsigned char* data = new unsigned char[ msg->len + 2u ];
 
 
    // Read payload and checksum
    /*int maxReads = 10;
    int remaining = msg->len + 2;
    int dataOffset = 0;
    while ( maxReads > 0 && remaining > 0)
    {
        int read = this->read( data + dataOffset, remaining );
        dataOffset += read;
        remaining -= read;
        maxReads--;
        if (remaining > 0) {
            //fprintf( stderr, "Partial message received, waiting 100µs\n" );
            usleep(100);
        }
    }
    if (remaining > 0)
    {
        throw TransmissionException(str(boost::format("Not enough data (%d, expected %d), Command = %02X") % res % (msg->len + 2) % msg->id));
    }*/
 
    // Read payload and checksum:
    // payload: msg->len
    // checksum: 2 byte
    int read = this->read(data, msg->len + 2);
 
    if (read != (int)msg->len + 2)
    {
        delete[] data;
        throw TransmissionException(str(boost::format("Not enough data (%d, expected %d), Command = %02X") % res % (msg->len + 2) % msg->id));
    }
 
    /*
    res = interface->read( msg->data, msg->len + 2 );
    if ( res < (int) (msg->len + 2) )
    {
        fprintf( stderr, "Not enough data (%d, expected %d)\n", res, msg->len + 2 );
        fprintf( stderr, "Command = %X\n", msg->id );
        return -1;
    }*/
 
    // Check checksum
    checksum = Checksum::Update_crc16(data, msg->len + 2, checksum);
 
    if (checksum != 0)
    {
        delete[] data;
        logText("CHECKSUM ERROR");
        throw ChecksumErrorException("Checksum error");
    }
 
    msg->data = std::vector<unsigned char>(data, data + msg->len + 2);
    delete[] data;
    logText("receive done.");
    return msg->len + 8;
}
 
int AbstractInterface::send(unsigned char id, unsigned int len, unsigned char* data)
{
    unsigned char header[MSG_PREAMBLE_LEN + 3];
    unsigned short crc;
    int i, res;
 
    logText("write preamble");
 
    // Preamble
    for (i = 0; i < MSG_PREAMBLE_LEN; i++)
    {
        header[i] = MSG_PREAMBLE_BYTE;
    }
 
    // Command ID
    header[MSG_PREAMBLE_LEN] = id;
 
    // Length
    header[MSG_PREAMBLE_LEN + 1] = lo(len);
    header[MSG_PREAMBLE_LEN + 2] = hi(len);
 
    // Checksum
    crc = Checksum::Crc16(header, 6);
    crc = Checksum::Update_crc16(data, len, crc);
 
 
    unsigned char* buf = new unsigned char[ 6 + len + 2 ];
    memcpy(buf, header, 6);
    memcpy(buf + 6, data, len);
    memcpy(buf + 6 + len, (unsigned char*) &crc, 2);
 
    res = write(buf, 6 + len + 2);
 
    if (res < 6 + (int)len + 2)
    {
        delete[] buf;
        close();
        throw TransmissionException("Failed to submit message checksum");
        //cerr << "Failed to submit message checksum" << endl;
    }
 
    delete[] buf;
 
    logText("send done.");
 
    return len + 8;
}
 
 
 
 
std::ostream& operator<<(std::ostream& strm, const AbstractInterface& a)
{
    return strm << a.toString();
}