Diff: main.cpp

Revision:

3:db3f7831bcea

Parent:

2:5d0d7997e461

Child:

4:215c9d6d1c80

--- a/main.cpp	Wed Apr 03 22:52:28 2019 +0000
+++ b/main.cpp	Wed Jul 17 13:17:40 2019 +0000
@@ -11,21 +11,18 @@
 #include "ble/services/UARTService.h"
 #include<string>
 #include "nrf51_rtc.h"
-#include "MPL3115A2.h"  // Barometer
-#include "MPU9250.h" //IMU
+#include "MPU9250.h" // IMU
+#include "BMP180.h" // Barometer
 
 #define ON 0
 #define OFF 1
 
 UARTService *uart;
 BLE &ble = BLE::Instance();
-MPU9250 mpu9250;
 Timer t;
 Ticker interrupt;
 DigitalOut led(p7); // Azul
 
-const int SlaveAddressI2C = 0xC0;  //This is the slave address of the device
-                                   // para o IMU
 int status=-1;  // Status da IMU
 char I2C_Read[8];
 
@@ -34,30 +31,20 @@
 char day1[10],day2[10]; //Store the day, to check if the day changed
 int frame=0; //store the frames
 int BaudRate=115200; //OpenLog's BaudRate
-float TimeInterrupt=20000; //Interruption time in [us]   1/20000[us] =50[Hz] ESTÁ EM 50HZ
+float Freq=60;
+float TimeInterrupt=1000000/Freq; //Interruption time in [us]   1/50000[us] =20[Hz]
 float t1,t2; //time values
 double aa, ta; //altimeter and temperature values
 double ti; // temperature values - IMU
+int pressure;
 bool flagNewCapture = false; // Realiza nova captura somente quando os dados
                              //  anteriores já foram gravados
-bool flagPrint = true;
 
 //Teste DTW
 float AcceABS=0; //Valor absoluto aceleração
 float AcceABSF=0; //Valor absoluto aceleração filtrado
 float alfa=0.8;
 
-//variaveis DTW global
-int TBS=0;
-int steps=0;
-float Calibration=33;
-bool flagStep=false;
-int iiDTW=0;
-int kkDTW=0;
-#define StepSize 73
-float Signal[StepSize+1];
-
-
 /////////////////Day and Time Configuration//////////////////////////////
 // TODO : Update through BLE
 char paciente[20]="TesteDTW";
@@ -74,40 +61,117 @@
 
 ////////////////Configurating peripherals/////////////////////////////
 Serial Open(p27,p26);   //tx,rx,baudrate // OpenLog
-MPL3115A2 Sensor1(SlaveAddressI2C, p13, p12, p14, p15); //SDA SCL INT1 INT2 //  
-                                                        //MPL SCL->12 SDA->13 INT1->14 INT2->15
-                                                        // barometro
-//////////// Change MPU conections in myMPU9250.h/////////////// p17, p16
+I2C i2c(p17, p16);    // SDA, SCL
+MPU9250 MPU9250(i2c);
+// BMP180 pressure and temperature sensor access class
+BMP180 bmp180(&i2c);
 // Estao nesta parte poque dependem de delcarações anteriores
-#include "SubDTW.h" // DTW
+#include "SubFastDTW.h" // DTW
 #include "time_config.h" //Time configuration and util
 #include "file_comands.h" //File comands
 #include "ble_comands.h" //ble
 
