3dmgx2.h
Go to the documentation of this file.
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2008-2010 Willow Garage
4  *
5  *
6  * This library is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU Lesser General Public
8  * License as published by the Free Software Foundation; either
9  * version 2.1 of the License, or (at your option) any later version.
10  *
11  * This library is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with this library; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19  */
20 
21 
22 #ifndef MS_3DMGX2_HH
23 #define MS_3DMGX2_HH
24 
25 #include <fstream>
26 #include <stdexcept>
27 #include <stdint.h>
28 
30 {
31 
33  #define DEF_EXCEPTION(name, parent) \
34  class name : public parent { \
35  public: \
36  name(const char* msg) : parent(msg) {} \
37  }
38 
39  DEF_EXCEPTION(Exception, std::runtime_error);
40  DEF_EXCEPTION(TimeoutException, Exception);
41  DEF_EXCEPTION(CorruptedDataException, Exception);
42 
43  #undef DEF_EXCEPTION
44 
46 
91  class IMU
92  {
93 // from C++11, constexpr specifier is requred for double
94 #if __cplusplus > 199711L
95 #define const constexpr
96 #endif
97  static const int TICKS_PER_SEC_GX2 = 19660800;
99  static const int TICKS_PER_SEC_GX3 = 62500;
101  static const int MAX_BYTES_SKIPPED = 1000;
103  static const unsigned int KF_NUM_SUM= 100;
105  static const double KF_K_1 = 0.00995031;
107  static const double KF_K_2 = 0.0000497506;
108 
109  public:
110 
112  static const double G = 9.80665;
113 #if __cplusplus > 199711L
114 #undef const
115 #endif
116 
118  enum cmd {
119  CMD_RAW = 0xC1,
123  CMD_ORIENT = 0xC5,
125  CMD_MAG_VEC = 0xC7,
132  CMD_EULER = 0xCE,
139  };
140 
142 
143  enum id_string {
148  };
149 
151  IMU();
152 
153  // Destructor
154  ~IMU();
155 
157 
163  void openPort(const char *port_name);
164 
166  void closePort();
167 
169 
175  void initTime(double fix_off);
176 
178 
188  void initGyros(double* bias_x = 0, double* bias_y = 0, double* bias_z = 0);
189 
191 
199  bool setContinuous(cmd command);
200 
202  void stopContinuous();
203 
205 
210  void receiveAccelAngrate(uint64_t *time, double accel[3], double angrate[3]);
211 
213 
218  void receiveDelvelDelang(uint64_t *time, double delvel[3], double delang[3]);
219 
221 
227  void receiveAccelAngrateMag(uint64_t *time, double accel[3], double angrate[3], double mag[3]);
228 
230 
236  void receiveEuler(uint64_t *time, double *roll, double *pitch, double *yaw);
237 
239 
245  void receiveAccelAngrateOrientation(uint64_t *time, double accel[3], double angrate[3], double orientation[9]);
246 
248 
255  void receiveAccelAngrateMagOrientation (uint64_t *time, double accel[3], double angrate[3], double mag[3], double orientation[9]);
256 
258 
263  void receiveRawAccelAngrate(uint64_t *time, double accel[3], double angrate[3]);
264 
266 
269  void setFixedOffset(double fix_off) {fixed_offset = fix_off;};
270 
272 
277  bool getDeviceIdentifierString(id_string type, char id[17]);
278 
279  private:
281  int transact(void *cmd, int cmd_len, void *rep, int rep_len, int timeout = 0);
282 
284  int send(void *cmd, int cmd_len);
285 
287  int receive(uint8_t command, void *rep, int rep_len, int timeout = 0, uint64_t* sys_time = NULL);
288 
290  uint64_t extractTime(uint8_t* addr);
291 
293  uint64_t filterTime(uint64_t imu_time, uint64_t sys_time);
294 
296  double toDouble(uint64_t time);
297 
299  uint64_t toUint64_t(double time);
300 
302  int fd;
303 
305  uint32_t wraps;
306 
308  uint32_t offset_ticks;
309 
311  uint32_t last_ticks;
312 
314  uint32_t diff_ticks;
315 
317  unsigned long long start_time;
318 
320  double time_est[2];
321 
323  double P_time_est[2][2];
324 
327 
329  unsigned int counter;
330 
333 
335  bool is_gx3;
336 
337  };
338 
339 }
340 #endif
int fd
The file descriptor.
Definition: 3dmgx2.h:302
static const int TICKS_PER_SEC_GX3
Definition: 3dmgx2.h:99
void initGyros(double *bias_x=0, double *bias_y=0, double *bias_z=0)
Initial gyros.
Definition: 3dmgx2.cc:224
unsigned long long start_time
The time at which the imu was started.
Definition: 3dmgx2.h:317
static const int MAX_BYTES_SKIPPED
Maximum bytes allowed to be skipped when seeking a message.
Definition: 3dmgx2.h:101
IMU()
Constructor.
Definition: 3dmgx2.cc:98
static const int TICKS_PER_SEC_GX2
IMU internal ticks/second.
Definition: 3dmgx2.h:98
void openPort(const char *port_name)
Open the port.
Definition: 3dmgx2.cc:113
void closePort()
Close the port.
Definition: 3dmgx2.cc:170
int send(void *cmd, int cmd_len)
Send a single packet frmo the IMU.
Definition: 3dmgx2.cc:620
void receiveDelvelDelang(uint64_t *time, double delvel[3], double delang[3])
Read a message of type "DELVEL_DELANG".
Definition: 3dmgx2.cc:422
double fixed_offset
Variables used by the kalman computation.
Definition: 3dmgx2.h:332
void receiveAccelAngrate(uint64_t *time, double accel[3], double angrate[3])
Read a message of type "ACCEL_ANGRATE".
Definition: 3dmgx2.cc:389
int transact(void *cmd, int cmd_len, void *rep, int rep_len, int timeout=0)
Send a command to the IMU and wait for a reply.
Definition: 3dmgx2.cc:608
void initTime(double fix_off)
Initialize timing variables.
Definition: 3dmgx2.cc:196
bool continuous
Whether continuous mode is enabled.
Definition: 3dmgx2.h:326
static const unsigned int KF_NUM_SUM
Number of KF samples to sum over.
Definition: 3dmgx2.h:103
void receiveRawAccelAngrate(uint64_t *time, double accel[3], double angrate[3])
Read a message of type "CMD_RAW".
Definition: 3dmgx2.cc:553
void setFixedOffset(double fix_off)
Set the fixed time offset.
Definition: 3dmgx2.h:269
A class for interfacing to the microstrain 3dmgx2 and inertialink IMUs.
Definition: 3dmgx2.h:91
cmd
Enumeration of possible IMU commands.
Definition: 3dmgx2.h:118
uint32_t wraps
The number of times the imu has wrapped.
Definition: 3dmgx2.h:305
void stopContinuous()
Take the device out of continous mode.
Definition: 3dmgx2.cc:277
void receiveAccelAngrateOrientation(uint64_t *time, double accel[3], double angrate[3], double orientation[9])
Read a message of type "ACCEL_ANGRATE_ORIENTATION".
Definition: 3dmgx2.cc:346
uint32_t last_ticks
The last number of ticks for computing wraparound.
Definition: 3dmgx2.h:311
void receiveEuler(uint64_t *time, double *roll, double *pitch, double *yaw)
Read a message of type "EULER".
Definition: 3dmgx2.cc:456
uint32_t offset_ticks
The number of ticks the initial offset is off by.
Definition: 3dmgx2.h:308
ROSLIB_DECL std::string command(const std::string &cmd)
uint32_t diff_ticks
The different in the number of ticks.
Definition: 3dmgx2.h:314
DEF_EXCEPTION(Exception, std::runtime_error)
bool setContinuous(cmd command)
Put the device in continuous mode.
Definition: 3dmgx2.cc:252
uint64_t extractTime(uint8_t *addr)
Extract time from a pointer into an imu buffer.
Definition: 3dmgx2.cc:587
int receive(uint8_t command, void *rep, int rep_len, int timeout=0, uint64_t *sys_time=NULL)
Receive a particular message from the IMU.
Definition: 3dmgx2.cc:671
uint64_t filterTime(uint64_t imu_time, uint64_t sys_time)
Run the filter on the imu time and system times.
Definition: 3dmgx2.cc:724
id_string
Enumeration of possible identifier strings for the getDeviceIdentifierString command.
Definition: 3dmgx2.h:143
double time_est[2]
The estimate of time offset and driftrate.
Definition: 3dmgx2.h:320
static const double G
Gravity (m/sec^2)
Definition: 3dmgx2.h:112
double toDouble(uint64_t time)
Convert the uint64_t time to a double for numerical computations.
Definition: 3dmgx2.cc:750
bool is_gx3
Is the IMU a GX3?
Definition: 3dmgx2.h:335
uint64_t toUint64_t(double time)
Convert the double time back to a uint64_t.
Definition: 3dmgx2.cc:760
bool getDeviceIdentifierString(id_string type, char id[17])
Read one of the device identifier strings.
Definition: 3dmgx2.cc:476
static const double KF_K_2
Second KF term.
Definition: 3dmgx2.h:107
static const double KF_K_1
First KF term.
Definition: 3dmgx2.h:105
void receiveAccelAngrateMagOrientation(uint64_t *time, double accel[3], double angrate[3], double mag[3], double orientation[9])
Read a message of type "ACCEL_ANGRATE_MAG_ORIENT".
Definition: 3dmgx2.cc:505
unsigned int counter
A counter used by the filter.
Definition: 3dmgx2.h:329
void receiveAccelAngrateMag(uint64_t *time, double accel[3], double angrate[3], double mag[3])
Read a message of type "ACCEL_ANGRATE_MAG".
Definition: 3dmgx2.cc:304
double P_time_est[2][2]
The covariances on time offset and driftrate.
Definition: 3dmgx2.h:323


microstrain_3dmgx2_imu
Author(s): Jeremy Leibs, Blaise Gassend
autogenerated on Tue Jul 2 2019 19:16:18