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

Inatividade.h

Committer:
Rogercl
Date:
2019-08-04
Revision:
6:e9a2bc040ada

File content as of revision 6:e9a2bc040ada:

 //variaveis inatividade

float normMean = 9.5;
float stdMean = 0.03;
int i=0;
float norma=0;
float desvio=0;
int contador_inativ=0;
int contador_desvio=0;
int flagInativ=0;
float AcelNormA=0;

void Inatividade()
{
    //variaveis inatividade
    float ACEL_CONV=9.807;
    float window_filt = 10;
    float window = Freq*2;
    float mult_norm_stdMean = 2; 
    float config_stdMean = 0.03;
    float AcelNorm=sqrt(ax*ax + ay*ay+ az*az)*ACEL_CONV;
    
    //Identifica inativo / ativo 
    if (i<(window_filt-1))
    {
        //norma = norma + vin[i]/window_filt;  
        norma = norma + AcelNorm/window_filt;  
    } 
    else
    {
        //norma = ((window_filt - 1)/window_filt) * norma + vin[i]/window_filt;
        norma = ((window_filt - 1)/window_filt) * norma + AcelNorm/window_filt;  
    }
    
    if (i>=window)
    {
        //INATIVO
        if (!(abs(norma - normMean) > mult_norm_stdMean * stdMean))    //barreira de desvio / norma
        {
            contador_inativ = contador_inativ + 1;
            
            //INATIVO
            if (contador_inativ>=Freq)
            {
                flagInativ=1;    
                iiSearch=0;
            }
        }
        //ATIVO
        else
        {
            flagInativ=0;
            contador_inativ=0;
        }
        
    }
    
    //desvio avg - corrige/adapta os paramentros de threshold da norma e std  
    if (i>0)
    {
        if (i<=(window_filt-1))
        {
            //desvio = desvio + abs(vin(i)-vin(i-1))/window_filt;
            desvio = desvio + abs(AcelNorm-AcelNormA)/window_filt;
        }
        else
        {
            //desvio = ((window_filt - 1)/window_filt)*desvio + abs(vin(i)-vin(i-1))/window_filt;
            desvio = ((window_filt - 1)/window_filt)*desvio + abs(AcelNorm-AcelNormA)/window_filt;
        }
    }
    
    //Desvio
    if (i>=window)
    {
        if  (!(abs(desvio - stdMean) > stdMean))  // barreira de desvio / norma
        {
            contador_desvio = contador_desvio + 1;
            if ((contador_desvio >= window/2) && (contador_desvio < 2*window))
            {
                normMean = 0.95 * normMean + 0.05 * norma;
            }
            else if (contador_desvio >= 2*window)
            {
                normMean = 0.85 * normMean + 0.15 * norma;
                if (flagInativ)
                {
                    stdMean = 0.95 * stdMean + 0.05 * desvio;

                    // limitante superior
                    if (stdMean<=2*config_stdMean) stdMean=stdMean;
                    else stdMean=2*config_stdMean;

                    // limitante inferior
                    if (stdMean>=config_stdMean/2) stdMean=stdMean;
                    else stdMean=config_stdMean/2;
                }
            }
        }
        else
        {
            contador_desvio = 0;
        } 
    }
    
    AcelNormA=AcelNorm;
    i=i+1;
}