Patrik Rendek / Mbed 2 deprecated SemestralkaCSS2

a

Dependencies:   X_NUCLEO_IKS01A1 mbed

Fork of HelloWorld_IKS01A1 by ST

main.cpp@11:4a74fda2b9b7, 2017-05-22 (annotated)

Committer:
rendek4
Date:
Mon May 22 18:42:40 2017 +0000
Revision:
11:4a74fda2b9b7
Parent:
10:211cd9c27d5e 
qqq

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wolfgang Betz 0:c71c9af137dd 1 /**
Wolfgang Betz 0:c71c9af137dd 2 ******************************************************************************
Wolfgang Betz 0:c71c9af137dd 3 * @file main.cpp
Wolfgang Betz 0:c71c9af137dd 4 * @author AST / EST
Wolfgang Betz 0:c71c9af137dd 5 * @version V0.0.1
Wolfgang Betz 0:c71c9af137dd 6 * @date 14-August-2015
rendek4 11:4a74fda2b9b7 7 * @brief Simple Example application for using the X_NUCLEO_IKS01A1
Wolfgang Betz 0:c71c9af137dd 8 * MEMS Inertial & Environmental Sensor Nucleo expansion board.
Wolfgang Betz 0:c71c9af137dd 9 ******************************************************************************
Wolfgang Betz 0:c71c9af137dd 10 * @attention
Wolfgang Betz 0:c71c9af137dd 11 *
Wolfgang Betz 0:c71c9af137dd 12 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
Wolfgang Betz 0:c71c9af137dd 13 *
Wolfgang Betz 0:c71c9af137dd 14 * Redistribution and use in source and binary forms, with or without modification,
Wolfgang Betz 0:c71c9af137dd 15 * are permitted provided that the following conditions are met:
Wolfgang Betz 0:c71c9af137dd 16 * 1. Redistributions of source code must retain the above copyright notice,
Wolfgang Betz 0:c71c9af137dd 17 * this list of conditions and the following disclaimer.
Wolfgang Betz 0:c71c9af137dd 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
Wolfgang Betz 0:c71c9af137dd 19 * this list of conditions and the following disclaimer in the documentation
Wolfgang Betz 0:c71c9af137dd 20 * and/or other materials provided with the distribution.
Wolfgang Betz 0:c71c9af137dd 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
Wolfgang Betz 0:c71c9af137dd 22 * may be used to endorse or promote products derived from this software
Wolfgang Betz 0:c71c9af137dd 23 * without specific prior written permission.
Wolfgang Betz 0:c71c9af137dd 24 *
Wolfgang Betz 0:c71c9af137dd 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
Wolfgang Betz 0:c71c9af137dd 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
Wolfgang Betz 0:c71c9af137dd 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
Wolfgang Betz 0:c71c9af137dd 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
Wolfgang Betz 0:c71c9af137dd 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
Wolfgang Betz 0:c71c9af137dd 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
Wolfgang Betz 0:c71c9af137dd 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
Wolfgang Betz 0:c71c9af137dd 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
Wolfgang Betz 0:c71c9af137dd 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
Wolfgang Betz 0:c71c9af137dd 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Wolfgang Betz 0:c71c9af137dd 35 *
Wolfgang Betz 0:c71c9af137dd 36 ******************************************************************************
rendek4 11:4a74fda2b9b7 37 */
Wolfgang Betz 0:c71c9af137dd 38
Wolfgang Betz 0:c71c9af137dd 39 /* Includes */
Wolfgang Betz 0:c71c9af137dd 40 #include "mbed.h"
Wolfgang Betz 0:c71c9af137dd 41 #include "x_nucleo_iks01a1.h"
Wolfgang Betz 0:c71c9af137dd 42
Wolfgang Betz 0:c71c9af137dd 43 /* Instantiate the expansion board */
Wolfgang Betz 4:b1526d074d83 44 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
Wolfgang Betz 0:c71c9af137dd 45
Wolfgang Betz 0:c71c9af137dd 46 /* Retrieve the composing elements of the expansion board */
Wolfgang Betz 0:c71c9af137dd 47 static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
Wolfgang Betz 0:c71c9af137dd 48 static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
Wolfgang Betz 0:c71c9af137dd 49 static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;
Wolfgang Betz 0:c71c9af137dd 50 static HumiditySensor *humidity_sensor = mems_expansion_board->ht_sensor;
Wolfgang Betz 3:a2d2342526db 51 static PressureSensor *pressure_sensor = mems_expansion_board->pt_sensor;
Wolfgang Betz 0:c71c9af137dd 52 static TempSensor *temp_sensor1 = mems_expansion_board->ht_sensor;
Wolfgang Betz 3:a2d2342526db 53 static TempSensor *temp_sensor2 = mems_expansion_board->pt_sensor;
Wolfgang Betz 0:c71c9af137dd 54
rendek4 11:4a74fda2b9b7 55 double pX;
rendek4 11:4a74fda2b9b7 56
Wolfgang Betz 0:c71c9af137dd 57 /* Helper function for printing floats & doubles */
Wolfgang Betz 0:c71c9af137dd 58 static char *printDouble(char* str, double v, int decimalDigits=2)
Wolfgang Betz 0:c71c9af137dd 59 {
rendek4 11:4a74fda2b9b7 60 int i = 1;
rendek4 11:4a74fda2b9b7 61 int intPart, fractPart;
rendek4 11:4a74fda2b9b7 62 int len;
rendek4 11:4a74fda2b9b7 63 char *ptr;
rendek4 11:4a74fda2b9b7 64
rendek4 11:4a74fda2b9b7 65 /* prepare decimal digits multiplicator */
rendek4 11:4a74fda2b9b7 66 for (; decimalDigits!=0; i*=10, decimalDigits--);
rendek4 11:4a74fda2b9b7 67
rendek4 11:4a74fda2b9b7 68 /* calculate integer & fractinal parts */
rendek4 11:4a74fda2b9b7 69 intPart = (int)v;
rendek4 11:4a74fda2b9b7 70 fractPart = (int)((v-(double)(int)v)*i);
Wolfgang Betz 0:c71c9af137dd 71
rendek4 11:4a74fda2b9b7 72 /* fill in integer part */
rendek4 11:4a74fda2b9b7 73 sprintf(str, "%i.", intPart);
rendek4 11:4a74fda2b9b7 74
rendek4 11:4a74fda2b9b7 75 /* prepare fill in of fractional part */
rendek4 11:4a74fda2b9b7 76 len = strlen(str);
rendek4 11:4a74fda2b9b7 77 ptr = &str[len];
Wolfgang Betz 0:c71c9af137dd 78
rendek4 11:4a74fda2b9b7 79 /* fill in leading fractional zeros */
rendek4 11:4a74fda2b9b7 80 for (i/=10; i>1; i/=10, ptr++) {
rendek4 11:4a74fda2b9b7 81 if(fractPart >= i) break;
rendek4 11:4a74fda2b9b7 82 *ptr = '0';
rendek4 11:4a74fda2b9b7 83 }
Wolfgang Betz 0:c71c9af137dd 84
rendek4 11:4a74fda2b9b7 85 /* fill in (rest of) fractional part */
rendek4 11:4a74fda2b9b7 86 sprintf(ptr, "%i", fractPart);
Wolfgang Betz 0:c71c9af137dd 87
rendek4 11:4a74fda2b9b7 88 return str;
rendek4 11:4a74fda2b9b7 89 }
rendek4 11:4a74fda2b9b7 90
rendek4 11:4a74fda2b9b7 91 double dolno (double data)
rendek4 11:4a74fda2b9b7 92 {
rendek4 11:4a74fda2b9b7 93 float Fc=0.1;
rendek4 11:4a74fda2b9b7 94 float alfa=0.1;
rendek4 11:4a74fda2b9b7 95 double x= (1- alfa) *pX+alfa*data;
rendek4 11:4a74fda2b9b7 96 pX=x;
rendek4 11:4a74fda2b9b7 97 return x;
rendek4 11:4a74fda2b9b7 98
rendek4 11:4a74fda2b9b7 99
Wolfgang Betz 0:c71c9af137dd 100
rendek4 11:4a74fda2b9b7 101 }
rendek4 11:4a74fda2b9b7 102 double pasmo (double in)
rendek4 11:4a74fda2b9b7 103 {
rendek4 11:4a74fda2b9b7 104 double hc = 0.02;
rendek4 11:4a74fda2b9b7 105 double lc = 0.7;
rendek4 11:4a74fda2b9b7 106 double lk = 0;
rendek4 11:4a74fda2b9b7 107 double hk = 0;
rendek4 11:4a74fda2b9b7 108 double out;
rendek4 11:4a74fda2b9b7 109 lk = lk + lc * (in - lk);
rendek4 11:4a74fda2b9b7 110 hk = hk + hc * (lk - hk);
rendek4 11:4a74fda2b9b7 111 out = lk - hk;
Wolfgang Betz 0:c71c9af137dd 112
Wolfgang Betz 0:c71c9af137dd 113
rendek4 11:4a74fda2b9b7 114 return out;
Wolfgang Betz 0:c71c9af137dd 115 }
Wolfgang Betz 0:c71c9af137dd 116
Wolfgang Betz 0:c71c9af137dd 117
Wolfgang Betz 0:c71c9af137dd 118 /* Simple main function */
rendek4 11:4a74fda2b9b7 119 int main()
rendek4 11:4a74fda2b9b7 120 {
rendek4 11:4a74fda2b9b7 121 uint8_t id;
rendek4 11:4a74fda2b9b7 122 float value1, value2;
rendek4 11:4a74fda2b9b7 123 char buffer1[32], buffer2[32];
rendek4 11:4a74fda2b9b7 124
rendek4 11:4a74fda2b9b7 125
rendek4 11:4a74fda2b9b7 126 printf("\r\n--- Starting new run ---\r\n");
rendek4 11:4a74fda2b9b7 127
rendek4 11:4a74fda2b9b7 128 humidity_sensor->read_id(&id);
rendek4 11:4a74fda2b9b7 129 printf("HTS221 humidity & temperature = 0x%X\r\n", id);
rendek4 11:4a74fda2b9b7 130
rendek4 11:4a74fda2b9b7 131 wait(3);
rendek4 11:4a74fda2b9b7 132
rendek4 11:4a74fda2b9b7 133 while(1) {
rendek4 11:4a74fda2b9b7 134 //printf("\r\n");
rendek4 11:4a74fda2b9b7 135
rendek4 11:4a74fda2b9b7 136 temp_sensor1->get_temperature(&value1);
rendek4 11:4a74fda2b9b7 137 humidity_sensor->get_humidity(&value2);
rendek4 11:4a74fda2b9b7 138 printf("%s%;\t",printDouble(buffer2, value2));
rendek4 11:4a74fda2b9b7 139 // printf("\r\n");
rendek4 11:4a74fda2b9b7 140 double filtered=dolno(value2);
rendek4 11:4a74fda2b9b7 141 printf( "%6.5lf;\t", filtered );
rendek4 11:4a74fda2b9b7 142 double p=pasmo(value2);
rendek4 11:4a74fda2b9b7 143 printf( "%6.5lf\r\n", p );
Wolfgang Betz 0:c71c9af137dd 144
rendek4 11:4a74fda2b9b7 145 wait(0.5);
rendek4 11:4a74fda2b9b7 146 }
rendek4 11:4a74fda2b9b7 147 }
rendek4 11:4a74fda2b9b7 148
rendek4 11:4a74fda2b9b7 149 ///////////////////////////////////////////////////////////////////////////////////////
rendek4 11:4a74fda2b9b7 150 /*
rendek4 11:4a74fda2b9b7 151 #include <iostream>
rendek4 11:4a74fda2b9b7 152 #include <cmath>
rendek4 11:4a74fda2b9b7 153 #include <iomanip>
rendek4 11:4a74fda2b9b7 154
rendek4 11:4a74fda2b9b7 155 using namespace std;
rendek4 11:4a74fda2b9b7 156
rendek4 11:4a74fda2b9b7 157 void createFilter(double gKernel[][5])
rendek4 11:4a74fda2b9b7 158 {
rendek4 11:4a74fda2b9b7 159 // set standard deviation to 1.0
rendek4 11:4a74fda2b9b7 160 double sigma = 1.0;
rendek4 11:4a74fda2b9b7 161 double r, s = 2.0 * sigma * sigma;
rendek4 11:4a74fda2b9b7 162
rendek4 11:4a74fda2b9b7 163 // sum is for normalization
rendek4 11:4a74fda2b9b7 164 double sum = 0.0;
rendek4 11:4a74fda2b9b7 165
rendek4 11:4a74fda2b9b7 166 // generate 5x5 kernel
rendek4 11:4a74fda2b9b7 167 for (int x = -2; x <= 2; x++)
rendek4 11:4a74fda2b9b7 168 {
rendek4 11:4a74fda2b9b7 169 for(int y = -2; y <= 2; y++)
rendek4 11:4a74fda2b9b7 170 {
rendek4 11:4a74fda2b9b7 171 r = sqrt(x*x + y*y);
rendek4 11:4a74fda2b9b7 172 gKernel[x + 2][y + 2] = (exp(-(r*r)/s))/(M_PI * s);
rendek4 11:4a74fda2b9b7 173 sum += gKernel[x + 2][y + 2];
rendek4 11:4a74fda2b9b7 174 }
rendek4 11:4a74fda2b9b7 175 }
Wolfgang Betz 0:c71c9af137dd 176
rendek4 11:4a74fda2b9b7 177 // normalize the Kernel
rendek4 11:4a74fda2b9b7 178 for(int i = 0; i < 5; ++i)
rendek4 11:4a74fda2b9b7 179 for(int j = 0; j < 5; ++j)
rendek4 11:4a74fda2b9b7 180 gKernel[i][j] /= sum;
rendek4 11:4a74fda2b9b7 181
rendek4 11:4a74fda2b9b7 182 }
Wolfgang Betz 0:c71c9af137dd 183
rendek4 11:4a74fda2b9b7 184 int main()
rendek4 11:4a74fda2b9b7 185 {
rendek4 11:4a74fda2b9b7 186 double gKernel[5][5];
rendek4 11:4a74fda2b9b7 187 createFilter(gKernel);
rendek4 11:4a74fda2b9b7 188 for(int i = 0; i < 5; ++i)
rendek4 11:4a74fda2b9b7 189 {
rendek4 11:4a74fda2b9b7 190 for (int j = 0; j < 5; ++j)
rendek4 11:4a74fda2b9b7 191 cout<<gKernel[i][j]<<"\t";
rendek4 11:4a74fda2b9b7 192 cout<<endl;
rendek4 11:4a74fda2b9b7 193 }
rendek4 11:4a74fda2b9b7 194 }
rendek4 11:4a74fda2b9b7 195 */
rendek4 11:4a74fda2b9b7 196
rendek4 11:4a74fda2b9b7 197
rendek4 11:4a74fda2b9b7 198
rendek4 11:4a74fda2b9b7 199
rendek4 11:4a74fda2b9b7 200 /* Thanks, that's obvious after reading it :) The high pass filter is just
rendek4 11:4a74fda2b9b7 201 the input minus the output of the low pass filter.
rendek4 11:4a74fda2b9b7 202
rendek4 11:4a74fda2b9b7 203 What I really need is a bandpass filter. I've combined the two filters like
rendek4 11:4a74fda2b9b7 204 this:
Wolfgang Betz 0:c71c9af137dd 205
rendek4 11:4a74fda2b9b7 206 hc = 0.02;
rendek4 11:4a74fda2b9b7 207 lc = 0.4;
rendek4 11:4a74fda2b9b7 208 lk = 0;
rendek4 11:4a74fda2b9b7 209 hk = 0;
rendek4 11:4a74fda2b9b7 210 loop {
rendek4 11:4a74fda2b9b7 211 in = nextSensorValue();
rendek4 11:4a74fda2b9b7 212 lk = lk + lc * (in - lk);
rendek4 11:4a74fda2b9b7 213 hk = hk + hc * (lk - hk);
rendek4 11:4a74fda2b9b7 214 out = lk - hk;
rendek4 11:4a74fda2b9b7 215 writeOut(out);
rendek4 11:4a74fda2b9b7 216 }
Wolfgang Betz 0:c71c9af137dd 217
rendek4 11:4a74fda2b9b7 218 This is already very simple and looks like it does what I want, but I
rendek4 11:4a74fda2b9b7 219 wonder if I can reduce the number of multiplications and additions even
rendek4 11:4a74fda2b9b7 220 further, because I want to implement it on a microcontroller, with the
rendek4 11:4a74fda2b9b7 221 additional complication that it doesn't support floating points. But I
rendek4 11:4a74fda2b9b7 222 think I can convert it to fixed point math. */
rendek4 11:4a74fda2b9b7 223
rendek4 11:4a74fda2b9b7 224
rendek4 11:4a74fda2b9b7 225
rendek4 11:4a74fda2b9b7 226 //highpass
Wolfgang Betz 0:c71c9af137dd 227
rendek4 11:4a74fda2b9b7 228 /*Sure...
rendek4 11:4a74fda2b9b7 229 x(k+1) = (1-c)*x(k) + c*u(k)
rendek4 11:4a74fda2b9b7 230 y(k) = u(k) - x(k)
rendek4 11:4a74fda2b9b7 231 c sets the cutoff frequency. There's a zero at z=1 and a pole at z=1-
rendek4 11:4a74fda2b9b7 232 c.
rendek4 11:4a74fda2b9b7 233 u(k) is the input; y(k) the output. x is a variable internal to the
rendek4 11:4a74fda2b9b7 234 filter.
rendek4 11:4a74fda2b9b7 235 Note that x(k) by itself as an output is just a low-pass function.
rendek4 11:4a74fda2b9b7 236 This is a first-order filter. */
rendek4 11:4a74fda2b9b7 237
rendek4 11:4a74fda2b9b7 238
rendek4 11:4a74fda2b9b7 239