File content as of revision 2:61afd3fd7d6e:

#include "mbed.h"
#include "rtos.h"
#include "header.h"
#include <time.h>
#include "SDFileSystem.h"
#include "MPU6050.h"

//SDFileSystem sd(PA_7, PA_6, PA_5, PB_6, "sd");
SDFileSystem sd(SPI_MOSI, SPI_MISO, SPI_SCK, SPI_CS, "sd");
Serial pc(USBTX,USBRX,9600);

Mutex mutex_ac;
Mutex mutex_cs;

/**********Funzioni***********/
int ok_produzione_ac = 1;
int ok_consumo_ac = 0;
int ok_produzione_cs = 1;
int ok_consumo_cs = 0;

/*********Inizia Thread Acquisizione********/
void funzione_acquisisci(){
    float sum = 0;
    uint32_t sumCount = 0;

    MPU6050 mpu6050;
    
    Timer t;      

   t.start();        
 
  // Read the WHO_AM_I register, this is a good test of communication
  uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
  //pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
  
  if (whoami == 0x68) // WHO_AM_I should always be 0x68
    {  
    //pc.printf("MPU6050 is online...");
    //wait(1);
     
        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
    
        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f){
            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
            mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
        }
        else
            pc.printf("Device did not the pass self-test!\n\r");
    }
    else{
    pc.printf("Could not connect to MPU6050: \n\r");
    pc.printf("%#x \n",  whoami);
 
    while(1) ; // Loop forever if communication doesn't happen
    }
    
    srand(time(NULL));
    int i;
    static const int off_set_a=400;
    time_t rawtime;
    struct tm* timeinfo;
    Timer temp3;
    temp3.start();
    for(i = 0; i < BLOCCO; i++){
       
        // If data ready bit set, all data registers have new data
        if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
            mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
            mpu6050.getAres();
    
            // Now we'll calculate the accleration value into actual g's
            ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
            ay = (float)accelCount[1]*aRes - accelBias[1];   
            az = (float)accelCount[2]*aRes - accelBias[2];  
            
            time ( &rawtime );
            timeinfo = localtime ( &rawtime );
            vettore[i].x = ax;
            vettore[i].y = ay;
            vettore[i].z = az;
            vettore[i].t = asctime(timeinfo);
   
            mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
            mpu6050.getGres();
 
            // Calculate the gyro value into actual degrees per second
            gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
            gy = (float)gyroCount[1]*gRes; // - gyroBias[1];  
            gz = (float)gyroCount[2]*gRes; // - gyroBias[2];   

            tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
            temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
        }  
   
        Now = t.read_us();
        deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
        lastUpdate = Now;
    
        sum += deltat;
        sumCount++;
    
        if(lastUpdate - firstUpdate > 10000000.0f) {
            beta = 0.04;  // decrease filter gain after stabilized
            zeta = 0.015; // increasey bias drift gain after stabilized
        }
    
        // Pass gyro rate as rad/s
        mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);

        // Serial print and/or display at 0.5 s rate independent of data rates
        delt_t = t.read_ms() - count;
        if (delt_t > 100) { // update LCD once per half-second independent of read rate
            yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
            pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
            roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
            pitch *= 180.0f / PI;
            yaw   *= 180.0f / PI; 
            roll  *= 180.0f / PI;
                                         
            myled= !myled;
            count = t.read_ms(); 
            sum = 0;
            sumCount = 0; 
        }
         pc.printf("A%03.0f%03.0f%03.0f",100*vettore[i].x+off_set_a,100*vettore[i].y+off_set_a,100*vettore[i].z+off_set_a);
         //pc.printf("X: %f | Y: %f | Z: %f | Time: %s\n\r",vettore[i].x,vettore[i].y,vettore[i].z,vettore[i].t);   
    } 
    temp3.stop();
    pc.printf("Tempo impiegato per l'acquisizione di 10 elementi: %f\n\r",temp3.read());   
   
}

