Erik van de Coevering / Mbed 2 deprecated Multicopter_2018

Latest version of my quadcopter controller with an LPC1768 and MPU9250.

Dependencies:   mbed

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@8:981f7e2365b6, 2018-07-31 (annotated)

Committer:
Anaesthetix
Date:
Tue Jul 31 20:36:57 2018 +0000
Revision:
8:981f7e2365b6
Parent:
7:d86c41443f6d 
Switched from Madgwick to Mahony as I'm having trouble with slow oscillations caused by the madgwick filter. Fixed an error on the PID algorithm also.

Who changed what in which revision?

UserRevisionLine numberNew 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 }