iCub-main
embObjInertials.h
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 #ifndef __embObjInertials_h__
4 #define __embObjInertials_h__
5 
6 #include <yarp/dev/DeviceDriver.h>
7 #include <yarp/dev/IAnalogSensor.h>
8 #include <yarp/os/PeriodicThread.h>
9 #include <string>
10 #include <list>
11 #include <mutex>
12 
13 #include <iCub/FactoryInterface.h>
14 #include <iCub/LoggerInterfaces.h>
15 
16 #include "IethResource.h"
17 #include <ethManager.h>
18 #include <abstractEthResource.h>
19 
20 
21 #include "FeatureInterface.h"
22 #include "EoAnalogSensors.h"
23 
24 
25 #include <yarp/os/LogStream.h>
26 
27 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
28 
29 namespace yarp {
30  namespace dev {
31  class embObjInertials;
32  }
33 }
34 
35 #include "serviceParser.h"
36 
37 
38 #define EMBOBJINERTIALS_PUBLISH_OLDSTYLE
39 
40 
41 // -- class embObjInertials
42 class yarp::dev::embObjInertials: public yarp::dev::IAnalogSensor,
43  public yarp::dev::DeviceDriver,
44  public yarp::dev::IThreeAxisGyroscopes,
45  public yarp::dev::IThreeAxisLinearAccelerometers,
46  public eth::IethResource
47 {
48 
49 public:
50 
51 
52 #if defined(EMBOBJINERTIALS_PUBLISH_OLDSTYLE)
53  // 6 channels because we have (pos, type, t, x, y, z)
54  enum { inertials_Channels = 6, inertials_FormatData = 16, inertials_maxNumber = eOas_inertials_maxnumber };
55 #else
56  // 4 channels because we have (t, x, y, z)
57  enum { inertials_Channels = 4, inertials_FormatData = 16, inertials_maxNumber = eOas_inertials_maxnumber };
58 #endif
59 
60 public:
61 
64 
65  // An open function yarp factory compatible
66  bool open(yarp::os::Searchable &config);
67  bool close();
68 
69  // IAnalogSensor interface
70  virtual int read(yarp::sig::Vector &out);
71  virtual int getState(int ch);
72  virtual int getChannels();
73  virtual int calibrateChannel(int ch, double v);
74  virtual int calibrateSensor();
75  virtual int calibrateSensor(const yarp::sig::Vector& value);
76  virtual int calibrateChannel(int ch);
77 
78  // IethResource interface
79  virtual bool initialised();
80  virtual eth::iethresType_t type();
81  virtual bool update(eOprotID32_t id32, double timestamp, void* rxdata);
82 
83 
84  /* IThreeAxisGyroscopes methods */
85  virtual size_t getNrOfThreeAxisGyroscopes() const override;
86  virtual yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override;
87  virtual bool getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override;
88  virtual bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override;
89  virtual bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
90 
91  /* IThreeAxisLinearAccelerometers methods */
92  virtual size_t getNrOfThreeAxisLinearAccelerometers() const override;
93  virtual yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override;
94  virtual bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override;
95  virtual bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override;
96  virtual bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
97 
98 
99 private:
100 
101  string boardIPstring;
102  string boardName;
103  eOipv4addr_t ipv4addr;
104 
105  eth::TheEthManager* ethManager;
107  ServiceParser* parser;
108 
109  bool opened;
110  bool verbosewhenok;
111 
112  unsigned int counterSat;
113  unsigned int counterError;
114  unsigned int counterTimeout;
115 
117  // parameters
118  servConfigInertials_t serviceConfig;
119 
120  mutable std::mutex mtx;
121 
122  vector<double> analogdata;
123  vector<uint16_t> gyrSensors;
124  vector<uint16_t> accSensors;
125 
126 
127  short status;
128  double timeStamp;
129 
130 private:
131 
132  // for all
133  bool fromConfig(yarp::os::Searchable &config);
134  bool initRegulars();
135  void cleanup(void);
136  void printServiceConfig(void);
137 
138  // for inertials
139  bool sendConfig2MTBboards(yarp::os::Searchable &config);
140 
141 
142  // for ??
143  void resetCounters();
144  void getCounters(unsigned int &saturations, unsigned int &errors, unsigned int &timeouts);
145 
146 };
147 
148 
149 #endif
150 
Interface for a factory device; a device that can create objects.
virtual size_t getNrOfThreeAxisGyroscopes() const override
virtual eth::iethresType_t type()
virtual bool getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
virtual size_t getNrOfThreeAxisLinearAccelerometers() const override
virtual int calibrateChannel(int ch, double v)
virtual int read(yarp::sig::Vector &out)
Read a vector from the sensor.
virtual yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override
virtual yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override
virtual bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override
virtual bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
virtual bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override
bool open(yarp::os::Searchable &config)
virtual int getState(int ch)
virtual bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
virtual bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override
static int v
Definition: iCub_Sim.cpp:42
iethresType_t
Definition: IethResource.h:61
Copyright (C) 2008 RobotCub Consortium.
out
Definition: sine.m:8