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@2:5d0d7997e461, 2019-04-03 (annotated)

Committer:
Rogercl
Date:
Wed Apr 03 22:52:28 2019 +0000
Revision:
2:5d0d7997e461
Parent:
1:d1002dc109b9
Child:
3:db3f7831bcea
teste

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