Algoritmo funcionando com a biblioteca de inatividade utilizando dos dados do acelerômetro e a biblioteca de PeakSearch se utilizando dos dados filtrados pelo filtro Kalman.
Dependencies: mbed MatrixMath Matrix nrf51_rtc BMP180 MPU9250
main.cpp
- Committer:
- Rogercl
- Date:
- 2019-03-07
- Revision:
- 0:095b19b8fb7e
- Child:
- 1:d1002dc109b9
File content as of revision 0:095b19b8fb7e:
///////////////// 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