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@0:095b19b8fb7e, 2019-03-07 (annotated)

Committer:
Rogercl
Date:
Thu Mar 07 23:30:19 2019 +0000
Revision:
0:095b19b8fb7e
Child:
1:d1002dc109b9
Modificado o BLE para facilitacao dos testes.; ; Necessario Debugar  para encontrar problemas.

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 0:095b19b8fb7e 38 float TimeInterrupt=20000; //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 0:095b19b8fb7e 84 ////////////////////////////Getting MPU Values/////////////////////////
Rogercl 0:095b19b8fb7e 85 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
Rogercl 0:095b19b8fb7e 86
Rogercl 0:095b19b8fb7e 87 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
Rogercl 0:095b19b8fb7e 88
Rogercl 0:095b19b8fb7e 89 mpu9250.readMagData(magCount); // Read the x/y/z adc values
Rogercl 0:095b19b8fb7e 90
Rogercl 0:095b19b8fb7e 91 tempCount = mpu9250.readTempData(); // Read the adc values
Rogercl 0:095b19b8fb7e 92
Rogercl 0:095b19b8fb7e 93 ////////////////////////////Getting Altimeter Values/////////////////////////
Rogercl 0:095b19b8fb7e 94 status = -1;
Rogercl 0:095b19b8fb7e 95 status = Sensor1.Read_Altitude_Data();
Rogercl 0:095b19b8fb7e 96
Rogercl 0:095b19b8fb7e 97 flagNewCapture = true;
Rogercl 0:095b19b8fb7e 98 }
Rogercl 0:095b19b8fb7e 99
Rogercl 0:095b19b8fb7e 100 } //end get_values()
Rogercl 0:095b19b8fb7e 101
Rogercl 0:095b19b8fb7e 102 /////////////////////Main Funcition////////////////////////
Rogercl 0:095b19b8fb7e 103
Rogercl 0:095b19b8fb7e 104 int main(void)
Rogercl 0:095b19b8fb7e 105 {
Rogercl 0:095b19b8fb7e 106 led=OFF; // led azul - que indica quando esta gravando no OpenLog
Rogercl 0:095b19b8fb7e 107
Rogercl 0:095b19b8fb7e 108 Open.set_flow_control(Serial::Disabled); // Without that the serial communication won't work
Rogercl 0:095b19b8fb7e 109 Open.baud(BaudRate); //Starting the serial comunication
Rogercl 0:095b19b8fb7e 110
Rogercl 0:095b19b8fb7e 111 getting_pacient_data();// Get the pacient name, date and time
Rogercl 0:095b19b8fb7e 112
Rogercl 0:095b19b8fb7e 113 // TODO: modificar para que o ultimo valor de hora esteja gravado ou no SD ou EEPROM,
Rogercl 0:095b19b8fb7e 114 // verificar se eh possivel manter o RTC ligado, mesmo com o micro desligado.
Rogercl 0:095b19b8fb7e 115 // ou então, criar um app que realize a atualização sempre que o dispositivo
Rogercl 0:095b19b8fb7e 116 // seja reiniciado.
Rogercl 0:095b19b8fb7e 117 time_init(); //initialize the RTC timer
Rogercl 0:095b19b8fb7e 118
Rogercl 0:095b19b8fb7e 119 // FIXME: atualmente sempre que reinicia o dispositivo ele sobrescreve o arquivo
Rogercl 0:095b19b8fb7e 120 // pre-existente, uma vez que a DataStamp setado eh igual
Rogercl 0:095b19b8fb7e 121 new_file(); //create a new file
Rogercl 0:095b19b8fb7e 122
Rogercl 0:095b19b8fb7e 123 ask_day(day1); // verificar se mudou o dia
Rogercl 0:095b19b8fb7e 124
Rogercl 0:095b19b8fb7e 125 //////////////////// Starting MPU ///////////////////////
Rogercl 0:095b19b8fb7e 126
Rogercl 0:095b19b8fb7e 127 // Read the WHO_AM_I register, this is a good test of communication
Rogercl 0:095b19b8fb7e 128 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
Rogercl 0:095b19b8fb7e 129 if (whoami == 0x71) // WHO_AM_I should always be 0x68
Rogercl 0:095b19b8fb7e 130 {
Rogercl 0:095b19b8fb7e 131 wait(1);
Rogercl 0:095b19b8fb7e 132 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
Rogercl 0:095b19b8fb7e 133 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
Rogercl 0:095b19b8fb7e 134 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
Rogercl 0:095b19b8fb7e 135 wait(2);
Rogercl 0:095b19b8fb7e 136 mpu9250.initMPU9250();
Rogercl 0:095b19b8fb7e 137 mpu9250.initAK8963(magCalibration);
Rogercl 0:095b19b8fb7e 138 wait(1);
Rogercl 0:095b19b8fb7e 139 } //end if
Rogercl 0:095b19b8fb7e 140 else
Rogercl 0:095b19b8fb7e 141 {
Rogercl 0:095b19b8fb7e 142 Open.printf("Could not connect to MPU9250: \n\r");
Rogercl 0:095b19b8fb7e 143 // TODO/FIXME: Verificar se eh possivel disparar um Hardware/Reset quando a
Rogercl 0:095b19b8fb7e 144 // MPU não for inicializada
Rogercl 0:095b19b8fb7e 145 while(1) ; // Loop forever if communication doesn't happen
Rogercl 0:095b19b8fb7e 146 } //end else
Rogercl 0:095b19b8fb7e 147
Rogercl 0:095b19b8fb7e 148 mpu9250.getAres(); // Get accelerometer sensitivity
Rogercl 0:095b19b8fb7e 149 mpu9250.getGres(); // Get gyro sensitivity
Rogercl 0:095b19b8fb7e 150 mpu9250.getMres(); // Get magnetometer sensitivity
Rogercl 0:095b19b8fb7e 151
Rogercl 0:095b19b8fb7e 152 // TODO : Carregar ficheiros de calibração em funcao da temperatura
Rogercl 0:095b19b8fb7e 153 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
Rogercl 0:095b19b8fb7e 154 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
Rogercl 0:095b19b8fb7e 155 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
Rogercl 0:095b19b8fb7e 156
Rogercl 0:095b19b8fb7e 157 //////////////////// Starting MPL ///////////////////////
Rogercl 0:095b19b8fb7e 158 if(!Sensor1.Init())
Rogercl 0:095b19b8fb7e 159 {
Rogercl 0:095b19b8fb7e 160 // Open.printf("Altimeter Initialized\n\r");
Rogercl 0:095b19b8fb7e 161 } //end if
Rogercl 0:095b19b8fb7e 162 else
Rogercl 0:095b19b8fb7e 163 {
Rogercl 0:095b19b8fb7e 164 Open.printf("Altimeter Failed To Initialize\n\r");
Rogercl 0:095b19b8fb7e 165 while(1);
Rogercl 0:095b19b8fb7e 166 } //end els
Rogercl 0:095b19b8fb7e 167
Rogercl 0:095b19b8fb7e 168 wait(1); // 1 seg
Rogercl 0:095b19b8fb7e 169 ////////////////Interrupçã0///////////////////////
Rogercl 0:095b19b8fb7e 170 interrupt.attach_us(&get_values,TimeInterrupt); // the address of the function to be attached and the interval
Rogercl 0:095b19b8fb7e 171 t.start(); // inicia a interrupcao
Rogercl 0:095b19b8fb7e 172
Rogercl 0:095b19b8fb7e 173 led=ON; // indica q a gravacao (openlog) esta apta a ser inciada
Rogercl 0:095b19b8fb7e 174 wait_ms(1);
Rogercl 0:095b19b8fb7e 175 while (true)
Rogercl 0:095b19b8fb7e 176 {
Rogercl 0:095b19b8fb7e 177 ble.waitForEvent();
Rogercl 0:095b19b8fb7e 178 if(strcmp(data_ble,"Stop")== 0||strcmp(data_ble,"Stop ")== 0) //strcmp return 0 whhen true
Rogercl 0:095b19b8fb7e 179 {
Rogercl 0:095b19b8fb7e 180 led=OFF;
Rogercl 0:095b19b8fb7e 181 char alc[10];
Rogercl 0:095b19b8fb7e 182 sprintf(alc,"%d",steps);
Rogercl 0:095b19b8fb7e 183 uart->writeString("Steps: ");
Rogercl 0:095b19b8fb7e 184 uart->writeString(alc);
Rogercl 0:095b19b8fb7e 185 uart->writeString("\n");
Rogercl 0:095b19b8fb7e 186 return 0;
Rogercl 0:095b19b8fb7e 187 }
Rogercl 0:095b19b8fb7e 188 if (flagNewCapture)
Rogercl 0:095b19b8fb7e 189 {
Rogercl 0:095b19b8fb7e 190 t1=t.read_us();
Rogercl 0:095b19b8fb7e 191 ////////////////////////////Getting MPU Values/////////////////////////
Rogercl 0:095b19b8fb7e 192 // Now we'll calculate the accleration value into actual g's
Rogercl 0:095b19b8fb7e 193 ax = (float)accelCount[0]*aRes; // get actual g value, this depends on scale being set
Rogercl 0:095b19b8fb7e 194 ay = (float)accelCount[1]*aRes;
Rogercl 0:095b19b8fb7e 195 az = (float)accelCount[2]*aRes;
Rogercl 0:095b19b8fb7e 196
Rogercl 0:095b19b8fb7e 197 AcceABS=sqrt(ax*ax + ay*ay + az*az);
Rogercl 0:095b19b8fb7e 198 AcceABSF=alfa*AcceABSF+(1-alfa)*AcceABS;
Rogercl 0:095b19b8fb7e 199
Rogercl 0:095b19b8fb7e 200 // Calculate the gyro value into actual degrees per second
Rogercl 0:095b19b8fb7e 201 gx = (float)gyroCount[0]*gRes; // get actual gyro value, this depends on scale being set
Rogercl 0:095b19b8fb7e 202 gy = (float)gyroCount[1]*gRes;
Rogercl 0:095b19b8fb7e 203 gz = (float)gyroCount[2]*gRes;
Rogercl 0:095b19b8fb7e 204
Rogercl 0:095b19b8fb7e 205 // Calculate the magnetometer values in milliGauss
Rogercl 0:095b19b8fb7e 206 // Include factory calibration per data sheet and user environmental corrections
Rogercl 0:095b19b8fb7e 207 mx = (float)magCount[0]*mRes*magCalibration[0]; // get actual magnetometer value, this depends on scale being set
Rogercl 0:095b19b8fb7e 208 my = (float)magCount[1]*mRes*magCalibration[1];
Rogercl 0:095b19b8fb7e 209 mz = (float)magCount[2]*mRes*magCalibration[2];
Rogercl 0:095b19b8fb7e 210
Rogercl 0:095b19b8fb7e 211 ti = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
Rogercl 0:095b19b8fb7e 212
Rogercl 0:095b19b8fb7e 213 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx, gy, gz, my, mx, mz);
Rogercl 0:095b19b8fb7e 214 aa=Sensor1.Altitude_m();
Rogercl 0:095b19b8fb7e 215 ta=Sensor1.Temp_C();
Rogercl 0:095b19b8fb7e 216
Rogercl 0:095b19b8fb7e 217 //////////////////////////// Is there a new step? //////////////////
Rogercl 0:095b19b8fb7e 218 SubDTW();
Rogercl 0:095b19b8fb7e 219
Rogercl 0:095b19b8fb7e 220 if (flagStep==true)
Rogercl 0:095b19b8fb7e 221 {
Rogercl 0:095b19b8fb7e 222 if (flagPrint==true)
Rogercl 0:095b19b8fb7e 223 {
Rogercl 0:095b19b8fb7e 224 ////////////////////////////Printing Values/////////////////////////
Rogercl 0:095b19b8fb7e 225 ask_time(buff);
Rogercl 0:095b19b8fb7e 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 0:095b19b8fb7e 227 sprintf(buff_Values,"Steps %d \r\n",steps);
Rogercl 0:095b19b8fb7e 228 strcat(buff,buff_Values);
Rogercl 0:095b19b8fb7e 229
Rogercl 0:095b19b8fb7e 230 ///////////////////////////////Cheking if the day changed/////////////////////////////////////
Rogercl 0:095b19b8fb7e 231 ask_day(day2);
Rogercl 0:095b19b8fb7e 232 if (strcmp (day2,day1) != 0) //compare todays day with the day that the file was created
Rogercl 0:095b19b8fb7e 233 {
Rogercl 0:095b19b8fb7e 234 new_file(); //cria novo arquivo
Rogercl 0:095b19b8fb7e 235 ask_day(day1);
Rogercl 0:095b19b8fb7e 236 } // end if
Rogercl 0:095b19b8fb7e 237 //////////////////////////////////////////////////////////////////////////////////////////////
Rogercl 0:095b19b8fb7e 238
Rogercl 0:095b19b8fb7e 239 Open.printf(buff); //Printing values on the SD card
Rogercl 0:095b19b8fb7e 240 flagNewCapture = false;
Rogercl 0:095b19b8fb7e 241 t2=t.read_us();
Rogercl 0:095b19b8fb7e 242 if ((t2 - t1) < TimeInterrupt)
Rogercl 0:095b19b8fb7e 243 {
Rogercl 0:095b19b8fb7e 244 wait_us((TimeInterrupt) - (t2-t1));
Rogercl 0:095b19b8fb7e 245 led=OFF;
Rogercl 0:095b19b8fb7e 246 }//end if time
Rogercl 0:095b19b8fb7e 247 led=ON;
Rogercl 0:095b19b8fb7e 248 flagPrint=false;
Rogercl 0:095b19b8fb7e 249 } //end if flag print
Rogercl 0:095b19b8fb7e 250 } //end if flag step
Rogercl 0:095b19b8fb7e 251 else
Rogercl 0:095b19b8fb7e 252 {
Rogercl 0:095b19b8fb7e 253 flagPrint=true;
Rogercl 0:095b19b8fb7e 254 }
Rogercl 0:095b19b8fb7e 255 } // end if flag new capture
Rogercl 0:095b19b8fb7e 256 } // end while true
Rogercl 0:095b19b8fb7e 257 } //end main