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
Diff: main.cpp
- Revision:
- 3:db3f7831bcea
- Parent:
- 2:5d0d7997e461
- Child:
- 4:215c9d6d1c80
diff -r 5d0d7997e461 -r db3f7831bcea main.cpp --- 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