iCub-main
ST_M1.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) 2015 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 __imuST_M1__
20 #define __imuST_M1__
21 
22 #include <mutex>
23 #include <string.h>
24 #include <termios.h> // terminal io (serial port) interface
25 
26 #include <yarp/dev/DeviceDriver.h>
27 #include <yarp/dev/IGenericSensor.h>
28 #include <yarp/os/Stamp.h>
29 #include <yarp/dev/IPreciselyTimed.h>
30 #include <yarp/os/PeriodicThread.h>
31 
32 #include <ST_M1_dataType.h>
33 
34 #include <arpa/inet.h>
35 
36 namespace yarp{
37  namespace dev{
38  class imuST_M1;
39  }
40 }
41 
42 
43 // union IMUData {
44 // char packet[53];
45 // Pippo pippo;
46 // };
47 
57 class yarp::dev::imuST_M1 : public yarp::dev::IGenericSensor,
58  public yarp::dev::IPreciselyTimed,
59  public yarp::dev::DeviceDriver,
60  public yarp::os::PeriodicThread
61 {
62 private:
63 
64  bool opened;
65  int fd_ser;
66  int baudrate;
67  int nchannels;
68  short comPort;
69  std::string comPortString;
70 
71  // data specific for this imu
72 // typedef std::map<int, imu_cmd_t*> cmd_map_t;
73 // cmd_map_t cmd_ptr_map;
74  int expected_packet_size;
75  int expected_payload_size;
76  char *buffer;
77 // IMUData data;
78 
79  int progressiv_num;
80  double *temp_euler;
81  double *temp_acc;
82  double *temp_gyro;
83  double *temp_mag;
84  double temp_data[12];
85 
86  std::mutex data_mutex;
87 
88  std::string comPortName;
89  ST_IMU_Frame rawData;
90  yarp::os::Stamp lastStamp;
91 
92 public:
93  imuST_M1();
94  ~imuST_M1();
95 
96  // IGenericSensor interface.
97  virtual bool read(yarp::sig::Vector &out);
98  virtual bool getChannels(int *nc);
99  virtual bool open(yarp::os::Searchable &config);
100  virtual bool calibrate(int ch, double v);
101  virtual bool close();
102  void sample_setting(void);
103 
104  virtual yarp::os::Stamp getLastInputStamp();
105 
106 private:
107  bool threadInit();
108  void run();
109  void threadRelease();
110  Pippo *pippo;
111  Pippo outVals;
112  bool verbose;
113 
114 private: // Device specific
115  float *euler_float;
116 
117 // void stop_continuous(void);
118 // // command 0xC2
119 // void get_Acc_Ang(float acc[3], float angRate[3], uint64_t *time);
120 // // command 0xC8
121 // void get_Acc_Ang_Orient(float acc[3], float angRate[3], float orientMat[9], uint64_t *time);
122 // // command 0xCB
123 // void get_Acc_Ang_Mag(float acc[3], float angRate[3], float mag[3], uint64_t *time);
124 // // command 0xCC
125 // void get_Acc_Ang_Mag_Orient(float acc[3], float angRate[3], float mag[3], float orientMat[9], uint64_t *time);
126 // // command 0xCE
127 // void get_Euler(float euler[3], uint64_t *time);
128 // // command 0xCF
129 // void get_Euler_AngularRate(float euler[3], float angRate[3], uint64_t *time);
130 // // command 0xDF
131 // void get_Quaternion(float quat[4], uint64_t *time);
132 //
133 // // serial handling
134 // inline int flush() {return tcflush(fd_ser, TCIOFLUSH); }
135 };
136 
137 
138 #endif // __imuST_M1__
imuST_M1 : driver for 3DM_GX3 IMU unit from MicroStrain
Definition: ST_M1.h:61
virtual bool close()
Definition: ST_M1.cpp:256
virtual bool read(yarp::sig::Vector &out)
Definition: ST_M1.cpp:299
void sample_setting(void)
Definition: ST_M1.cpp:194
virtual bool calibrate(int ch, double v)
Definition: ST_M1.cpp:291
virtual yarp::os::Stamp getLastInputStamp()
Definition: ST_M1.cpp:326
virtual bool getChannels(int *nc)
Definition: ST_M1.cpp:284
virtual bool open(yarp::os::Searchable &config)
Definition: ST_M1.cpp:41
static int v
Definition: iCub_Sim.cpp:42
Copyright (C) 2008 RobotCub Consortium.
out
Definition: sine.m:8