File content as of revision 11:366f1186c121:

#include "mbed.h"
#include "SPI.h"
#include "IMUs.h"
#include "Structures.h"
#include "Quaternions.h"
#include "DMA_SPI.h"

DigitalOut myled(LED1);
Serial pc(USBTX, USBRX); 

int masterRx = 0;
int16_t slaveRx = 0;
int i = 2;
int k = 0;
int16_t zgHigher = 0;
int16_t zgLower = 0;
int16_t zGyro = 0;
int countx = 0;
int p = 32776;
double const SSF = 0.06097560975609756097560975609756f;                           //FSEL = 0: 0.00763358778625954198473282442748, FSEL = 3:  0.06097560975609756097560975609756f

int OffsetX = 254;
int OffsetY = -14;
int OffsetZ = 81;

int OffsetXA = -306;
int OffsetYA = -131;
int OffsetZA = -531;

IMU IMU0 (0, -306.0f, -131.0f, -351.0f, 254.0f, -14.0f, 81.0f, 0, 0);
IMU IMU1 (0, -306.0f, -131.0f, -351.0f, 254.0f, -14.0f, 81.0f, 0, 0);
IMU_Data Dat;
vector Datt, Datt1;
int D2T;


Timer t;
float dTime = 0.0f;
    
void calibrateOffset();

int flag = 0;
int flag2 = 0;
int rx_data = 0;
int badSampleFlag = 0;
int16_t rxx;
IMUcheck infoIMU;


/*

*/
//------------------Printing-All-values----------------------//
int16_t IMUarray[12]; //Store each separate reading in an array
uint16_t IDarray[12]; //Holds the identification of each data piece
char idx = 0;         //IMUarray Pointer
char dataCount = 0;   //Keeps track of how many data points have been read in using SPI


void ProcessAndPrint();
//------------------Printing-All-values----------------------//

//-------------Testing-Variables-Remove-Later----------------//
int blinkCounter = 0;
//-------------Testing-Variables-Remove-Later----------------//


//------------------------------------------------------------------------------Function Declarations
IMUcheck checkData(int16_t dataArray[10][12]);
//------------------------------------------------------------------------------Function Declarations

int main() {
    pc.baud(115200);
    
    
    IDarray[0] = 1;
    IDarray[1] = 0;
    IDarray[2] = 9;
    IDarray[3] = 8;
    IDarray[4] = 17;
    IDarray[5] = 16;
    IDarray[6] = 3;
    IDarray[7] = 2; //2
    IDarray[8] = 11;
    IDarray[9] = 10;
    IDarray[10] = 19;
    IDarray[11] = 18;
    
    /*
    IDarray[0] = 1 + 32;
    IDarray[1] = 0 + 32;
    IDarray[2] = 9 + 32;
    IDarray[3] = 8 + 32;
    IDarray[4] = 17 + 32;
    IDarray[5] = 16 + 32;
    IDarray[6] = 3 + 32;
    IDarray[7] = 2 + 32; //2
    IDarray[8] = 11 + 32;
    IDarray[9] = 10 + 32;
    IDarray[10] = 19 + 32;
    IDarray[11] = 18 + 32;
    */
    //init_spi1();
    t.start();
    SPI_DMA_init();
    
   
    data_to_transmit[0] = 1;
    data_to_transmit[1] = 2;
    data_to_transmit[2] = 3;
    data_to_transmit[3] = 4;
    data_to_transmit[4] = 5;
    data_to_transmit[5] = 6;
    data_to_transmit[6] = 7;
    data_to_transmit[7] = 8;
    data_to_transmit[8] = 9;
    data_to_transmit[9] = 10;
    data_to_transmit[10] = 11;
    data_to_transmit[11] = 12;
    

    while(1) {

    
    
    
    if(newDataFlag == 1) {
        newDataFlag = 0; 
        infoIMU = checkData(SampleFIFO);
        if(infoIMU.errorFlag == 0) {
            for(int x = 0; x <= 11; x++) {
                IMUarray[x] = SampleFIFO[pointerFS][x];
            }
            if(infoIMU.id == 0) {
                Datt = IMU0.CalculateCFAngles(IMUarray);
                //pc.printf("IMU 0\n\r");
            }
            else {
                //pc.printf("IMU 1\n\r");
                Datt1 = IMU1.CalculateCFAngles(IMUarray);
            }
            //pc.printf("IMU 0: X = %+2f,    Y = %+2f,   Z = %+2f    IMU 1: X = %+2f,    Y = %+2f,   Z = %+2f\n\r", Datt.x, Datt.y, Datt.z, Datt1.x, Datt1.y, Datt1.z);
        }
    }//if(newDataFlag == 1)
    

    }
    
}




