My first attempt at writing a library for the UM6LT. Not complete and not well documented but it does the trick for me.
UM6LT.cpp
- Committer:
- acloitre
- Date:
- 2012-09-30
- Revision:
- 0:5651731cfb32
File content as of revision 0:5651731cfb32:
#include "UM6LT.h" #include "mbed.h" UM6LT::UM6LT(PinName tx, PinName rx):serial_(tx,rx) {serial_.baud(115200);stdDelayMs = 2;} //---------------------------------------------------------------------------------------------- //----------------------------------- Private Functions ------------------------------------ //---------------------------------------------------------------------------------------------- int UM6LT::verifyChecksum(int byte1, int byte2){ int checksumDATA = 0; div_t div_res1; div_t div_res2; int quot1 = byte1; int quot2 = byte2; int coef[16] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 32768}; for(int i=0; i<8; ++i){ div_res1 = div(quot1,2); div_res2 = div(quot2,2); checksumDATA = checksumDATA + div_res1.rem * coef[i+8] + div_res2.rem * coef[i]; quot1 = div_res1.quot; quot2 = div_res2.quot; } return checksumDATA; } //---------------------------------------------------------------------------------------------- void UM6LT::createChecksum(int checksum_dec, int& byte1, int& byte2){ div_t div_res; int quot = checksum_dec; int coef[24] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 4, 8, 16, 32, 64, 128, 0, 0, 0, 0, 0, 0, 0, 0}; byte1 = 0; byte2 = 0; for(int i=0; i<16; ++i){ div_res = div(quot,2); byte1 = byte1 + div_res.rem * coef[i]; byte2 = byte2 + div_res.rem * coef[i + 8]; quot = div_res.quot; } } //---------------------------------------------------------------------------------------------- void UM6LT::createByte(int* bitsList, int& byte) { int coef[] = {1, 2, 4, 8, 16, 32, 64, 128}; for(int i=0; i<8; i++){ byte = byte + bitsList[i]*coef[i]; } } //---------------------------------------------------------------------------------------------- void UM6LT::decomposeByte(int* bitsList, int byte){ div_t div_res; int quot = byte; for(int i=0; i<8; i++){ div_res = div(quot, 2); bitsList[i] = div_res.rem; quot = div_res.quot; } } //---------------------------------------------------------------------------------------------- int UM6LT::twosComplement(int byte1, int byte2){ int sumData = 0; div_t div_res1; div_t div_res2; int quot1 = byte1; int quot2 = byte2; int coef[16] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 32768}; for(int i=0; i<7; ++i){ div_res1 = div(quot1,2); div_res2 = div(quot2,2); sumData = sumData + div_res1.rem * coef[i+8] + div_res2.rem * coef[i]; quot1 = div_res1.quot; quot2 = div_res2.quot; } div_res1 = div(quot1,2); div_res2 = div(quot2,2); if(div_res1.rem == 1){ sumData = sumData + div_res2.rem * coef[7] - coef[15]; } else{ sumData = sumData + div_res2.rem * coef[7]; } return sumData; } //---------------------------------------------------------------------------------------------- void UM6LT::oneWordWrite(int hasData, int isBatch, int BL, int address, int* data) { int N, PT, checksumDATA; if(hasData){ N = BL*4; //Number of data bytes } else{ N = 0; } int word [N+7]; int coef [] = {4, 8, 16, 32}; div_t div_res; int quot = BL; PT = 128*hasData + 64*isBatch; for(int i=0; i<4; ++i){ div_res = div(quot,2); PT = PT + div_res.rem * coef[i]; quot = div_res.quot; } checksumDATA = 115 + 110 + 112 + PT + address; if(hasData){ for(int i=0; i<N; i++){ checksumDATA = checksumDATA + data[i]; word[i+5] = data[i]; } } int byte1 = 0; int byte2 = 0; createChecksum(checksumDATA, byte1, byte2); word[0] = 115; //'s' word[1] = 110; //'n' word[2] = 112; //'p' word[3] = PT; word[4] = address; word[N+5] = byte1; word[N+6] = byte2; if(serial_.writeable()) { for(int i=0; i<N+7; i++){ serial_.putc(word[i]); } } else{ printf("IMU not writeable.\r\n"); } } //---------------------------------------------------------------------------------------------- void UM6LT::oneWordRead(int& PT, int& N, int& address, int* data){ int sumData = 0; int PT_byte [8]; char snp [] = {'0', '0', '0'}; if(serial_.readable()){ while((snp[0] != 's' || snp[1] != 'n' || snp[2] != 'p') && serial_.readable()){ snp[0] = snp[1]; snp[1] = snp[2]; snp[2] = serial_.getc(); } if(snp[0] == 's' && snp[1]=='n' && snp[2] == 'p'){ PT = serial_.getc(); address = serial_.getc(); decomposeByte(PT_byte, PT); int hasData = PT_byte[7]; int CF = PT_byte[0]; if(CF){ // find something to do if command failed printf("Last command sent to UM6 failed.\r\n"); } else if(hasData){ int BL = PT_byte[5]*8 + PT_byte[4]*4 + PT_byte[3]*2 + PT_byte[2]; N = 4*BL; for(int i=0; i<N; i++){ data[i] = serial_.getc(); sumData = sumData + data[i]; } } else{ N = 0; } int byte1 = serial_.getc(); int byte2 = serial_.getc(); int checksumUM6LT = verifyChecksum(byte1, byte2); int checksumDATA = 115 + 110 + 112 + PT + address + sumData; if(checksumDATA != checksumUM6LT){ // Find something to do if checksum not good printf("Calculated checksum does not match the one from the UM6.\r\n"); data[0] = -1; } } else{ printf("Could not read an expected word from UM6. Its buffer is now empty\r\n"); } } else{ // Find something to do if IMU not readable printf("Minor synchronization issue with UM6.\r\n"); data[0] = -1; } } //---------------------------------------------------------------------------------------------- void UM6LT::requestData(int address, int expectedBL){ int hasData = 0; int isBatch = 1; int data[1]; if(serial_.readable()){ while(serial_.readable()){ data[0] = serial_.getc(); } } if(serial_.writeable()){ oneWordWrite(hasData, isBatch, expectedBL, address, data); } else{ printf("IMU not writeable. Data request not sent.\r\n"); } } //---------------------------------------------------------------------------------------------- int UM6LT::sendOneCommand(int address){ int PT = -1; int N = -1; int addressRead = -1; int data [1] = {0}; int okToMoveOn = 0; int count = 0; while(!okToMoveOn && count<5){ requestData(address, 0); wait_ms(stdDelayMs); oneWordRead(PT, N, addressRead, data); okToMoveOn = noCommError(addressRead, address, PT); count++; } if(okToMoveOn){ switch(address){ case UM6_FLASH_COMMIT: printf("All current configurations were written to flash.\r\n"); break; case UM6_RESET_EKF: printf("EKF algorithm successfully reset.\r\n"); break; case UM6_SET_ACCEL_REF: printf("Acceleration reference vector updated.\r\n"); break; case UM6_SET_MAG_REF: printf("Magnetic reference vector updated.\r\n"); break; case UM6_RESET_TO_FACTORY: printf("UM6 reset to factory settings.\r\n"); break; default: printf("This command is not coded. Can't say what happened. address: 0x%x\r\n", address); break; } return 1; } else{ WhatCommError(addressRead, address, PT); switch(address){ case UM6_FLASH_COMMIT: printf("Current configurations could not be written to flash.\r\n"); break; case UM6_RESET_EKF: printf("EKF algorithm could not be reset.\r\n"); break; case UM6_SET_ACCEL_REF: printf("Acceleration reference vector failed to update.\r\n"); break; case UM6_SET_MAG_REF: printf("Magnetic reference vector failed to update.\r\n"); break; case UM6_RESET_TO_FACTORY: printf("UM6 could not be reset to factory settings.\r\n"); break; default: printf("This command is not coded. Can't say what happened. address: 0x%x\r\n", address); break; } return 0; } } //---------------------------------------------------------------------------------------------- int UM6LT::getTwoBatches(int address, int* dataToPost){ dataToPost[0] = 0; dataToPost[1] = 0; dataToPost[2] = 0; dataToPost[3] = 0; int PT = -1; int N = -1; int addressRead = -1; int dataToRead [8] = {0,0,0,0,0,0,0,0}; int okToMoveOn = 0; int count = 0; while(!okToMoveOn && count<5){ requestData(address, 2); wait_ms(stdDelayMs); oneWordRead(PT, N, addressRead, dataToRead); okToMoveOn = noCommError(addressRead, address, PT); count++; } if(okToMoveOn){ switch(address){ case UM6_EULER_PHI_THETA: dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ANGLE_FACTOR); dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ANGLE_FACTOR); dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_ANGLE_FACTOR); break; case UM6_ACCEL_PROC_XY: dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ACCEL_FACTOR*1000); dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ACCEL_FACTOR*1000); dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_ACCEL_FACTOR*1000); break; case UM6_MAG_PROC_XY: dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_MAG_FACTOR*1000); dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_MAG_FACTOR*1000); dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_MAG_FACTOR*1000); break; case UM6_GYRO_PROC_XY: dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ANGLE_RATE_FACTOR); dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ANGLE_RATE_FACTOR); dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_ANGLE_RATE_FACTOR); break; case UM6_QUAT_AB: dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_QUATERNION_FACTOR*1000); dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_QUATERNION_FACTOR*1000); dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_QUATERNION_FACTOR*1000); dataToPost[3] = (int)(twosComplement(dataToRead[6], dataToRead[7])*UM6_QUATERNION_FACTOR*1000); break; default: dataToPost[0] = -1; dataToPost[1] = -1; dataToPost[2] = -1; dataToPost[3] = -1; printf("Data acquisition not programmed for this address: %d\r\n", address); break; } return 1; } else{ WhatCommError(address, UM6_ACCEL_PROC_XY, PT); switch(address){ case UM6_EULER_PHI_THETA: printf("Could not acquire the values of the Euler angles.\r\n"); break; case UM6_ACCEL_PROC_XY: printf("Could not acquire the components of the acceleration vector.\r\n"); break; case UM6_MAG_PROC_XY: printf("Could not acquire the components of the magnetic vector.\r\n"); break; case UM6_GYRO_PROC_XY: printf("Could not acquire the values of angle rates.\r\n"); break; case UM6_QUAT_AB: printf("Could not acquire the components of the quaternion.\r\n"); break; default: printf("Data acquisition not programmed for this address: %d\r\n", address); break; } return 0; } } //---------------------------------------------------------------------------------------------- int UM6LT::noCommError(int addressRead, int addressExpected, int PT){ if(addressRead == 0xFD){ return 0; } else if(addressRead == 0xFE){ return 0; } else if(addressRead == 0xFF){ return 0; } else if(addressRead == addressExpected){ return 1; } else{ return 0; } } //---------------------------------------------------------------------------------------------- void UM6LT::WhatCommError(int addressRead, int addressExpected, int PT){ if(addressRead == 0xFD){ printf("\r\n !!!!!! \r\nLast word sent to UM6 had a bad checksum.\r\n !!!!!! \r\n"); } else if(addressRead == 0xFE){ printf("\r\n !!!!!! \r\nLast word was sent to UM6 at an unknown address.\r\n !!!!!! \r\n"); printf("address read: 0x%x\r\naddress expected: 0x%x\r\n", addressRead, addressExpected); } else if(addressRead == 0xFF){ printf("\r\n !!!!!! \r\nLast word sent to UM6 had a bad batch size.\r\n !!!!!! \r\n"); printf("PT: %d\r\n", PT); } else if(addressRead == addressExpected){ printf("No communication error detected...\r\n"); printf("PT: %d\r\n", PT); printf("address read: 0x%x\r\naddress expected: 0x%x\r\n", addressRead, addressExpected); } else{ printf("\r\n !!!!!! \r\nLast word read was not the one expected.\r\n !!!!!! \r\n"); printf("PT: %d\r\n", PT); printf("address read: 0x%x\r\naddress expected: 0x%x\r\n", addressRead, addressExpected); } } //---------------------------------------------------------------------------------------------- //---------------------------------- Public Functions -------------------------------------- //---------------------------------------------------------------------------------------------- void UM6LT::baud(int baudrate){serial_.baud(baudrate);} //---------------------------------------------------------------------------------------------- int UM6LT::readable(void){return serial_.readable();} //---------------------------------------------------------------------------------------------- char UM6LT::getc(void){return serial_.getc();} //---------------------------------------------------------------------------------------------- void UM6LT::putc(char byte){serial_.putc(byte);} //---------------------------------------------------------------------------------------------- void UM6LT::setCommParams(int broadcastRate, int baudrate, int* dataToTransmit, int broadcastEnabled){ // Broadcast rate should be an integer between 20 and 300 (in Hertz) // Baudrate should be either 9600, 14400, 19200, 38400, 57600 or 115200 // DataToTransmit defines what data the UM6LT will provide. It's a list of 0 and 1 for a total length of N=9. See UM6LT documentation int data [4] = {0, 0, 0, 0}; int bitsList2 [8]; int bitsList3 [8]; int bitsList4 [8]; int broadcastRateByte = ((broadcastRate - 20)*255)/280; int baudrateByte [3]; switch(baudrate){ case 9600: baudrateByte[0] = 0; baudrateByte[1] = 0; baudrateByte[2] = 0; break; case 14400: baudrateByte[0] = 1; baudrateByte[1] = 0; baudrateByte[2] = 0; break; case 19200: baudrateByte[0] = 0; baudrateByte[1] = 1; baudrateByte[2] = 0; break; case 38400: baudrateByte[0] = 1; baudrateByte[1] = 1; baudrateByte[2] = 0; break; case 57600: baudrateByte[0] = 0; baudrateByte[1] = 0; baudrateByte[2] = 1; break; case 115200: baudrateByte[0] = 1; baudrateByte[1] = 0; baudrateByte[2] = 1; break; default: baudrateByte[0] = 1; baudrateByte[1] = 0; baudrateByte[2] = 1; printf("Baudrate not listed for UM6LT. Default = 115200.\r\n"); break; } bitsList2[0] = baudrateByte[0]; bitsList2[1] = baudrateByte[1]; bitsList2[2] = baudrateByte[2]; for(int i=0; i<5; i++){ bitsList2[i+3] = 0; bitsList3[i] = 0; } for(int i=0; i<3; i++){ bitsList3[i+5] = dataToTransmit[i]; } for(int i=0; i<6; i++){ bitsList4[i] = dataToTransmit[i+3]; } bitsList4[6] = broadcastEnabled; bitsList4[7] = 0; data[3] = broadcastRateByte; createByte(bitsList2, data[2]); createByte(bitsList3, data[1]); createByte(bitsList4, data[0]); int hasData = 1; int isBatch = 0; int address = UM6_COMMUNICATION; int BL = 1; int PTread = -1; int Nread = -1; int addressRead = -1; int dataRead[1]; while(addressRead != address){ oneWordWrite(hasData, isBatch, BL, address, data); wait_ms(stdDelayMs); oneWordRead(PTread, Nread, addressRead, dataRead); } if(PTread%2 == 1){ printf("Communication configuration failed\r\n"); } else{ printf("UM6 communication configuration completed\r\n"); } } //---------------------------------------------------------------------------------------------- void UM6LT::setConfigParams(int wantPPS, int wantQuatUpdate, int wantCal, int wantAccelUpdate, int wantMagUpdate){ int data [4] = {0, 0, 0, 0}; int bitsList [8] = {0, 0, 0, 0, 0, 0, 0, 0}; bitsList[3] = wantPPS; bitsList[4] = wantQuatUpdate; bitsList[5] = wantCal; bitsList[6] = wantAccelUpdate; bitsList[7] = wantMagUpdate; createByte(bitsList, data[0]); int hasData = 1; int isBatch = 0; int address = UM6_MISC_CONFIG; int BL = 1; int PTread = -1; int Nread = -1; int addressRead = -1; int dataRead[1]; while(addressRead != address){ oneWordWrite(hasData, isBatch, BL, address, data); wait_ms(stdDelayMs); oneWordRead(PTread, Nread, addressRead, dataRead); } if(PTread%2 == 1){ printf("Miscellaneous configuration failed\r\n"); } else{ printf("UM6 miscellaneous configuration completed\r\n"); } } //---------------------------------------------------------------------------------------------- int UM6LT::getStatus(void){ int PT, N, address; PT = -1; N = -1; address = -1; int data[4] = {1, 1, 1, 1}; int bitsList1[8]; int bitsList2[8]; int bitsList3[8]; int bitsList4[8]; int okToMoveOn = 0; int count = 0; while(!okToMoveOn && count<5){ requestData(UM6_STATUS, 1); wait_ms(stdDelayMs); oneWordRead(PT, N, address, data); okToMoveOn = noCommError(address, UM6_STATUS, PT); count++; } if(okToMoveOn){ decomposeByte(bitsList1, data[3]); decomposeByte(bitsList2, data[2]); decomposeByte(bitsList3, data[1]); decomposeByte(bitsList4, data[0]); if(bitsList1[0] == 1){ printf("Self-test completed\r\n"); return 0; } else if(bitsList2[5] == 1){ printf("Mag data timed out\r\n"); return 0; } else if(bitsList2[6] == 1){ printf("Accel data timed out\r\n"); return 0; } else if(bitsList2[7] == 1){ printf("Gyro data timed out\r\n"); return 0; } else if(bitsList3[0] == 1){ printf("EKF rebooted b/c of state estimate divergence\r\n"); return 0; } else if(bitsList3[1] == 1){ printf("Bus error with Mag sensor\r\n"); return 0; } else if(bitsList3[2] == 1){ printf("Bus error with Accel sensor\r\n"); return 0; } else if(bitsList3[3] == 1){ printf("Bus error with Gyro sensor\r\n"); return 0; } else if(bitsList3[4] == 1){ printf("Self-test operation failed on Mag z-axis\r\n"); return 0; } else if(bitsList3[5] == 1){ printf("Self-test operation failed on Mag y-axis\r\n"); return 0; } else if(bitsList3[6] == 1){ printf("Self-test operation failed on Mag x-axis\r\n"); return 0; } else if(bitsList3[7] == 1){ printf("Self-test operation failed on Accel z-axis\r\n"); return 0; } else if(bitsList4[0] == 1){ printf("Self-test operation failed on Accel y-axis\r\n"); return 0; } else if(bitsList4[1] == 1){ printf("Self-test operation failed on Accel x-axis\r\n"); return 0; } else if(bitsList4[2] == 1){ printf("Self-test operation failed on Gyro z-axis\r\n"); return 0; } else if(bitsList4[3] == 1){ printf("Self-test operation failed on Gyro y-axis\r\n"); return 0; } else if(bitsList4[4] == 1){ printf("Self-test operation failed on Gyro x-axis\r\n"); return 0; } else if(bitsList4[5] == 1){ printf("Gyro start-up initialization failed -> Gyro is dead\r\n"); return 0; } else if(bitsList4[6] == 1){ printf("Accel start-up initialization failed -> Accel is dead\r\n"); return 0; } else if(bitsList4[7] == 1){ printf("Mag start-up initialization failed -> Mag is dead\r\n"); return 0; } else{ printf("No problem to report.\r\n"); return 1; } } else{ WhatCommError(address, UM6_STATUS, PT); return 0; } } //---------------------------------------------------------------------------------------------- int UM6LT::zeroGyros(int& gyroBiasX, int& gyroBiasY, int& gyroBiasZ){ int PT = -1; int N = -1; int address = -1; int data [8] = {0, 0, 0, 0, 0, 0, 0, 0}; int okToMoveOn = 0; int count = 0; printf("Do not move the imu: gyro zeroing in progress.\r\n"); while(!okToMoveOn && count<5){ requestData(UM6_ZERO_GYROS, 0); wait_ms(stdDelayMs); oneWordRead(PT, N, address, data); okToMoveOn = noCommError(address, UM6_ZERO_GYROS, PT); count++; } if(okToMoveOn){ okToMoveOn = 0; count = 0; PT = -1; N = -1; address = -1; wait(1); while(!okToMoveOn && count<5){ wait_ms(stdDelayMs); oneWordRead(PT, N, address, data); okToMoveOn = noCommError(address, UM6_GYRO_BIAS_XY, PT); count++; } if(okToMoveOn){ gyroBiasX = twosComplement(data[0], data[1]); gyroBiasY = twosComplement(data[2], data[3]); gyroBiasZ = twosComplement(data[4], data[5]); printf("Gyro zeroing completed.\r\n"); return 1; } else{ WhatCommError(address, UM6_GYRO_BIAS_XY, PT); printf("Could not acquire the values of gyro biases.\r\n"); return 0; } } else{ WhatCommError(address, UM6_ZERO_GYROS, PT); printf("Could not trigger gyro zeroing script.\r\n"); return 0; } } //---------------------------------------------------------------------------------------------- int UM6LT::autoSetAccelRef(void){ int address = UM6_SET_ACCEL_REF; if(sendOneCommand(address)){ return 1; } else{ return 0; } } //---------------------------------------------------------------------------------------------- int UM6LT::autoSetMagRef(void){ int address = UM6_SET_MAG_REF; if(sendOneCommand(address)){ return 1; } else{ return 0; } } //---------------------------------------------------------------------------------------------- int UM6LT::resetEKF(void){ int address = UM6_RESET_EKF; if(sendOneCommand(address)){ return 1; } else{ return 0; } } //---------------------------------------------------------------------------------------------- int UM6LT::writeToFlash(void){ int address = UM6_FLASH_COMMIT; if(sendOneCommand(address)){ return 1; } else{ return 0; } } //---------------------------------------------------------------------------------------------- int UM6LT::resetToFactory(void){ int address = UM6_RESET_TO_FACTORY; if(sendOneCommand(address)){ return 1; } else{ return 0; } } //---------------------------------------------------------------------------------------------- int UM6LT::getAngles(int& roll, int& pitch, int& yaw){ roll = 0; pitch= 0; yaw= 0; int data[4] = {0, 0, 0, 0}; int address = UM6_EULER_PHI_THETA; if(getTwoBatches(address, data)){ roll = data[0]; pitch = data[1]; yaw = data[2]; return 1; } else{ return 0; } } //---------------------------------------------------------------------------------------------- int UM6LT::getAccel(int& accelX, int& accelY, int& accelZ){ accelX = 0; accelY = 0; accelZ = 0; int data[4] = {0, 0, 0, 0}; int address = UM6_ACCEL_PROC_XY; if(getTwoBatches(address, data)){ accelX = data[0]; accelY = data[1]; accelZ = data[2]; return 1; } else{ return 0; } } //---------------------------------------------------------------------------------------------- int UM6LT::getMag(int& magX, int& magY, int& magZ){ magX = 0; magY = 0; magZ = 0; int data[4] = {0, 0, 0, 0}; int address = UM6_MAG_PROC_XY; if(getTwoBatches(address, data)){ magX = data[0]; magY = data[1]; magZ = data[2]; return 1; } else{ return 0; } } //---------------------------------------------------------------------------------------------- int UM6LT::getAngleRates(int& rollRate, int& pitchRate, int& yawRate){ rollRate = 0; pitchRate = 0; yawRate = 0; int data[4] = {0, 0, 0, 0}; int address = UM6_GYRO_PROC_XY; if(getTwoBatches(address, data)){ rollRate = data[0]; pitchRate = data[1]; yawRate = data[2]; return 1; } else{ return 0; } } //---------------------------------------------------------------------------------------------- int UM6LT::getQuaternion(int& a, int& b, int& c, int& d){ a = 0; b = 0; c = 0; d = 0; int data[4] = {0, 0, 0, 0}; int address = UM6_QUAT_AB; if(getTwoBatches(address, data)){ a = data[0]; b = data[1]; c = data[2]; d = data[3]; return 1; } else{ return 0; } } //----------------------------------------------------------------------------------------------