Latest version of my quadcopter controller with an LPC1768 and MPU9250.
Currently running on a custom PCB with 30.5 x 30.5mm mounts. There are also 2 PC apps that go with the software; one to set up the PID controller and one to balance the motors and props. If anyone is interested, send me a message and I'll upload them.
Filters/LPfilter.cpp@7:d86c41443f6d, 2018-07-17 (annotated)
- Committer:
- Anaesthetix
- Date:
- Tue Jul 17 14:56:05 2018 +0000
- Revision:
- 7:d86c41443f6d
- Parent:
- 4:fab65ad01ab4
Added 8th order low-pass IIR filters
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Anaesthetix | 4:fab65ad01ab4 | 1 | // Coded by Erik van de Coevering |
Anaesthetix | 7:d86c41443f6d | 2 | // DSP-style low-pass IIR filters designed with Matlab's FDAtool. |
Anaesthetix | 4:fab65ad01ab4 | 3 | // For information on how to implement these filters, check this pdf: http://spinlab.wpi.edu/courses/ece4703_2009/how_to_interpret_matlab_dfii_sos.pdf |
Anaesthetix | 4:fab65ad01ab4 | 4 | |
Anaesthetix | 0:0929d3d566cf | 5 | #include "LPfilter.h" |
Anaesthetix | 7:d86c41443f6d | 6 | /* |
Anaesthetix | 0:0929d3d566cf | 7 | float LPfilter::run(float input) { |
Anaesthetix | 0:0929d3d566cf | 8 | filterbuffer[2] = filterbuffer[1]; |
Anaesthetix | 0:0929d3d566cf | 9 | filterbuffer[1] = filterbuffer[0]; |
Anaesthetix | 0:0929d3d566cf | 10 | filterbuffer[0] = input*LP_A + filterbuffer[1]*LP_B + filterbuffer[2]*LP_C; |
Anaesthetix | 0:0929d3d566cf | 11 | |
Anaesthetix | 0:0929d3d566cf | 12 | out = filterbuffer[0]*LP_D + filterbuffer[1]*LP_E + filterbuffer[2]*LP_F; |
Anaesthetix | 0:0929d3d566cf | 13 | return out; |
Anaesthetix | 0:0929d3d566cf | 14 | } |
Anaesthetix | 0:0929d3d566cf | 15 | |
Anaesthetix | 0:0929d3d566cf | 16 | float LPfilter2::run(float input) { |
Anaesthetix | 0:0929d3d566cf | 17 | filterbuffer1[2] = filterbuffer1[1]; |
Anaesthetix | 0:0929d3d566cf | 18 | filterbuffer1[1] = filterbuffer1[0]; |
Anaesthetix | 0:0929d3d566cf | 19 | filterbuffer1[0] = input*0.003762202981699 + filterbuffer1[1]*1.893415601023 + filterbuffer1[2]*-0.9084644129493; |
Anaesthetix | 0:0929d3d566cf | 20 | |
Anaesthetix | 0:0929d3d566cf | 21 | out_temp = filterbuffer1[0] + filterbuffer1[1]*2.0f + filterbuffer1[2]; |
Anaesthetix | 0:0929d3d566cf | 22 | |
Anaesthetix | 0:0929d3d566cf | 23 | filterbuffer2[2] = filterbuffer2[1]; |
Anaesthetix | 0:0929d3d566cf | 24 | filterbuffer2[1] = filterbuffer2[0]; |
Anaesthetix | 0:0929d3d566cf | 25 | filterbuffer2[0] = out_temp*0.003533495923378 + filterbuffer2[1]*1.778313488139 + filterbuffer2[2]*-0.7924474718329; |
Anaesthetix | 0:0929d3d566cf | 26 | |
Anaesthetix | 0:0929d3d566cf | 27 | out = filterbuffer2[0] + filterbuffer2[1]*2.0f + filterbuffer2[2]; |
Anaesthetix | 0:0929d3d566cf | 28 | return out; |
Anaesthetix | 0:0929d3d566cf | 29 | } |
Anaesthetix | 0:0929d3d566cf | 30 | |
Anaesthetix | 0:0929d3d566cf | 31 | float LPfilter2_1::run(float input) { |
Anaesthetix | 0:0929d3d566cf | 32 | filterbuffer1[2] = filterbuffer1[1]; |
Anaesthetix | 0:0929d3d566cf | 33 | filterbuffer1[1] = filterbuffer1[0]; |
Anaesthetix | 0:0929d3d566cf | 34 | filterbuffer1[0] = input*0.007954132308248 + filterbuffer1[1]*1.836787188551 + filterbuffer1[2]*-0.8686037177835; |
Anaesthetix | 0:0929d3d566cf | 35 | |
Anaesthetix | 0:0929d3d566cf | 36 | out_temp = filterbuffer1[0] + filterbuffer1[1]*2.0f + filterbuffer1[2]; |
Anaesthetix | 0:0929d3d566cf | 37 | |
Anaesthetix | 0:0929d3d566cf | 38 | filterbuffer2[2] = filterbuffer2[1]; |
Anaesthetix | 0:0929d3d566cf | 39 | filterbuffer2[1] = filterbuffer2[0]; |
Anaesthetix | 0:0929d3d566cf | 40 | filterbuffer2[0] = out_temp*0.007277930990548 + filterbuffer2[1]*1.680637168775 + filterbuffer2[2]*-0.7097488927369; |
Anaesthetix | 0:0929d3d566cf | 41 | |
Anaesthetix | 0:0929d3d566cf | 42 | out = filterbuffer2[0] + filterbuffer2[1]*2.0f + filterbuffer2[2]; |
Anaesthetix | 0:0929d3d566cf | 43 | return out; |
Anaesthetix | 0:0929d3d566cf | 44 | } |
Anaesthetix | 7:d86c41443f6d | 45 | */ |
Anaesthetix | 7:d86c41443f6d | 46 | //8th order IIR filter, with Fs = 1500Hz and Fc = 45Hz |
Anaesthetix | 0:0929d3d566cf | 47 | |
Anaesthetix | 7:d86c41443f6d | 48 | float LPfilter8::run(float input) { |
Anaesthetix | 7:d86c41443f6d | 49 | filterbuffer1[2] = filterbuffer1[1]; |
Anaesthetix | 7:d86c41443f6d | 50 | filterbuffer1[1] = filterbuffer1[0]; |
Anaesthetix | 7:d86c41443f6d | 51 | filterbuffer1[0] = input*0.008544036438708 + filterbuffer1[1]*1.895289756537 + filterbuffer1[2]*-0.9294659022914; |
Anaesthetix | 7:d86c41443f6d | 52 | |
Anaesthetix | 7:d86c41443f6d | 53 | out_temp1 = filterbuffer1[0] + filterbuffer1[1]*2.0f + filterbuffer1[2]; |
Anaesthetix | 7:d86c41443f6d | 54 | |
Anaesthetix | 7:d86c41443f6d | 55 | filterbuffer2[2] = filterbuffer2[1]; |
Anaesthetix | 7:d86c41443f6d | 56 | filterbuffer2[1] = filterbuffer2[0]; |
Anaesthetix | 7:d86c41443f6d | 57 | filterbuffer2[0] = out_temp1*0.008021326615853 + filterbuffer2[1]*1.779339107215 + filterbuffer2[2]*-0.811424413678; |
Anaesthetix | 7:d86c41443f6d | 58 | |
Anaesthetix | 7:d86c41443f6d | 59 | out_temp2 = filterbuffer2[0] + filterbuffer2[1]*2.0f + filterbuffer2[2]; |
Anaesthetix | 7:d86c41443f6d | 60 | |
Anaesthetix | 7:d86c41443f6d | 61 | filterbuffer3[2] = filterbuffer3[1]; |
Anaesthetix | 7:d86c41443f6d | 62 | filterbuffer3[1] = filterbuffer3[0]; |
Anaesthetix | 7:d86c41443f6d | 63 | filterbuffer3[0] = out_temp2*0.007662537043122 + filterbuffer3[1]*1.699750237618 + filterbuffer3[2]*-0.7304003857905; |
Anaesthetix | 7:d86c41443f6d | 64 | |
Anaesthetix | 7:d86c41443f6d | 65 | out_temp3 = filterbuffer3[0] + filterbuffer3[1]*2.0f + filterbuffer3[2]; |
Anaesthetix | 7:d86c41443f6d | 66 | |
Anaesthetix | 7:d86c41443f6d | 67 | filterbuffer4[2] = filterbuffer4[1]; |
Anaesthetix | 7:d86c41443f6d | 68 | filterbuffer4[1] = filterbuffer4[0]; |
Anaesthetix | 7:d86c41443f6d | 69 | filterbuffer4[0] = out_temp3*0.007481430998498 + filterbuffer4[1]*1.659576201179 + filterbuffer4[2]*-0.6895019251732; |
Anaesthetix | 7:d86c41443f6d | 70 | |
Anaesthetix | 7:d86c41443f6d | 71 | out = filterbuffer4[0] + filterbuffer4[1]*2.0f + filterbuffer4[2]; |
Anaesthetix | 7:d86c41443f6d | 72 | |
Anaesthetix | 7:d86c41443f6d | 73 | return out; |
Anaesthetix | 7:d86c41443f6d | 74 | } |