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:
- 0:0929d3d566cf
- Child:
- 2:61ad38ea550d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/calccomp.h Mon Jul 09 16:31:40 2018 +0000 @@ -0,0 +1,139 @@ +#include "mbed.h" + +DigitalOut led1(LED1); // for stick arming (leds are active low) + +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 +{ + //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 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; + +// float KPx, KDx, KPy, KDy; +// static float KIx = 0; +// static float KIy = 0; + + //Scale rx channels + rud = (((float)(ctrl[0] - ruddercenter))/2.5); + ycntrl = ((float)(ctrl[3] - elevcenter))/8; + throttle = ctrl[2] - 20; + xcntrl = ((float)(ctrl[1] - aileroncenter))/8; + + //Limit throttle + if(throttle > 1950) throttle = 1950; + + //Start by mixing throttle + m1 = throttle; + m2 = throttle; + m3 = throttle; + m4 = throttle; + /* + // Calc PID values and prevent integral windup on KIx + KPx = (angles[0] + xcntrl) * 1.05f; + KIx = KIx + ((angles[0] + xcntrl) * 0.001f); + + if(KIx > 30) KIx = 30; + if(KIx < -30) KIx = -30; + KDx = (angles[3] + (angles[0] + xcntrl)*0.8f) * 0.45f; + + 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; + + //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) * 1.05f; + KIy = KIx + ((angles[1] + ycntrl) * 0.001f); + + if(KIy > 30) KIy = 30; + if(KIy < -30) KIy = -30; + KDx = (angles[4] + (angles[1] + ycntrl)*0.8f) * 0.45f; + + ycomp = KPy + KIy + KDy; + */ + //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; + + //Mix elevator + m1 = m1 + ycomp; + m2 = m2 + ycomp; + m3 = m3 - ycomp; + m4 = m4 - ycomp; + + + //Calc rudder compensation and mix + ruddercomp = (rud + (angles[5]*1.0f)); //has drift + zcomp = ruddercomp*-1; + //zcomp = 0; + + //Mix rudder + m1 = m1 - zcomp; + m2 = m2 + zcomp; + m3 = m3 - zcomp; + m4 = m4 + zcomp; + + //Prevent motors from stalling +// if(m1 < 1075 && throttle > 1000) m1 = 1280; +// if(m2 < 1075 && throttle > 1000) m2 = 1280; +// if(m3 < 1075 && throttle > 1000) m3 = 1280; +// if(m4 < 1075 && throttle > 1000) m4 = 1280; + + //When throttle down or if not armed, stop motors + if(throttle < 1100 || armed == false) { + m1=1000; + m2=1000; + m3=1000; + m4=1000; + } + + //Stick arming + if(throttle < 1100 && rud > 100) { + armed = true; + led1 = 0; + } + if(throttle < 1100 && rud < -100) { + armed = false; + led1 = 1; + } + + + + //Output to motors + motor[0] = m1; + motor[1] = m2; + motor[2] = m3; + motor[3] = m4; +} +