CanBusInertialMTB.h

Go to the documentation of this file.

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
// Copyright: (C) 2013 iCub Facility, Istituto Italiano di Tecnologia
// Authors: Marco Randazzo and Lorenzo Natale <marco.randazzo@iit.it>
// CopyPolicy: Released under the terms of the GNU GPL v2.0.
 
#ifndef __CANBUSINERTIALMTB_H__
#define __CANBUSINERTIALMTB_H__
 
#include <mutex>
#include <yarp/os/PeriodicThread.h>
#include <yarp/dev/IAnalogSensor.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/CanBusInterface.h>
#include <yarp/sig/Vector.h>
 
#include <vector>
 
using namespace yarp::os;
using namespace yarp::dev;
 
 
 
 
struct MTBInertialBoardInfo
{
        unsigned short     boardId;
        double             accTimeStamp;
        double             gyroTimeStamp;
        unsigned char      enabledSensors;
        unsigned char      sensorPeriod;
        bool               enabledGyro;
        unsigned int       nrOfChannels; 
        unsigned int       vectorOffset; 
};
 
class CanBusInertialMTB : public PeriodicThread, public yarp::dev::IAnalogSensor, public DeviceDriver
{
private:
    bool validateConf(yarp::os::Searchable& config,
                      std::vector<int> & canAddresses);
 
    void setPrivateBoardStatus(int boardIndex, short status);
    void setPrivateBoardAccStatus(int boardIndex, short status);
    void setPrivateBoardGyroStatus(int boardIndex, short status);
 
 
protected:
    PolyDriver         driver;
    ICanBus            *pCanBus;
    ICanBufferFactory  *pCanBufferFactory;
    CanBuffer          inBuffer;
    CanBuffer          outBuffer;
 
    std::mutex mtx;
 
    std::vector<MTBInertialBoardInfo> boards;
 
    int nrOfTotalChannels;
 
    yarp::sig::Vector  data;
    yarp::sig::Vector  privateData;
    std::vector<short> sharedStatus;
    std::vector<short> privateStatus;
    short sharedGlobalStatus;
    short privateGlobalStatus;
 
    bool              initted;
    int               count;
public:
    CanBusInertialMTB(int period=20) : PeriodicThread((double)period/1000.0),
                                       initted(false),
                                       count(0)
    {}
 
 
    ~CanBusInertialMTB()
    {
    }
 
    virtual bool open(yarp::os::Searchable& config);
    virtual bool close();
 
    //IAnalogSensor interface
    virtual int read(yarp::sig::Vector &out);
    virtual int getState(int ch);
    virtual int getChannels();
    int calibrateSensor();
    virtual int calibrateChannel(int ch, double v);
 
    virtual int calibrateSensor(const yarp::sig::Vector& v);
    virtual int calibrateChannel(int ch);
 
    virtual bool threadInit();
    virtual void threadRelease();
    virtual void run();
};
 
#endif