Diff: main.cpp

Revision:

0:095b19b8fb7e

Child:

1:d1002dc109b9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 07 23:30:19 2019 +0000
@@ -0,0 +1,257 @@
+///////////////// 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"
+#include "ble/Gap.h"
+#include "ble/services/UARTService.h"
+#include<string>
+#include "nrf51_rtc.h"
+#include "MPL3115A2.h"  // Barometer
+#include "MPU9250.h" //IMU
+
+
+#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];
+
+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
+int frame=0; //store the frames
+int BaudRate=115200; //OpenLog's BaudRate
+float TimeInterrupt=20000; //Interruption time in [us]   1/20000[us] =50[Hz]
+float t1,t2; //time values
+double aa, ta; //altimeter and temperature values
+double ti; // temperature values - IMU
+bool flagNewCapture = false; // Realiza nova captura somente quando os dados
+                             //  anteriores já foram gravados
+bool flagPrint = true;
+
+//Teste DTW
+float AcceABS; //Valor absoluto aceleração
+float AcceABSF; //Valor absoluto aceleração filtrado
+float alfa=0.8;
+
+/////////////////Day and Time Configuration//////////////////////////////
+// TODO : Update through BLE
+char paciente[20]="TesteDTW";
+char DD[3],MM[3],AA[3],hh[3],mm[3],ss[3];
+/*
+int Day=02;
+int Month=06;
+int Year=2018;
+int Hour=10;
+int Minutes=24;
+int Seconds=00;
+*/
+//////////////////////////////////////////////////////////////////////////
+
+////////////////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
+// Estao nesta parte poque dependem de delcarações anteriores
+#include "SubDTW.h" // DTW
+#include "time_config.h" //Time configuration and util
+#include "file_comands.h" //File comands
+#include "ble_comands.h" //ble
+
+
+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   
+        
+        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
+         
+        ////////////////////////////Getting Altimeter Values/////////////////////////       
+        status = -1;
+        status = Sensor1.Read_Altitude_Data();        
+
+        flagNewCapture = true;
+    }
+        
+} //end get_values() 
+
+/////////////////////Main Funcition////////////////////////
+
+int main(void)
+{
+    led=OFF; // led azul - que indica quando esta gravando no OpenLog
+    
+    Open.set_flow_control(Serial::Disabled);  // Without that the serial communication won't work
+    Open.baud(BaudRate);  //Starting the serial comunication
+    
+    getting_pacient_data();// Get the pacient name, date and time
+    
+    // TODO: modificar para que o ultimo valor de hora esteja gravado ou no SD ou EEPROM,
+    //          verificar se eh possivel manter o RTC ligado, mesmo com o micro desligado.
+    //          ou então, criar um app que realize a atualização sempre que o dispositivo
+    //          seja reiniciado. 
+    time_init(); //initialize the RTC timer
+    
+    // FIXME: atualmente sempre que reinicia o dispositivo ele sobrescreve o arquivo
+    //      pre-existente, uma vez que a DataStamp setado eh igual             
+    new_file();  //create a new file
+    
+    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
+
+    wait(1); // 1 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  
+    
+    led=ON; // indica q a gravacao (openlog) esta apta a ser inciada
+    wait_ms(1);
+    while (true) 
+    {   
+        ble.waitForEvent(); 
+        if(strcmp(data_ble,"Stop")== 0||strcmp(data_ble,"Stop ")== 0) //strcmp return 0 whhen true
+        {
+             led=OFF;
+             char alc[10];
+             sprintf(alc,"%d",steps);
+             uart->writeString("Steps: ");
+             uart->writeString(alc);
+             uart->writeString("\n");
+             return 0;
+        }        
+        if (flagNewCapture)
+        {            
+            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;  
+            
+            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;   
+             
+            // 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];  
+
+            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();  
+            
+            //////////////////////////// Is there a new step? //////////////////
+            SubDTW();
+            
+            if (flagStep==true)
+            {
+                if (flagPrint==true)
+                {
+            ////////////////////////////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);
+                    sprintf(buff_Values,"Steps %d \r\n",steps);
+                    strcat(buff,buff_Values);
+            
+            ///////////////////////////////Cheking if the day changed/////////////////////////////////////
+                     ask_day(day2);
+                    if (strcmp (day2,day1) != 0) //compare todays day with the day that the file was created
+                    {   
+                        new_file(); //cria novo arquivo
+                        ask_day(day1);
+                    } // end if     
+                    //////////////////////////////////////////////////////////////////////////////////////////////     
+        
+                    Open.printf(buff);      //Printing values on the SD card       
+                    flagNewCapture = false; 
+                    t2=t.read_us();
+                    if ((t2 - t1) < TimeInterrupt)
+                    {
+                        wait_us((TimeInterrupt) - (t2-t1)); 
+                        led=OFF;
+                    }//end if time
+                    led=ON;
+                    flagPrint=false;
+                } //end if flag print        
+            } //end if flag step
+            else
+            {
+                flagPrint=true;
+            }
+        } // end if flag new capture        
+    } // end while true
+} //end main