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@6:e9a2bc040ada, 2019-08-04 (annotated)

Committer:
Rogercl
Date:
Sun Aug 04 11:38:08 2019 +0000
Revision:
6:e9a2bc040ada
Parent:
5:d87c25f009d1 
Algoritmo funcionando com a biblioteca de inatividade utilizando dos dados do acelerometro e a biblioteca de PeakSearch se utilizando dos dados filtrados pelo filtro Kalman.

Who changed what in which revision?

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