ryan lin / chair_BNO055

wrapper class for BNO055

Dependencies:   BNO055

Dependents:   wheelchaircontrol wheelchaircontrolRos

filter.cpp@3:531a74cecb89, 2018-08-12 (annotated)

Committer:
ryanlin97
Date:
Sun Aug 12 00:33:19 2018 +0000
Revision:
3:531a74cecb89
Parent:
1:3258d62af038 
changed sda and scl definitions to d4 and d5 for small mbed board

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 1:3258d62af038 1 float lowPass(float sample)
ryanlin97 1:3258d62af038 2 {
ryanlin97 1:3258d62af038 3 static const float a[4] = {1.00000000e+00,-2.77555756e-16,3.33333333e-01,-1.85037171e-17};
ryanlin97 1:3258d62af038 4 static const float b[4] = {0.16666667,0.5,0.5,0.16666667};
ryanlin97 1:3258d62af038 5 // x array for holding recent inputs (newest input as index 0, delay of 1 at index 1, etc.
ryanlin97 1:3258d62af038 6 static volatile float x[4] = {0};
ryanlin97 1:3258d62af038 7 // x array for holding recent inputs (newest input as index 0, delay of 1 at index 1, etc.
ryanlin97 1:3258d62af038 8 static volatile float y[4] = {0};
ryanlin97 1:3258d62af038 9 x[0] = sample;
ryanlin97 1:3258d62af038 10 // Calculate the output filtered signal based on a weighted sum of previous inputs/outputs
ryanlin97 1:3258d62af038 11 y[0] = (b[0]*x[0]+b[1]*x[1]+b[2]*x[2]+b[3]*x[3])-(a[1]*y[1]+a[2]*y[2]+a[3]*y[3]);
ryanlin97 1:3258d62af038 12 y[0] /= a[0];
ryanlin97 1:3258d62af038 13 // Shift the input signals by one timestep to prepare for the next call to this function
ryanlin97 1:3258d62af038 14 x[3] = x[2];
ryanlin97 1:3258d62af038 15 x[2] = x[1];
ryanlin97 1:3258d62af038 16 x[1] = x[0];
ryanlin97 1:3258d62af038 17 // Shift the previously calculated output signals by one time step to prepare for the next call to this function
ryanlin97 1:3258d62af038 18 y[3] = y[2];
ryanlin97 1:3258d62af038 19 y[2] = y[1];
ryanlin97 1:3258d62af038 20 y[1] = y[0];
ryanlin97 1:3258d62af038 21 return y[0];
ryanlin97 1:3258d62af038 22 }
ryanlin97 1:3258d62af038 23
ryanlin97 1:3258d62af038 24 float boxcar(float sample)
ryanlin97 1:3258d62af038 25 {
ryanlin97 1:3258d62af038 26 static const int boxcarWidth = 30; // Change this value to alter boxcar length
ryanlin97 1:3258d62af038 27 static float recentSamples[boxcarWidth] = {0}; // hold onto recent samples
ryanlin97 1:3258d62af038 28 static int readIndex = 0; // the index of the current reading
ryanlin97 1:3258d62af038 29 static float total = 0; // the running total
ryanlin97 1:3258d62af038 30 static float average = 0; // the average
ryanlin97 1:3258d62af038 31 // subtract the last reading:
ryanlin97 1:3258d62af038 32 total = total - recentSamples[readIndex];
ryanlin97 1:3258d62af038 33 // add new sample to list (overwrite oldest sample)
ryanlin97 1:3258d62af038 34 recentSamples[readIndex] = sample;
ryanlin97 1:3258d62af038 35 // add the reading to the total:
ryanlin97 1:3258d62af038 36 total = total + recentSamples[readIndex];
ryanlin97 1:3258d62af038 37 // advance to the next position in the array:
ryanlin97 1:3258d62af038 38 readIndex = readIndex + 1;
ryanlin97 1:3258d62af038 39 // if we're at the end of the array...
ryanlin97 1:3258d62af038 40 if (readIndex >= boxcarWidth) {
ryanlin97 1:3258d62af038 41 // ...wrap around to the beginning:
ryanlin97 1:3258d62af038 42 readIndex = 0;
ryanlin97 1:3258d62af038 43 }
ryanlin97 1:3258d62af038 44 // calculate the average:
ryanlin97 1:3258d62af038 45 average = total / boxcarWidth;
ryanlin97 1:3258d62af038 46 // send it to the computer as ASCII digits
ryanlin97 1:3258d62af038 47 return average;
ryanlin97 1:3258d62af038 48 }
ryanlin97 1:3258d62af038 49 float complement(float x, float y , float ratio)
ryanlin97 1:3258d62af038 50 {
ryanlin97 1:3258d62af038 51 return (ratio*x + (1 - ratio)*y);
ryanlin97 1:3258d62af038 52 }