SHARE
TWEET

whatnick

a guest Feb 13th, 2010 371 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2. #include <termios.h> // terminal io (serial port) interface
  3. #include <fcntl.h>   // File control definitions
  4. #include <errno.h>   // Error number definitions
  5. #include <assert.h>
  6.  
  7. #include <iostream>
  8.  
  9. #define PI (3.141592653589793)
  10.  
  11.  
  12. typedef int ComPortHandle;
  13. typedef unsigned char Byte;
  14.  
  15.  
  16. using std::cout;
  17. using std::cerr;
  18. using std::endl;
  19.  
  20. int TestByteOrder()
  21. {
  22.    short int word = 0x0001;
  23.    char *byte = (char *) &word;
  24.    return(byte[0] ? LITTLE_ENDIAN : BIG_ENDIAN);
  25. }
  26.  
  27.  
  28. /*----------------------------------------------------------------------
  29.  * convert2short
  30.  * Convert two adjacent bytes to an integer.
  31.  *
  32.  * parameters:  buffer : pointer to first of two buffer bytes.
  33.  * returns:     the converted value aa a signed short -32 to +32k.
  34.  *--------------------------------------------------------------------*/
  35. short convert2short(unsigned char* buffer) {
  36.         short x;
  37.         if(TestByteOrder() != BIG_ENDIAN) {
  38.            x = (buffer[0] <<8) + (buffer[1] & 0xFF);
  39.         }else{
  40.                 x = (short)buffer;
  41.         }
  42.         return x;
  43. }
  44. /*----------------------------------------------------------------------
  45.  * convert2ushort
  46.  * Convert two adjacent bytes to a short.
  47.  *
  48.  * parameters:  buffer : pointer to first of two buffer bytes.
  49.  * returns:     the converted value as a unsigned short 0-64k.
  50.  *--------------------------------------------------------------------*/
  51. unsigned short convert2ushort(unsigned char* buffer) {
  52.         unsigned short x;
  53.         if(TestByteOrder() != BIG_ENDIAN) {
  54.            x = (buffer[0] <<8) + (buffer[1] & 0xFF);
  55.         }else{
  56.                 x = (unsigned short)buffer;
  57.         }
  58.         return x;
  59. }
  60.  
  61. /*----------------------------------------------------------------------
  62.  * FloatFromBytes
  63.  * Converts bytes to Float.
  64.  *
  65.  * parameters:  pBytes : received buffer containing pointer to 4 bytes
  66.  *
  67.  * returns:     a float value.
  68.  *--------------------------------------------------------------------*/
  69. float FloatFromBytes(const unsigned char* pBytes)
  70. {
  71.         float f = 0;
  72.         if(TestByteOrder() != BIG_ENDIAN) {
  73.            ((Byte*)(&f))[0] = pBytes[3];
  74.            ((Byte*)(&f))[1] = pBytes[2];
  75.            ((Byte*)(&f))[2] = pBytes[1];
  76.            ((Byte*)(&f))[3] = pBytes[0];
  77.         }else{
  78.            ((Byte*)(&f))[0] = pBytes[0];
  79.            ((Byte*)(&f))[1] = pBytes[1];
  80.            ((Byte*)(&f))[2] = pBytes[2];
  81.            ((Byte*)(&f))[3] = pBytes[3];
  82.         }
  83.        
  84.         return f;
  85. }
  86.  
  87. /*----------------------------------------------------------------------
  88.  * i3dmgx2_Checksum
  89.  * Calculate checksum on a received data buffer.
  90.  *
  91.  * Note: The last two bytes, which contain the received checksum,
  92.  *       are not included in the calculation.
  93.  *
  94.  * parameters:  buffer : pointer to the start of the received buffer.
  95.  *              length - the length (in chars) of the buffer.
  96.  *
  97.  * returns:     the calculated checksum.
  98.  *--------------------------------------------------------------------*/
  99. unsigned short i3dmgx2_Checksum(const unsigned char* pBytes, int count)
  100. {
  101.         unsigned short i3_checksum = 0;
  102.         int i = 0;
  103.  
  104.         for(i = 0; i < count; ++i)
  105.         {
  106.                 i3_checksum+=pBytes[i];
  107.         }
  108.  
  109.         return i3_checksum;
  110. }
  111.  
  112. /////////////////////////////////////////////////////////////////////////////////////////////////////
  113. //
  114. // utility functions for working with a com port in Linux
  115. //
  116. /////////////////////////////////////////////////////////////////////////////////////////////////////
  117.  
  118.  
  119. //=======================================================================================
  120. // Purge
  121. //---------------------------------------------------------------------------------------
  122. // Clears the com port's read and write buffers
  123. //=======================================================================================
  124. bool Purge(ComPortHandle comPortHandle)
  125. {
  126.         if (tcflush(comPortHandle,TCIOFLUSH)==-1)
  127.         {
  128.                 cerr << "flush failed" << endl;
  129.                 return false;
  130.         }
  131.        
  132.         return true;
  133. }
  134.  
  135.  
  136. //=======================================================================================
  137. // OpenComPort
  138. //---------------------------------------------------------------------------------------
  139. // Opens a com port with the correct settings for communicating with a MicroStrain
  140. // 3DM-GX2 sensor
  141. //=======================================================================================
  142. ComPortHandle OpenComPort(const char* comPortPath)
  143. {
  144.         ComPortHandle comPort = open(comPortPath, O_RDWR | O_NOCTTY);
  145.  
  146.         if (comPort== -1) //Opening of port failed
  147.         {
  148.                 cerr << "Unable to open com Port\n"
  149.                         "Error:(" << errno << ") " << strerror(errno) << endl;
  150.                 return -1;
  151.         }
  152.        
  153.         //Get the current options for the port...
  154.         termios options;
  155.         tcgetattr(comPort, &options);
  156.  
  157.         //set the baud rate to 115200
  158.         int baudRate = B115200;
  159.         cfsetospeed(&options, baudRate);
  160.         cfsetispeed(&options, baudRate);
  161.  
  162.         //set the number of data bits.
  163.         options.c_cflag &= ~CSIZE;  // Mask the character size bits
  164.         options.c_cflag |= CS8;
  165.  
  166.         //set the number of stop bits to 1
  167.         options.c_cflag &= ~CSTOPB;
  168.        
  169.          //Set parity to None
  170.         options.c_cflag &=~PARENB;
  171.        
  172.         //set for non-canonical (raw processing, no echo, etc.)
  173.         options.c_iflag = IGNPAR; // ignore parity check close_port(int
  174.         options.c_oflag = 0; // raw output
  175.         options.c_lflag = 0; // raw input
  176.  
  177.         //Time-Outs -- won't work with NDELAY option in the call to open
  178.         options.c_cc[VMIN]  = 0;   // block reading until RX x characers. If x = 0, it is non-blocking.
  179.         options.c_cc[VTIME] = 10;   // Inter-Character Timer -- i.e. timeout= x*.1 s
  180.  
  181.         //Set local mode and enable the receiver
  182.         options.c_cflag |= (CLOCAL | CREAD);
  183.  
  184.         //Purge serial port buffers
  185.         Purge(comPort);
  186.  
  187.         //Set the new options for the port...
  188.         int status=tcsetattr(comPort, TCSANOW, &options);
  189.  
  190.         if (status != 0) //For error message
  191.         {
  192.         cerr << "Configuring comport failed" << endl;
  193.         return status;
  194.     }
  195.  
  196.         //Purge serial port buffers
  197.         Purge(comPort);
  198.        
  199.         return comPort;
  200. }
  201.  
  202.  
  203. //=======================================================================================
  204. // CloseComPort
  205. //---------------------------------------------------------------------------------------
  206. // Closes a port that was previously opened with OpenComPort
  207. //=======================================================================================
  208. void CloseComPort(ComPortHandle comPort)
  209. {
  210.         close(comPort);
  211. }
  212.  
  213. //=======================================================================================
  214. // readComPort
  215. //---------------------------------------------------------------------------------------
  216. // read the specivied number of bytes from the com port
  217. //=======================================================================================
  218. int readComPort(ComPortHandle comPort, Byte* bytes, int bytesToRead)
  219. {
  220.         int bytesRead = read(comPort, bytes, bytesToRead);       
  221. }
  222.  
  223. //=======================================================================================
  224. // writeComPort
  225. //---------------------------------------------------------------------------------------
  226. // send bytes to the com port
  227. //=======================================================================================
  228. int writeComPort(ComPortHandle comPort, const Byte* bytesToWrite, int size)
  229. {
  230.         return write(comPort, bytesToWrite, size);
  231. }
  232.  
  233.  
  234. /////////////////////////////////////////////////////////////////////////////////////////////////////
  235. //
  236. // functions for communicationg with the 3dm-gx1
  237. //
  238. /////////////////////////////////////////////////////////////////////////////////////////////////////
  239.  
  240.  
  241. struct EulerAngles
  242. {
  243.         float roll;
  244.         float pitch;
  245.         float yaw;
  246. };
  247.  
  248.  
  249. //=======================================================================================
  250. // ReadEulerAngles
  251. //=======================================================================================
  252. bool ReadEulerAngles(ComPortHandle comPort, EulerAngles& eulerAngles)
  253. {
  254.         static const Byte COMMAND_BYTE  = 0xCE;
  255.         writeComPort(comPort, &COMMAND_BYTE, 1);
  256.        
  257.         static const int RESPONSE_SIZE = 19;
  258.         Byte response[RESPONSE_SIZE] = {0};
  259.         int size = readComPort(comPort, &response[0], RESPONSE_SIZE);
  260.        
  261.        
  262.         //cout << size << endl;
  263.         //cout << RESPONSE_SIZE << endl;
  264.         //must get all the bytes we want, or it's not a valid read
  265.         if(size != RESPONSE_SIZE)
  266.         {
  267.                 cerr << "Invalid response size" << endl;
  268.                 return false;
  269.         }
  270.        
  271.         // verify first byte matches the command byte
  272.         if(response[0] != COMMAND_BYTE)
  273.         {
  274.                 cout << response[0] << COMMAND_BYTE << endl;
  275.                 cerr << "Invalid response" << endl;
  276.                 return false;
  277.         }
  278.        
  279.         //Verify the checksum
  280.         short responseChecksum = convert2ushort(&response[RESPONSE_SIZE-2]);
  281.         short calculatedChecksum = i3dmgx2_Checksum(&response[0], RESPONSE_SIZE-2);
  282.                
  283.         if(calculatedChecksum != responseChecksum)
  284.         {
  285.                 cerr << "calculatedChecksum" << calculatedChecksum << endl;
  286.                 cerr << "responseChecksum"  <<  responseChecksum << endl;
  287.                 cerr << "Invalid Checksum"  << endl;
  288.                 return false;
  289.         }
  290.        
  291.         //conversion factor used to convert the returned values to degrees
  292.         static const float SCALE_AS_DEGREES = 180.0/PI;
  293.        
  294.         eulerAngles.roll  = FloatFromBytes(&response[1])*SCALE_AS_DEGREES;
  295.         eulerAngles.pitch = FloatFromBytes(&response[5])*SCALE_AS_DEGREES;
  296.         eulerAngles.yaw   = FloatFromBytes(&response[9])*SCALE_AS_DEGREES;
  297.        
  298.         return true;
  299. }
  300.  
  301.  
  302. //=======================================================================================
  303. // main
  304. //=======================================================================================
  305. int main(int argc, char* argv[])
  306. {
  307.         //open the comport
  308.         printf("Opening comport...\n");
  309.         ComPortHandle comPort = OpenComPort("/dev/ttyUSB0");
  310.         if(comPort > 0)
  311.         {      
  312.        
  313.                 //every second get the euler angles from the sensor, and print the results
  314.                 cout<<"Roll,Pitch,Yaw"<<endl;          
  315.                 while(true)
  316.                 {
  317.                         sleep(1);
  318.                         EulerAngles eulerAngles;
  319.                         if(ReadEulerAngles(comPort, eulerAngles))
  320.                         {
  321.                                 cout << eulerAngles.roll << "," << eulerAngles.pitch << "," << eulerAngles.yaw << endl;
  322.                                 //printf("Roll:%0.2f Pitch:%0.2f Yaw:%0.2f\n", eulerAngles.roll, eulerAngles.pitch, eulerAngles.yaw);
  323.                         }
  324.                 }
  325.        
  326.        
  327.                 CloseComPort(comPort);
  328.         }
  329.        
  330.  
  331.         return 0;
  332. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top