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
  {
    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;    

    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