File content as of revision 10:5b96211275d4:

#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, 3);
IMU_Data Dat;
vector Datt;
int D2T;


Timer t;
float dTime = 0.0f;

vector vertical;
vector globalAccel;
vector correctionGlobalAccel;
vector correctionBodyAccel;
Quaternion gyroQ;
vector Eangles;
Quaternion CF;
vector intGyro;
    vector GyroVals;
    vector AccelVals;
    vector accelTilt;
    Quaternion accelQ;
    
    void calibrateOffset();


int stateRXNE = 0;
int stateTXE = 0;
int stateBSY = 0;
int DMA1_CH2_Transfer_Complete_Flag = 0;
int DMA1_CH3_Transfer_Complete_Flag = 0;
int DMA1_CH2_Transfer_Error_Flag = 0;
int DMA1_CH3_Transfer_Error_Flag = 0;
int stateDMAch2interrupt = 0;
int stateDMAch3interrupt = 0;
int flag = 0;
int flag2 = 0;
int rx_data = 0;
int16_t rxx;


/*

*/
//------------------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();
    SPI_DMA_init();
    gyroQ.w = 1;
    gyroQ.x = 0.0001;
    gyroQ.y = 0.0001;
    gyroQ.z = 0.0001;
    
    CF.w = 1;
    CF.x = 0.0001;
    CF.y = 0.0001;
    CF.z = 0.0001;
    
    gyroQ = normaliseQuaternion(gyroQ);
    CF = normaliseQuaternion(CF);
    
    vertical.x = 0.0f;
    vertical.y = 0.0f;
    vertical.z = 1.0f;

   
    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;
    //data_to_transmit[12] = 13;
    

    while(1) {

    

     stateRXNE = SPI1->SR&0x01;
     stateTXE = SPI1->SR&0x02;
     stateBSY = SPI1->SR&(1u<<7);
     
    DMA1_CH2_Transfer_Complete_Flag = DMA1->ISR&(1u<<5);
    DMA1_CH3_Transfer_Complete_Flag = DMA1->ISR&(1u<<9);
    DMA1_CH2_Transfer_Error_Flag = DMA1->ISR&(1u<<7);
    DMA1_CH3_Transfer_Error_Flag = DMA1->ISR&(1u<<11);
    //D2T
    
     pc.printf("RXNE: %d,       TXE: %d,        BSY: %d,        CH2 Transfer Complete: %d,      CH2 Transfer Complete: %d,       CH2 Transfer Error: %d,       CH3 Transfer Error: %d\n\r",stateRXNE, stateTXE, stateBSY, DMA1_CH2_Transfer_Complete_Flag, DMA1_CH3_Transfer_Complete_Flag, DMA1_CH2_Transfer_Error_Flag, DMA1_CH3_Transfer_Error_Flag);
     
     if(DMA1->ISR&(1u<<5)) {                                                    //Check whether data read transfer is complete
        for(int x = 0; x <= 12; x++) {
            IMUarray[x] = received_data[x];
        }
        CLEAR_DMA1_CH2_IFCR_GFLAG();                                            //Clear global channel interrupt flag for channel 2
     }
     
     if(DMA1->ISR&(1u<<9)) {                                                    //Check whteher data transmit transfer is complete  
        //Read data from the array that stores received data
        for(int x = 0; x <= 12; x++) {
            data_to_transmit[x] = x+1;
        }
        CLEAR_DMA1_CH3_IFCR_GFLAG();                                            //Clear global channel interrupt flag for channel 3
     }
     /*
     if(DMA1->ISR&(1u<<11) && flag == 0) {
        if(SPI1->SR&(1u<<7) == 0) {                                                  //Check whether SPI is busy before disabling      
            SPI1->CR1 &= ~SPI_CR1_SPE;                                              //Disable the SPI
            DMA1->IFCR |= (1u << (4*(C3S - 1)));
            DMA1_Channel3->CCR |= (0x01<<CCR_EN); 
            SPI1->CR1 |= SPI_CR1_SPE;                                               //Enable SPI
            flag = 1;
        }
     }
     
    if(DMA1->ISR&(1u<<7) && flag2 == 0) {
        DMA1->IFCR |= (15u << (4*(C2S - 1)));
        flag2 = 1;
     }
     
    if(DMA1->ISR&(1u<<9) && flag == 0) {
            DMA1->IFCR |= (1u << (4*(C3S - 1)));
        
     }
     
    if(DMA1->ISR&(1u<<5) && flag2 == 0) {
        DMA1->IFCR |= (15u << (4*(C2S - 1)));
        for(int x = 0; x <= 10; x++) {
            rxx = received_data[x];
        }
     }

*/


























//------------------------------------------------------------------------------ 
     /*   
        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;
                  //calibrateOffset(); 
                  //IMU0.concatenateData(IMUarray);
                  Datt = IMU0.CalculateQCFAngles(IMUarray);
                  
           
                     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)

    */
       
    }
}







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