rt_usb_9axisimu.hpp
Go to the documentation of this file.
1 /*
2  * rt_usb_9axisimu.hpp
3  *
4  * License: BSD-3-Clause
5  *
6  * Copyright (c) 2015-2020 RT Corporation <support@rt-net.jp>
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * 1. Redistributions of source code must retain the above copyright notice,
13  * this list of conditions and the following disclaimer.
14  * 2. Redistributions in binary form must reproduce the above copyright
15  * notice, this list of conditions and the following disclaimer in the
16  * documentation and/or other materials provided with the distribution.
17  * 3. Neither the name of RT Corporation nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 #ifndef RT_USB_9AXISIMU_H_
35 #define RT_USB_9AXISIMU_H_
36 
37 #include <fcntl.h>
38 #include <stdlib.h>
39 #include <sys/ioctl.h>
40 #include <sys/stat.h>
41 #include <sys/types.h>
42 #include <termios.h>
43 #include <unistd.h>
44 
45 #include <cmath>
46 #include <sstream>
47 
48 /**********************************************************************************************************
49  *
50  * Constants
51  *
52  **********************************************************************************************************/
53 
54 namespace rt_usb_9axisimu
55 {
56 class Consts
57 {
58 public:
60  {
90  };
91 
93  {
106  };
107 
108  // Convertor
109  const double CONVERTOR_RAW2G;
110  const double CONVERTOR_RAW2DPS;
114  const double CONVERTOR_G2A;
115  const double CONVERTOR_D2R;
116  const double CONVERTOR_UT2T;
120 
122  : CONVERTOR_RAW2G(2048) // for linear_acceleration (raw data to [g])
123  , CONVERTOR_RAW2DPS(16.4) // for angular_velocity (raw data to [degree/s])
124  , CONVERTOR_RAW2UT(0.3) // for magnetic_field (raw data to [uT])
125  , CONVERTOR_RAW2C_1(340) // for temperature (raw data to celsius)
126  , CONVERTOR_RAW2C_2(35) // for temperature (raw data to celsius)
127  , CONVERTOR_G2A(9.80665) // for linear_acceleration (g to m/s^2)
128  , CONVERTOR_D2R(M_PI / 180.0) // for angular_velocity (degree to radian)
129  , CONVERTOR_UT2T(1000000) // for magnetic_field (uT to Tesla)
130  , DEFAULT_LINEAR_ACCELERATION_STDDEV(0.023145) // Default of square root of the
131  // linear_acceleration_covariance diagonal elements in
132  // m/s^2.
133  , DEFAULT_ANGULAR_VELOCITY_STDDEV(0.0010621) // Default of square root of the
134  // angular_velocity_covariance diagonal elements in
135  // rad/s.
136  , DEFAULT_MAGNETIC_FIELD_STDDEV(0.00000080786) // Default of square root of the
137  // magnetic_field_covariance diagonal elements in
138  // Tesla.
139  {
140  }
141 
143  {
144  }
145 
146  // Method to adjust convertors to firmware version
147  void ChangeConvertor(const int firmware_ver)
148  {
149  if (firmware_ver == 5)
150  {
151  CONVERTOR_RAW2UT = 0.3;
152  CONVERTOR_RAW2C_1 = 340;
153  CONVERTOR_RAW2C_2 = 35;
154  }
155  else if (firmware_ver >= 6)
156  {
157  CONVERTOR_RAW2UT = 0.15;
158  CONVERTOR_RAW2C_1 = 333.87;
159  CONVERTOR_RAW2C_2 = 21;
160  }
161  }
162 };
163 
164 /**********************************************************************************************************
165  *
166  * Serial port abstraction
167  *
168  **********************************************************************************************************/
169 
171 {
172 private:
173  std::string port_name_; // ex) "/dev/ttyACM0"
174  struct termios old_settings_;
175  int port_fd_;
176 
177 public:
178  SerialPort(const char* port = "") : port_name_(port), port_fd_(-1)
179  {
180  }
181 
183  {
184  closeSerialPort();
185  }
186 
187  bool openPort(const char* port)
188  {
189  port_name_ = port;
190  return openSerialPort();
191  }
192 
194  {
195  int fd = 0;
196 
197  if (port_fd_ > 0)
198  {
199  return true;
200  }
201 
202  fd = open(port_name_.c_str(), O_RDWR | O_NOCTTY); // Open serial port
203  if (fd < 0)
204  {
205  return false; // Port open error
206  }
207 
208  struct termios settings;
209 
210  tcgetattr(fd, &old_settings_);
211 
212  cfsetispeed(&settings, B57600);
213  cfmakeraw(&settings);
214 
215  tcsetattr(fd, TCSANOW, &settings);
216 
217  port_fd_ = fd;
218 
219  return (fd > 0);
220  }
221 
223  {
224  if (port_fd_ > 0)
225  {
226  tcsetattr(port_fd_, TCSANOW, &old_settings_);
227  close(port_fd_); // Close serial port
228  port_fd_ = -1;
229  }
230  }
231 
232  int readFromDevice(unsigned char* buf, unsigned int buf_len)
233  {
234  if (port_fd_ < 0)
235  {
236  return -1;
237  }
238 
239  return read(port_fd_, buf, buf_len);
240  }
241 
242  int writeToDevice(unsigned char* data, unsigned int data_len)
243  {
244  if (port_fd_ < 0)
245  {
246  return -1;
247  }
248 
249  return write(port_fd_, data, data_len);
250  }
251 };
252 
253 /**************************************************************************
254  *
255  * IMU
256  *
257  **************************************************************************/
258 
259 // Class to store either raw integer imu data or converted physical quantity
260 template <typename Type>
261 class ImuData
262 {
263 public:
266  Type ax, ay, az, gx, gy, gz, mx, my, mz, temperature;
267 
269  {
270  reset();
271  }
272 
274  {
275  }
276 
277  inline void reset()
278  {
279  firmware_ver = 5;
280  timestamp = -1;
281  ax = ay = az = gx = gy = gz = mx = my = mz = temperature = 0;
282  }
283 };
284 
286 {
287 private:
291 
292 public:
294  {
295  reset();
296  }
297 
299  {
300  }
301 
302  void reset()
303  {
304  imu_raw_data_.reset();
305  imu_.reset();
306  }
307 
309  {
310  imu_raw_data_ = i;
311  }
312 
314  {
315  imu_ = i;
316  }
317 
318  // Method to convert raw integer imu_ data to physical quantity
320  {
321  // Adjust convertors to firmware version
322  consts_.ChangeConvertor(imu_raw_data_.firmware_ver);
323 
324  imu_.firmware_ver = imu_raw_data_.firmware_ver;
325  imu_.timestamp = imu_raw_data_.timestamp;
326 
327  // Convert raw data to [g]
328  imu_.ax = imu_raw_data_.ax / consts_.CONVERTOR_RAW2G;
329  imu_.ay = imu_raw_data_.ay / consts_.CONVERTOR_RAW2G;
330  imu_.az = imu_raw_data_.az / consts_.CONVERTOR_RAW2G;
331 
332  // Convert raw data to [degree/s]
333  imu_.gx = imu_raw_data_.gx / consts_.CONVERTOR_RAW2DPS;
334  imu_.gy = imu_raw_data_.gy / consts_.CONVERTOR_RAW2DPS;
335  imu_.gz = imu_raw_data_.gz / consts_.CONVERTOR_RAW2DPS;
336 
337  // Convert raw data to [uT]
338  imu_.mx = imu_raw_data_.mx * consts_.CONVERTOR_RAW2UT;
339  imu_.my = imu_raw_data_.my * consts_.CONVERTOR_RAW2UT;
340  imu_.mz = imu_raw_data_.mz * consts_.CONVERTOR_RAW2UT;
341 
342  // Convert raw data to celsius
343  imu_.temperature = imu_raw_data_.temperature / consts_.CONVERTOR_RAW2C_1 + consts_.CONVERTOR_RAW2C_2;
344  }
345 
347  {
348  return imu_;
349  }
350 };
351 }; // namespace rt_usb_9axisimu
352 
353 #endif
ImuData< int16_t > imu_raw_data_
SerialPort(const char *port="")
const double DEFAULT_ANGULAR_VELOCITY_STDDEV
void setImuRawData(ImuData< int16_t > &i)
const double DEFAULT_LINEAR_ACCELERATION_STDDEV
bool openPort(const char *port)
void setImuData(ImuData< double > &i)
const double DEFAULT_MAGNETIC_FIELD_STDDEV
void ChangeConvertor(const int firmware_ver)
int writeToDevice(unsigned char *data, unsigned int data_len)
ImuData< double > getImuData()
int readFromDevice(unsigned char *buf, unsigned int buf_len)


rt_usb_9axisimu_driver
Author(s): RT Corporation
autogenerated on Mon Dec 14 2020 03:34:37