void acquisisci(){
    while (true) {
        
        mutex_ac.lock();
        while(ok_produzione_ac == 0){
            mutex_ac.unlock();
            //Thread::wait(100);
            mutex_ac.lock();        
        }
                        
        funzione_acquisisci();
        
        ok_produzione_ac = 0;
        ok_consumo_ac = 1;
        
        mutex_ac.unlock();
    }
}
/*******Fine Thread Acquisizione********/



/**********Inizio thread copia*******/
void funzione_copia(){
      int i;
      Timer temp2;
      temp2.start();
      for(i = 0; i < BLOCCO; i++){
        
            appoggio[i].x = vettore[i].x;
            appoggio[i].y = vettore[i].y;
            appoggio[i].z = vettore[i].z; 
            appoggio[i].t = vettore[i].t; 
       
      }
      temp2.stop();
      pc.printf("Tempo impiegato per la copia di 10 elementi: %f\n\r",temp2.read()); 
}

void copia()
{
    while (true) {
        
        mutex_ac.lock();
        while(ok_consumo_ac==0){
            mutex_ac.unlock();
            //Thread::wait(100);
            mutex_ac.lock();    
        }
        
        mutex_cs.lock();
        while(ok_produzione_cs==0){
            mutex_cs.unlock();
            //Thread::wait(500);
            mutex_cs.lock();    
        }
        
        funzione_copia();
        
        ok_produzione_cs = 0;
        ok_consumo_cs = 1;
        mutex_cs.unlock();
        
        ok_produzione_ac = 1;
        ok_consumo_ac = 0;
        mutex_ac.unlock();
    }
}
/**********Fine thread copia*********/



/********Inizio thread salvataggio***********/
void funzione_salva(){
    int i;
    static const int off_set_a=400;
    Timer temp1;
          
    mkdir("/sd/mydir",0777); 
        
    FILE *fp = fopen("/sd/mydir/sdtest.txt", "a");
    if(fp == NULL) 
        pc.printf("Could not open file for write\n\r");
    else{
        temp1.start();        
        for(i = 0; i < BLOCCO; i++){
            
                //fprintf(fp,"X: %3.3f | Y: %3.3f | Z: %3.3f | Time: %s\r\n",appoggio[i].x,appoggio[i].y,appoggio[i].z,appoggio[i].t);                   
                  fprintf(fp,"\n\rA%03.0f%03.0f%03.0f\n\r",100*vettore[i].x+off_set_a,100*vettore[i].y+off_set_a,100*vettore[i].z+off_set_a); 
        }
        temp1.stop();
     pc.printf("Tempo impiegato per la scrittura di 10 elementi su file: %f\n\r",temp1.read());
        
        fclose(fp);
    }
}

void salva()
{
    while (true) {
        
        mutex_cs.lock();
        while(ok_consumo_cs==0){
            mutex_cs.unlock();
            //Thread::wait(100);
            mutex_cs.lock();    
        }
        
        funzione_salva();
        
        ok_produzione_cs = 1;
        ok_consumo_cs = 0;
        mutex_cs.unlock();
    }
}
/************Fine thread salvataggio*************/

/*********Fine Funzioni**********/

    Thread Acquisizione; //(acquisisci, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
    Thread Copia; //(copia, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
    Thread Salvataggio; //(salva, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);

int main()
{
    i2c.frequency(400000);
    int i;
    for(i = 0; i < BLOCCO; i++){
        
            vettore[i].x = 0;
            vettore[i].y = 0;
            vettore[i].z = 0;
            //vettore[i][j].t = 0;
            appoggio[i].x = 0;
            appoggio[i].y = 0;
            appoggio[i].z = 0; 
            //appoggio[i][j].t = '';      
              
    }
    
   while (true) {
        Acquisizione.start(callback(acquisisci));
        Copia.start(callback(copia));
        Salvataggio.start(callback(salva));
              
    }
}