3dmgx2.h
Go to the documentation of this file.
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2008-2010  Willow Garage
00004  *                      
00005  *
00006  *  This library is free software; you can redistribute it and/or
00007  *  modify it under the terms of the GNU Lesser General Public
00008  *  License as published by the Free Software Foundation; either
00009  *  version 2.1 of the License, or (at your option) any later version.
00010  *
00011  *  This library is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  *  Lesser General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU Lesser General Public
00017  *  License along with this library; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  */
00020 
00021 
00022 #ifndef MS_3DMGX2_HH
00023 #define MS_3DMGX2_HH
00024 
00025 #include <fstream>
00026 #include <stdexcept>
00027 #include <stdint.h>
00028 
00029 namespace microstrain_3dmgx2_imu
00030 {
00031 
00033   #define DEF_EXCEPTION(name, parent) \
00034   class name  : public parent { \
00035   public: \
00036     name(const char* msg) : parent(msg) {} \
00037   }
00038 
00039   DEF_EXCEPTION(Exception, std::runtime_error);
00040   DEF_EXCEPTION(TimeoutException, Exception);
00041   DEF_EXCEPTION(CorruptedDataException, Exception);
00042 
00043   #undef DEF_EXCEPTION
00044 
00046 
00091   class IMU
00092   {
00094     static const int TICKS_PER_SEC_GX2  = 19660800;
00095     static const int TICKS_PER_SEC_GX3  = 62500;
00097     static const int MAX_BYTES_SKIPPED  = 1000;
00099     static const unsigned int KF_NUM_SUM= 100;
00101     static const double KF_K_1          = 0.00995031;
00103     static const double KF_K_2          = 0.0000497506;
00104 
00105   public: 
00106 
00108     static const double G               = 9.80665;    
00109 
00111     enum cmd {
00112       CMD_RAW                      =  0xC1,
00113       CMD_ACCEL_ANGRATE            =  0xC2,
00114       CMD_DELVEL_DELANG            =  0xC3,
00115       CMD_CONTINUOUS               =  0xC4,
00116       CMD_ORIENT                   =  0xC5,
00117       CMD_ATT_UPDATE               =  0xC6,
00118       CMD_MAG_VEC                  =  0xC7,
00119       CMD_ACCEL_ANGRATE_ORIENT     =  0xC8,
00120       CMD_WRITE_ACCEL_BIAS         =  0xC9,
00121       CMD_WRITE_GYRO_BIAS          =  0xCA,
00122       CMD_ACCEL_ANGRATE_MAG        =  0xCB,
00123       CMD_ACCEL_ANGRATE_MAG_ORIENT =  0xCC,
00124       CMD_CAPTURE_GYRO_BIAS        =  0xCD,
00125       CMD_EULER                    =  0xCE,
00126       CMD_EULER_ANGRATE            =  0xCF,
00127       CMD_TEMPERATURES             =  0xD1,
00128       CMD_GYROSTAB_ANGRATE_MAG     =  0xD2,
00129       CMD_DELVEL_DELANG_MAG        =  0xD3,
00130       CMD_DEV_ID_STR               =  0xEA,
00131       CMD_STOP_CONTINUOUS          =  0xFA
00132     };
00133 
00135 
00136     enum id_string {
00137       ID_MODEL_NUMBER   = 0,
00138       ID_SERIAL_NUMBER  = 1,
00139       ID_DEVICE_NAME    = 2,
00140       ID_DEVICE_OPTIONS = 3
00141     };
00142 
00144     IMU();
00145 
00146     // Destructor
00147     ~IMU();
00148 
00150 
00156     void openPort(const char *port_name);
00157 
00159     void closePort();
00160 
00162 
00168     void initTime(double fix_off);
00169 
00171 
00181     void initGyros(double* bias_x = 0, double* bias_y = 0, double* bias_z = 0);
00182 
00184 
00192     bool setContinuous(cmd command);
00193 
00195     void stopContinuous();
00196 
00198 
00203     void receiveAccelAngrate(uint64_t *time, double accel[3], double angrate[3]);
00204 
00206 
00211     void receiveDelvelDelang(uint64_t *time, double delvel[3], double delang[3]);
00212 
00214 
00220     void receiveAccelAngrateMag(uint64_t *time, double accel[3], double angrate[3], double mag[3]);
00221 
00223 
00229     void receiveEuler(uint64_t *time, double *roll, double *pitch, double *yaw);
00230 
00232 
00238     void receiveAccelAngrateOrientation(uint64_t *time, double accel[3], double angrate[3], double orientation[9]);
00239 
00241 
00248         void receiveAccelAngrateMagOrientation (uint64_t *time, double accel[3], double angrate[3], double mag[3], double orientation[9]);
00249 
00251 
00256         void receiveRawAccelAngrate(uint64_t *time, double accel[3], double angrate[3]);
00257 
00259 
00262     void setFixedOffset(double fix_off) {fixed_offset = fix_off;};
00263 
00265 
00270     bool getDeviceIdentifierString(id_string type, char id[17]);
00271 
00272   private:
00274     int transact(void *cmd, int cmd_len, void *rep, int rep_len, int timeout = 0);
00275 
00277     int send(void *cmd, int cmd_len);
00278 
00280     int receive(uint8_t command, void *rep, int rep_len, int timeout = 0, uint64_t* sys_time = NULL);
00281 
00283     uint64_t extractTime(uint8_t* addr);
00284 
00286     uint64_t filterTime(uint64_t imu_time, uint64_t sys_time);
00287 
00289     double toDouble(uint64_t time);
00290 
00292     uint64_t toUint64_t(double time);
00293 
00295     int fd;
00296 
00298     uint32_t wraps;
00299 
00301     uint32_t offset_ticks;
00302 
00304     uint32_t last_ticks;
00305 
00307     uint32_t diff_ticks;
00308 
00310     unsigned long long start_time;
00311 
00313     double time_est[2];
00314 
00316     double P_time_est[2][2];
00317 
00319     bool continuous;
00320 
00322     unsigned int counter;
00323 
00325     double fixed_offset, offset, d_offset, sum_meas;
00326 
00328     bool is_gx3;
00329 
00330   };
00331 
00332 }
00333 #endif


microstrain_3dmgx2_imu
Author(s): Jeremy Leibs, Blaise Gassend
autogenerated on Thu Jan 2 2014 11:21:15