Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <termios.h> // terminal io (serial port) interface
- #include <fcntl.h> // File control definitions
- #include <errno.h> // Error number definitions
- #include <assert.h>
- #include <iostream>
- #define PI (3.141592653589793)
- typedef int ComPortHandle;
- typedef unsigned char Byte;
- using std::cout;
- using std::cerr;
- using std::endl;
- int TestByteOrder()
- {
- short int word = 0x0001;
- char *byte = (char *) &word;
- return(byte[0] ? LITTLE_ENDIAN : BIG_ENDIAN);
- }
- /*----------------------------------------------------------------------
- * convert2short
- * Convert two adjacent bytes to an integer.
- *
- * parameters: buffer : pointer to first of two buffer bytes.
- * returns: the converted value aa a signed short -32 to +32k.
- *--------------------------------------------------------------------*/
- short convert2short(unsigned char* buffer) {
- short x;
- if(TestByteOrder() != BIG_ENDIAN) {
- x = (buffer[0] <<8) + (buffer[1] & 0xFF);
- }else{
- x = (short)buffer;
- }
- return x;
- }
- /*----------------------------------------------------------------------
- * convert2ushort
- * Convert two adjacent bytes to a short.
- *
- * parameters: buffer : pointer to first of two buffer bytes.
- * returns: the converted value as a unsigned short 0-64k.
- *--------------------------------------------------------------------*/
- unsigned short convert2ushort(unsigned char* buffer) {
- unsigned short x;
- if(TestByteOrder() != BIG_ENDIAN) {
- x = (buffer[0] <<8) + (buffer[1] & 0xFF);
- }else{
- x = (unsigned short)buffer;
- }
- return x;
- }
- /*----------------------------------------------------------------------
- * FloatFromBytes
- * Converts bytes to Float.
- *
- * parameters: pBytes : received buffer containing pointer to 4 bytes
- *
- * returns: a float value.
- *--------------------------------------------------------------------*/
- float FloatFromBytes(const unsigned char* pBytes)
- {
- float f = 0;
- if(TestByteOrder() != BIG_ENDIAN) {
- ((Byte*)(&f))[0] = pBytes[3];
- ((Byte*)(&f))[1] = pBytes[2];
- ((Byte*)(&f))[2] = pBytes[1];
- ((Byte*)(&f))[3] = pBytes[0];
- }else{
- ((Byte*)(&f))[0] = pBytes[0];
- ((Byte*)(&f))[1] = pBytes[1];
- ((Byte*)(&f))[2] = pBytes[2];
- ((Byte*)(&f))[3] = pBytes[3];
- }
- return f;
- }
- /*----------------------------------------------------------------------
- * i3dmgx2_Checksum
- * Calculate checksum on a received data buffer.
- *
- * Note: The last two bytes, which contain the received checksum,
- * are not included in the calculation.
- *
- * parameters: buffer : pointer to the start of the received buffer.
- * length - the length (in chars) of the buffer.
- *
- * returns: the calculated checksum.
- *--------------------------------------------------------------------*/
- unsigned short i3dmgx2_Checksum(const unsigned char* pBytes, int count)
- {
- unsigned short i3_checksum = 0;
- int i = 0;
- for(i = 0; i < count; ++i)
- {
- i3_checksum+=pBytes[i];
- }
- return i3_checksum;
- }
- /////////////////////////////////////////////////////////////////////////////////////////////////////
- //
- // utility functions for working with a com port in Linux
- //
- /////////////////////////////////////////////////////////////////////////////////////////////////////
- //=======================================================================================
- // Purge
- //---------------------------------------------------------------------------------------
- // Clears the com port's read and write buffers
- //=======================================================================================
- bool Purge(ComPortHandle comPortHandle)
- {
- if (tcflush(comPortHandle,TCIOFLUSH)==-1)
- {
- cerr << "flush failed" << endl;
- return false;
- }
- return true;
- }
- //=======================================================================================
- // OpenComPort
- //---------------------------------------------------------------------------------------
- // Opens a com port with the correct settings for communicating with a MicroStrain
- // 3DM-GX2 sensor
- //=======================================================================================
- ComPortHandle OpenComPort(const char* comPortPath)
- {
- ComPortHandle comPort = open(comPortPath, O_RDWR | O_NOCTTY);
- if (comPort== -1) //Opening of port failed
- {
- cerr << "Unable to open com Port\n"
- "Error:(" << errno << ") " << strerror(errno) << endl;
- return -1;
- }
- //Get the current options for the port...
- termios options;
- tcgetattr(comPort, &options);
- //set the baud rate to 115200
- int baudRate = B115200;
- cfsetospeed(&options, baudRate);
- cfsetispeed(&options, baudRate);
- //set the number of data bits.
- options.c_cflag &= ~CSIZE; // Mask the character size bits
- options.c_cflag |= CS8;
- //set the number of stop bits to 1
- options.c_cflag &= ~CSTOPB;
- //Set parity to None
- options.c_cflag &=~PARENB;
- //set for non-canonical (raw processing, no echo, etc.)
- options.c_iflag = IGNPAR; // ignore parity check close_port(int
- options.c_oflag = 0; // raw output
- options.c_lflag = 0; // raw input
- //Time-Outs -- won't work with NDELAY option in the call to open
- options.c_cc[VMIN] = 0; // block reading until RX x characers. If x = 0, it is non-blocking.
- options.c_cc[VTIME] = 10; // Inter-Character Timer -- i.e. timeout= x*.1 s
- //Set local mode and enable the receiver
- options.c_cflag |= (CLOCAL | CREAD);
- //Purge serial port buffers
- Purge(comPort);
- //Set the new options for the port...
- int status=tcsetattr(comPort, TCSANOW, &options);
- if (status != 0) //For error message
- {
- cerr << "Configuring comport failed" << endl;
- return status;
- }
- //Purge serial port buffers
- Purge(comPort);
- return comPort;
- }
- //=======================================================================================
- // CloseComPort
- //---------------------------------------------------------------------------------------
- // Closes a port that was previously opened with OpenComPort
- //=======================================================================================
- void CloseComPort(ComPortHandle comPort)
- {
- close(comPort);
- }
- //=======================================================================================
- // readComPort
- //---------------------------------------------------------------------------------------
- // read the specivied number of bytes from the com port
- //=======================================================================================
- int readComPort(ComPortHandle comPort, Byte* bytes, int bytesToRead)
- {
- int bytesRead = read(comPort, bytes, bytesToRead);
- }
- //=======================================================================================
- // writeComPort
- //---------------------------------------------------------------------------------------
- // send bytes to the com port
- //=======================================================================================
- int writeComPort(ComPortHandle comPort, const Byte* bytesToWrite, int size)
- {
- return write(comPort, bytesToWrite, size);
- }
- /////////////////////////////////////////////////////////////////////////////////////////////////////
- //
- // functions for communicationg with the 3dm-gx1
- //
- /////////////////////////////////////////////////////////////////////////////////////////////////////
- struct EulerAngles
- {
- float roll;
- float pitch;
- float yaw;
- };
- //=======================================================================================
- // ReadEulerAngles
- //=======================================================================================
- bool ReadEulerAngles(ComPortHandle comPort, EulerAngles& eulerAngles)
- {
- static const Byte COMMAND_BYTE = 0xCE;
- writeComPort(comPort, &COMMAND_BYTE, 1);
- static const int RESPONSE_SIZE = 19;
- Byte response[RESPONSE_SIZE] = {0};
- int size = readComPort(comPort, &response[0], RESPONSE_SIZE);
- //cout << size << endl;
- //cout << RESPONSE_SIZE << endl;
- //must get all the bytes we want, or it's not a valid read
- if(size != RESPONSE_SIZE)
- {
- cerr << "Invalid response size" << endl;
- return false;
- }
- // verify first byte matches the command byte
- if(response[0] != COMMAND_BYTE)
- {
- cout << response[0] << COMMAND_BYTE << endl;
- cerr << "Invalid response" << endl;
- return false;
- }
- //Verify the checksum
- short responseChecksum = convert2ushort(&response[RESPONSE_SIZE-2]);
- short calculatedChecksum = i3dmgx2_Checksum(&response[0], RESPONSE_SIZE-2);
- if(calculatedChecksum != responseChecksum)
- {
- cerr << "calculatedChecksum" << calculatedChecksum << endl;
- cerr << "responseChecksum" << responseChecksum << endl;
- cerr << "Invalid Checksum" << endl;
- return false;
- }
- //conversion factor used to convert the returned values to degrees
- static const float SCALE_AS_DEGREES = 180.0/PI;
- eulerAngles.roll = FloatFromBytes(&response[1])*SCALE_AS_DEGREES;
- eulerAngles.pitch = FloatFromBytes(&response[5])*SCALE_AS_DEGREES;
- eulerAngles.yaw = FloatFromBytes(&response[9])*SCALE_AS_DEGREES;
- return true;
- }
- //=======================================================================================
- // main
- //=======================================================================================
- int main(int argc, char* argv[])
- {
- //open the comport
- printf("Opening comport...\n");
- ComPortHandle comPort = OpenComPort("/dev/ttyUSB0");
- if(comPort > 0)
- {
- //every second get the euler angles from the sensor, and print the results
- cout<<"Roll,Pitch,Yaw"<<endl;
- while(true)
- {
- sleep(1);
- EulerAngles eulerAngles;
- if(ReadEulerAngles(comPort, eulerAngles))
- {
- cout << eulerAngles.roll << "," << eulerAngles.pitch << "," << eulerAngles.yaw << endl;
- //printf("Roll:%0.2f Pitch:%0.2f Yaw:%0.2f\n", eulerAngles.roll, eulerAngles.pitch, eulerAngles.yaw);
- }
- }
- CloseComPort(comPort);
- }
- return 0;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement