30 #include <netinet/in.h>
43 #define IMU_EXCEPT(except, msg, ...) \
46 snprintf(buf, 1000, msg" (in microstrain_3dmgx2_imu::IMU:%s)", ##__VA_ARGS__, __FUNCTION__); \
57 static inline unsigned short bswap_16(
unsigned short x) {
58 return (x>>8) | (x<<8);
62 static inline unsigned int bswap_32(
unsigned int x) {
72 *((
unsigned char*)(&tmp) + 3) = *(addr);
73 *((
unsigned char*)(&tmp) + 2) = *(addr+1);
74 *((
unsigned char*)(&tmp) + 1) = *(addr+2);
75 *((
unsigned char*)(&tmp)) = *(addr+3);
85 struct timespec curtime;
86 clock_gettime(CLOCK_REALTIME, &curtime);
87 return (
unsigned long long)(curtime.tv_sec) * 1000000000 + (
unsigned long long)(curtime.tv_nsec);
89 struct timeval timeofday;
90 gettimeofday(&timeofday,NULL);
91 return (
unsigned long long)(timeofday.tv_sec) * 1000000000 + (
unsigned long long)(timeofday.tv_usec) * 1000;
118 fd = open(port_name, O_RDWR | O_SYNC | O_NONBLOCK | O_NOCTTY, S_IRUSR | S_IWUSR );
121 const char *extra_msg =
"";
125 extra_msg =
"You probably don't have premission to open the port for reading and writing.";
128 extra_msg =
"The requested port does not exist. Is the IMU connected? Was the port name misspelled?";
132 IMU_EXCEPT(microstrain_3dmgx2_imu::Exception,
"Unable to open serial port [%s]. %s. %s", port_name, strerror(errno), extra_msg);
138 fl.l_whence = SEEK_SET;
143 if (fcntl(fd, F_SETLK, &fl) != 0)
144 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);
148 if (tcgetattr(fd, &term) < 0)
149 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);
152 cfsetispeed(&term, B115200);
153 cfsetospeed(&term, B115200);
155 if (tcsetattr(fd, TCSAFLUSH, &term) < 0 )
156 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);
162 if (tcflush(fd, TCIOFLUSH) != 0)
163 IMU_EXCEPT(microstrain_3dmgx2_imu::Exception,
"Tcflush failed. Please report this error if you see it.");
180 }
catch (microstrain_3dmgx2_imu::Exception &e) {
186 IMU_EXCEPT(microstrain_3dmgx2_imu::Exception,
"Unable to close serial port; [%s]", strerror(errno));
204 transact(
cmd,
sizeof(
cmd), rep,
sizeof(rep), 1000);
208 offset_ticks =
bswap_32(*(uint32_t*)(rep + k));
209 last_ticks = offset_ticks;
218 fixed_offset = fix_off;
231 cmd[0] = CMD_CAPTURE_GYRO_BIAS;
236 transact(
cmd,
sizeof(
cmd), rep,
sizeof(rep), 30000);
257 cmd[0] = CMD_CONTINUOUS;
262 transact(
cmd,
sizeof(
cmd), rep,
sizeof(rep), 1000);
281 cmd[0] = CMD_STOP_CONTINUOUS;
289 send(
cmd, is_gx3 ? 3 : 1);
293 if (tcflush(fd, TCIOFLUSH) != 0)
294 IMU_EXCEPT(microstrain_3dmgx2_imu::Exception,
"Tcflush failed");
313 receive(CMD_ACCEL_ANGRATE_MAG, rep,
sizeof(rep), 1000, &sys_time);
318 for (i = 0; i < 3; i++)
326 for (i = 0; i < 3; i++)
334 for (i = 0; i < 3; i++) {
339 imu_time = extractTime(rep+37);
340 *time = filterTime(imu_time, sys_time);
355 receive(CMD_ACCEL_ANGRATE_ORIENT, rep,
sizeof(rep), 1000, &sys_time);
360 for (i = 0; i < 3; i++)
368 for (i = 0; i < 3; i++)
376 for (i = 0; i < 9; i++) {
381 imu_time = extractTime(rep+61);
382 *time = filterTime(imu_time, sys_time);
397 receive(CMD_ACCEL_ANGRATE, rep,
sizeof(rep), 1000, &sys_time);
401 for (i = 0; i < 3; i++)
409 for (i = 0; i < 3; i++)
415 imu_time = extractTime(rep+25);
416 *time = filterTime(imu_time, sys_time);
430 receive(CMD_DELVEL_DELANG, rep,
sizeof(rep), 1000, &sys_time);
434 for (i = 0; i < 3; i++)
442 for (i = 0; i < 3; i++)
448 imu_time = extractTime(rep+25);
449 *time = filterTime(imu_time, sys_time);
463 receive(CMD_EULER, rep,
sizeof(rep), 1000, &sys_time);
469 imu_time = extractTime(rep + 13);
470 *time = filterTime(imu_time, sys_time);
481 cmd[0] = CMD_DEV_ID_STR;
484 transact(
cmd,
sizeof(
cmd), rep,
sizeof(rep), 1000);
486 if (
cmd[0] != CMD_DEV_ID_STR ||
cmd[1] != type)
490 memcpy(
id, rep+2, 16);
492 if( type==ID_DEVICE_NAME ){
493 is_gx3 = (strstr(
id,
"GX3") != NULL);
500 #define CMD_ACCEL_ANGRATE_MAG_ORIENT_REP_LEN 79
501 #define CMD_RAW_ACCEL_ANGRATE_LEN 31
513 receive( CMD_ACCEL_ANGRATE_MAG_ORIENT, rep,
sizeof(rep), 1000, &sys_time);
517 for (i = 0; i < 3; i++)
525 for (i = 0; i < 3; i++)
533 for (i = 0; i < 3; i++) {
540 for (i = 0; i < 9; i++) {
544 imu_time = extractTime(rep + 73);
546 *time = filterTime(imu_time, sys_time);
565 for (i = 0; i < 3; i++)
573 for (i = 0; i < 3; i++)
579 imu_time = extractTime(rep+25);
580 *time = filterTime(imu_time, sys_time);
589 uint32_t ticks =
bswap_32(*(uint32_t*)(addr));
591 if (ticks < last_ticks) {
597 uint64_t all_ticks = ((uint64_t)wraps << 32) - offset_ticks + ticks;
599 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)));
612 return receive(*(uint8_t*)
cmd, rep, rep_len, timeout);
625 bytes = write(fd,
cmd, cmd_len);
627 IMU_EXCEPT(microstrain_3dmgx2_imu::Exception,
"error writing to IMU [%s]", strerror(errno));
629 if (bytes != cmd_len)
630 IMU_EXCEPT(microstrain_3dmgx2_imu::Exception,
"whole message not written to IMU");
634 if (tcdrain(fd) != 0)
635 IMU_EXCEPT(microstrain_3dmgx2_imu::Exception,
"tcdrain failed");
646 struct pollfd ufd[1];
648 ufd[0].events = POLLIN;
653 if ( (retval = poll(ufd, 1, timeout)) < 0 )
654 IMU_EXCEPT(microstrain_3dmgx2_imu::Exception,
"poll failed [%s]", strerror(errno));
657 IMU_EXCEPT(microstrain_3dmgx2_imu::TimeoutException,
"timeout reached");
659 nbytes = read(fd, (uint8_t *) buff, count);
662 IMU_EXCEPT(microstrain_3dmgx2_imu::Exception,
"read failed [%s]", strerror(errno));
673 int nbytes, bytes, skippedbytes;
677 struct pollfd ufd[1];
679 ufd[0].events = POLLIN;
682 *(uint8_t*)(rep) = 0;
684 while (*(uint8_t*)(rep) !=
command && skippedbytes < MAX_BYTES_SKIPPED)
691 if (sys_time != NULL)
698 while (bytes < rep_len)
700 nbytes =
read_with_timeout(fd, (uint8_t *)rep + bytes, rep_len - bytes, timeout);
703 IMU_EXCEPT(microstrain_3dmgx2_imu::Exception,
"read failed [%s]", strerror(errno));
710 uint16_t checksum = 0;
711 for (
int i = 0; i < rep_len - 2; i++) {
712 checksum += ((uint8_t*)rep)[i];
716 if (checksum !=
bswap_16(*(uint16_t*)((uint8_t*)rep+rep_len-2)))
717 IMU_EXCEPT(microstrain_3dmgx2_imu::CorruptedDataException,
"invalid checksum.\n Make sure the IMU sensor is connected to this computer.");
727 if (counter < KF_NUM_SUM){
729 sum_meas += (toDouble(imu_time) - toDouble(sys_time));
737 double meas_diff = (sum_meas/KF_NUM_SUM) - offset;
738 offset += KF_K_1 * meas_diff;
739 d_offset += KF_K_2 * meas_diff;
742 counter = 0; sum_meas = 0;
744 return imu_time - toUint64_t( offset ) + toUint64_t( fixed_offset );
752 double res = trunc(time/1e9);
753 res += (((double)time)/1e9) - res;
762 return (uint64_t)(time * 1e9);