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.
calccomp.h
- Committer:
- Anaesthetix
- Date:
- 2018-07-14
- Revision:
- 6:033ad7377fee
- Parent:
- 5:0f4204941604
- Child:
- 8:981f7e2365b6
File content as of revision 6:033ad7377fee:
// 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_x, KI_x, KD_x, KP_y, KI_y, KD_y, KP_z, KI_z, KD_z; 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 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; //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; // 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_y; KIx = KIx + ((angles[0] - xcntrl) * KI_y * 0.001); if(KIx > 50) KIx = 50; if(KIx < -50) KIx = -50; KDx = (angles[3] + (angles[0] - xcntrl)) * KD_y; xcomp = KPx + KIx + KDx; xcomp2 = xcomp*-1; //Mix pitch m1 = m1 - xcomp2; m2 = m2 + xcomp2; m3 = m3 + xcomp2; 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); if(KIy > 50) KIy = 50; if(KIy < -50) KIy = -50; KDy = (angles[4] + (angles[1] + ycntrl)) * KD_x; ycomp = KPy + KIy + KDy; ycomp = ycomp*-1; //Mix roll m1 = m1 + ycomp; m2 = m2 + ycomp; m3 = m3 - ycomp; m4 = m4 - ycomp; //Calc yaw compensation and mix error_z = angles[5] + rud; KPz = error_z * KP_z; KIz = KIz + (error_z * 0.001f * KI_z); if(KIz > 80) KIz = 80; if(KIz < -80) KIz = -80; KDz = (error_z - error_z1) * KD_z; error_z1 = error_z; zcomp = (KPz + KIz + KDz) * -1.0f; //Mix yaw m1 = m1 - zcomp; m2 = m2 + zcomp; m3 = m3 - zcomp; m4 = m4 + zcomp; //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; }