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-07-18
- Revision:
- 4:215c9d6d1c80
- Parent:
- 3:db3f7831bcea
- Child:
- 5:d87c25f009d1
File content as of revision 4:215c9d6d1c80:
///////////////// 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 "MPU9250.h" // IMU #include "BMP180.h" // Barometer #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 int frame=0; //store the frames int BaudRate=115200; //OpenLog's BaudRate 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 //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 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 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 "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) { // 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 tempCount = MPU9250.readTempData(); // Read the adc values //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() /////////////////////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 /////////////////////// SensorStart(); 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 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"); Open.printf("Quantidade total de passos: %d",steps); 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 - 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 - 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] - 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) { ////////////////////////////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 /* 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