+void SensorStart()
+{
+    
+    Timer t;
+
+    char buffer[14];
+
+    uint8_t whoami;
+    //___ Set up I2C: use fast (400 kHz) I2C ___
+    i2c.frequency(400000);  
+  
+    wait(10); // to allow terminal to cooect on PC
+  
+      while(1) 
+      {
+            if (bmp180.init() != 0) 
+            {
+              Open.printf("Error communicating with BMP180r\n");
+            } 
+            else 
+            {
+              //Open.printf("Initialized BMP180r\n");
+              break;
+            }
+            wait(1);
+      }
+  
+  //Open.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
+  
+  t.start(); // Timer ON
+  
+  // Read the WHO_AM_I register, this is a good test of communication
+  whoami = MPU9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
+  
+  //Open.printf("I AM 0x%x\r\n", whoami); Open.printf("I SHOULD BE 0x71\r\n");
+  if (I2Cstate != 0) // error on I2C
+    Open.printf("I2C failure while reading WHO_AM_I register");
+  
+  if (whoami == 0x71) // WHO_AM_I should always be 0x71
+  {  
+    //Open.printf("MPU9250 WHO_AM_I is 0x%x\r\n", whoami);
+    //Open.printf("MPU9250 is online...\r\n");
+    sprintf(buffer, "0x%x", whoami);
+    wait(0.5);
+    
+    MPU9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
+    
+    MPU9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values (accelerometer and gyroscope self test)
+    MPU9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometer, load biases in bias registers  
+
+    //Open.printf("z accel bias = %f\r\n", accelBias[2]);
+    wait(1);
+    
+    // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+    MPU9250.initMPU9250();
+    
+    // Initialize device for active mode read of magnetometer, 16 bit resolution, 100Hz.
+    MPU9250.initAK8963(magCalibration);
+    wait(0.5);
+   }
+   
+   else // Connection failure
+   {
+    Open.printf("Could not connect to MPU9250: \r\n");
+    Open.printf("%#x \n",  whoami);    
+    sprintf(buffer, "WHO_AM_I 0x%x", whoami);
+    while(1) ; // Loop forever if communication doesn't happen
+    }
+  
+    MPU9250.getAres(); // Get accelerometer sensitivity
+    MPU9250.getGres(); // Get gyro sensitivity
+    MPU9250.getMres(); // Get magnetometer sensitivity
+    magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
+    magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
+    magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
+}
 
 void get_values() //Function that get all the data from the sensors
 {       
     frame=frame+1;
     
     if (!flagNewCapture)
-    {     
-        ////////////////////////////Getting MPU Values///////////////////////// 
-        mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
+    {      
+        // Pass gyro rate as rad/s
+        MPU9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);  
+                   
+        MPU9250.readAccelData(accelCount);  // Read the x/y/z adc values             
+        MPU9250.readGyroData(gyroCount);  // Read the x/y/z adc values  
+        MPU9250.readMagData(magCount);  // Read the x/y/z adc values   
         
-        mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
-        
-        mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
+        tempCount = MPU9250.readTempData();  // Read the adc values
         
-        tempCount = mpu9250.readTempData();  // Read the adc values
-         
-        ////////////////////////////Getting Altimeter Values/////////////////////////       
-        status = -1;
-        status = Sensor1.Read_Altitude_Data();        
+    //BMP180        
+        bmp180.startTemperature();
+        wait_ms(5);     // Wait for conversion to complete
+        bmp180.startPressure(BMP180::ULTRA_LOW_POWER);
+        wait_ms(10);    // Wait for conversion to complete
+        flagNewCapture = true;  
+    } 
+} //end get_values() 
 
-        flagNewCapture = true;
-    }
-        
-} //end get_values() 
 
 /////////////////////Main Funcition////////////////////////
 
@@ -133,49 +197,9 @@
     ask_day(day1); // verificar se mudou o dia
     
     //////////////////// Starting MPU /////////////////////// 
-    
-    // Read the WHO_AM_I register, this is a good test of communication
-    uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
-    if (whoami == 0x71) // WHO_AM_I should always be 0x68
-    {  
-        wait(1);
-        mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
-        mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values 
-        mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
-        wait(2);
-        mpu9250.initMPU9250(); 
-        mpu9250.initAK8963(magCalibration);
-        wait(1);
-    } //end if
-   else
-   {
-        Open.printf("Could not connect to MPU9250: \n\r");
-        // TODO/FIXME: Verificar se eh possivel disparar um Hardware/Reset quando a
-        //      MPU não for inicializada
-        while(1) ; // Loop forever if communication doesn't happen
-    } //end else
-    
-    mpu9250.getAres(); // Get accelerometer sensitivity
-    mpu9250.getGres(); // Get gyro sensitivity
-    mpu9250.getMres(); // Get magnetometer sensitivity
-    
-    // TODO : Carregar ficheiros de calibração em funcao da temperatura
-    magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
-    magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
-    magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
-    
-        //////////////////// Starting MPL ///////////////////////
-    if(!Sensor1.Init())
-    {
-        // Open.printf("Altimeter Initialized\n\r");
-    } //end if
-    else
-    {
-        Open.printf("Altimeter Failed To Initialize\n\r");
-        while(1);
-    } //end els
+    SensorStart();  
 
