iCub-main
3dm_gx3.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  * Author: Alberto Cardellino
5  * email: alberto.cardellino@iit.it
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17  */
18 
19 #ifndef __3DM_GX3__
20 #define __3DM_GX3__
21 
22 #include <yarp/dev/DeviceDriver.h>
23 #include <yarp/dev/GenericSensorInterfaces.h>
24 #include <yarp/os/Stamp.h>
25 #include <yarp/dev/PreciselyTimed.h>
26 #include <yarp/os/PeriodicThread.h>
27 #include <mutex>
28 #include <string.h>
29 #include <dataTypes.h>
30 #include <termios.h> // terminal io (serial port) interface
31 
32 
33 namespace yarp{
34  namespace dev{
35  class imu3DM_GX3;
36  }
37 }
38 
40 {
42  short comPort;
43 };
44 
50 class yarp::dev::imu3DM_GX3 : public yarp::dev::IGenericSensor,
51  public yarp::dev::IPreciselyTimed,
52  public yarp::dev::DeviceDriver,
53  public yarp::os::PeriodicThread
54 {
55 private:
56 
57  int fd_ser;
58  int baudrate;
59  int nchannels;
60 
61  // data specific for this imu
62  typedef std::map<int, imu_cmd_t*> cmd_map_t;
63  cmd_map_t cmd_ptr_map;
64 
65  // Data structure specific for each command
66  imu_cmd_t C2_cmd;
67  imu_cmd_t C8_cmd;
68  imu_cmd_t CB_cmd;
69  imu_cmd_t CC_cmd;
70  imu_cmd_t CE_cmd;
71  imu_cmd_t CF_cmd;
72  imu_cmd_t DF_cmd;
73 
74  std::mutex data_mutex;
75 
76  std::string comPortName;
77  data_3DM_GX3_t rawData;
78  yarp::os::Stamp lastStamp;
79 
80 public:
81  imu3DM_GX3();
82  ~imu3DM_GX3();
83 
84  // IGenericSensor interface.
85  virtual bool read(yarp::sig::Vector &out);
86  virtual bool getChannels(int *nc);
87  virtual bool open(yarp::os::Searchable &config);
88  virtual bool calibrate(int ch, double v);
89  virtual bool close();
90  void sample_setting(void);
91 
92  virtual yarp::os::Stamp getLastInputStamp();
93 
94  // Open the device
95  bool open(const XSensMTxParameters &par);
96 
97 private:
98  bool threadInit();
99  void run();
100  void threadRelease();
101 
102 private: // Device specific
103 
104  void stop_continuous(void);
105  // command 0xC2
106  void get_Acc_Ang(float acc[3], float angRate[3], uint64_t *time);
107  // command 0xC8
108  void get_Acc_Ang_Orient(float acc[3], float angRate[3], float orientMat[9], uint64_t *time);
109  // command 0xCB
110  void get_Acc_Ang_Mag(float acc[3], float angRate[3], float mag[3], uint64_t *time);
111  // command 0xCC
112  void get_Acc_Ang_Mag_Orient(float acc[3], float angRate[3], float mag[3], float orientMat[9], uint64_t *time);
113  // command 0xCE
114  void get_Euler(float euler[3], uint64_t *time);
115  // command 0xCF
116  void get_Euler_AngularRate(float euler[3], float angRate[3], uint64_t *time);
117  // command 0xDF
118  void get_Quaternion(float quat[4], uint64_t *time);
119 
120  // serial handling
121  inline int flush() {return tcflush(fd_ser, TCIOFLUSH);
122  }
123 };
124 
125 
126 #endif // __3DM_GX3__
yarp::dev::imu3DM_GX3::imu3DM_GX3
imu3DM_GX3()
Definition: 3dm_gx3.cpp:23
quat
_4f_vect_t quat
Definition: dataTypes.h:175
out
out
Definition: sine.m:8
yarp::dev::imu3DM_GX3::getLastInputStamp
virtual yarp::os::Stamp getLastInputStamp()
Definition: 3dm_gx3.cpp:226
XSensMTxParameters::comPort
short comPort
Definition: 3dm_gx3.h:42
yarp::dev::imu3DM_GX3::~imu3DM_GX3
~imu3DM_GX3()
Definition: 3dm_gx3.cpp:67
angRate
_3f_vect_t angRate
Definition: dataTypes.h:4
dataTypes.h
orientMat
_3f_matx_t orientMat
Definition: dataTypes.h:107
yarp::dev::imu3DM_GX3::getChannels
virtual bool getChannels(int *nc)
Definition: 3dm_gx3.cpp:182
yarp::dev::imu3DM_GX3::read
virtual bool read(yarp::sig::Vector &out)
Definition: 3dm_gx3.cpp:197
yarp::dev::imu3DM_GX3::sample_setting
void sample_setting(void)
Definition: 3dm_gx3.cpp:141
imu_cmd_t
Definition: dataTypes.h:201
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
data_3DM_GX3_t
Definition: dataTypes.h:186
XSensMTxParameters
Definition: 3dm_gx3.h:39
v
static int v
Definition: iCub_Sim.cpp:42
XSensMTxParameters::comPortString
std::string comPortString
Definition: 3dm_gx3.h:41
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
yarp::dev::imu3DM_GX3::calibrate
virtual bool calibrate(int ch, double v)
Definition: 3dm_gx3.cpp:189
acc
_3f_vect_t acc
Definition: dataTypes.h:3
mag
_3f_vect_t mag
Definition: dataTypes.h:131
yarp::dev::imu3DM_GX3::close
virtual bool close()
Definition: 3dm_gx3.cpp:170
time
degrees time
Definition: sine.m:5
yarp::dev::imu3DM_GX3::open
virtual bool open(yarp::os::Searchable &config)
Definition: 3dm_gx3.cpp:74
yarp::dev::imu3DM_GX3
Driver for 3DM_GX3 IMU unit from MicroStrain.
Definition: 3dm_gx3.h:50