Diff: main.cpp

Revision:

3:c9fbf54ed265

Parent:

2:61afd3fd7d6e

Child:

4:fa71806deb67

--- a/main.cpp	Tue Dec 05 19:37:35 2017 +0000
+++ b/main.cpp	Fri Mar 30 16:29:45 2018 +0000
@@ -1,39 +1,16 @@
 #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;
-
+int main()
+{
+    i2c.frequency(400000);
     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");
+    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
+    if (whoami == 0x68) // WHO_AM_I should always be 0x68
     {  
     //pc.printf("MPU6050 is online...");
     //wait(1);
@@ -55,226 +32,53 @@
     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 = '';      
-              
+            vettore[i].xx = 0;
+            vettore[i].yy = 0;
+            vettore[i].zz = 0;
+     
     }
     
    while (true) {
-        Acquisizione.start(callback(acquisisci));
-        Copia.start(callback(copia));
-        Salvataggio.start(callback(salva));
-              
+     // Read the WHO_AM_I register, this is a good test of communication
+    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
+                vettore[i].x = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
+                vettore[i].y = (float)accelCount[1]*aRes - accelBias[1];   
+                vettore[i].z = (float)accelCount[2]*aRes - accelBias[2];  
+            
+                mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
+                mpu6050.getGres();
+ 
+            // Calculate the gyro value into actual degrees per second
+                vettore[i].xx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
+                vettore[i].yy = (float)gyroCount[1]*gRes; // - gyroBias[1];  
+                vettore[i].zz = (float)gyroCount[2]*gRes; // - gyroBias[2];  
+                             
+            }  
+   
+            pc.printf("%03.0f %03.0f %03.0f %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,100*vettore[i].xx+off_set_a,100*vettore[i].yy+off_set_a,100*vettore[i].zz+off_set_a);
+         
+        } 
+        //temp3.stop();
+        //pc.printf("Tempo impiegato per l'acquisizione di 1000 elementi: %f\n\r",temp3.read());     
     }
 }