iCub-main
CanBusInertialMTB.h
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 // Copyright: (C) 2013 iCub Facility, Istituto Italiano di Tecnologia
4 // Authors: Marco Randazzo and Lorenzo Natale <marco.randazzo@iit.it>
5 // CopyPolicy: Released under the terms of the GNU GPL v2.0.
6 
7 #ifndef __CANBUSINERTIALMTB_H__
8 #define __CANBUSINERTIALMTB_H__
9 
10 #include <mutex>
11 #include <yarp/os/PeriodicThread.h>
12 #include <yarp/dev/IAnalogSensor.h>
13 #include <yarp/dev/PolyDriver.h>
14 #include <yarp/dev/CanBusInterface.h>
15 #include <yarp/sig/Vector.h>
16 
17 #include <vector>
18 
19 using namespace yarp::os;
20 using namespace yarp::dev;
21 
22 
23 
24 
26 {
27  unsigned short boardId;
28  double accTimeStamp;
29  double gyroTimeStamp;
30  unsigned char enabledSensors;
31  unsigned char sensorPeriod;
33  unsigned int nrOfChannels;
34  unsigned int vectorOffset;
35 };
36 
60 class CanBusInertialMTB : public PeriodicThread, public yarp::dev::IAnalogSensor, public DeviceDriver
61 {
62 private:
66  bool validateConf(yarp::os::Searchable& config,
67  std::vector<int> & canAddresses);
68 
69  void setPrivateBoardStatus(int boardIndex, short status);
70  void setPrivateBoardAccStatus(int boardIndex, short status);
71  void setPrivateBoardGyroStatus(int boardIndex, short status);
72 
73 
74 protected:
75  PolyDriver driver;
76  ICanBus *pCanBus;
77  ICanBufferFactory *pCanBufferFactory;
78  CanBuffer inBuffer;
79  CanBuffer outBuffer;
80 
81  std::mutex mtx;
82 
83  std::vector<MTBInertialBoardInfo> boards;
84 
86 
87  yarp::sig::Vector data;
88  yarp::sig::Vector privateData;
89  std::vector<short> sharedStatus;
90  std::vector<short> privateStatus;
93 
94  bool initted;
95  int count;
96 public:
97  CanBusInertialMTB(int period=20) : PeriodicThread((double)period/1000.0),
98  initted(false),
99  count(0)
100  {}
101 
102 
104  {
105  }
106 
107  virtual bool open(yarp::os::Searchable& config);
108  virtual bool close();
109 
110  //IAnalogSensor interface
111  virtual int read(yarp::sig::Vector &out);
112  virtual int getState(int ch);
113  virtual int getChannels();
114  int calibrateSensor();
115  virtual int calibrateChannel(int ch, double v);
116 
117  virtual int calibrateSensor(const yarp::sig::Vector& v);
118  virtual int calibrateChannel(int ch);
119 
120  virtual bool threadInit();
121  virtual void threadRelease();
122  virtual void run();
123 };
124 
125 #endif
canbusinertialmtb : driver for CAN communication with inertial sensors (accelerometers,...
std::vector< short > sharedStatus
std::vector< short > privateStatus
ICanBufferFactory * pCanBufferFactory
std::vector< MTBInertialBoardInfo > boards
yarp::sig::Vector privateData
yarp::sig::Vector data
CanBusInertialMTB(int period=20)
static int v
Definition: iCub_Sim.cpp:42
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
Definition: ethParser.cpp:92
out
Definition: sine.m:8
unsigned char enabledSensors
unsigned short boardId
unsigned char sensorPeriod
unsigned int vectorOffset
offset of the board data in the output vector
unsigned int nrOfChannels
6 if extAccAndGyro, 3 if acc