-    wait(1); // 1 seg
+    wait(0.5); // 0.5 seg
     ////////////////Interrupçã0///////////////////////
     interrupt.attach_us(&get_values,TimeInterrupt); // the address of the function to be attached and the interval 
     t.start();  // inicia a interrupcao  
@@ -201,57 +225,31 @@
             t1=t.read_us();           
                         ////////////////////////////Getting MPU Values/////////////////////////  
            // Now we'll calculate the accleration value into actual g's
-            ax = (float)accelCount[0]*aRes;  // get actual g value, this depends on scale being set
-            ay = (float)accelCount[1]*aRes;   
-            az = (float)accelCount[2]*aRes;  
+            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];
             
             AcceABS=sqrt(ax*ax + ay*ay + az*az);
             AcceABSF=alfa*AcceABSF+(1-alfa)*AcceABS;
             
             // Calculate the gyro value into actual degrees per second
-            gx = (float)gyroCount[0]*gRes;  // get actual gyro value, this depends on scale being set
-            gy = (float)gyroCount[1]*gRes;  
-            gz = (float)gyroCount[2]*gRes;   
-             
+            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];
             // Calculate the magnetometer values in milliGauss
             // Include factory calibration per data sheet and user environmental corrections
-            mx = (float)magCount[0]*mRes*magCalibration[0];  // get actual magnetometer value, this depends on scale being set
-            my = (float)magCount[1]*mRes*magCalibration[1];  
-            mz = (float)magCount[2]*mRes*magCalibration[2];  
+            mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
+            my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
+            mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
 
             ti = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
-            
-            mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx, gy, gz, my, mx, mz);    
-            aa=Sensor1.Altitude_m();
-            ta=Sensor1.Temp_C();  
+            aa= 44330.0f*( 1.0f - pow((pressure/101325.0f), (1.0f/5.255f))); // Calculate altitude in meters 
             wait_us(1);
             //////////////////////////// Is there a new step? //////////////////
-            if (iiDTW>StepSize)
-            {
-                Open.printf("iiDTW>StepSize \r\n");
-                int jjDTW;
-                for (jjDTW=0;jjDTW<=(StepSize-1);jjDTW=jjDTW+1)
-               {
-                    Signal[jjDTW]=Signal[jjDTW+1];
-               }
-                Signal[StepSize]=AcceABSF;
-                kkDTW=kkDTW+1;
-                
-               if (kkDTW>=50);
-                {
-                    kkDTW=0;
-                    SubDTW();
-                }
-            }
-            else
-            {
-                Signal[iiDTW]=AcceABSF;
-                iiDTW=iiDTW+1;
-                kkDTW=kkDTW+1;
-            }  
+            SubFastDTW(AcceABSF);
 
             
-            if (flagPrint==true)
+            if (flagPrint==1)
             {
         ////////////////////////////Printing Values/////////////////////////  
                 ask_time(buff); 
@@ -267,12 +265,9 @@
                     ask_day(day1);
                 } // end if     
         //////////////////////////////////////////////////////////////////////////////////////////////     
-                if (flagStep==true)
-                {
-                    Open.printf(buff);      //Printing values on the SD card   
-                } //end if flag step
+                Open.printf(buff);      //Printing values on the SD card   
                 
-                flagNewCapture = false; 
+                
                 t2=t.read_us();
                 if ((t2 - t1) < TimeInterrupt)
                 {
@@ -280,12 +275,9 @@
                     led=OFF;
                 }//end if time
                 led=ON;
-                flagPrint=false;
+                flagPrint=0;
             } //end if flag print        
-            else
-            {
-                flagPrint=true;
-            }
+            flagNewCapture = false; 
         } // end if flag new capture        
     } // end while true
 } //end main