Diff: main.cpp

Revision:

11:4a74fda2b9b7

Parent:

10:211cd9c27d5e

--- a/main.cpp	Fri Mar 24 10:14:11 2017 +0000
+++ b/main.cpp	Mon May 22 18:42:40 2017 +0000
@@ -4,7 +4,7 @@
  * @author  AST / EST
  * @version V0.0.1
  * @date    14-August-2015
- * @brief   Simple Example application for using the X_NUCLEO_IKS01A1 
+ * @brief   Simple Example application for using the X_NUCLEO_IKS01A1
  *          MEMS Inertial & Environmental Sensor Nucleo expansion board.
  ******************************************************************************
  * @attention
@@ -34,7 +34,7 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
-*/ 
+*/
 
 /* Includes */
 #include "mbed.h"
@@ -52,83 +52,188 @@
 static TempSensor *temp_sensor1 = mems_expansion_board->ht_sensor;
 static TempSensor *temp_sensor2 = mems_expansion_board->pt_sensor;
 
+double pX;
+
 /* Helper function for printing floats & doubles */
 static char *printDouble(char* str, double v, int decimalDigits=2)
 {
-  int i = 1;
-  int intPart, fractPart;
-  int len;
-  char *ptr;
+    int i = 1;
+    int intPart, fractPart;
+    int len;
+    char *ptr;
+
+    /* prepare decimal digits multiplicator */
+    for (; decimalDigits!=0; i*=10, decimalDigits--);
+
+    /* calculate integer & fractinal parts */
+    intPart = (int)v;
+    fractPart = (int)((v-(double)(int)v)*i);
 
-  /* prepare decimal digits multiplicator */
-  for (;decimalDigits!=0; i*=10, decimalDigits--);
+    /* fill in integer part */
+    sprintf(str, "%i.", intPart);
+
+    /* prepare fill in of fractional part */
+    len = strlen(str);
+    ptr = &str[len];
 
-  /* calculate integer & fractinal parts */
-  intPart = (int)v;
-  fractPart = (int)((v-(double)(int)v)*i);
+    /* fill in leading fractional zeros */
+    for (i/=10; i>1; i/=10, ptr++) {
+        if(fractPart >= i) break;
+        *ptr = '0';
+    }
 
-  /* fill in integer part */
-  sprintf(str, "%i.", intPart);
+    /* fill in (rest of) fractional part */
+    sprintf(ptr, "%i", fractPart);
 
-  /* prepare fill in of fractional part */
-  len = strlen(str);
-  ptr = &str[len];
+    return str;
+}
+
+double dolno (double data)
+{
+    float Fc=0.1;
+    float alfa=0.1;
+    double x= (1- alfa) *pX+alfa*data;
+    pX=x;
+    return x;
+
+
 
-  /* fill in leading fractional zeros */
-  for (i/=10;i>1; i/=10, ptr++) {
-    if(fractPart >= i) break;
-    *ptr = '0';
-  }
+}
+double pasmo (double in)
+{
+    double hc = 0.02;
+    double lc = 0.7;
+    double lk = 0;
+    double  hk = 0;
+    double out;
+    lk = lk + lc * (in - lk);
+    hk = hk + hc * (lk - hk);
+    out = lk - hk;
 
-  /* fill in (rest of) fractional part */
-  sprintf(ptr, "%i", fractPart);
 
-  return str;
+    return out;
 }
 
 
 /* Simple main function */
