Revision 6:e9a2bc040ada, committed 2019-08-04

Comitter:

Rogercl

Date:

Sun Aug 04 11:38:08 2019 +0000

Parent:

5:d87c25f009d1

Commit message:

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.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Inatividade.h	Sun Aug 04 11:38:08 2019 +0000
@@ -0,0 +1,108 @@
+ //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;
+}
\ No newline at end of file

--- a/main.cpp	Sat Jul 27 12:17:25 2019 +0000
+++ b/main.cpp	Sun Aug 04 11:38:08 2019 +0000
@@ -60,7 +60,7 @@
 #include "time_config.h" //Time configuration and util
 #include "file_comands.h" //File comands
 #include "ble_comands.h" //ble
- 
+#include "Inatividade.h" //Inatividade
 void SensorStart()
 {
     
@@ -241,31 +241,38 @@
             ti = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
             aa= 44330.0f*( 1.0f - pow((pressure/101325.0f), (1.0f/5.255f))); // Calculate altitude in meters 
             wait_us(1);
-            //////////////////////////// Is there a new step? //////////////////
-            kalman(ax, ay, az, gx, gy, gz, &Yaw, &Pitch, &Roll, &P, (1/Freq));
-            int result=findPeaks(Pitch, Threshold/4, Threshold);
-            if(result>0)
+            //////////////////////////// Inatividade ///////////////////////////
+            Inatividade();
+
+            if (flagInativ==0)
             {
-                steps=steps+result;
-                result=0;
-                iiSearch=0;
-        ////////////////////////////Printing Values/////////////////////////  
-                ask_time(buff); 
-                //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);
-                sprintf(buff_Values,"Steps %d \r\n",steps);
-                strcat(buff,buff_Values);
-        
-        ///////////////////////////////Cheking if the day changed/////////////////////////////////////
-                 ask_day(day2);
-                if (strcmp (day2,day1) != 0) //compare todays day with the day that the file was created
-                {   
-                    new_file(); //cria novo arquivo
-                    ask_day(day1);
-                } // end if     
-        //////////////////////////////////////////////////////////////////////////////////////////////     
-                Open.printf(buff);      //Printing values on the SD card   
-            } //end if flag print        
-            flagNewCapture = false; 
+                
+                //////////////////////////// Is there a new step? //////////////////
+                kalman(ax, ay, az, gx, gy, gz, &Yaw, &Pitch, &Roll, &P, (1/Freq));
+                int result=findPeaks(Pitch, Threshold/4, Threshold);
+                if(result>0)
+                {
+                    steps=steps+result;
+                    result=0;
+                    iiSearch=0;
+            ////////////////////////////Printing Values/////////////////////////  
+                    ask_time(buff); 
+                    //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);
+                    sprintf(buff_Values,"Steps %d \r\n",steps);
+                    strcat(buff,buff_Values);
+            
+            ///////////////////////////////Cheking if the day changed/////////////////////////////////////
+                     ask_day(day2);
+                    if (strcmp (day2,day1) != 0) //compare todays day with the day that the file was created
+                    {   
+                        new_file(); //cria novo arquivo
+                        ask_day(day1);
+                    } // end if     
+            //////////////////////////////////////////////////////////////////////////////////////////////     
+                    Open.printf(buff);      //Printing values on the SD card   
+                } //end if flag print        
+            } //end if inatividade
+        flagNewCapture = false; 
         } // end if flag new capture        
     } // end while true
 } //end main
\ No newline at end of file