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@1:d1002dc109b9, 2019-03-09 (annotated)

Committer:
Rogercl
Date:
Sat Mar 09 12:06:30 2019 +0000
Revision:
1:d1002dc109b9
Parent:
0:095b19b8fb7e
Child:
2:5d0d7997e461
Erro + printfs;

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