-int main() {
-  uint8_t id;
-  float value1, value2;
-  char buffer1[32], buffer2[32];
-  int32_t axes[3];
-  
-  printf("\r\n--- Starting new run ---\r\n");
+int main()
+{
+    uint8_t id;
+    float value1, value2;
+    char buffer1[32], buffer2[32];
+
+
+    printf("\r\n--- Starting new run ---\r\n");
+
+    humidity_sensor->read_id(&id);
+    printf("HTS221  humidity & temperature    = 0x%X\r\n", id);
+
+    wait(3);
+
+    while(1) {
+        //printf("\r\n");
+
+        temp_sensor1->get_temperature(&value1);
+        humidity_sensor->get_humidity(&value2);
+        printf("%s%;\t",printDouble(buffer2, value2));
+        //  printf("\r\n");
+        double filtered=dolno(value2);
+        printf( "%6.5lf;\t", filtered );
+        double p=pasmo(value2);
+        printf( "%6.5lf\r\n", p );
 
-  humidity_sensor->read_id(&id);
-  printf("HTS221  humidity & temperature    = 0x%X\r\n", id);
-  pressure_sensor->read_id(&id);
-  printf("LPS25H  pressure & temperature    = 0x%X\r\n", id);
-  magnetometer->read_id(&id);
-  printf("LIS3MDL magnetometer              = 0x%X\r\n", id);
-  gyroscope->read_id(&id);
-  printf("LSM6DS0 accelerometer & gyroscope = 0x%X\r\n", id);
-  
-  wait(3);
- 
-  while(1) {
-    printf("\r\n");
+        wait(0.5);
+    }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////
+/*
+#include <iostream>
+#include <cmath>
+#include <iomanip>
+
+using namespace std;
+
+void createFilter(double gKernel[][5])
+{
+    // set standard deviation to 1.0
+    double sigma = 1.0;
+    double r, s = 2.0 * sigma * sigma;
+
+    // sum is for normalization
+    double sum = 0.0;
+
+    // generate 5x5 kernel
+    for (int x = -2; x <= 2; x++)
+    {
+        for(int y = -2; y <= 2; y++)
+        {
+            r = sqrt(x*x + y*y);
+            gKernel[x + 2][y + 2] = (exp(-(r*r)/s))/(M_PI * s);
+            sum += gKernel[x + 2][y + 2];
+        }
+    }
 
-    temp_sensor1->get_temperature(&value1);
-    humidity_sensor->get_humidity(&value2);
-    printf("HTS221: [temp] %7s°C,   [hum] %s%%\r\n", printDouble(buffer1, value1), printDouble(buffer2, value2));
-    
-    temp_sensor2->get_fahrenheit(&value1);
-    pressure_sensor->get_pressure(&value2);
-    printf("LPS25H: [temp] %7s°F, [press] %smbar\r\n", printDouble(buffer1, value1), printDouble(buffer2, value2));
+    // normalize the Kernel
+    for(int i = 0; i < 5; ++i)
+        for(int j = 0; j < 5; ++j)
+            gKernel[i][j] /= sum;
+
+}
 
-    printf("---\r\n");
+int main()
+{
+    double gKernel[5][5];
+    createFilter(gKernel);
+    for(int i = 0; i < 5; ++i)
+    {
+        for (int j = 0; j < 5; ++j)
+            cout<<gKernel[i][j]<<"\t";
+        cout<<endl;
+    }
+}
+*/
+
+
+
+
+/* Thanks, that's obvious after reading it :) The high pass filter is just
+the input minus the output of the low pass filter.
+
+What I really need is a bandpass filter. I've combined the two filters like
+this:
 
-    magnetometer->get_m_axes(axes);
-    printf("LIS3MDL [mag/mgauss]:  %7ld, %7ld, %7ld\r\n", axes[0], axes[1], axes[2]);
-
-    accelerometer->get_x_axes(axes);
-    printf("LSM6DS0 [acc/mg]:      %7ld, %7ld, %7ld\r\n", axes[0], axes[1], axes[2]);
+hc = 0.02;
+lc = 0.4;
+lk = 0;
+hk = 0;
+loop {
+in = nextSensorValue();
+lk = lk + lc * (in - lk);
+hk = hk + hc * (lk - hk);
+out = lk - hk;
+writeOut(out);
+}
 
-    gyroscope->get_g_axes(axes);
-    printf("LSM6DS0 [gyro/mdps]:   %7ld, %7ld, %7ld\r\n", axes[0], axes[1], axes[2]);
+This is already very simple and looks like it does what I want, but I
+wonder if I can reduce the number of multiplications and additions even
+further, because I want to implement it on a microcontroller, with the
+additional complication that it doesn't support floating points. But I
+think I can convert it to fixed point math.  */
+
+
+
+//highpass
 
-    wait(1.5);
-  }
-}
+/*Sure...
+x(k+1) = (1-c)*x(k) + c*u(k)
+y(k) = u(k) - x(k)
+c sets the cutoff frequency. There's a zero at z=1 and a pole at z=1-
+c.
+u(k) is the input; y(k) the output. x is a variable internal to the
+filter.
+Note that x(k) by itself as an output is just a low-pass function.
+This is a first-order filter. */
+
+
+