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
Inatividade.h@6:e9a2bc040ada, 2019-08-04 (annotated)
- Committer:
- Rogercl
- Date:
- Sun Aug 04 11:38:08 2019 +0000
- Revision:
- 6:e9a2bc040ada
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?
User | Revision | Line number | New contents of line |
---|---|---|---|
Rogercl | 6:e9a2bc040ada | 1 | //variaveis inatividade |
Rogercl | 6:e9a2bc040ada | 2 | |
Rogercl | 6:e9a2bc040ada | 3 | float normMean = 9.5; |
Rogercl | 6:e9a2bc040ada | 4 | float stdMean = 0.03; |
Rogercl | 6:e9a2bc040ada | 5 | int i=0; |
Rogercl | 6:e9a2bc040ada | 6 | float norma=0; |
Rogercl | 6:e9a2bc040ada | 7 | float desvio=0; |
Rogercl | 6:e9a2bc040ada | 8 | int contador_inativ=0; |
Rogercl | 6:e9a2bc040ada | 9 | int contador_desvio=0; |
Rogercl | 6:e9a2bc040ada | 10 | int flagInativ=0; |
Rogercl | 6:e9a2bc040ada | 11 | float AcelNormA=0; |
Rogercl | 6:e9a2bc040ada | 12 | |
Rogercl | 6:e9a2bc040ada | 13 | void Inatividade() |
Rogercl | 6:e9a2bc040ada | 14 | { |
Rogercl | 6:e9a2bc040ada | 15 | //variaveis inatividade |
Rogercl | 6:e9a2bc040ada | 16 | float ACEL_CONV=9.807; |
Rogercl | 6:e9a2bc040ada | 17 | float window_filt = 10; |
Rogercl | 6:e9a2bc040ada | 18 | float window = Freq*2; |
Rogercl | 6:e9a2bc040ada | 19 | float mult_norm_stdMean = 2; |
Rogercl | 6:e9a2bc040ada | 20 | float config_stdMean = 0.03; |
Rogercl | 6:e9a2bc040ada | 21 | float AcelNorm=sqrt(ax*ax + ay*ay+ az*az)*ACEL_CONV; |
Rogercl | 6:e9a2bc040ada | 22 | |
Rogercl | 6:e9a2bc040ada | 23 | //Identifica inativo / ativo |
Rogercl | 6:e9a2bc040ada | 24 | if (i<(window_filt-1)) |
Rogercl | 6:e9a2bc040ada | 25 | { |
Rogercl | 6:e9a2bc040ada | 26 | //norma = norma + vin[i]/window_filt; |
Rogercl | 6:e9a2bc040ada | 27 | norma = norma + AcelNorm/window_filt; |
Rogercl | 6:e9a2bc040ada | 28 | } |
Rogercl | 6:e9a2bc040ada | 29 | else |
Rogercl | 6:e9a2bc040ada | 30 | { |
Rogercl | 6:e9a2bc040ada | 31 | //norma = ((window_filt - 1)/window_filt) * norma + vin[i]/window_filt; |
Rogercl | 6:e9a2bc040ada | 32 | norma = ((window_filt - 1)/window_filt) * norma + AcelNorm/window_filt; |
Rogercl | 6:e9a2bc040ada | 33 | } |
Rogercl | 6:e9a2bc040ada | 34 | |
Rogercl | 6:e9a2bc040ada | 35 | if (i>=window) |
Rogercl | 6:e9a2bc040ada | 36 | { |
Rogercl | 6:e9a2bc040ada | 37 | //INATIVO |
Rogercl | 6:e9a2bc040ada | 38 | if (!(abs(norma - normMean) > mult_norm_stdMean * stdMean)) //barreira de desvio / norma |
Rogercl | 6:e9a2bc040ada | 39 | { |
Rogercl | 6:e9a2bc040ada | 40 | contador_inativ = contador_inativ + 1; |
Rogercl | 6:e9a2bc040ada | 41 | |
Rogercl | 6:e9a2bc040ada | 42 | //INATIVO |
Rogercl | 6:e9a2bc040ada | 43 | if (contador_inativ>=Freq) |
Rogercl | 6:e9a2bc040ada | 44 | { |
Rogercl | 6:e9a2bc040ada | 45 | flagInativ=1; |
Rogercl | 6:e9a2bc040ada | 46 | iiSearch=0; |
Rogercl | 6:e9a2bc040ada | 47 | } |
Rogercl | 6:e9a2bc040ada | 48 | } |
Rogercl | 6:e9a2bc040ada | 49 | //ATIVO |
Rogercl | 6:e9a2bc040ada | 50 | else |
Rogercl | 6:e9a2bc040ada | 51 | { |
Rogercl | 6:e9a2bc040ada | 52 | flagInativ=0; |
Rogercl | 6:e9a2bc040ada | 53 | contador_inativ=0; |
Rogercl | 6:e9a2bc040ada | 54 | } |
Rogercl | 6:e9a2bc040ada | 55 | |
Rogercl | 6:e9a2bc040ada | 56 | } |
Rogercl | 6:e9a2bc040ada | 57 | |
Rogercl | 6:e9a2bc040ada | 58 | //desvio avg - corrige/adapta os paramentros de threshold da norma e std |
Rogercl | 6:e9a2bc040ada | 59 | if (i>0) |
Rogercl | 6:e9a2bc040ada | 60 | { |
Rogercl | 6:e9a2bc040ada | 61 | if (i<=(window_filt-1)) |
Rogercl | 6:e9a2bc040ada | 62 | { |
Rogercl | 6:e9a2bc040ada | 63 | //desvio = desvio + abs(vin(i)-vin(i-1))/window_filt; |
Rogercl | 6:e9a2bc040ada | 64 | desvio = desvio + abs(AcelNorm-AcelNormA)/window_filt; |
Rogercl | 6:e9a2bc040ada | 65 | } |
Rogercl | 6:e9a2bc040ada | 66 | else |
Rogercl | 6:e9a2bc040ada | 67 | { |
Rogercl | 6:e9a2bc040ada | 68 | //desvio = ((window_filt - 1)/window_filt)*desvio + abs(vin(i)-vin(i-1))/window_filt; |
Rogercl | 6:e9a2bc040ada | 69 | desvio = ((window_filt - 1)/window_filt)*desvio + abs(AcelNorm-AcelNormA)/window_filt; |
Rogercl | 6:e9a2bc040ada | 70 | } |
Rogercl | 6:e9a2bc040ada | 71 | } |
Rogercl | 6:e9a2bc040ada | 72 | |
Rogercl | 6:e9a2bc040ada | 73 | //Desvio |
Rogercl | 6:e9a2bc040ada | 74 | if (i>=window) |
Rogercl | 6:e9a2bc040ada | 75 | { |
Rogercl | 6:e9a2bc040ada | 76 | if (!(abs(desvio - stdMean) > stdMean)) // barreira de desvio / norma |
Rogercl | 6:e9a2bc040ada | 77 | { |
Rogercl | 6:e9a2bc040ada | 78 | contador_desvio = contador_desvio + 1; |
Rogercl | 6:e9a2bc040ada | 79 | if ((contador_desvio >= window/2) && (contador_desvio < 2*window)) |
Rogercl | 6:e9a2bc040ada | 80 | { |
Rogercl | 6:e9a2bc040ada | 81 | normMean = 0.95 * normMean + 0.05 * norma; |
Rogercl | 6:e9a2bc040ada | 82 | } |
Rogercl | 6:e9a2bc040ada | 83 | else if (contador_desvio >= 2*window) |
Rogercl | 6:e9a2bc040ada | 84 | { |
Rogercl | 6:e9a2bc040ada | 85 | normMean = 0.85 * normMean + 0.15 * norma; |
Rogercl | 6:e9a2bc040ada | 86 | if (flagInativ) |
Rogercl | 6:e9a2bc040ada | 87 | { |
Rogercl | 6:e9a2bc040ada | 88 | stdMean = 0.95 * stdMean + 0.05 * desvio; |
Rogercl | 6:e9a2bc040ada | 89 | |
Rogercl | 6:e9a2bc040ada | 90 | // limitante superior |
Rogercl | 6:e9a2bc040ada | 91 | if (stdMean<=2*config_stdMean) stdMean=stdMean; |
Rogercl | 6:e9a2bc040ada | 92 | else stdMean=2*config_stdMean; |
Rogercl | 6:e9a2bc040ada | 93 | |
Rogercl | 6:e9a2bc040ada | 94 | // limitante inferior |
Rogercl | 6:e9a2bc040ada | 95 | if (stdMean>=config_stdMean/2) stdMean=stdMean; |
Rogercl | 6:e9a2bc040ada | 96 | else stdMean=config_stdMean/2; |
Rogercl | 6:e9a2bc040ada | 97 | } |
Rogercl | 6:e9a2bc040ada | 98 | } |
Rogercl | 6:e9a2bc040ada | 99 | } |
Rogercl | 6:e9a2bc040ada | 100 | else |
Rogercl | 6:e9a2bc040ada | 101 | { |
Rogercl | 6:e9a2bc040ada | 102 | contador_desvio = 0; |
Rogercl | 6:e9a2bc040ada | 103 | } |
Rogercl | 6:e9a2bc040ada | 104 | } |
Rogercl | 6:e9a2bc040ada | 105 | |
Rogercl | 6:e9a2bc040ada | 106 | AcelNormA=AcelNorm; |
Rogercl | 6:e9a2bc040ada | 107 | i=i+1; |
Rogercl | 6:e9a2bc040ada | 108 | } |