1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153
|
int main()
{
Gyro *gyro;
//connect and reset / calibrate:
gyro = new Gyro("/dev/ttyUSB0", 3);
}
/**
------
relevant content of gyro class:
------------------
*/
Gyro::Gyro(string path, unsigned int calibrateTime = 30) : path(path) {
com = new CPCom(path);
gyro_model = CRUIZCORE_XG1010;
//The UART default configuration for CruizCore xg1010 is
//115,200 bps, 8 data bit, 1 stop bit, and no parity.
//reset by transmitting: $MIB,RESET*87
unsigned char cmd[14];
cmd[0] = '$';
cmd[1] = 'M';
cmd[2] = 'I';
cmd[3] = 'B';
cmd[4] = ',';
cmd[5] = 'R';
cmd[6] = 'E';
cmd[7] = 'S';
cmd[8] = 'E';
cmd[9] = 'T';
cmd[10]= '*';
cmd[11]= '8';
cmd[12]= '7';
cmd[13]= '\0';
int len;
int r;
//printf("%s\n", cmd);
this->com->cpwrite(cmd, 13);
this->com->cpflush();
printf("The procedure will take ~4 seconds\n\n");
usleep(4000000);
//init tmp string to null
unsigned char response[142];
response[0] = '\0';
//to avoid a printf reading random memory if read returns non null terminated strings
response[141] = '\0';
r = this->com->cpread(response, 140);
this->com->cpflush();
if(r > 0)
{
printf("%s\n has been reset and calibrated\n", response);
}
}
Gyro::~Gyro() {
delete com;
}
/**
relevant part of com class:
*/
CPCom::CPCom(const string comPortPath) {
this->_cph = open(comPortPath.c_str(), O_RDWR | O_NOCTTY);
if (this->_cph == -1){ //Opening of port failed
cout << "Unable to open IMU com port " << comPortPath << endl << "Errno = " << errno << endl;
printf("error:%s\n",strerror(errno));
return;
}
//Get the current options for the port...
struct termios options;
tcgetattr(this->_cph, &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] = 100; // Inter-Character Timer -- i.e. timeout= x*.1 s
//Set local mode and enable the receiver
options.c_cflag |= (CLOCAL | CREAD);
//flush serial port buffers
this->cpflush();
//Set the new options for the port...
int status = tcsetattr(this->_cph, TCSANOW, &options);
if (status != 0){ //For error message
cout << "Configuring comport failed with status error = " << status << endl;
return;
}
//flush serial port buffers
this->cpflush();
}
CPCom::~CPCom() {
close(this->_cph);
}
bool CPCom::cpflush() {
if ( tcflush(this->_cph,TCIOFLUSH) == -1 ){
cout << "IMU Flush Failed" << endl;
return false;
}
return true;
}
int CPCom::cpread(unsigned char* bytes, int bytesToRead) {
int r = read(this->_cph, bytes, bytesToRead);
if(r == -1)
{
printf("error:%s\n",strerror(errno));
}
return r;
}
int CPCom::cpwrite(unsigned char* bytes, int bytesToWrite) {
return write(this->_cph, bytes, bytesToWrite);
}
|