Revision 1:1567f5a4e96f, committed 2017-10-30

Comitter:

zdenka

Date:

Mon Oct 30 16:12:27 2017 +0000

Parent:

0:0472d2623223

Commit message:

accelerometer samplefilter

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SampleFilter.c	Mon Oct 30 16:12:27 2017 +0000
@@ -0,0 +1,38 @@
+#include "Samplefilter.h"
+
+static double filter_taps[SAMPLEFILTER_TAP_NUM] = {
+  0.009364555465021395,
+  0.04162548629009952,
+  0.08783132195564536,
+  0.14608632119801232,
+  0.19260258113649567,
+  0.21122159144894026,
+  0.19260258113649567,
+  0.14608632119801232,
+  0.08783132195564536,
+  0.04162548629009952,
+  0.009364555465021395
+};
+
+void SampleFilter_init(SampleFilter* f) {
+  int i;
+  for(i = 0; i < SAMPLEFILTER_TAP_NUM; ++i)
+    f->history[i] = 0;
+  f->last_index = 0;
+}
+
+void SampleFilter_put(SampleFilter* f, double input) {
+  f->history[f->last_index++] = input;
+  if(f->last_index == SAMPLEFILTER_TAP_NUM)
+    f->last_index = 0;
+}
+
+double SampleFilter_get(SampleFilter* f) {
+  double acc = 0;
+  int index = f->last_index, i;
+  for(i = 0; i < SAMPLEFILTER_TAP_NUM; ++i) {
+    index = index != 0 ? index-1 : SAMPLEFILTER_TAP_NUM-1;
+    acc += f->history[index] * filter_taps[i];
+  };
+  return acc;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Samplefilter.h	Mon Oct 30 16:12:27 2017 +0000
@@ -0,0 +1,34 @@
+#ifndef SAMPLEFILTER_H_
+#define SAMPLEFILTER_H_
+
+/*
+
+FIR filter designed with
+ http://t-filter.appspot.com
+
+sampling frequency: 800 Hz
+
+* 0 Hz - 50 Hz
+  gain = 1
+  desired ripple = 5 dB
+  actual ripple = 2.914975479357161 dB
+
+* 150 Hz - 400 Hz
+  gain = 0
+  desired attenuation = -40 dB
+  actual attenuation = -43.03832555525106 dB
+
+*/
+
+#define SAMPLEFILTER_TAP_NUM 11
+
+typedef struct {
+  double history[SAMPLEFILTER_TAP_NUM];
+  unsigned int last_index;
+} SampleFilter;
+
+void SampleFilter_init(SampleFilter* f);
+void SampleFilter_put(SampleFilter* f, double input);
+double SampleFilter_get(SampleFilter* f);
+
+#endif
\ No newline at end of file

--- a/main.cpp	Mon Oct 16 14:47:53 2017 +0000
+++ b/main.cpp	Mon Oct 30 16:12:27 2017 +0000
@@ -1,6 +1,6 @@
 #include "mbed.h"
 #include "MMA8451Q.h"
-
+#include "SampleFilter.c"
 #if   defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
   PinName const SDA = PTE25;
   PinName const SCL = PTE24;
@@ -15,20 +15,39 @@
 #endif
 
 #define MMA8451_I2C_ADDRESS (0x1d<<1)
-Serial pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX,115200);
+Ticker timeout;
+Ticker casovac;
+int pom=1;
+float a=0.95;
+float predposlednex=0.0;
+MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
+   static float x, y, z;
+   SampleFilter filter;
+   
+void attick(){
+    if(pom==1){
+            
 
+        x = (acc.getAccX());
+       // y = (acc.getAccY());
+        //z = (acc.getAccZ());
+        //pc.printf("skuska\n\r");
+        SampleFilter_put(&filter,x);
+        pc.printf("%f\n\r", SampleFilter_get(&filter));
 
+        }
+    }
+    void atstop(){pom=0;}
 int main(void)
-{
-    MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
-    PwmOut rled(LED1);
-    PwmOut gled(LED2);
-    PwmOut bled(LED3);
 
-    printf("MMA8451 ID: %d\n", acc.getWhoAmI());
-
+{   
+SampleFilter_init(&filter);
+    casovac.attach(&attick,0.00125);
+    timeout.attach(&atstop,15.0);
+    //pc.printf("zaciatok\n\r");
     while (true) {
-        float x, y, z;
+       /* float x, y, z;
         x = abs(acc.getAccX());
         y = abs(acc.getAccY());
         z = abs(acc.getAccZ());
@@ -36,6 +55,6 @@
         gled = 1.0f - y;
         bled = 1.0f - z;
         wait(0.1f);
-        printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n\r", x, y, z);
+        printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n\r", x, y, z);*/
     }
 }