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.
Diff: calccomp.h
- Revision:
- 8:981f7e2365b6
- Parent:
- 6:033ad7377fee
--- a/calccomp.h Tue Jul 17 14:56:05 2018 +0000 +++ b/calccomp.h Tue Jul 31 20:36:57 2018 +0000 @@ -1,11 +1,17 @@ // Coded by: Erik van de Coevering -// Axis are mixed up, will fix soon. #include "mbed.h" +#include "MAfilter.h" DigitalOut led1(LED1); // for stick arming (leds are active low) extern float KP_x, KI_x, KD_x, KP_y, KI_y, KD_y, KP_z, KI_z, KD_z; +float KPx, KPy, KIx, KIy, KDx, KDy, KPz, KIz, KDz, error_x1, error_x, error_y1, error_y; +MAfilter10 ma_errorx, ma_errory; +static signed int m1 = 0; +static signed int m2 = 0; +static signed int m3 = 0; +static signed int m4 = 0; void calccomp(int* ctrl, float* angles, int* motor) //ctrl: 0-rud 1-elev 2-throttle 3-aileron angles: 0-ax 1-ay 2-az 3-gx 4-gy 5-gz { @@ -21,14 +27,9 @@ int rud = 0; int zcomp = 0; int throttle = 0; - static signed int m1 = 0; - static signed int m2 = 0; - static signed int m3 = 0; - static signed int m4 = 0; static bool armed = false; float xcntrl = 0; float ycntrl = 0; - float KPx, KPy, KIx, KIy, KDx, KDy, KPz, KIz, KDz; float error_z = 0; float error_z1 = 0; @@ -47,14 +48,16 @@ m3 = throttle; m4 = throttle; - // Current values used on a 250 size mini racer (still needs tuning): P: 1.9, I: 2.4, D: 0.45 + // Current values used on a 250 size mini racer (still needs tuning): P: 2.7, I: 2.0, D: 0.4 // Calc PID values and prevent integral windup on KIx - KPx = (angles[0] - xcntrl) * KP_y; - KIx = KIx + ((angles[0] - xcntrl) * KI_y * 0.001); + error_x = angles[0] - xcntrl; + KPx = error_x * KP_y; + KIx = KIx + (error_x * 0.001f * KI_y); // PID gains mixed up, will fix soon. - if(KIx > 50) KIx = 50; - if(KIx < -50) KIx = -50; - KDx = (angles[3] + (angles[0] - xcntrl)) * KD_y; + if(KIx > 10) KIx = 10; + if(KIx < -10) KIx = -10; + + KDx = (angles[3] + (error_x)) * KD_y; // Should be a derative of the error, but this way feels much more responsive xcomp = KPx + KIx + KDx; xcomp2 = xcomp*-1; @@ -66,12 +69,13 @@ m4 = m4 - xcomp2; // Calc PID values and prevent integral windup on KIy - KPy = (angles[1] + ycntrl) * KP_x; - KIy = KIy + ((angles[1] + ycntrl) * KI_x * 0.001); + error_y = angles[1] + ycntrl; + KPy = error_y * KP_x; + KIy = KIy + (error_y * 0.001f * KI_x); - if(KIy > 50) KIy = 50; - if(KIy < -50) KIy = -50; - KDy = (angles[4] + (angles[1] + ycntrl)) * KD_x; + if(KIy > 10) KIy = 10; + if(KIy < -10) KIy = -10; + KDy = (angles[4] + (error_y)) * KD_x; ycomp = KPy + KIy + KDy; ycomp = ycomp*-1; @@ -89,13 +93,14 @@ KPz = error_z * KP_z; KIz = KIz + (error_z * 0.001f * KI_z); - if(KIz > 80) KIz = 80; - if(KIz < -80) KIz = -80; + if(KIz > 20) KIz = 20; + if(KIz < -20) KIz = -20; KDz = (error_z - error_z1) * KD_z; + error_z1 = error_z; - zcomp = (KPz + KIz + KDz) * -1.0f; + zcomp = (KPz + KDz) * -1.0f; //Mix yaw m1 = m1 - zcomp; @@ -128,4 +133,4 @@ motor[1] = m2; motor[2] = m3; motor[3] = m4; -} \ No newline at end of file +}