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:
- 3:a49ab9168fb2
- Parent:
- 2:61ad38ea550d
- Child:
- 4:fab65ad01ab4
--- a/calccomp.h Mon Jul 09 19:18:20 2018 +0000 +++ b/calccomp.h Tue Jul 10 13:19:53 2018 +0000 @@ -6,29 +6,30 @@ 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 { - //Rx variables - int ruddercenter = 1562; - int elevcenter = 1554; - int aileroncenter = 1550; - - //Variables for calccomp - float xcomp = 0; - float ycomp = 0; - int xcomp2 = 0; + //Rx variables + int ruddercenter = 1562; + int elevcenter = 1554; + int aileroncenter = 1550; + + //Variables for calccomp + float xcomp = 0; + float ycomp = 0; + int xcomp2 = 0; // int ycomp2 = 0; - float ruddercomp = 0; + float ruddercomp = 0; // float ratio = 0.3; - 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; - + 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; + //Scale rx channels rud = (((float)(ctrl[0] - ruddercenter))/2.5); ycntrl = ((float)(ctrl[3] - elevcenter))/8; @@ -43,45 +44,47 @@ m2 = throttle; m3 = throttle; m4 = throttle; - /* - // Calc PID values and prevent integral windup on KIx - KPx = (angles[0] + xcntrl) * Kp; - KIx = KIx + ((angles[0] + xcntrl) * Kd); - - if(KIx > 30) KIx = 30; - if(KIx < -30) KIx = -30; - KDx = (angles[3] + (angles[0] + xcntrl)) Kd; - - xcomp = KPx + KIx + KDx; - xcomp2 = xcomp*-1; - */ + + // Current values used on a 250 size mini racer (still needs tuning): P: 1.55, I: 2.4, D: 0.4 + // Calc PID values and prevent integral windup on KIx + KPx = (angles[0] - xcntrl) * Kp; + KIx = KIx + ((angles[0] - xcntrl) * Ki * 0.001); + + if(KIx > 30) KIx = 30; + if(KIx < -30) KIx = -30; + KDx = (angles[3] + (angles[0] - xcntrl)) * Kd; + + xcomp = KPx + KIx + KDx; + xcomp2 = xcomp*-1; + /* //Calc aileron compensation and mix xcomp = ((((angles[0])-xcntrl)*Kp) + ((angles[3]+((angles[0]-xcntrl)*1.0f))*Kd)); xcomp2 = (xcomp*-1); - // xcomp2 = 0; - + // xcomp2 = 0; + */ //Mix aileron m1 = m1 - xcomp2; m2 = m2 + xcomp2; m3 = m3 + xcomp2; m4 = m4 - xcomp2; - /* - // Calc PID values and prevent integral windup on KIx - KPy = (angles[1] + ycntrl) * Kd; - KIy = KIx + ((angles[1] + ycntrl) * Ki); - - if(KIy > 30) KIy = 30; - if(KIy < -30) KIy = -30; - KDx = (angles[4] + (angles[1] + ycntrl)) * Kd; - - ycomp = KPy + KIy + KDy; - */ + + // Calc PID values and prevent integral windup on KIx + KPy = (angles[1] + ycntrl) * Kp; + KIy = KIy + ((angles[1] + ycntrl) * Ki * 0.001); + + if(KIy > 30) KIy = 30; + if(KIy < -30) KIy = -30; + KDy = (angles[4] + (angles[1] + ycntrl)) * Kd; + + ycomp = KPy + KIy + KDy; + ycomp = ycomp*-1; + /* //ycntrl = ycntrl+5; //Calc elevator compensation and mix ycomp = ((((angles[1])+(ycntrl))*Kp) + ((angles[4]+((angles[1]+ycntrl)*1.0f))*Kd)); ycomp = ycomp*-1; - // ycomp2 = 0; - + // ycomp2 = 0; + */ //Mix elevator m1 = m1 + ycomp; m2 = m2 + ycomp; @@ -92,7 +95,7 @@ //Calc rudder compensation and mix ruddercomp = (rud + (angles[5]*1.0f)); //has drift zcomp = ruddercomp*-1; - //zcomp = 0; + //zcomp = 0; //Mix rudder m1 = m1 - zcomp; @@ -106,7 +109,7 @@ // if(m3 < 1075 && throttle > 1000) m3 = 1280; // if(m4 < 1075 && throttle > 1000) m4 = 1280; - //When throttle down or if not armed, stop motors + //When throttle down or if not armed, stop motors if(throttle < 1100 || armed == false) { m1=1000; m2=1000; @@ -125,10 +128,10 @@ } - + //Output to motors - motor[0] = m1; - motor[1] = m2; - motor[2] = m3; - motor[3] = m4; + motor[0] = m1; + motor[1] = m2; + motor[2] = m3; + motor[3] = m4; }