Modules-old/Microstrain/Microstrain.cpp
////////////////////////////////////////////////////////////////////////////////
// Receive a reply from the IMU.
// Returns the number of bytes read.
Receive a reply from the IMU.
Returns the number of bytes read.
Receive a reply from the IMU . Returns the number of bytes read .
int
Microstrain::receive(uint8_t command, void *rep, int rep_len, int timeout, uint64_t* sys_time)
{
  int nbytes, bytes, skippedbytes;
  skippedbytes = 0;
  struct pollfd ufd[1];
  ufd[0].fd = fd;
  ufd[0].events = POLLIN;
  *(uint8_t*)(rep) = 0;
  while (*(uint8_t*)(rep) != command && skippedbytes < MAX_BYTES_SKIPPED)
  {
    read_with_timeout(fd, rep, 1, timeout);
    skippedbytes++;
  }
  if (sys_time != NULL)
    *sys_time = time_helper();
  bytes = 1;
  while (bytes < rep_len)
  {
    nbytes = read_with_timeout(fd, (uint8_t *)rep + bytes, rep_len - bytes, timeout);
    if (nbytes < 0)
      IMU_EXCEPT(microstrain_3dmgx3_imu::Exception, "read failed  [%s]", strerror(errno));
    bytes += nbytes;
  }
  uint16_t checksum = 0;
  for (int i = 0; i < rep_len - 2; i++) {
    checksum += ((uint8_t*)rep)[i];
  }
  if (checksum != bswap_16(*(uint16_t*)((uint8_t*)rep+rep_len-2)))
    IMU_EXCEPT(microstrain_3dmgx3_imu::CorruptedDataException, "invalid checksum.\n Make sure the IMU sensor is connected to this computer.");
  return bytes;
}
int Microstrain :: receive ( uint8_t command , void * rep , int rep_len , int timeout , uint64_t * sys_time ) { int nbytes , bytes , skippedbytes ; skippedbytes = 0 ; struct pollfd ufd [ 1 ] ; ufd [ 0 ] . fd = fd ; ufd [ 0 ] . events = POLLIN ; * ( uint8_t * ) ( rep ) = 0 ; while ( * ( uint8_t * ) ( rep ) != command && skippedbytes < MAX_BYTES_SKIPPED ) { read_with_timeout ( fd , rep , 1 , timeout ) ; skippedbytes ++ ; } if ( sys_time != NULL ) * sys_time = time_helper ( ) ; bytes = 1 ; while ( bytes < rep_len ) { nbytes = read_with_timeout ( fd , ( uint8_t * ) rep + bytes , rep_len - bytes , timeout ) ; if ( nbytes < 0 ) IMU_EXCEPT ( microstrain_3dmgx3_imu :: Exception , " " , strerror ( errno ) ) ; bytes += nbytes ; } uint16_t checksum = 0 ; for ( int i = 0 ; i < rep_len - 2 ; i ++ ) { checksum += ( ( uint8_t * ) rep ) [ i ] ; } if ( checksum != bswap_16 ( * ( uint16_t * ) ( ( uint8_t * ) rep + rep_len - 2 ) ) ) IMU_EXCEPT ( microstrain_3dmgx3_imu :: CorruptedDataException , " \n " ) ; return bytes ; }
Receive a reply from the IMU.
Receive a reply from the IMU .
// Skip everything until we find our "header" // We now have 1 byte // Read the rest of the message: // Checksum is always final 2 bytes of transaction // If wrong throw Exception