File content as of revision 4:e36c7042d3bb:

#include "mbed.h"
#include "SPI.h"
#include "Quaternions.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;

Timer t;
float dTime = 0.0f;

vector vertical;
vector globalAccel;
vector correctionGlobalAccel;
vector correctionBodyAccel;
Quaternion gyroQ;
vector Eangles;


float xx;
float yy;
float zz;



/*

*/
//------------------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
uint16_t id = 0;      //Used to store extracted data ID

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

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

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;
    
    init_spi1();
    t.start();
    
    gyroQ.w = 1;
    gyroQ.x = 0.0001;
    gyroQ.y = 0.0001;
    gyroQ.z = 0.0001;
    
    
    
    gyroQ = normaliseQuaternion(gyroQ);
    
    vertical.x = 0.0f;
    vertical.y = 0.0f;
    vertical.z = 1.0f;
    
    pc.printf("Begin \n\r");
    
    while(1) {
        if(i == 1) {
        for(int x = 1; x < 128; x *= 2) { 
        slaveRx = transfer_spi_slave(x);
        //pc.putc(slaveRx); 
        pc.printf("%d \n\r",slaveRx); 
        }
        for(int x = 128; x > 1; x /= 2) { 
        slaveRx = transfer_spi_slave(x);
        //pc.putc(slaveRx);
        pc.printf("%d \n\r",slaveRx);  
        } 
        }//if(i == 1) 
        
        
        
//------------------------------------------------------------------------------         
        if(i == 0) { 
        slaveRx = transfer_spi_slave(10);
        countx++;
        if(countx == 1) {
            k = slaveRx; 
            k &= ~(255 << 8);
            k = k << 8;
           // if(k == 19) {
                zgHigher = k; 
                //zgHigher = zgHigher << 8;
            //}//if(k == 19)
        }//if(count == 11)
        
        
        else if(countx == 2) {
            k = slaveRx;
            k &= ~(255 << 8); 
            //k = k << 8;
            //if(k == 18) {
                zgLower = k;
                //zgLower = zgLower << 8;
                zGyro = zgHigher + zgLower;
                pc.printf("%+d \n\r",zGyro);
            //}//if(k == 19)
                countx = 0;
        }//else if(count == 12)
        
        }//if(i == 0) 
//------------------------------------------------------------------------------ 
        
        if(i == 2) {
            slaveRx = transfer_spi_slave(10);                                   //get IMU data
            
            
            id = slaveRx;                                                       //Save sample to id for id extraction
            id &= ~(255);                                                       //get rid of the actual data to only be left with the id
            id =  id >> 8;                                                      //shift the id to the right for comparison
            
            //pc.printf("ID: %d \n\r", id);                                       //Print each id to see what sequence is obtained. Only the correct sequence will make the code run/
            
            if(IDarray[dataCount] == id) {                                      //compare if the order of data is right and if not wait until it is.
                dataCount++;                                                    //Increase dataCount as new value has been read in.
                IMUarray[idx] = slaveRx;                                        //save the newly read value to current free space in the array
                idx++;                                                          //increment the pointer to point to next free space in the array
            
                if(dataCount == 12) {
                    //reset idx and dataCount
                    dataCount = 0;
                    idx = 0;
                    ProcessAndPrint(); 
                    
                     t.stop();
                     dTime = t.read();
                     t.reset();
                     t.start();      
                }//if(dataCount == 12) 
             }//if(IDarray[dataCount] == id)
             else {                          
                                                                                //-----Code-Used-For-Testing-----//
                //pc.printf("Failed at: %d \n\r", dataCount);                   //Print an error if there is one
                if(blinkCounter == 10) {                                        //Slow the blinking down to make it visible if there are errors
                   myled = !myled;                                              //Change state of the LED if error occurs
                   blinkCounter = 0;                                            //Reset Blink counter        
                   }
                else {
                    blinkCounter++;   
                }                                                           
                                                                                //-----Code-Used-For-Testing-----//
                                                                                
                    dataCount = 0;                                              //ID sequence is worng so reset the counter
                    idx = 0;                                                    //ID sequence is worng so reset the counter
             }                                                                  
       }//if(i == 2)


       
    }
}




