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:
- 5:0f4204941604
- Parent:
- 4:fab65ad01ab4
- Child:
- 6:033ad7377fee
--- a/calccomp.h Thu Jul 12 13:53:55 2018 +0000 +++ b/calccomp.h Fri Jul 13 14:27:30 2018 +0000 @@ -1,10 +1,12 @@ -// Coded by Erik van de Coevering +// Coded by: Erik van de Coevering + +// Axis are mixed up, will fix soon. #include "mbed.h" DigitalOut led1(LED1); // for stick arming (leds are active low) +extern float Kp, Ki, Kd; -extern float Kp, Ki, Kd; 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 { @@ -28,7 +30,9 @@ static bool armed = false; float xcntrl = 0; float ycntrl = 0; - float KPx, KPy, KIx, KIy, KDx, KDy; + float KPx, KPy, KIx, KIy, KDx, KDy, KPz, KIz, KDz; + float error_z = 0; + float error_z1 = 0; //Scale rx channels rud = (((float)(ctrl[0] - ruddercenter))/2.5); @@ -45,7 +49,7 @@ m3 = throttle; m4 = throttle; - // Current values used on a 250 size mini racer (still needs tuning): P: 1.45, I: 1.5, D: 0.34 + // Current values used on a 250 size mini racer (still needs tuning): P: 1.9, I: 2.4, D: 0.45 // Calc PID values and prevent integral windup on KIx KPx = (angles[0] - xcntrl) * Kp; KIx = KIx + ((angles[0] - xcntrl) * Ki * 0.001); @@ -64,12 +68,12 @@ m4 = m4 - xcomp2; // Calc PID values and prevent integral windup on KIy - KPy = (angles[1] + ycntrl) * Kp; - KIy = KIy + ((angles[1] + ycntrl) * Ki * 0.001); + KPy = (angles[1] + ycntrl) * Kp * 0.8f; + KIy = KIy + ((angles[1] + ycntrl) * Ki * 0.8f * 0.001); if(KIy > 50) KIy = 50; if(KIy < -50) KIy = -50; - KDy = (angles[4] + (angles[1] + ycntrl)) * Kd; + KDy = (angles[4] + (angles[1] + ycntrl)) * Kd * 0.8f; ycomp = KPy + KIy + KDy; ycomp = ycomp*-1; @@ -81,10 +85,19 @@ m4 = m4 - ycomp; - //Calc rudder compensation and mix - ruddercomp = (rud + (angles[5]*1.0f)); //has drift - zcomp = ruddercomp*-1; - //zcomp = 0; + //Yaw PID gains hardcoded for now + error_z = angles[5] + rud; + + KPz = error_z * 1.5f; + KIz = KIz + (error_z * 0.001f); + + if(KIz > 50) KIz = 50; + if(KIz < -50) KIz = -50; + + KDz = (error_z - error_z1) * 0.3f; + error_z1 = error_z; + + zcomp = (KPz + KIz + KDz) * -1.0f; //Mix rudder m1 = m1 - zcomp;