File content as of revision 15:791f35b0f220:

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

DigitalOut myled(LED1);
Serial pc(USBTX, USBRX); 
InterruptIn dataRequest(PA_0);

//---------------------------IMU-Objects-Initialisation-----------------------------------------//
/* 
    --Initialisation Parameters
    ***IMU ID number assignement
    ***Accelerometer x-axis offset cancellation value
    ***Accelerometer y-axis offset cancellation value
    ***Accelerometer z-axis offset cancellation value
    ***Gyroscope x-axis offset cancellation value
    ***Gyroscope y-axis offset cancellation value
    ***Gyroscope z-axis offset cancellation value
    ***Accelerometer Sensitivity Scale Factor selection number 0 - 3
    ***Gyroscope Sensitivity Scale Factor selection number 0 - 3
*/
IMU IMU0 (0, 2354.0f, 3128.0f, 1674.0f, -737.0f, -609.0f, -135.0f, 0, 0);
IMU IMU1 (0, 7738.0f, -6734.0f, 2333.0f, -1364.0f, 234.0f, 49.0f, 0, 0);
IMU IMU2 (0, -107.0f, -494.0f, -631.0f, -19.0f, -137.0f, -71.0f, 0, 0);
//---------------------------IMU-Objects-Initialisation-----------------------------------------//

//These typedefs contain x,y,z orientation data
vector IMU0_Data, IMU1_Data, IMU2_Data;


//Sensitivity scale factor used to get IMU offset values
float SSF = 0.00006103515625f; 

//Timer Object used to get sampling period for gyro integration
Timer t;
float dTime = 0.0f;
int D2T;

IMUcheck infoIMU;

//Points to the IMU whose turn it is to send data for wireless transmission.   
char IMU_Data_Pointer = 0;                                                     

//------------------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

//------------------Printing-All-values----------------------//




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

//Function used to receive request from simulation
void requestISR(void) {
    dataLoadedFlag = 0;                                                         //New request has been made but newest data is not loaded yet
    dataRequestFlag = 1;                                                        //Let L432 know that data has been requested
    myled = !myled;                                                             //Flash LED to indicate
    
    if(IMU_Data_Pointer == 2) {                                                 //Was previous data sent from IMU2?
        IMU_Data_Pointer = 0;                                                   //Yes - reset back to IMU0
    }
    else {                                                                      
        IMU_Data_Pointer++;                                                     //No - point to the next IMU data to be sent.
    }
}


//Used to split data into SPI managable chunks
void prepareSPItx(int imuID, vector IMUn) {
    txSPIUnion data;                                                            //Use a union data type to split float into 2 16-bit values
    
    for(int x = 0; x <= 11; x++) {                                              //Use the for loop to load data into correct places in the array

        switch(x) {
           case 0:
                IMU_Data_Array[x] = imuID;                                                              
           break;
           
           case 1:
                data.f = IMUn.x;                                                //Place x-axis oreintation data into the union float
                IMU_Data_Array[x] = data.n[0];                                  //save MSB 16 bits to array
           break;
           
           case 2:
                data.f = IMUn.x;                                                //Place x-axis oreintation data into the union float
                IMU_Data_Array[x] = data.n[1];                                  //save LSB 16 bits to array
           break;
           
           case 3:
                data.f = IMUn.y;                                                //Place y-axis oreintation data into the union float
                IMU_Data_Array[x] = data.n[0];                                  //save MSB 16 bits to array
           break;
           
           case 4:
                data.f = IMUn.y;                                                //Place y-axis oreintation data into the union float
                IMU_Data_Array[x] = data.n[1];                                  //save LSB 16 bits to array
           break;
           
           case 5:
                data.f = IMUn.z;                                                //Place z-axis oreintation data into the union float
                IMU_Data_Array[x] = data.n[0];                                  //save MSB 16 bits to array
           break;
           
           case 6:
                data.f = IMUn.z;                                                //Place z-axis oreintation data into the union float
                IMU_Data_Array[x] = data.n[1];                                  //save LSB 16 bits to array
           break;
           
           default:
                IMU_Data_Array[x] = 0;
           break;         
        }//switch(x)  
             
    }//for(int x = 0; x <= 11; x++) {
}//void prepareSPItx