/*
    -The purpose of this function
*/
void ProcessAndPrint() {
    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
    
    vector GyroVals;
    vector AccelVals;
    vector accelTilt;
    Quaternion incRot;
//-----------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;
    
    float   faccelX = 0.0f;
    float   faccelY = 0.0f;
    float   faccelZ = 0.0f;
    
    float   fgyroX = 0.0f;
    float   fgyroY = 0.0f;
    float   fgyroZ = 0.0f;

//-----------Concatentated-Data-Pieces------------------------------------------
    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
                faccelX = (float)accelX * 0.00006103515625f;                    //Multiply the acceleration by sensitivity scale factor to get it into g  1/16,384
            break;
            case 1:
                accelY = MSB + LSB;                                             //Combine Accelerometer y-axis data
                faccelY = (float)accelY * 0.00006103515625f;                    //Multiply the acceleration by sensitivity scale factor to get it into g  1/16,384
            break;
            case 2:
                accelZ = MSB + LSB;                                             //Combine Accelerometer z-axis data
                faccelZ = (float)accelZ * 0.00006103515625f;                    //Multiply the acceleration by sensitivity scale factor to get it into g  1/16,384
            break;
            case 3:
                gyroX = MSB + LSB;                                              //Combine Gyroscope x-axis data
                fgyroX = (float)gyroX * 0.06097560975609756097560975609756f;    //Multiply the sample by sensitivity scale factor to get it into degress per second  1/16.4
                
            break;
            case 4:
                gyroY = MSB + LSB;                                              //Combine Gyroscope y-axis data
                fgyroY = (float)gyroY * 0.06097560975609756097560975609756f;    //Multiply the sample by sensitivity scale factor to get it into degress per second   1/16.4

            break;
            case 5:
                gyroZ = MSB + LSB;                                              //Combine Gyroscope z-axis data
                fgyroZ = (float)gyroZ * 0.06097560975609756097560975609756f;    //Multiply the sample by sensitivity scale factor to get it into degress per second   1/16.4
            break;
            default:
            break; 
        }//switch(x) 
    }//for(char x = 0; x <= 5; x++)
       
//Quaternion-Gyro-Integration--------------------------------------------------------------    
    //Get the Gyro and Accel values--------------------------------------------------------    
        //Convert these vales to radians per second and store them in the vector
        GyroVals.x = fgyroX * 0.0174533;
        GyroVals.y = fgyroY * 0.0174533;
        GyroVals.z =  fgyroZ * 0.0174533;
        
        AccelVals.x = faccelX;
        AccelVals.y = faccelY;
        AccelVals.z = faccelZ;
    //Get the Gyro and Accel values--------------------------------------------------------  
    
        gyroQ = updateQuaternion(gyroQ, GyroVals, dTime); 
        gyroQ = normaliseQuaternion(gyroQ);
        Eangles = eulerA(gyroQ);
//Quaternion-Gyro-Integration--------------------------------------------------------------


//Angle-Calculations-Based-on-Accalerometer------------------------------------------------
    //https://www.dfrobot.com/wiki/index.php/How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing
    
    accelTilt.x = (atan2(AccelVals.y, AccelVals.z) * 57.29577951f); 
    accelTilt.y = (atan2((-AccelVals.x), sqrt(pow(AccelVals.y, 2) + pow(AccelVals.z, 2) ))) * 57.29577951f); 
    //Canntot calculate accel tilt for Z. Gyro date in combination with magnetometer have to be used to obtain reliable z-axis tilt.
//Angle-Calculations-Based-on-Accalerometer------------------------------------------------       

    //Add the correction to gyro readings and update the quaternion------------------------     
        //pc.printf("%+6f,     %+6f,    %+6f      %+6f\n\r ", xx, yy, zz, dTime); 
        pc.printf("%+6f,     %+6f,     %+6f,    %+6f,    %+6f,    %+6f,    %+6f\n\r ", gyroQ.w, gyroQ.x, gyroQ.y, gyroQ.z, Eangles.x, Eangles.y, Eangles.z); 
        //pc.printf("%+6f,     %+6f,     %+6f\n\r ",Eangles.x, Eangles.y, Eangles.z); 
        //pc.printf("Accel X: %+6f,    Accel Y: %+6f,    Accel Z: %+6f,    Gyro X: %+6f,    Gyro Y: %+6f,    Gyro Z: %+6f\n\r", faccelX, faccelY, faccelZ, fgyroX, fgyroY, fgyroZ);
   
}//void ProcessAndPrint()













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