File content as of revision 1:703ba8e8b2cd:

#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){
    
    /*MAJOR CHANGE: 
        - UM6_QUAT_CD ADDED
        - UM6_EULER_PSI ADDED
        - UM6_MAG_PROC_Z ADDED
        - UM6_ACCEL_PROC_Z ADDED
        -
    */
    
    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);
                break;
            case UM6_EULER_PSI:
                dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*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);
                break;
            case UM6_ACCEL_PROC_Z:
                dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*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);
                break;
            case UM6_MAG_PROC_Z:
                dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*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);
                break;  
            case UM6_GYRO_PROC_Z:
                dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*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);
                break;
            case UM6_QUAT_CD:
                dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_QUATERNION_FACTOR*1000);
                dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*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){
/*MAJOR UPDATE:
    - getTwoBatches(...) updated
*/
    roll = 0;
    pitch= 0;
    yaw= 0;
    int data_1[4] = {0, 0, 0, 0};
    int data_2[4] = {0, 0, 0, 0};
    int address_phi_theta = UM6_EULER_PHI_THETA;
    int address_psi = UM6_EULER_PSI;
    
    if(getTwoBatches(address_phi_theta, data_1) && getTwoBatches(address_psi, data_2)){
        roll = data_1[0];
        pitch = data_1[1];
        yaw = data_2[0];
        return 1;
    }
    else{
        return 0;
    }
    
}

//----------------------------------------------------------------------------------------------
/*
    SUPER NEW!!!
    
    MAJOR UPDATE:
    - getTwoBatches(...) updated
*/

int UM6LT::getPitchAngle(){
    
    int pitch = 0;
    int data[4] = {0, 0, 0, 0};
    int address_phi_theta = UM6_EULER_PHI_THETA;
    
    if(getTwoBatches(address_phi_theta, data)){
        return pitch = data[1];
    }
    else{
        return 0;
    }
    
}
//----------------------------------------------------------------------------------------------


int UM6LT::getAccel(int& accelX, int& accelY, int& accelZ){
/*MAJOR UPDATE:
    - getTwoBatches(...) updated
*/
    accelX = 0;
    accelY = 0;
    accelZ = 0;
    int data_1[4] = {0, 0, 0, 0};
    int data_2[4] = {0, 0, 0, 0};
    int address_XY = UM6_ACCEL_PROC_XY;
    int address_Z = UM6_ACCEL_PROC_Z;
    
    if(getTwoBatches(address_XY, data_1) && getTwoBatches(address_Z, data_2)){
        accelX = data_1[0];
        accelY = data_1[1];
        accelZ = data_2[0];
        return 1;
    }
    else{
        return 0;
    }
    
}

//----------------------------------------------------------------------------------------------


int UM6LT::getMag(int& magX, int& magY, int& magZ){
/*MAJOR UPDATE:
    - getTwoBatches(...) updated
*/
    magX = 0;
    magY = 0;
    magZ = 0;
    int data_1[4] = {0, 0, 0, 0};
    int data_2[4] = {0, 0, 0, 0};
    int address_XY = UM6_MAG_PROC_XY;
    int address_Z = UM6_MAG_PROC_Z;
    
    if(getTwoBatches(address_XY, data_1) && getTwoBatches(address_Z, data_2)){
        magX = data_1[0];
        magY = data_1[1];
        magZ = data_2[0];
        return 1;
    }
    else{
        return 0;
    }
    
}

//----------------------------------------------------------------------------------------------


int UM6LT::getAngleRates(int& rollRate, int& pitchRate, int& yawRate){
/*MAJOR UPDATE:
    - getTwoBatches(...) updated
*/
    rollRate = 0;
    pitchRate = 0;
    yawRate = 0;
    int data_1[4] = {0, 0, 0, 0};
    int data_2[4] = {0, 0, 0, 0};
    int address_XY = UM6_GYRO_PROC_XY;
    int address_Z = UM6_GYRO_PROC_Z;
    
    if(getTwoBatches(address_XY, data_1) && getTwoBatches(address_Z, data_2)){
        rollRate = data_1[0];
        pitchRate = data_1[1];
        yawRate = data_2[0];
        return 1;
    }
    else{
        return 0;
    }
    
}

//----------------------------------------------------------------------------------------------

int UM6LT::getQuaternion(int& a, int& b, int& c, int& d){
/*MAJOR UPDATE:
    - getTwoBatches(...) updated
*/
    a = 0;
    b = 0;
    c = 0;
    d = 0;
    int data_1[4] = {0, 0, 0, 0};
    int data_2[4] = {0, 0, 0, 0};
    int address_AB = UM6_QUAT_AB;
    int address_CD = UM6_QUAT_CD;
    
    if(getTwoBatches(address_AB, data_1) && getTwoBatches(address_CD, data_2)){
        a = data_1[0];
        b = data_1[1];
        c = data_2[0];
        d = data_2[1];
        return 1;
    }
    else{
        return 0;
    }
    
}

//----------------------------------------------------------------------------------------------