can_string_generic.h

Go to the documentation of this file.

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
/* 
 * Copyright (C) 2008 The RobotCub Consortium
 * Author: Marco Randazzo, Marco Maggiali
 * website: www.robotcub.org
 * Permission is granted to copy, distribute, and/or modify this program
 * under the terms of the GNU General Public License, version 2 or any
 * later version published by the Free Software Foundation.
 *
 * A copy of the license can be found at
 * http://www.robotcub.org/icub/license/gpl.txt
 *
 * This program 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
*/
 
#ifndef __can_string_generic__
#define __can_string_generic__
 
#include "messages.h"
 
#include <yarp/dev/CanBusInterface.h>
#include <string.h>
#include <yarp/os/LogStream.h>
#include <yarp/os/Log.h>
#define MAX_STRINGS 4
 
class can_string_generic
{
private:
 
struct data_struct{
    char text_buffer [256];
    bool complete;
    bool maybe_last_part;
    char board_id;
    int expected_length;
    int current_length;
};
    data_struct data[MAX_STRINGS];
 
public:
    inline can_string_generic();
 
    inline ~can_string_generic();
 
    inline int add_string(void* can_packet);
 
    inline char* print(int buffer_num, const char* canDevName, int netNum);
    
    inline void  clear_string(int buffer_num);
};
 
can_string_generic::can_string_generic()
{
    int j=0;
    for (j=0; j<MAX_STRINGS; j++)   clear_string(j);
}
 
can_string_generic::~can_string_generic()
{
}
 
void can_string_generic::clear_string(int buffer_num)
{
    if (buffer_num >= MAX_STRINGS) return ;
 
    for (int i = 0; i < 256; i++) data[buffer_num].text_buffer[i]=0;
    data[buffer_num].complete=false;
    data[buffer_num].maybe_last_part=false;
    data[buffer_num].expected_length = 0;
    data[buffer_num].board_id=0;
    data[buffer_num].current_length = 0;
 
}
 
int can_string_generic::add_string(void* can_packet)
{
    int j=0;
    int string_id = 0;
    int offset = 0;
    yarp::dev::CanMessage* t;
    unsigned int id=0;
    unsigned char len=0;
 
    t = (yarp::dev::CanMessage*) (can_packet); 
    id=t->getId();
    len=t->getLen();
    unsigned char *candata=t->getData();
    string_id = (candata[1]>>4);
    offset = (candata[1]&0x0F);
    data[string_id].board_id = char(id>>4&0xf);
 
    if (string_id>=MAX_STRINGS) 
    {
        yError("msg from board %d contains an ERROR! (>MAX_STRINGS)\n",data[string_id ].board_id);
        return -1;
    }
 
    for (j=0 ; j<len-2; j++)
        data[string_id].text_buffer[j+offset*6]=candata[j+2];
 
    if (candata[0]==ICUBCANPROTO_PER_MC_MSG__PRINT + 128)
    {
        data[string_id].maybe_last_part = true; 
        data[string_id].expected_length=offset*6+len-2;
    }
 
    if (data[string_id].maybe_last_part)
    {
        data[string_id].current_length=strlen(data[string_id].text_buffer);
 
        if (data[string_id].expected_length==data[string_id].current_length)
            data[string_id].complete = true; //check me
    }
/*
    if (data[string_id].complete)
    {
        print(string_id);
        clear_string(string_id);
    }
*/
/*
    //DEBUG ONLY
    ACE_OS::printf("%d %d\r\n",string_id,offset);
    
    for (j=0 ; j<50; j++)
        ACE_OS::printf("%c",data[string_id].text_buffer[j]);
 
    ACE_OS::printf("\r\n");
 
    for (j=0 ; j<50; j++)
        ACE_OS::printf("%d",data[string_id].text_buffer[j]);
 
    ACE_OS::printf("\r\n");
*/
    if (data[string_id].complete) return string_id;
    else return -1;
}
 
char* can_string_generic::print(int buffer_num, const char* canDevName, int netNum)
{
    if (buffer_num>=MAX_STRINGS) return 0;
 
    yDebug("%s [%d] msg from board %d: %s \n", canDevName, netNum, data[buffer_num].board_id , data[buffer_num].text_buffer);
    
    return data[buffer_num].text_buffer;
}
 
 
#endif