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