IMUcheck checkData(int16_t dataArray[10][12]) {
   int16_t firstSample, lastSample;                                             //Used to check first and last sample of batch
   uint16_t id = 0;                                                             //Used to store extracted data ID
   IMUcheck dataStatus;                                                         //contains IMU id and error status
   
   dataStatus.errorFlag = 0;                                                    //Initialise as 0 by default
    
    firstSample = SampleFIFO[pointerFS][0];                                     //first sample loaded here
    lastSample = SampleFIFO[pointerFS][11];                                     //last sample loaded here
    
    firstSample &= ~(8191);                                                     //remove first 13 bits
    firstSample = firstSample >> 13;                                            //shift by right by 13
    lastSample &= ~(8191);                                                      //remove first 13 bits
    lastSample = lastSample >> 13;                                             //shift by right by 13
        
    if(firstSample != lastSample) {                                             //Check if the IDs match
        dataStatus.errorFlag = 1;                                               //if both sample ID are not equal then batch is wrong
        return;                                                                  //Leave function early if error occured
    }
    else {                                                                      //otherwise if both match
        dataStatus.id = firstSample;                                            //attach the status to dataStatus id
       // printf("%d \n\r", dataStatus.id);
    }
    
    for(int x = 0; x <= 11; x++) {
        id = SampleFIFO[pointerFS][x];                                          //Save sample to id for id extraction
        id &= ~(255);                                                           //Remove the actual data to only be left with the id
        id &= ~(57344);                                                         //Remove IMU identification data
        id = id >> 8;                                                           //shift the id to the right for comparison
        
        if(id != IDarray[x]) {                                                  //if the data identification does not match
            if(blinkCounter == 200) {
                myled = !myled;                                                 //Toggle LED to signal that error occured
                blinkCounter = 0;
            }
            else {
                blinkCounter++;
            }//if(blinkCounter == 200) 
            
            dataStatus.errorFlag = 1;                                           //Raise errorFlag
            break;                                                              //break out of the for loop
        }//if(id != IDarray[x])
    }//for(int x = 0; x <= 11; x++)      
    
    return dataStatus;                                       
    
}//IMUcheck checkData(int16_t dataArray[10][12])


