My first attempt at writing a library for the UM6LT. Not complete and not well documented but it does the trick for me.
Revision 0:5651731cfb32, committed 2012-09-30
- Comitter:
- acloitre
- Date:
- Sun Sep 30 17:59:52 2012 +0000
- Commit message:
- [mbed] converted /UM6LT_v1/UM6LT
Changed in this revision
UM6LT.cpp | Show annotated file Show diff for this revision Revisions of this file |
UM6LT.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r 5651731cfb32 UM6LT.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/UM6LT.cpp Sun Sep 30 17:59:52 2012 +0000 @@ -0,0 +1,997 @@ +#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; + } + +} + +//----------------------------------------------------------------------------------------------
diff -r 000000000000 -r 5651731cfb32 UM6LT.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/UM6LT.h Sun Sep 30 17:59:52 2012 +0000 @@ -0,0 +1,162 @@ +#ifndef MBED_UM6LT_H +#define MBED_UM6LT_H + +#include "mbed.h" + +// UM6 Configuration Registers + + #define UM6_COMMUNICATION 0x00 + #define UM6_MISC_CONFIG 0x01 + #define UM6_MAG_REF_X 0x02 + #define UM6_MAG_REF_Y 0x03 + #define UM6_MAG_REF_Z 0x04 + #define UM6_ACCEL_REF_X 0x05 + #define UM6_ACCEL_REF_Y 0x06 + #define UM6_ACCEL_REF_Z 0x07 + #define UM6_EKF_MAG_VARIANCE 0x08 + #define UM6_EKF_ACCEL_VARIANCE 0x09 + #define UM6_EKF_PROCESS_VARIANCE 0x0A + #define UM6_GYRO_BIAS_XY 0x0B + #define UM6_GYRO_BIAS_Z 0x0C + #define UM6_ACCEL_BIAS_XY 0x0D + #define UM6_ACCEL_BIAS_Z 0x0E + #define UM6_MAG_BIAS_XY 0x0F + #define UM6_MAG_BIAS_Z 0x10 + #define UM6_ACCEL_CAL_00 0x11 + #define UM6_ACCEL_CAL_01 0x12 + #define UM6_ACCEL_CAL_02 0x13 + #define UM6_ACCEL_CAL_10 0x14 + #define UM6_ACCEL_CAL_11 0x15 + #define UM6_ACCEL_CAL_12 0x16 + #define UM6_ACCEL_CAL_20 0x17 + #define UM6_ACCEL_CAL_21 0x18 + #define UM6_ACCEL_CAL_22 0x19 + #define UM6_GYRO_CAL_00 0x1A + #define UM6_GYRO_CAL_01 0x1B + #define UM6_GYRO_CAL_02 0x1C + #define UM6_GYRO_CAL_10 0x1D + #define UM6_GYRO_CAL_11 0x1E + #define UM6_GYRO_CAL_12 0x1F + #define UM6_GYRO_CAL_20 0x20 + #define UM6_GYRO_CAL_21 0x21 + #define UM6_GYRO_CAL_22 0x22 + #define UM6_MAG_CAL_00 0x23 + #define UM6_MAG_CAL_01 0x24 + #define UM6_MAG_CAL_02 0x25 + #define UM6_MAG_CAL_10 0x26 + #define UM6_MAG_CAL_11 0x27 + #define UM6_MAG_CAL_12 0x28 + #define UM6_MAG_CAL_20 0x29 + #define UM6_MAG_CAL_21 0x2A + #define UM6_MAG_CAL_22 0x2B + +// UM6 Data Registers + + #define UM6_STATUS 0x55 + #define UM6_GYRO_RAW_XY 0x56 + #define UM6_GYRO_RAW_Z 0x57 + #define UM6_ACCEL_RAW_XY 0x58 + #define UM6_ACCEL_RAW_Z 0x59 + #define UM6_MAG_RAW_XY 0x5A + #define UM6_MAG_RAW_Z 0x5B + #define UM6_GYRO_PROC_XY 0x5C + #define UM6_GYRO_PROC_Z 0x5D + #define UM6_ACCEL_PROC_XY 0x5E + #define UM6_ACCEL_PROC_Z 0x5F + #define UM6_MAG_PROC_XY 0x60 + #define UM6_MAG_PROC_Z 0x61 + #define UM6_EULER_PHI_THETA 0x62 + #define UM6_EULER_PSI 0x63 + #define UM6_QUAT_AB 0x64 + #define UM6_QUAT_CD 0x65 + #define UM6_ERROR_COV_00 0x66 + #define UM6_ERROR_COV_01 0x67 + #define UM6_ERROR_COV_02 0x68 + #define UM6_ERROR_COV_03 0x69 + #define UM6_ERROR_COV_10 0x6A + #define UM6_ERROR_COV_11 0x6B + #define UM6_ERROR_COV_12 0x6C + #define UM6_ERROR_COV_13 0x6D + #define UM6_ERROR_COV_20 0x6E + #define UM6_ERROR_COV_21 0x6F + #define UM6_ERROR_COV_22 0x70 + #define UM6_ERROR_COV_23 0x71 + #define UM6_ERROR_COV_30 0x72 + #define UM6_ERROR_COV_31 0x73 + #define UM6_ERROR_COV_32 0x74 + #define UM6_ERROR_COV_33 0x75 + +// UM6 Command Registers + + #define UM6_GET_FW_VERSION 0xAA + #define UM6_FLASH_COMMIT 0xAB + #define UM6_ZERO_GYROS 0xAC + #define UM6_RESET_EKF 0xAD + #define UM6_GET_DATA 0xAE + #define UM6_SET_ACCEL_REF 0xAF + #define UM6_SET_MAG_REF 0xB0 + #define UM6_RESET_TO_FACTORY 0xB1 + + #define UM6_BAD_CHECKSUM 0xFD + #define UM6_UNKNOWN_ADDRESS 0xFE + #define UM6_INVALID_BATCH_SIZE 0xFF + +// UM6 scale factors + + #define UM6_ANGLE_FACTOR 0.0109863 + #define UM6_ACCEL_FACTOR 0.000183105 + #define UM6_MAG_FACTOR 0.000305176 + #define UM6_ANGLE_RATE_FACTOR 0.0610352 + #define UM6_QUATERNION_FACTOR 0.0000335693 + +class UM6LT { + + public: + + UM6LT (PinName tx, PinName rx); + + void baud(int baudrate); + int readable(void); + char getc(void); + void putc(char byte); + + void setCommParams(int broadcastRate, int baudrate, int* dataToTransmit, int broadcastEnabled); + void setConfigParams(int wantPPS, int wantQuatUpdate, int wantCal, int wantAccelUpdate, int wantMagUpdate); + int getStatus(void); + + int zeroGyros(int& gyroBiasX, int& gyroBiasY, int& gyroBiasZ); + int autoSetAccelRef(void); + int autoSetMagRef(void); + int resetEKF(void); + int writeToFlash(void); + int resetToFactory(void); + + int getAngles(int& roll, int& pitch, int& yaw); // in degrees + int getAccel(int& accelX, int& accelY, int& accelZ); // in milli-gravity + int getMag(int& magX, int& magY, int& magZ); // in milli-unit. the norm of the vector should be one + int getAngleRates(int& rollRate, int& pitchRate, int& yawRate); // in degree/sec + int getQuaternion(int& a, int& b, int& c, int& d); // in milli-unit + + private: + + Serial serial_; + int stdDelayMs; + + int verifyChecksum(int byte1, int byte2); + void createChecksum(int checksum_dec, int& byte1, int& byte2); + void createByte(int* bitsList, int& byte); + void decomposeByte(int* bitsList, int byte); + int twosComplement(int byte1, int byte2); + + void oneWordWrite(int hasData, int isBatch, int BL, int address, int* data); + void oneWordRead(int& PT, int& N, int& address, int* data); + void requestData(int address, int expectedBL); + int sendOneCommand(int address); + int getTwoBatches(int address, int* data); + + int noCommError(int address, int addressExpected, int PT); + void WhatCommError(int addressRead, int addressExpected, int PT); + +}; + +#endif /* MBED_UM6LT_H */ \ No newline at end of file