main.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    main.cpp
00004  * @author  AST / EST
00005  * @version V0.0.1
00006  * @date    14-August-2015
00007  * @brief   Simple Example application for using the X_NUCLEO_IKS01A1
00008  *          MEMS Inertial & Environmental Sensor Nucleo expansion board.
00009  ******************************************************************************
00010  * @attention
00011  *
00012  * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
00013  *
00014  * Redistribution and use in source and binary forms, with or without modification,
00015  * are permitted provided that the following conditions are met:
00016  *   1. Redistributions of source code must retain the above copyright notice,
00017  *      this list of conditions and the following disclaimer.
00018  *   2. Redistributions in binary form must reproduce the above copyright notice,
00019  *      this list of conditions and the following disclaimer in the documentation
00020  *      and/or other materials provided with the distribution.
00021  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00022  *      may be used to endorse or promote products derived from this software
00023  *      without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00028  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00029  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00030  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00031  *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00033  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00034  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  ******************************************************************************
00037 */
00038 
00039 /* Includes */
00040 #include "mbed.h"
00041 #include "x_nucleo_iks01a1.h"
00042 
00043 /* Instantiate the expansion board */
00044 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
00045 
00046 /* Retrieve the composing elements of the expansion board */
00047 static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
00048 static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
00049 static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;
00050 static HumiditySensor *humidity_sensor = mems_expansion_board->ht_sensor;
00051 static PressureSensor *pressure_sensor = mems_expansion_board->pt_sensor;
00052 static TempSensor *temp_sensor1 = mems_expansion_board->ht_sensor;
00053 static TempSensor *temp_sensor2 = mems_expansion_board->pt_sensor;
00054 
00055 double pX;
00056 
00057 /* Helper function for printing floats & doubles */
00058 static char *printDouble(char* str, double v, int decimalDigits=2)
00059 {
00060     int i = 1;
00061     int intPart, fractPart;
00062     int len;
00063     char *ptr;
00064 
00065     /* prepare decimal digits multiplicator */
00066     for (; decimalDigits!=0; i*=10, decimalDigits--);
00067 
00068     /* calculate integer & fractinal parts */
00069     intPart = (int)v;
00070     fractPart = (int)((v-(double)(int)v)*i);
00071 
00072     /* fill in integer part */
00073     sprintf(str, "%i.", intPart);
00074 
00075     /* prepare fill in of fractional part */
00076     len = strlen(str);
00077     ptr = &str[len];
00078 
00079     /* fill in leading fractional zeros */
00080     for (i/=10; i>1; i/=10, ptr++) {
00081         if(fractPart >= i) break;
00082         *ptr = '0';
00083     }
00084 
00085     /* fill in (rest of) fractional part */
00086     sprintf(ptr, "%i", fractPart);
00087 
00088     return str;
00089 }
00090 
00091 double dolno (double data)
00092 {
00093     float Fc=0.1;
00094     float alfa=0.1;
00095     double x= (1- alfa) *pX+alfa*data;
00096     pX=x;
00097     return x;
00098 
00099 
00100 
00101 }
00102 double pasmo (double in)
00103 {
00104     double hc = 0.02;
00105     double lc = 0.7;
00106     double lk = 0;
00107     double  hk = 0;
00108     double out;
00109     lk = lk + lc * (in - lk);
00110     hk = hk + hc * (lk - hk);
00111     out = lk - hk;
00112 
00113 
00114     return out;
00115 }
00116 
00117 
00118 /* Simple main function */
00119 int main()
00120 {
00121     uint8_t id;
00122     float value1, value2;
00123     char buffer1[32], buffer2[32];
00124 
00125 
00126     printf("\r\n--- Starting new run ---\r\n");
00127 
00128     humidity_sensor->read_id(&id);
00129     printf("HTS221  humidity & temperature    = 0x%X\r\n", id);
00130 
00131     wait(3);
00132 
00133     while(1) {
00134         //printf("\r\n");
00135 
00136         temp_sensor1->get_temperature(&value1);
00137         humidity_sensor->get_humidity(&value2);
00138         printf("%s%;\t",printDouble(buffer2, value2));
00139         //  printf("\r\n");
00140         double filtered=dolno(value2);
00141         printf( "%6.5lf;\t", filtered );
00142         double p=pasmo(value2);
00143         printf( "%6.5lf\r\n", p );
00144 
00145         wait(0.5);
00146     }
00147 }
00148 
00149 ///////////////////////////////////////////////////////////////////////////////////////
00150 /*
00151 #include <iostream>
00152 #include <cmath>
00153 #include <iomanip>
00154 
00155 using namespace std;
00156 
00157 void createFilter(double gKernel[][5])
00158 {
00159     // set standard deviation to 1.0
00160     double sigma = 1.0;
00161     double r, s = 2.0 * sigma * sigma;
00162 
00163     // sum is for normalization
00164     double sum = 0.0;
00165 
00166     // generate 5x5 kernel
00167     for (int x = -2; x <= 2; x++)
00168     {
00169         for(int y = -2; y <= 2; y++)
00170         {
00171             r = sqrt(x*x + y*y);
00172             gKernel[x + 2][y + 2] = (exp(-(r*r)/s))/(M_PI * s);
00173             sum += gKernel[x + 2][y + 2];
00174         }
00175     }
00176 
00177     // normalize the Kernel
00178     for(int i = 0; i < 5; ++i)
00179         for(int j = 0; j < 5; ++j)
00180             gKernel[i][j] /= sum;
00181 
00182 }
00183 
00184 int main()
00185 {
00186     double gKernel[5][5];
00187     createFilter(gKernel);
00188     for(int i = 0; i < 5; ++i)
00189     {
00190         for (int j = 0; j < 5; ++j)
00191             cout<<gKernel[i][j]<<"\t";
00192         cout<<endl;
00193     }
00194 }
00195 */
00196 
00197 
00198 
00199 
00200 /* Thanks, that's obvious after reading it :) The high pass filter is just
00201 the input minus the output of the low pass filter.
00202 
00203 What I really need is a bandpass filter. I've combined the two filters like
00204 this:
00205 
00206 hc = 0.02;
00207 lc = 0.4;
00208 lk = 0;
00209 hk = 0;
00210 loop {
00211 in = nextSensorValue();
00212 lk = lk + lc * (in - lk);
00213 hk = hk + hc * (lk - hk);
00214 out = lk - hk;
00215 writeOut(out);
00216 }
00217 
00218 This is already very simple and looks like it does what I want, but I
00219 wonder if I can reduce the number of multiplications and additions even
00220 further, because I want to implement it on a microcontroller, with the
00221 additional complication that it doesn't support floating points. But I
00222 think I can convert it to fixed point math.  */
00223 
00224 
00225 
00226 //highpass
00227 
00228 /*Sure...
00229 x(k+1) = (1-c)*x(k) + c*u(k)
00230 y(k) = u(k) - x(k)
00231 c sets the cutoff frequency. There's a zero at z=1 and a pole at z=1-
00232 c.
00233 u(k) is the input; y(k) the output. x is a variable internal to the
00234 filter.
00235 Note that x(k) by itself as an output is just a low-pass function.
00236 This is a first-order filter. */
00237 
00238 
00239