34 #ifndef RT_USB_9AXISIMU_H_ 35 #define RT_USB_9AXISIMU_H_ 39 #include <sys/ioctl.h> 41 #include <sys/types.h> 122 : CONVERTOR_RAW2G(2048)
123 , CONVERTOR_RAW2DPS(16.4)
124 , CONVERTOR_RAW2UT(0.3)
125 , CONVERTOR_RAW2C_1(340)
126 , CONVERTOR_RAW2C_2(35)
127 , CONVERTOR_G2A(9.80665)
128 , CONVERTOR_D2R(M_PI / 180.0)
129 , CONVERTOR_UT2T(1000000)
130 , DEFAULT_LINEAR_ACCELERATION_STDDEV(0.023145)
133 , DEFAULT_ANGULAR_VELOCITY_STDDEV(0.0010621)
136 , DEFAULT_MAGNETIC_FIELD_STDDEV(0.00000080786)
149 if (firmware_ver == 5)
151 CONVERTOR_RAW2UT = 0.3;
152 CONVERTOR_RAW2C_1 = 340;
153 CONVERTOR_RAW2C_2 = 35;
155 else if (firmware_ver >= 6)
157 CONVERTOR_RAW2UT = 0.15;
158 CONVERTOR_RAW2C_1 = 333.87;
159 CONVERTOR_RAW2C_2 = 21;
174 struct termios old_settings_;
178 SerialPort(
const char* port =
"") : port_name_(port), port_fd_(-1)
190 return openSerialPort();
202 fd = open(port_name_.c_str(), O_RDWR | O_NOCTTY);
208 struct termios settings;
210 tcgetattr(fd, &old_settings_);
212 cfsetispeed(&settings, B57600);
213 cfmakeraw(&settings);
215 tcsetattr(fd, TCSANOW, &settings);
226 tcsetattr(port_fd_, TCSANOW, &old_settings_);
239 return read(port_fd_, buf, buf_len);
249 return write(port_fd_, data, data_len);
260 template <
typename Type>
281 ax = ay = az = gx = gy = gz = mx = my = mz = temperature = 0;
304 imu_raw_data_.
reset();
ImuData< int16_t > imu_raw_data_
SerialPort(const char *port="")
const double DEFAULT_ANGULAR_VELOCITY_STDDEV
void setImuRawData(ImuData< int16_t > &i)
void convertRawDataUnit()
const double DEFAULT_LINEAR_ACCELERATION_STDDEV
bool openPort(const char *port)
void setImuData(ImuData< double > &i)
const double CONVERTOR_D2R
const double DEFAULT_MAGNETIC_FIELD_STDDEV
void ChangeConvertor(const int firmware_ver)
int writeToDevice(unsigned char *data, unsigned int data_len)
const double CONVERTOR_RAW2G
ImuData< double > getImuData()
const double CONVERTOR_UT2T
int readFromDevice(unsigned char *buf, unsigned int buf_len)
const double CONVERTOR_G2A
const double CONVERTOR_RAW2DPS