Roger Lopes / Mbed 2 deprecated MARe_VT_ble_PeakSearch_Kalman_Inativ

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@4:215c9d6d1c80, 2019-07-18 (annotated)

Committer:
Rogercl
Date:
Thu Jul 18 12:06:34 2019 +0000
Revision:
4:215c9d6d1c80
Parent:
3:db3f7831bcea
Child:
5:d87c25f009d1
Working, need to check stepRef

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Rogercl 0:095b19b8fb7e 1 ///////////////// Breakout Board//////////////////
Rogercl 0:095b19b8fb7e 2 // OpenLog tx->27 rx->26
Rogercl 0:095b19b8fb7e 3 //MPL SCL->12 SDA->13 INT1->14 INT2->15
Rogercl 0:095b19b8fb7e 4 //MPU SDA->17 SCL->16
Rogercl 0:095b19b8fb7e 5
Rogercl 0:095b19b8fb7e 6
Rogercl 0:095b19b8fb7e 7 #include "mbed.h"
Rogercl 0:095b19b8fb7e 8 #include <events/mbed_events.h>
Rogercl 0:095b19b8fb7e 9 #include "ble/BLE.h"
Rogercl 0:095b19b8fb7e 10 #include "ble/Gap.h"
Rogercl 0:095b19b8fb7e 11 #include "ble/services/UARTService.h"
Rogercl 0:095b19b8fb7e 12 #include<string>
Rogercl 0:095b19b8fb7e 13 #include "nrf51_rtc.h"
Rogercl 3:db3f7831bcea 14 #include "MPU9250.h" // IMU
Rogercl 3:db3f7831bcea 15 #include "BMP180.h" // Barometer
Rogercl 0:095b19b8fb7e 16
Rogercl 0:095b19b8fb7e 17 #define ON 0
Rogercl 0:095b19b8fb7e 18 #define OFF 1
Rogercl 0:095b19b8fb7e 19
Rogercl 0:095b19b8fb7e 20 UARTService *uart;
Rogercl 0:095b19b8fb7e 21 BLE &ble = BLE::Instance();
Rogercl 0:095b19b8fb7e 22 Timer t;
Rogercl 0:095b19b8fb7e 23 Ticker interrupt;
Rogercl 0:095b19b8fb7e 24 DigitalOut led(p7); // Azul
Rogercl 0:095b19b8fb7e 25
Rogercl 0:095b19b8fb7e 26 int status=-1; // Status da IMU
Rogercl 0:095b19b8fb7e 27 char I2C_Read[8];
Rogercl 0:095b19b8fb7e 28
Rogercl 0:095b19b8fb7e 29 char buff[500]; //used to printf all the data to OpenLog (Serial)
Rogercl 0:095b19b8fb7e 30 char buff_Values[500]; //used in the process to get de data to OpenLog (Serial)
Rogercl 0:095b19b8fb7e 31 char day1[10],day2[10]; //Store the day, to check if the day changed
Rogercl 0:095b19b8fb7e 32 int frame=0; //store the frames
Rogercl 0:095b19b8fb7e 33 int BaudRate=115200; //OpenLog's BaudRate
Rogercl 3:db3f7831bcea 34 float Freq=60;
Rogercl 3:db3f7831bcea 35 float TimeInterrupt=1000000/Freq; //Interruption time in [us] 1/50000[us] =20[Hz]
Rogercl 0:095b19b8fb7e 36 float t1,t2; //time values
Rogercl 0:095b19b8fb7e 37 double aa, ta; //altimeter and temperature values
Rogercl 0:095b19b8fb7e 38 double ti; // temperature values - IMU
Rogercl 3:db3f7831bcea 39 int pressure;
Rogercl 0:095b19b8fb7e 40 bool flagNewCapture = false; // Realiza nova captura somente quando os dados
Rogercl 0:095b19b8fb7e 41 // anteriores já foram gravados
Rogercl 0:095b19b8fb7e 42
Rogercl 0:095b19b8fb7e 43 //Teste DTW
Rogercl 2:5d0d7997e461 44 float AcceABS=0; //Valor absoluto aceleração
Rogercl 2:5d0d7997e461 45 float AcceABSF=0; //Valor absoluto aceleração filtrado
Rogercl 0:095b19b8fb7e 46 float alfa=0.8;
Rogercl 0:095b19b8fb7e 47
Rogercl 0:095b19b8fb7e 48 /////////////////Day and Time Configuration//////////////////////////////
Rogercl 0:095b19b8fb7e 49 // TODO : Update through BLE
Rogercl 0:095b19b8fb7e 50 char paciente[20]="TesteDTW";
Rogercl 0:095b19b8fb7e 51 char DD[3],MM[3],AA[3],hh[3],mm[3],ss[3];
Rogercl 0:095b19b8fb7e 52 /*
Rogercl 0:095b19b8fb7e 53 int Day=02;
Rogercl 0:095b19b8fb7e 54 int Month=06;
Rogercl 0:095b19b8fb7e 55 int Year=2018;
Rogercl 0:095b19b8fb7e 56 int Hour=10;
Rogercl 0:095b19b8fb7e 57 int Minutes=24;
Rogercl 0:095b19b8fb7e 58 int Seconds=00;
Rogercl 0:095b19b8fb7e 59 */
Rogercl 0:095b19b8fb7e 60 //////////////////////////////////////////////////////////////////////////
Rogercl 0:095b19b8fb7e 61
Rogercl 0:095b19b8fb7e 62 ////////////////Configurating peripherals/////////////////////////////
Rogercl 0:095b19b8fb7e 63 Serial Open(p27,p26); //tx,rx,baudrate // OpenLog
Rogercl 3:db3f7831bcea 64 I2C i2c(p17, p16); // SDA, SCL
Rogercl 3:db3f7831bcea 65 MPU9250 MPU9250(i2c);
Rogercl 3:db3f7831bcea 66 // BMP180 pressure and temperature sensor access class
Rogercl 3:db3f7831bcea 67 BMP180 bmp180(&i2c);
Rogercl 0:095b19b8fb7e 68 // Estao nesta parte poque dependem de delcarações anteriores
Rogercl 3:db3f7831bcea 69 #include "SubFastDTW.h" // DTW
Rogercl 0:095b19b8fb7e 70 #include "time_config.h" //Time configuration and util
Rogercl 0:095b19b8fb7e 71 #include "file_comands.h" //File comands
Rogercl 0:095b19b8fb7e 72 #include "ble_comands.h" //ble
Rogercl 0:095b19b8fb7e 73
Rogercl 3:db3f7831bcea 74 void SensorStart()
Rogercl 3:db3f7831bcea 75 {
Rogercl 3:db3f7831bcea 76
Rogercl 3:db3f7831bcea 77 Timer t;
Rogercl 3:db3f7831bcea 78
Rogercl 3:db3f7831bcea 79 char buffer[14];
Rogercl 3:db3f7831bcea 80
Rogercl 3:db3f7831bcea 81 uint8_t whoami;
Rogercl 3:db3f7831bcea 82 //___ Set up I2C: use fast (400 kHz) I2C ___
Rogercl 3:db3f7831bcea 83 i2c.frequency(400000);
Rogercl 3:db3f7831bcea 84
Rogercl 3:db3f7831bcea 85 wait(10); // to allow terminal to cooect on PC
Rogercl 3:db3f7831bcea 86
Rogercl 3:db3f7831bcea 87 while(1)
Rogercl 3:db3f7831bcea 88 {
Rogercl 3:db3f7831bcea 89 if (bmp180.init() != 0)
Rogercl 3:db3f7831bcea 90 {
Rogercl 3:db3f7831bcea 91 Open.printf("Error communicating with BMP180r\n");
Rogercl 3:db3f7831bcea 92 }
Rogercl 3:db3f7831bcea 93 else
Rogercl 3:db3f7831bcea 94 {
Rogercl 3:db3f7831bcea 95 //Open.printf("Initialized BMP180r\n");
Rogercl 3:db3f7831bcea 96 break;
Rogercl 3:db3f7831bcea 97 }
Rogercl 3:db3f7831bcea 98 wait(1);
Rogercl 3:db3f7831bcea 99 }
Rogercl 3:db3f7831bcea 100
Rogercl 3:db3f7831bcea 101 //Open.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
Rogercl 3:db3f7831bcea 102
Rogercl 3:db3f7831bcea 103 t.start(); // Timer ON
Rogercl 3:db3f7831bcea 104
Rogercl 3:db3f7831bcea 105 // Read the WHO_AM_I register, this is a good test of communication
Rogercl 3:db3f7831bcea 106 whoami = MPU9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
Rogercl 3:db3f7831bcea 107
Rogercl 3:db3f7831bcea 108 //Open.printf("I AM 0x%x\r\n", whoami); Open.printf("I SHOULD BE 0x71\r\n");
Rogercl 3:db3f7831bcea 109 if (I2Cstate != 0) // error on I2C
Rogercl 3:db3f7831bcea 110 Open.printf("I2C failure while reading WHO_AM_I register");
Rogercl 3:db3f7831bcea 111
Rogercl 3:db3f7831bcea 112 if (whoami == 0x71) // WHO_AM_I should always be 0x71
Rogercl 3:db3f7831bcea 113 {
Rogercl 3:db3f7831bcea 114 //Open.printf("MPU9250 WHO_AM_I is 0x%x\r\n", whoami);
Rogercl 3:db3f7831bcea 115 //Open.printf("MPU9250 is online...\r\n");
Rogercl 3:db3f7831bcea 116 sprintf(buffer, "0x%x", whoami);
Rogercl 3:db3f7831bcea 117 wait(0.5);
Rogercl 3:db3f7831bcea 118
Rogercl 3:db3f7831bcea 119 MPU9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
Rogercl 3:db3f7831bcea 120
Rogercl 3:db3f7831bcea 121 MPU9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values (accelerometer and gyroscope self test)
Rogercl 3:db3f7831bcea 122 MPU9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometer, load biases in bias registers
Rogercl 3:db3f7831bcea 123
Rogercl 3:db3f7831bcea 124 //Open.printf("z accel bias = %f\r\n", accelBias[2]);
Rogercl 3:db3f7831bcea 125 wait(1);
Rogercl 3:db3f7831bcea 126
Rogercl 3:db3f7831bcea 127 // Initialize device for active mode read of acclerometer, gyroscope, and temperature
Rogercl 3:db3f7831bcea 128 MPU9250.initMPU9250();
Rogercl 3:db3f7831bcea 129
Rogercl 3:db3f7831bcea 130 // Initialize device for active mode read of magnetometer, 16 bit resolution, 100Hz.
Rogercl 3:db3f7831bcea 131 MPU9250.initAK8963(magCalibration);
Rogercl 3:db3f7831bcea 132 wait(0.5);
Rogercl 3:db3f7831bcea 133 }
Rogercl 3:db3f7831bcea 134
Rogercl 3:db3f7831bcea 135 else // Connection failure
Rogercl 3:db3f7831bcea 136 {
Rogercl 3:db3f7831bcea 137 Open.printf("Could not connect to MPU9250: \r\n");
Rogercl 3:db3f7831bcea 138 Open.printf("%#x \n", whoami);
Rogercl 3:db3f7831bcea 139 sprintf(buffer, "WHO_AM_I 0x%x", whoami);
Rogercl 3:db3f7831bcea 140 while(1) ; // Loop forever if communication doesn't happen
Rogercl 3:db3f7831bcea 141 }
Rogercl 3:db3f7831bcea 142
Rogercl 3:db3f7831bcea 143 MPU9250.getAres(); // Get accelerometer sensitivity
Rogercl 3:db3f7831bcea 144 MPU9250.getGres(); // Get gyro sensitivity
Rogercl 3:db3f7831bcea 145 MPU9250.getMres(); // Get magnetometer sensitivity
Rogercl 3:db3f7831bcea 146 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
Rogercl 3:db3f7831bcea 147 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
Rogercl 3:db3f7831bcea 148 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
Rogercl 3:db3f7831bcea 149 }
Rogercl 0:095b19b8fb7e 150
Rogercl 0:095b19b8fb7e 151 void get_values() //Function that get all the data from the sensors
Rogercl 0:095b19b8fb7e 152 {
Rogercl 0:095b19b8fb7e 153 frame=frame+1;
Rogercl 0:095b19b8fb7e 154
Rogercl 0:095b19b8fb7e 155 if (!flagNewCapture)
Rogercl 3:db3f7831bcea 156 {
Rogercl 3:db3f7831bcea 157 // Pass gyro rate as rad/s
Rogercl 3:db3f7831bcea 158 MPU9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
Rogercl 3:db3f7831bcea 159
Rogercl 3:db3f7831bcea 160 MPU9250.readAccelData(accelCount); // Read the x/y/z adc values
Rogercl 3:db3f7831bcea 161 MPU9250.readGyroData(gyroCount); // Read the x/y/z adc values
Rogercl 3:db3f7831bcea 162 MPU9250.readMagData(magCount); // Read the x/y/z adc values
Rogercl 0:095b19b8fb7e 163
Rogercl 3:db3f7831bcea 164 tempCount = MPU9250.readTempData(); // Read the adc values
Rogercl 0:095b19b8fb7e 165
Rogercl 3:db3f7831bcea 166 //BMP180
Rogercl 3:db3f7831bcea 167 bmp180.startTemperature();
Rogercl 4:215c9d6d1c80 168 //wait_ms(5); // Wait for conversion to complete
Rogercl 3:db3f7831bcea 169 bmp180.startPressure(BMP180::ULTRA_LOW_POWER);
Rogercl 4:215c9d6d1c80 170 //wait_ms(10); // Wait for conversion to complete
Rogercl 3:db3f7831bcea 171 flagNewCapture = true;
Rogercl 3:db3f7831bcea 172 }
Rogercl 3:db3f7831bcea 173 } //end get_values()
Rogercl 0:095b19b8fb7e 174
Rogercl 0:095b19b8fb7e 175
Rogercl 0:095b19b8fb7e 176 /////////////////////Main Funcition////////////////////////
Rogercl 0:095b19b8fb7e 177
Rogercl 0:095b19b8fb7e 178 int main(void)
Rogercl 0:095b19b8fb7e 179 {
Rogercl 0:095b19b8fb7e 180 led=OFF; // led azul - que indica quando esta gravando no OpenLog
Rogercl 0:095b19b8fb7e 181
Rogercl 0:095b19b8fb7e 182 Open.set_flow_control(Serial::Disabled); // Without that the serial communication won't work
Rogercl 0:095b19b8fb7e 183 Open.baud(BaudRate); //Starting the serial comunication
Rogercl 0:095b19b8fb7e 184
Rogercl 0:095b19b8fb7e 185 getting_pacient_data();// Get the pacient name, date and time
Rogercl 0:095b19b8fb7e 186
Rogercl 0:095b19b8fb7e 187 // TODO: modificar para que o ultimo valor de hora esteja gravado ou no SD ou EEPROM,
Rogercl 0:095b19b8fb7e 188 // verificar se eh possivel manter o RTC ligado, mesmo com o micro desligado.
Rogercl 0:095b19b8fb7e 189 // ou então, criar um app que realize a atualização sempre que o dispositivo
Rogercl 0:095b19b8fb7e 190 // seja reiniciado.
Rogercl 0:095b19b8fb7e 191 time_init(); //initialize the RTC timer
Rogercl 0:095b19b8fb7e 192
Rogercl 0:095b19b8fb7e 193 // FIXME: atualmente sempre que reinicia o dispositivo ele sobrescreve o arquivo
Rogercl 0:095b19b8fb7e 194 // pre-existente, uma vez que a DataStamp setado eh igual
Rogercl 0:095b19b8fb7e 195 new_file(); //create a new file
Rogercl 0:095b19b8fb7e 196
Rogercl 0:095b19b8fb7e 197 ask_day(day1); // verificar se mudou o dia
Rogercl 0:095b19b8fb7e 198
Rogercl 0:095b19b8fb7e 199 //////////////////// Starting MPU ///////////////////////
Rogercl 3:db3f7831bcea 200 SensorStart();
Rogercl 0:095b19b8fb7e 201
Rogercl 3:db3f7831bcea 202 wait(0.5); // 0.5 seg
Rogercl 0:095b19b8fb7e 203 ////////////////Interrupçã0///////////////////////
Rogercl 0:095b19b8fb7e 204 interrupt.attach_us(&get_values,TimeInterrupt); // the address of the function to be attached and the interval
Rogercl 0:095b19b8fb7e 205 t.start(); // inicia a interrupcao
Rogercl 0:095b19b8fb7e 206
Rogercl 0:095b19b8fb7e 207 led=ON; // indica q a gravacao (openlog) esta apta a ser inciada
Rogercl 0:095b19b8fb7e 208 wait_ms(1);
Rogercl 0:095b19b8fb7e 209 while (true)
Rogercl 0:095b19b8fb7e 210 {
Rogercl 0:095b19b8fb7e 211 ble.waitForEvent();
Rogercl 0:095b19b8fb7e 212 if(strcmp(data_ble,"Stop")== 0||strcmp(data_ble,"Stop ")== 0) //strcmp return 0 whhen true
Rogercl 0:095b19b8fb7e 213 {
Rogercl 0:095b19b8fb7e 214 led=OFF;
Rogercl 0:095b19b8fb7e 215 char alc[10];
Rogercl 0:095b19b8fb7e 216 sprintf(alc,"%d",steps);
Rogercl 0:095b19b8fb7e 217 uart->writeString("Steps: ");
Rogercl 0:095b19b8fb7e 218 uart->writeString(alc);
Rogercl 0:095b19b8fb7e 219 uart->writeString("\n");
Rogercl 2:5d0d7997e461 220 Open.printf("Quantidade total de passos: %d",steps);
Rogercl 0:095b19b8fb7e 221 return 0;
Rogercl 0:095b19b8fb7e 222 }
Rogercl 0:095b19b8fb7e 223 if (flagNewCapture)
Rogercl 0:095b19b8fb7e 224 {
Rogercl 0:095b19b8fb7e 225 t1=t.read_us();
Rogercl 0:095b19b8fb7e 226 ////////////////////////////Getting MPU Values/////////////////////////
Rogercl 0:095b19b8fb7e 227 // Now we'll calculate the accleration value into actual g's
Rogercl 3:db3f7831bcea 228 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
Rogercl 3:db3f7831bcea 229 ay = (float)accelCount[1]*aRes - accelBias[1];
Rogercl 3:db3f7831bcea 230 az = (float)accelCount[2]*aRes - accelBias[2];
Rogercl 0:095b19b8fb7e 231
Rogercl 0:095b19b8fb7e 232 AcceABS=sqrt(ax*ax + ay*ay + az*az);
Rogercl 0:095b19b8fb7e 233 AcceABSF=alfa*AcceABSF+(1-alfa)*AcceABS;
Rogercl 0:095b19b8fb7e 234
Rogercl 0:095b19b8fb7e 235 // Calculate the gyro value into actual degrees per second
Rogercl 3:db3f7831bcea 236 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
Rogercl 3:db3f7831bcea 237 gy = (float)gyroCount[1]*gRes - gyroBias[1];
Rogercl 3:db3f7831bcea 238 gz = (float)gyroCount[2]*gRes - gyroBias[2];
Rogercl 0:095b19b8fb7e 239 // Calculate the magnetometer values in milliGauss
Rogercl 0:095b19b8fb7e 240 // Include factory calibration per data sheet and user environmental corrections
Rogercl 3:db3f7831bcea 241 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
Rogercl 3:db3f7831bcea 242 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
Rogercl 3:db3f7831bcea 243 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
Rogercl 0:095b19b8fb7e 244
Rogercl 0:095b19b8fb7e 245 ti = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
Rogercl 3:db3f7831bcea 246 aa= 44330.0f*( 1.0f - pow((pressure/101325.0f), (1.0f/5.255f))); // Calculate altitude in meters
Rogercl 2:5d0d7997e461 247 wait_us(1);
Rogercl 0:095b19b8fb7e 248 //////////////////////////// Is there a new step? //////////////////
Rogercl 3:db3f7831bcea 249 SubFastDTW(AcceABSF);
Rogercl 1:d1002dc109b9 250
Rogercl 0:095b19b8fb7e 251
Rogercl 3:db3f7831bcea 252 if (flagPrint==1)
Rogercl 1:d1002dc109b9 253 {
Rogercl 1:d1002dc109b9 254 ////////////////////////////Printing Values/////////////////////////
Rogercl 1:d1002dc109b9 255 ask_time(buff);
Rogercl 1:d1002dc109b9 256 //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);
Rogercl 1:d1002dc109b9 257 sprintf(buff_Values,"Steps %d \r\n",steps);
Rogercl 1:d1002dc109b9 258 strcat(buff,buff_Values);
Rogercl 0:095b19b8fb7e 259
Rogercl 1:d1002dc109b9 260 ///////////////////////////////Cheking if the day changed/////////////////////////////////////
Rogercl 1:d1002dc109b9 261 ask_day(day2);
Rogercl 1:d1002dc109b9 262 if (strcmp (day2,day1) != 0) //compare todays day with the day that the file was created
Rogercl 1:d1002dc109b9 263 {
Rogercl 1:d1002dc109b9 264 new_file(); //cria novo arquivo
Rogercl 1:d1002dc109b9 265 ask_day(day1);
Rogercl 1:d1002dc109b9 266 } // end if
Rogercl 2:5d0d7997e461 267 //////////////////////////////////////////////////////////////////////////////////////////////
Rogercl 3:db3f7831bcea 268 Open.printf(buff); //Printing values on the SD card
Rogercl 4:215c9d6d1c80 269 /*
Rogercl 1:d1002dc109b9 270 t2=t.read_us();
Rogercl 1:d1002dc109b9 271 if ((t2 - t1) < TimeInterrupt)
Rogercl 1:d1002dc109b9 272 {
Rogercl 1:d1002dc109b9 273 wait_us((TimeInterrupt) - (t2-t1));
Rogercl 1:d1002dc109b9 274 led=OFF;
Rogercl 1:d1002dc109b9 275 }//end if time
Rogercl 1:d1002dc109b9 276 led=ON;
Rogercl 4:215c9d6d1c80 277 */
Rogercl 3:db3f7831bcea 278 flagPrint=0;
Rogercl 1:d1002dc109b9 279 } //end if flag print
Rogercl 3:db3f7831bcea 280 flagNewCapture = false;
Rogercl 0:095b19b8fb7e 281 } // end if flag new capture
Rogercl 0:095b19b8fb7e 282 } // end while true
Rogercl 0:095b19b8fb7e 283 } //end main