int main() {
    
    IMU_Data_Pointer = 2;                                                       //Intially 2 but will be reset to zero when first request signal comes in.
    
    //attach rising edge signal interrupt to pin
    dataRequest.rise(&requestISR);
    
    //set baud rate
    pc.baud(115200);

    //Initial arbitary values
    IMU0_Data.x = 51;
    IMU0_Data.y = 51;
    IMU0_Data.z = 51;
    
    IMU1_Data.x = 52;
    IMU1_Data.y = 52;
    IMU1_Data.z = 52;
    
    IMU2_Data.x = 53; 
    IMU2_Data.y = 53;
    IMU2_Data.z = 53;

    //Used to identify each incoming data piece.
    //Also checks whether order is correct and no corruption has occured.
    IDarray[0] = 1;
    IDarray[1] = 0;
    IDarray[2] = 9;
    IDarray[3] = 8;
    IDarray[4] = 17;
    IDarray[5] = 16;
    IDarray[6] = 3;
    IDarray[7] = 2; 
    IDarray[8] = 11;
    IDarray[9] = 10;
    IDarray[10] = 19;
    IDarray[11] = 18;
    
    //Timer used to time the sampling period for gyro integration
    t.start();
    
    //Initialise Direct Memory Access Serial Peripheral Interface
    SPI_DMA_init();
    
   while(1) {
    if(newDataFlag == 1) {                                                      //New data arrived?
        newDataFlag = 0;                                                        //Yes - Reset the flag and continue checking data and saving
        infoIMU = checkData(SampleFIFO);                                        //Check if data sent correctly
        if(infoIMU.errorFlag == 0) {                                            //Has error been detected?
            for(int x = 0; x <= 11; x++) {                                      //No - Save into array for processing
                IMUarray[x] = SampleFIFO[pointerFS][x];
            }
            switch(infoIMU.id) {                                                //Depending on ID of data received, process data and save to correct variable
                case 0:
                    IMU0_Data = IMU0.CalculateCFAngles(IMUarray);
                break;
                
                case 1:
                    IMU1_Data = IMU1.CalculateCFAngles(IMUarray);
                break;
                
                case 2:
                    IMU2_Data = IMU2.CalculateCFAngles(IMUarray);
                break;
                
                default:
                break;
            }
            //pc.printf("IMU 0: X = %+2f,    Y = %+2f,   Z = %+2f    IMU 1: X = %+2f,    Y = %+2f,   Z = %+2f\n\r", IMU0_Data.x, IMU0_Data.y, IMU0_Data.z, IMU1_Data.x, IMU1_Data.y, IMU1_Data.z);
            //pc.printf("IMU 0: X = %+2f,    Y = %+2f,   Z = %+2f\n\r", IMU1_Data.x, IMU1_Data.y, IMU1_Data.z);
        }
    }//if(newDataFlag == 1)
    
    //Load appropriate data into array for transmission
    if(dataLoadedFlag == 0) {                                                   //if data hasnt been loaded when request occured then load data
        switch(IMU_Data_Pointer) {
            case 0:
                prepareSPItx(1, IMU0_Data);   //IMU0 Data  
            break;
            
            case 1:
                prepareSPItx(2, IMU1_Data);   //IMU1 Data 
            break;
            
            case 2:
                prepareSPItx(3, IMU2_Data);   //IMU2 Data 
            break;
            
            default:
            break;
        }//switch(IMU_Data_Pointer)  
        dataLoadedFlag = 1;                                                     //Signal that new data has been loaded for transmission.
    }

    }//while(1) 
}//int main() 




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
    
    //Get ID number
    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
    }
    
    //Check each data piece
    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 
            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--------------------------------------------------------------------