Audren Cloitre
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 |
--- /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;
+ }
+
+}
+
+//----------------------------------------------------------------------------------------------
--- /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