3dmgx2.cc
Go to the documentation of this file.
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2008-20010  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 #include <assert.h>
00022 #include <errno.h>
00023 #include <fcntl.h>
00024 #include <math.h>
00025 #include <stdio.h>
00026 #include <string.h>
00027 #include <sys/stat.h>
00028 #include <termios.h>
00029 #include <unistd.h>
00030 #include <netinet/in.h>
00031 #include <stdlib.h>
00032 
00033 #include <sys/time.h>
00034 
00035 //#include <ros/console.h>
00036 
00037 #include "microstrain_3dmgx2_imu/3dmgx2.h"
00038 
00039 #include "poll.h"
00040 
00041 
00043 #define IMU_EXCEPT(except, msg, ...) \
00044   { \
00045     char buf[1000]; \
00046     snprintf(buf, 1000, msg" (in microstrain_3dmgx2_imu::IMU:%s)", ##__VA_ARGS__, __FUNCTION__); \
00047     throw except(buf); \
00048   }
00049 
00050 // Some systems (e.g., OS X) require explicit externing of static class
00051 // members.
00052 extern const double microstrain_3dmgx2_imu::IMU::G;
00053 extern const double microstrain_3dmgx2_imu::IMU::KF_K_1;
00054 extern const double microstrain_3dmgx2_imu::IMU::KF_K_2;
00055 
00057 static inline unsigned short bswap_16(unsigned short x) {
00058   return (x>>8) | (x<<8);
00059 }
00060 
00062 static inline unsigned int bswap_32(unsigned int x) {
00063   return (bswap_16(x&0xffff)<<16) | (bswap_16(x>>16));
00064 }
00065 
00066 
00068 static float extract_float(uint8_t* addr) {
00069 
00070   float tmp;
00071 
00072   *((unsigned char*)(&tmp) + 3) = *(addr);
00073   *((unsigned char*)(&tmp) + 2) = *(addr+1);
00074   *((unsigned char*)(&tmp) + 1) = *(addr+2);
00075   *((unsigned char*)(&tmp)) = *(addr+3);
00076 
00077   return tmp;
00078 }
00079 
00080 
00082 static unsigned long long time_helper()
00083 {
00084 #if POSIX_TIMERS > 0
00085   struct timespec curtime;
00086   clock_gettime(CLOCK_REALTIME, &curtime);
00087   return (unsigned long long)(curtime.tv_sec) * 1000000000 + (unsigned long long)(curtime.tv_nsec);  
00088 #else
00089   struct timeval timeofday;
00090   gettimeofday(&timeofday,NULL);
00091   return (unsigned long long)(timeofday.tv_sec) * 1000000000 + (unsigned long long)(timeofday.tv_usec) * 1000;  
00092 #endif
00093 }
00094 
00095 
00097 // Constructor
00098 microstrain_3dmgx2_imu::IMU::IMU() : fd(-1), continuous(false), is_gx3(false)
00099 { }
00100 
00101 
00103 // Destructor
00104 microstrain_3dmgx2_imu::IMU::~IMU()
00105 {
00106   closePort();
00107 }
00108 
00109 
00111 // Open the IMU port
00112 void
00113 microstrain_3dmgx2_imu::IMU::openPort(const char *port_name)
00114 {
00115   closePort(); // In case it was previously open, try to close it first.
00116 
00117   // Open the port
00118   fd = open(port_name, O_RDWR | O_SYNC | O_NONBLOCK | O_NOCTTY, S_IRUSR | S_IWUSR );
00119   if (fd < 0)
00120   {
00121     const char *extra_msg = "";
00122     switch (errno)
00123     {
00124       case EACCES:
00125         extra_msg = "You probably don't have premission to open the port for reading and writing.";
00126         break;
00127       case ENOENT:
00128         extra_msg = "The requested port does not exist. Is the IMU connected? Was the port name misspelled?";
00129         break;
00130     }
00131 
00132     IMU_EXCEPT(microstrain_3dmgx2_imu::Exception, "Unable to open serial port [%s]. %s. %s", port_name, strerror(errno), extra_msg);
00133   }
00134 
00135   // Lock the port
00136   struct flock fl;
00137   fl.l_type   = F_WRLCK;
00138   fl.l_whence = SEEK_SET;
00139   fl.l_start = 0;
00140   fl.l_len   = 0;
00141   fl.l_pid   = getpid();
00142 
00143   if (fcntl(fd, F_SETLK, &fl) != 0)
00144     IMU_EXCEPT(microstrain_3dmgx2_imu::Exception, "Device %s is already locked. Try 'lsof | grep %s' to find other processes that currently have the port open.", port_name, port_name);
00145 
00146   // Change port settings
00147   struct termios term;
00148   if (tcgetattr(fd, &term) < 0)
00149     IMU_EXCEPT(microstrain_3dmgx2_imu::Exception, "Unable to get serial port attributes. The port you specified (%s) may not be a serial port.", port_name);
00150 
00151   cfmakeraw( &term );
00152   cfsetispeed(&term, B115200);
00153   cfsetospeed(&term, B115200);
00154 
00155   if (tcsetattr(fd, TCSAFLUSH, &term) < 0 )
00156     IMU_EXCEPT(microstrain_3dmgx2_imu::Exception, "Unable to set serial port attributes. The port you specified (%s) may not be a serial port.", port_name); 
00157 
00158   // Stop continuous mode
00159   stopContinuous();
00160 
00161   // Make sure queues are empty before we begin
00162   if (tcflush(fd, TCIOFLUSH) != 0)
00163     IMU_EXCEPT(microstrain_3dmgx2_imu::Exception, "Tcflush failed. Please report this error if you see it.");
00164 }
00165 
00166 
00168 // Close the IMU port
00169 void
00170 microstrain_3dmgx2_imu::IMU::closePort()
00171 {
00172   if (fd != -1)
00173   {
00174     if (continuous)
00175     {
00176       try {
00177         //ROS_DEBUG("stopping continuous");
00178         stopContinuous();
00179 
00180       } catch (microstrain_3dmgx2_imu::Exception &e) {
00181         // Exceptions here are fine since we are closing anyways
00182       }
00183     }
00184 
00185     if (close(fd) != 0)
00186       IMU_EXCEPT(microstrain_3dmgx2_imu::Exception, "Unable to close serial port; [%s]", strerror(errno));
00187     fd = -1;
00188   }
00189 }
00190 
00191 
00192 
00194 // Initialize time information
00195 void
00196 microstrain_3dmgx2_imu::IMU::initTime(double fix_off)
00197 {
00198   wraps = 0;
00199 
00200   uint8_t cmd[1];
00201   uint8_t rep[31];
00202   cmd[0] = CMD_RAW;
00203 
00204   transact(cmd, sizeof(cmd), rep, sizeof(rep), 1000);
00205   start_time = time_helper();
00206 
00207   int k = 25;
00208   offset_ticks = bswap_32(*(uint32_t*)(rep + k));
00209   last_ticks = offset_ticks;
00210 
00211   // reset kalman filter state
00212   offset = 0;
00213   d_offset = 0;
00214   sum_meas = 0;
00215   counter = 0;
00216 
00217   // fixed offset
00218   fixed_offset = fix_off;
00219 }
00220 
00222 // Initialize IMU gyros
00223 void
00224 microstrain_3dmgx2_imu::IMU::initGyros(double* bias_x, double* bias_y, double* bias_z)
00225 {
00226   wraps = 0;
00227 
00228   uint8_t cmd[5];
00229   uint8_t rep[19];
00230 
00231   cmd[0] = CMD_CAPTURE_GYRO_BIAS;
00232   cmd[1] = 0xC1;
00233   cmd[2] = 0x29;
00234   *(unsigned short*)(&cmd[3]) = bswap_16(10000);
00235 
00236   transact(cmd, sizeof(cmd), rep, sizeof(rep), 30000);
00237 
00238   if (bias_x)
00239     *bias_x = extract_float(rep + 1);
00240   
00241   if (bias_y)
00242     *bias_y = extract_float(rep + 5);
00243 
00244   if (bias_z)
00245     *bias_z = extract_float(rep + 9);
00246 }
00247 
00248 
00250 // Put the IMU into continuous mode
00251 bool
00252 microstrain_3dmgx2_imu::IMU::setContinuous(cmd command)
00253 {
00254   uint8_t cmd[4];
00255   uint8_t rep[8];
00256 
00257   cmd[0] = CMD_CONTINUOUS;
00258   cmd[1] = 0xC1; //Confirms user intent
00259   cmd[2] = 0x29; //Confirms user intent
00260   cmd[3] = command;
00261 
00262   transact(cmd, sizeof(cmd), rep, sizeof(rep), 1000);
00263   
00264   // Verify that continuous mode is set on correct command:
00265   if (rep[1] != command) {
00266     return false;
00267   }
00268 
00269   continuous = true;
00270   return true;
00271 }
00272 
00273 
00275 // Take the IMU out of continuous mode
00276 void
00277 microstrain_3dmgx2_imu::IMU::stopContinuous()
00278 {
00279   uint8_t cmd[3];
00280 
00281   cmd[0] = CMD_STOP_CONTINUOUS;
00282   
00283   cmd[1] = 0x75; // gx3 - confirms user intent
00284 
00285   cmd[2] = 0xb4; // gx3 - confirms user intent
00286 
00287   send(cmd, sizeof(cmd));
00288 
00289   send(cmd, is_gx3 ? 3 : 1);
00290 
00291   usleep(1000000);
00292 
00293   if (tcflush(fd, TCIOFLUSH) != 0)
00294     IMU_EXCEPT(microstrain_3dmgx2_imu::Exception, "Tcflush failed");
00295 
00296   continuous = false;
00297 }
00298 
00299 
00300 
00302 // Receive ACCEL_ANGRATE_MAG message
00303 void
00304 microstrain_3dmgx2_imu::IMU::receiveAccelAngrateMag(uint64_t *time, double accel[3], double angrate[3], double mag[3])
00305 {
00306   int i, k;
00307   uint8_t rep[43];
00308 
00309   uint64_t sys_time;
00310   uint64_t imu_time;
00311 
00312   //ROS_DEBUG("About to do receive.");
00313   receive(CMD_ACCEL_ANGRATE_MAG, rep, sizeof(rep), 1000, &sys_time);
00314   //ROS_DEBUG("Receive finished.");
00315 
00316   // Read the acceleration:
00317   k = 1;
00318   for (i = 0; i < 3; i++)
00319   {
00320     accel[i] = extract_float(rep + k) * G;
00321     k += 4;
00322   }
00323 
00324   // Read the angular rates
00325   k = 13;
00326   for (i = 0; i < 3; i++)
00327   {
00328     angrate[i] = extract_float(rep + k);
00329     k += 4;
00330   }
00331 
00332   // Read the magnetometer reading.
00333   k = 25;
00334   for (i = 0; i < 3; i++) {
00335     mag[i] = extract_float(rep + k);
00336     k += 4;
00337   }
00338 
00339   imu_time = extractTime(rep+37);
00340   *time = filterTime(imu_time, sys_time);
00341 }
00342 
00344 // Receive ACCEL_ANGRATE_ORIENTATION message
00345 void
00346 microstrain_3dmgx2_imu::IMU::receiveAccelAngrateOrientation(uint64_t *time, double accel[3], double angrate[3], double orientation[9])
00347 {
00348   int i, k;
00349   uint8_t rep[67];
00350 
00351   uint64_t sys_time;
00352   uint64_t imu_time;
00353 
00354   //ROS_DEBUG("About to do receive.");
00355   receive(CMD_ACCEL_ANGRATE_ORIENT, rep, sizeof(rep), 1000, &sys_time);
00356   //ROS_DEBUG("Finished receive.");
00357 
00358   // Read the acceleration:
00359   k = 1;
00360   for (i = 0; i < 3; i++)
00361   {
00362     accel[i] = extract_float(rep + k) * G;
00363     k += 4;
00364   }
00365 
00366   // Read the angular rates
00367   k = 13;
00368   for (i = 0; i < 3; i++)
00369   {
00370     angrate[i] = extract_float(rep + k);
00371     k += 4;
00372   }
00373 
00374   // Read the orientation matrix
00375   k = 25;
00376   for (i = 0; i < 9; i++) {
00377     orientation[i] = extract_float(rep + k);
00378     k += 4;
00379   }
00380 
00381   imu_time = extractTime(rep+61);
00382   *time = filterTime(imu_time, sys_time);
00383 }
00384 
00385 
00387 // Receive ACCEL_ANGRATE message
00388 void
00389 microstrain_3dmgx2_imu::IMU::receiveAccelAngrate(uint64_t *time, double accel[3], double angrate[3])
00390 {
00391   int i, k;
00392   uint8_t rep[31];
00393 
00394   uint64_t sys_time;
00395   uint64_t imu_time;
00396 
00397   receive(CMD_ACCEL_ANGRATE, rep, sizeof(rep), 1000, &sys_time);
00398 
00399   // Read the acceleration:
00400   k = 1;
00401   for (i = 0; i < 3; i++)
00402   {
00403     accel[i] = extract_float(rep + k) * G;
00404     k += 4;
00405   }
00406 
00407   // Read the angular rates
00408   k = 13;
00409   for (i = 0; i < 3; i++)
00410   {
00411     angrate[i] = extract_float(rep + k);
00412     k += 4;
00413   }
00414 
00415   imu_time = extractTime(rep+25);
00416   *time = filterTime(imu_time, sys_time);
00417 }
00418 
00420 // Receive DELVEL_DELANG message
00421 void
00422 microstrain_3dmgx2_imu::IMU::receiveDelvelDelang(uint64_t *time, double delvel[3], double delang[3])
00423 {
00424   int i, k;
00425   uint8_t rep[31];
00426 
00427   uint64_t sys_time;
00428   uint64_t imu_time;
00429 
00430   receive(CMD_DELVEL_DELANG, rep, sizeof(rep), 1000, &sys_time);
00431 
00432   // Read the delta angles:
00433   k = 1;
00434   for (i = 0; i < 3; i++)
00435   {
00436     delang[i] = extract_float(rep + k);
00437     k += 4;
00438   }
00439 
00440   // Read the delta velocities
00441   k = 13;
00442   for (i = 0; i < 3; i++)
00443   {
00444     delvel[i] = extract_float(rep + k) * G;
00445     k += 4;
00446   }
00447 
00448   imu_time = extractTime(rep+25);
00449   *time = filterTime(imu_time, sys_time);
00450 }
00451 
00452 
00454 // Receive EULER message
00455 void
00456 microstrain_3dmgx2_imu::IMU::receiveEuler(uint64_t *time, double *roll, double *pitch, double *yaw)
00457 {
00458   uint8_t rep[19];
00459 
00460   uint64_t sys_time;
00461   uint64_t imu_time;
00462 
00463   receive(CMD_EULER, rep, sizeof(rep), 1000, &sys_time);
00464 
00465   *roll  = extract_float(rep + 1);
00466   *pitch = extract_float(rep + 5);
00467   *yaw   = extract_float(rep + 9);
00468 
00469   imu_time  = extractTime(rep + 13);
00470   *time = filterTime(imu_time, sys_time);
00471 }
00472     
00474 // Receive Device Identifier String
00475 
00476 bool microstrain_3dmgx2_imu::IMU::getDeviceIdentifierString(id_string type, char id[17])
00477 {
00478   uint8_t cmd[2];
00479   uint8_t rep[20];
00480 
00481   cmd[0] = CMD_DEV_ID_STR;
00482   cmd[1] = type;
00483 
00484   transact(cmd, sizeof(cmd), rep, sizeof(rep), 1000);
00485   
00486   if (cmd[0] != CMD_DEV_ID_STR || cmd[1] != type)
00487     return false;
00488 
00489   id[16] = 0;
00490   memcpy(id, rep+2, 16);
00491 
00492   if( type==ID_DEVICE_NAME ){
00493     is_gx3 = (strstr(id,"GX3") != NULL);
00494   }
00495 
00496   return true;
00497 }
00498 
00499 /* ideally it would be nice to feed these functions back into willowimu */
00500 #define CMD_ACCEL_ANGRATE_MAG_ORIENT_REP_LEN 79
00501 #define CMD_RAW_ACCEL_ANGRATE_LEN 31
00502 
00503 // Receive ACCEL_ANGRATE_MAG_ORIENT message
00504 void 
00505 microstrain_3dmgx2_imu::IMU::receiveAccelAngrateMagOrientation (uint64_t *time, double accel[3], double angrate[3], double mag[3], double orientation[9]) 
00506 {
00507         uint8_t  rep[CMD_ACCEL_ANGRATE_MAG_ORIENT_REP_LEN];
00508 
00509         int k, i;
00510         uint64_t sys_time;
00511         uint64_t imu_time;
00512 
00513         receive( CMD_ACCEL_ANGRATE_MAG_ORIENT, rep, sizeof(rep), 1000, &sys_time); 
00514 
00515   // Read the acceleration:
00516   k = 1;
00517   for (i = 0; i < 3; i++)
00518   {
00519     accel[i] = extract_float(rep + k) * G;
00520     k += 4;
00521   }
00522 
00523   // Read the angular rates
00524   k = 13;
00525   for (i = 0; i < 3; i++)
00526   {
00527     angrate[i] = extract_float(rep + k);
00528     k += 4;
00529   }
00530 
00531   // Read the magnetic field matrix
00532   k = 25;
00533   for (i = 0; i < 3; i++) {
00534     mag[i] = extract_float(rep + k);
00535     k += 4;
00536   }
00537 
00538  // Read the orientation matrix
00539   k = 37;
00540   for (i = 0; i < 9; i++) {
00541     orientation[i] = extract_float(rep + k);
00542     k += 4;
00543   }
00544         imu_time  = extractTime(rep + 73);
00545 
00546         *time = filterTime(imu_time, sys_time);
00547 }
00548 
00550 // Receive RAW message
00551 // (copy of receive accel angrate but with raw cmd)
00552 void
00553 microstrain_3dmgx2_imu::IMU::receiveRawAccelAngrate(uint64_t *time, double accel[3], double angrate[3])
00554 {
00555   int i, k;
00556   uint8_t rep[CMD_RAW_ACCEL_ANGRATE_LEN];
00557 
00558   uint64_t sys_time;
00559   uint64_t imu_time;
00560 
00561   receive(microstrain_3dmgx2_imu::IMU::CMD_RAW, rep, sizeof(rep), 1000, &sys_time);
00562 
00563   // Read the accelerator AD register values 0 - 65535 given as float
00564   k = 1;
00565   for (i = 0; i < 3; i++)
00566   {
00567     accel[i] = extract_float(rep + k);
00568     k += 4;
00569   }
00570 
00571   // Read the angular rates AD registor values 0 - 65535 (given as float
00572   k = 13;
00573   for (i = 0; i < 3; i++)
00574   {
00575     angrate[i] = extract_float(rep + k);
00576     k += 4;
00577   }
00578 
00579   imu_time = extractTime(rep+25);
00580   *time = filterTime(imu_time, sys_time);
00581 }
00582 
00583 
00585 // Extract time and process rollover
00586 uint64_t
00587 microstrain_3dmgx2_imu::IMU::extractTime(uint8_t* addr)
00588 {
00589   uint32_t ticks = bswap_32(*(uint32_t*)(addr));
00590 
00591   if (ticks < last_ticks) {
00592     wraps += 1;
00593   }
00594 
00595   last_ticks = ticks;
00596 
00597   uint64_t all_ticks = ((uint64_t)wraps << 32) - offset_ticks + ticks;
00598 
00599   return  start_time + (is_gx3 ? (uint64_t)(all_ticks * (1000000000.0 / TICKS_PER_SEC_GX3)) : (uint64_t)(all_ticks * (1000000000.0 / TICKS_PER_SEC_GX2))); // syntax a bit funny because C++ compiler doesn't like conditional ?: operator near the static consts (???)
00600 
00601 }
00602 
00603 
00604 
00606 // Send a packet and wait for a reply from the IMU.
00607 // Returns the number of bytes read.
00608 int microstrain_3dmgx2_imu::IMU::transact(void *cmd, int cmd_len, void *rep, int rep_len, int timeout)
00609 {
00610   send(cmd, cmd_len);
00611   
00612   return receive(*(uint8_t*)cmd, rep, rep_len, timeout);
00613 }
00614 
00615 
00617 // Send a packet to the IMU.
00618 // Returns the number of bytes written.
00619 int
00620 microstrain_3dmgx2_imu::IMU::send(void *cmd, int cmd_len)
00621 {
00622   int bytes;
00623 
00624   // Write the data to the port
00625   bytes = write(fd, cmd, cmd_len);
00626   if (bytes < 0)
00627     IMU_EXCEPT(microstrain_3dmgx2_imu::Exception, "error writing to IMU [%s]", strerror(errno));
00628 
00629   if (bytes != cmd_len)
00630     IMU_EXCEPT(microstrain_3dmgx2_imu::Exception, "whole message not written to IMU");
00631 
00632   // Make sure the queue is drained
00633   // Synchronous IO doesnt always work
00634   if (tcdrain(fd) != 0)
00635     IMU_EXCEPT(microstrain_3dmgx2_imu::Exception, "tcdrain failed");
00636 
00637   return bytes;
00638 }
00639 
00640 
00641 static int read_with_timeout(int fd, void *buff, size_t count, int timeout)
00642 {
00643   ssize_t nbytes;
00644   int retval;
00645 
00646   struct pollfd ufd[1];
00647   ufd[0].fd = fd;
00648   ufd[0].events = POLLIN;
00649 
00650   if (timeout == 0)
00651     timeout = -1; // For compatibility with former behavior, 0 means no timeout. For poll, negative means no timeout.
00652   
00653   if ( (retval = poll(ufd, 1, timeout)) < 0 )
00654     IMU_EXCEPT(microstrain_3dmgx2_imu::Exception, "poll failed  [%s]", strerror(errno));
00655 
00656   if (retval == 0)
00657     IMU_EXCEPT(microstrain_3dmgx2_imu::TimeoutException, "timeout reached");
00658         
00659   nbytes = read(fd, (uint8_t *) buff, count);
00660 
00661   if (nbytes < 0)
00662     IMU_EXCEPT(microstrain_3dmgx2_imu::Exception, "read failed  [%s]", strerror(errno));
00663 
00664   return nbytes;
00665 }
00666 
00668 // Receive a reply from the IMU.
00669 // Returns the number of bytes read.
00670 int
00671 microstrain_3dmgx2_imu::IMU::receive(uint8_t command, void *rep, int rep_len, int timeout, uint64_t* sys_time)
00672 {
00673   int nbytes, bytes, skippedbytes;
00674 
00675   skippedbytes = 0;
00676 
00677   struct pollfd ufd[1];
00678   ufd[0].fd = fd;
00679   ufd[0].events = POLLIN;
00680   
00681   // Skip everything until we find our "header"
00682   *(uint8_t*)(rep) = 0;
00683   
00684   while (*(uint8_t*)(rep) != command && skippedbytes < MAX_BYTES_SKIPPED)
00685   {
00686     read_with_timeout(fd, rep, 1, timeout);
00687 
00688     skippedbytes++;
00689   }
00690 
00691   if (sys_time != NULL)
00692     *sys_time = time_helper();
00693   
00694   // We now have 1 byte
00695   bytes = 1;
00696 
00697   // Read the rest of the message:
00698   while (bytes < rep_len)
00699   {
00700     nbytes = read_with_timeout(fd, (uint8_t *)rep + bytes, rep_len - bytes, timeout);
00701     
00702     if (nbytes < 0)
00703       IMU_EXCEPT(microstrain_3dmgx2_imu::Exception, "read failed  [%s]", strerror(errno));
00704     
00705     bytes += nbytes;
00706   }
00707 
00708   // Checksum is always final 2 bytes of transaction
00709 
00710   uint16_t checksum = 0;
00711   for (int i = 0; i < rep_len - 2; i++) {
00712     checksum += ((uint8_t*)rep)[i];
00713   }
00714 
00715   // If wrong throw Exception
00716   if (checksum != bswap_16(*(uint16_t*)((uint8_t*)rep+rep_len-2)))
00717     IMU_EXCEPT(microstrain_3dmgx2_imu::CorruptedDataException, "invalid checksum.\n Make sure the IMU sensor is connected to this computer.");
00718   
00719   return bytes;
00720 }
00721 
00723 // Kalman filter for time estimation
00724 uint64_t microstrain_3dmgx2_imu::IMU::filterTime(uint64_t imu_time, uint64_t sys_time)
00725 {
00726   // first calculate the sum of KF_NUM_SUM measurements
00727   if (counter < KF_NUM_SUM){
00728     counter ++;
00729     sum_meas += (toDouble(imu_time) - toDouble(sys_time));
00730   }
00731   // update kalman filter with fixed innovation
00732   else{
00733     // system update
00734     offset += d_offset;
00735 
00736     // measurement update
00737     double meas_diff = (sum_meas/KF_NUM_SUM) - offset;
00738     offset   += KF_K_1 * meas_diff;
00739     d_offset += KF_K_2 * meas_diff;
00740 
00741     // reset counter and average
00742     counter = 0; sum_meas = 0;
00743   }
00744   return imu_time - toUint64_t( offset ) + toUint64_t( fixed_offset );
00745 }
00746 
00747 
00749 // convert uint64_t time to double time
00750 double microstrain_3dmgx2_imu::IMU::toDouble(uint64_t time)
00751 {
00752   double res = trunc(time/1e9);
00753   res += (((double)time)/1e9) - res;
00754   return res;
00755 }
00756 
00757 
00759 // convert double time to uint64_t time
00760 uint64_t  microstrain_3dmgx2_imu::IMU::toUint64_t(double time)
00761 {
00762   return (uint64_t)(time * 1e9);
00763 }


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