//---------------------------------------------------------------------------OFFSET-CALIBRATION--------------------------------------------------------------------
void calibrateOffset()  {
        
    int16_t MSB = 0;                                    //Store Most Significant Byte of data piece in this variable for processing
    int16_t LSB = 0;                                    //Store Least Significant Byte of data piece in this variable for processing
    char    arrPointer = 0;                                //Array Pointer
    
//-----------Concatentated-Data-Pieces------------------------------------------
    int16_t gyroX = 0;
    int16_t gyroY = 0;
    int16_t gyroZ = 0;
    
    int16_t accelX = 0;
    int16_t accelY = 0;
    int16_t accelZ = 0;
    
    vector accelRaw;
    vector accelG;
    vector gyroRaw;
    vector gyroDPS;
    
    static unsigned int sampleCounter = 1;
    static vector accelRawAvg;
    static vector accelGAvg;
    static vector gyroRawAvg;
    static vector gyroDPSAvg;
        
        
        for(char x = 0; x <= 5; x++) {
        MSB = IMUarray[arrPointer];                                             //Odd data pieces are MSBs
        MSB &= ~(255<<8);                                                       //Mask the MSB bits as they are not part of data
        MSB = MSB << 8;                                                         //Shift the Value as its the MSB of the data piece
        arrPointer++;                                                           //Increment array pointer
        LSB = IMUarray[arrPointer];                                             //Even data pieces are LSBs
        LSB &= ~(255 << 8);                                                     //Mask the MSB bits as they are not part of data
        arrPointer++;                                                           //Increment array pointer
        
        switch(x) {
            case 0:
                accelX = MSB + LSB;                                             //Combine Accelerometer x-axis data
                accelRaw.x = (double)accelX;                                    //accelRaw
                accelG.x = (double)accelX * 0.00006103515625f;                  //accelSSF
                
            break;
            case 1:
                accelY = MSB + LSB;                                             //Combine Accelerometer y-axis data
                accelRaw.y = (double)accelY;
                accelG.y = (double)accelY * 0.00006103515625f;
            break;
            case 2:
                accelZ = MSB + LSB;                                             //Combine Accelerometer z-axis data
                accelRaw.z = (double)accelZ;
                accelG.z = (double)accelZ * 0.00006103515625f;
            break;
            case 3:
                gyroX = MSB + LSB;                                              //Combine Gyroscope x-axis data
                gyroRaw.x = (double)gyroX;                                      //gyroRaw
                gyroDPS.x = (double)gyroX * SSF;                                //gyroSSF
                
            break;
            case 4:
                gyroY = MSB + LSB;                                              //Combine Gyroscope y-axis data
                gyroRaw.y = (double)gyroY;
                gyroDPS.y = (double)gyroY * SSF;

            break;
            case 5:
                gyroZ = MSB + LSB;                                              //Combine Gyroscope z-axis data
                gyroRaw.z = (double)gyroZ;
                gyroDPS.z = (double)gyroZ * SSF;
            break;
            default:
            break; 
        }//switch(x) 
    }//for(char x = 0; x <= 5; x++)
    
//Take-Running-Averages------------------------------------------------------------------------
    //Raw accel averages     
    accelRawAvg.x = accelRawAvg.x + ((accelRaw.x - accelRawAvg.x)/sampleCounter);
    accelRawAvg.y = accelRawAvg.y + ((accelRaw.y - accelRawAvg.y)/sampleCounter);
    accelRawAvg.z = accelRawAvg.z + ((accelRaw.z - accelRawAvg.z)/sampleCounter);
    
    //SSF accel averages
    accelGAvg.x = accelGAvg.x + ((accelG.x - accelGAvg.x)/sampleCounter);
    accelGAvg.y = accelGAvg.y + ((accelG.y - accelGAvg.y)/sampleCounter);
    accelGAvg.z = accelGAvg.z + ((accelG.z - accelGAvg.z)/sampleCounter);
    
    //Raw gyroo averages
    gyroRawAvg.x = gyroRawAvg.x + ((gyroRaw.x - gyroRawAvg.x)/sampleCounter);
    gyroRawAvg.y = gyroRawAvg.y + ((gyroRaw.y - gyroRawAvg.y)/sampleCounter);
    gyroRawAvg.z = gyroRawAvg.z + ((gyroRaw.z - gyroRawAvg.z)/sampleCounter);
    
    //SSF gyro averages
    gyroDPSAvg.x = gyroDPSAvg.x + ((gyroDPS.x - gyroDPSAvg.x)/sampleCounter);
    gyroDPSAvg.y = gyroDPSAvg.y + ((gyroDPS.y - gyroDPSAvg.y)/sampleCounter);
    gyroDPSAvg.z = gyroDPSAvg.z + ((gyroDPS.z - gyroDPSAvg.z)/sampleCounter);
//Take-Running-Averages------------------------------------------------------------------------     

//Print Messages-------------------------------------------------------------------------------    
    if(sampleCounter == 1) {
    pc.printf("Collecting Raw and Sensitivity Scale Factor multiplied Gyroscope and Accelerometer Offsets...\n\r");     
    };
       
    
    if(sampleCounter == 5000) {
        pc.printf("RawAX              RawAY              RawAZ              RawGX              RawGY              RawGZ              SampleN\n\r");
        pc.printf("%+-6.2f            %+-6.2f            %+-6.2f            %+-6.2f            %+-6.2f            %+-6.2f            %-10d\n\r", accelRawAvg.x, accelRawAvg.y, accelRawAvg.z, gyroRawAvg.x, gyroRawAvg.y, gyroRawAvg.z, sampleCounter);     
        pc.printf("\n\r");
        pc.printf("\n\r");
        pc.printf("\n\r");
        
        //Add the sensitivity scale factor multiplied data
        pc.printf("SSFAX              SSFAY              SSFAZ              SSFGX              SSFGY              SSFGZ              SampleN\n\r");
        pc.printf("%+-6.2f            %+-6.2f            %+-6.2f            %+-6.2f            %+-6.2f            %+-6.2f            %-10d\n\r", accelGAvg.x, accelGAvg.y, accelGAvg.z, gyroDPSAvg.x, gyroDPSAvg.y, gyroDPSAvg.z, sampleCounter);
    
    };
    sampleCounter++;
//Print Messages-------------------------------------------------------------------------------     
}    
//---------------------------------------------------------------------------OFFSET-CALIBRATION--------------------------------------------------------------------







//------------------------------------------Artifacts----------------------------------------

    //----------------------Insert Whole---------------------------------------------------
    //Rotate the accel data Global reference frame by qvq*---------------------------------
        //globalAccel = rotateGlobal(AccelVals, gyroQ);
    //Rotate the accel data Global reference frame by qvq*--------------------------------- 
    
    //get the correction values and rotate back to IMU reference---------------------------     
       // correctionGlobalAccel = crossProduct(globalAccel, vertical);
       // correctionBodyAccel = rotateLocal(correctionGlobalAccel, gyroQ);
    //get the correction values and rotate back to IMU reference---------------------------   
    
    //Add the correction to gyro readings and update the quaternion------------------------ 
        //GyroVals = sumVector(GyroVals, correctionBodyAccel); 
        //incRot = toQuaternionNotation123(GyroVals, dTime); 
        //gyroQ =  getQuaternionProduct(gyroQ, incRot); 
        //gyroQ = normaliseQuaternion(gyroQ);
    //----------------------Insert Whole---------------------------------------------------
    
//------------------------------------------Artifacts----------------------------------------