3dmgx2.h

Go to the documentation of this file.

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2008-2010  Willow Garage
 *                      
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
 
 
#ifndef MS_3DMGX2_HH
#define MS_3DMGX2_HH
 
#include <fstream>
#include <stdexcept>
#include <stdint.h>
 
namespace microstrain_3dmgx2_imu
{
 
  #define DEF_EXCEPTION(name, parent) \
  class name  : public parent { \
  public: \
    name(const char* msg) : parent(msg) {} \
  }
 
  DEF_EXCEPTION(Exception, std::runtime_error);
  DEF_EXCEPTION(TimeoutException, Exception);
  DEF_EXCEPTION(CorruptedDataException, Exception);
 
  #undef DEF_EXCEPTION
 
 
  class IMU
  {
// from C++11, constexpr specifier is requred for double
#if __cplusplus > 199711L
#define const constexpr
#endif
    static const int TICKS_PER_SEC_GX2  = 19660800;
    static const int TICKS_PER_SEC_GX3  = 62500;
    static const int MAX_BYTES_SKIPPED  = 1000;
    static const unsigned int KF_NUM_SUM= 100;
    static const double KF_K_1          = 0.00995031;
    static const double KF_K_2          = 0.0000497506;
 
  public: 
 
    static const double G               = 9.80665;    
#if __cplusplus > 199711L
#undef const
#endif
 
    enum cmd {
      CMD_RAW                      =  0xC1,
      CMD_ACCEL_ANGRATE            =  0xC2,
      CMD_DELVEL_DELANG            =  0xC3,
      CMD_CONTINUOUS               =  0xC4,
      CMD_ORIENT                   =  0xC5,
      CMD_ATT_UPDATE               =  0xC6,
      CMD_MAG_VEC                  =  0xC7,
      CMD_ACCEL_ANGRATE_ORIENT     =  0xC8,
      CMD_WRITE_ACCEL_BIAS         =  0xC9,
      CMD_WRITE_GYRO_BIAS          =  0xCA,
      CMD_ACCEL_ANGRATE_MAG        =  0xCB,
      CMD_ACCEL_ANGRATE_MAG_ORIENT =  0xCC,
      CMD_CAPTURE_GYRO_BIAS        =  0xCD,
      CMD_EULER                    =  0xCE,
      CMD_EULER_ANGRATE            =  0xCF,
      CMD_TEMPERATURES             =  0xD1,
      CMD_GYROSTAB_ANGRATE_MAG     =  0xD2,
      CMD_DELVEL_DELANG_MAG        =  0xD3,
      CMD_DEV_ID_STR               =  0xEA,
      CMD_STOP_CONTINUOUS          =  0xFA
    };
 
 
    enum id_string {
      ID_MODEL_NUMBER   = 0,
      ID_SERIAL_NUMBER  = 1,
      ID_DEVICE_NAME    = 2,
      ID_DEVICE_OPTIONS = 3
    };
 
    IMU();
 
    // Destructor
    ~IMU();
 
 
    void openPort(const char *port_name);
 
    void closePort();
 
 
    void initTime(double fix_off);
 
 
    void initGyros(double* bias_x = 0, double* bias_y = 0, double* bias_z = 0);
 
 
    bool setContinuous(cmd command);
 
    void stopContinuous();
 
 
    void receiveAccelAngrate(uint64_t *time, double accel[3], double angrate[3]);
 
 
    void receiveDelvelDelang(uint64_t *time, double delvel[3], double delang[3]);
 
 
    void receiveAccelAngrateMag(uint64_t *time, double accel[3], double angrate[3], double mag[3]);
 
 
    void receiveEuler(uint64_t *time, double *roll, double *pitch, double *yaw);
 
 
    void receiveAccelAngrateOrientation(uint64_t *time, double accel[3], double angrate[3], double orientation[9]);
 
 
        void receiveAccelAngrateMagOrientation (uint64_t *time, double accel[3], double angrate[3], double mag[3], double orientation[9]);
 
 
        void receiveRawAccelAngrate(uint64_t *time, double accel[3], double angrate[3]);
 
 
    void setFixedOffset(double fix_off) {fixed_offset = fix_off;};
 
 
    bool getDeviceIdentifierString(id_string type, char id[17]);
 
  private:
    int transact(void *cmd, int cmd_len, void *rep, int rep_len, int timeout = 0);
 
    int send(void *cmd, int cmd_len);
 
    int receive(uint8_t command, void *rep, int rep_len, int timeout = 0, uint64_t* sys_time = NULL);
 
    uint64_t extractTime(uint8_t* addr);
 
    uint64_t filterTime(uint64_t imu_time, uint64_t sys_time);
 
    double toDouble(uint64_t time);
 
    uint64_t toUint64_t(double time);
 
    int fd;
 
    uint32_t wraps;
 
    uint32_t offset_ticks;
 
    uint32_t last_ticks;
 
    uint32_t diff_ticks;
 
    unsigned long long start_time;
 
    double time_est[2];
 
    double P_time_est[2][2];
 
    bool continuous;
 
    unsigned int counter;
 
    double fixed_offset, offset, d_offset, sum_meas;
 
    bool is_gx3;
 
  };
 
}
#endif