Diff: main.cpp

Revision:

5:d87c25f009d1

Parent:

4:215c9d6d1c80

Child:

6:e9a2bc040ada

diff -r 215c9d6d1c80 -r d87c25f009d1 main.cpp
--- a/main.cpp	Thu Jul 18 12:06:34 2019 +0000
+++ b/main.cpp	Sat Jul 27 12:17:25 2019 +0000
@@ -1,9 +1,3 @@
-///////////////// Breakout Board//////////////////
-// OpenLog tx->27  rx->26
-//MPL SCL->12 SDA->13 INT1->14 INT2->15
-//MPU SDA->17  SCL->16
-
-
 #include "mbed.h"
 #include <events/mbed_events.h>
 #include "ble/BLE.h"
@@ -13,19 +7,20 @@
 #include "nrf51_rtc.h"
 #include "MPU9250.h" // IMU
 #include "BMP180.h" // Barometer
-
+#include "kalman.h" //Filtro Kalman
+ 
 #define ON 0
 #define OFF 1
-
+ 
 UARTService *uart;
 BLE &ble = BLE::Instance();
 Timer t;
 Ticker interrupt;
 DigitalOut led(p7); // Azul
-
+ 
 int status=-1;  // Status da IMU
 char I2C_Read[8];
-
+ 
 char buff[500]; //used to printf all the data to OpenLog (Serial)
 char buff_Values[500]; //used in the process to get de data to OpenLog (Serial)
 char day1[10],day2[10]; //Store the day, to check if the day changed
@@ -39,15 +34,10 @@
 int pressure;
 bool flagNewCapture = false; // Realiza nova captura somente quando os dados
                              //  anteriores já foram gravados
-
-//Teste DTW
-float AcceABS=0; //Valor absoluto aceleração
-float AcceABSF=0; //Valor absoluto aceleração filtrado
-float alfa=0.8;
-
+ 
 /////////////////Day and Time Configuration//////////////////////////////
 // TODO : Update through BLE
-char paciente[20]="TesteDTW";
+char paciente[10]="PeakKalma";
 char DD[3],MM[3],AA[3],hh[3],mm[3],ss[3];
 /*
 int Day=02;
@@ -58,7 +48,7 @@
 int Seconds=00;
 */
 //////////////////////////////////////////////////////////////////////////
-
+ 
 ////////////////Configurating peripherals/////////////////////////////
 Serial Open(p27,p26);   //tx,rx,baudrate // OpenLog
 I2C i2c(p17, p16);    // SDA, SCL
@@ -66,23 +56,23 @@
 // BMP180 pressure and temperature sensor access class
 BMP180 bmp180(&i2c);
 // Estao nesta parte poque dependem de delcarações anteriores
-#include "SubFastDTW.h" // DTW
+#include "findPeaks.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
+    wait(2); // to allow terminal to cooect on PC
   
       while(1) 
       {
@@ -95,7 +85,7 @@
               //Open.printf("Initialized BMP180r\n");
               break;
             }
-            wait(1);
+            wait(0.5);
       }
   
   //Open.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
@@ -120,9 +110,9 @@
     
     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);
+    wait(0.5);
     
     // Initialize device for active mode read of acclerometer, gyroscope, and temperature
     MPU9250.initMPU9250();
@@ -147,7 +137,7 @@
     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;
@@ -171,10 +161,10 @@
         flagNewCapture = true;  
     } 
 } //end get_values() 
-
-
+ 
+ 
 /////////////////////Main Funcition////////////////////////
-
+ 
 int main(void)
 {
     led=OFF; // led azul - que indica quando esta gravando no OpenLog
@@ -198,8 +188,15 @@
     
     //////////////////// Starting MPU /////////////////////// 
     SensorStart();  
-
-    wait(0.5); // 0.5 seg
+    
+    //kalman
+    wait(0.5); // 1 seg
+    float Yaw, Pitch, Roll;
+    Matrix  P(3,3);
+    P << 1  << 0  << 0
+      << 0  << 1  << 0
+      << 0  << 0  << 1;
+ 
     ////////////////Interrupçã0///////////////////////
     interrupt.attach_us(&get_values,TimeInterrupt); // the address of the function to be attached and the interval 
     t.start();  // inicia a interrupcao  
@@ -217,7 +214,7 @@
              uart->writeString("Steps: ");
              uart->writeString(alc);
              uart->writeString("\n");
-             Open.printf("Quantidade total de passos: %d",steps);
+             Open.printf("Quantidade total de passos: %d \r\n",steps);
              return 0;
         }        
         if (flagNewCapture)
@@ -229,8 +226,7 @@
             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 - gyroBias[0];  // get actual gyro value, this depends on scale being set
@@ -241,16 +237,18 @@
             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
             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? //////////////////
-            SubFastDTW(AcceABSF);
-
-            
-            if (flagPrint==1)
+            kalman(ax, ay, az, gx, gy, gz, &Yaw, &Pitch, &Roll, &P, (1/Freq));
+            int result=findPeaks(Pitch, Threshold/4, Threshold);
+            if(result>0)
             {
+                steps=steps+result;
+                result=0;
+                iiSearch=0;
         ////////////////////////////Printing Values/////////////////////////  
                 ask_time(buff); 
                 //sprintf(buff_Values,"%2.4f,%2.4f,%2.4f,%2.4f,%2.4f,%2.4f,%2.4f,%2.4f,%2.4f,%2.2f,%2.0f,%2.0f,%d\r\n",ax, ay, az, gx, gy, gz, mx, my, mz, aa, ta, ti, frame);
@@ -266,18 +264,8 @@
                 } // end if     
         //////////////////////////////////////////////////////////////////////////////////////////////     
                 Open.printf(buff);      //Printing values on the SD card   
-                /*
-                t2=t.read_us();
-                if ((t2 - t1) < TimeInterrupt)
-                {
-                    wait_us((TimeInterrupt) - (t2-t1)); 
-                    led=OFF;
-                }//end if time
-                led=ON;
-                */
-                flagPrint=0;
             } //end if flag print        
             flagNewCapture = false; 
         } // end if flag new capture        
     } // end while true
-} //end main
+} //end main
\ No newline at end of file