File content as of revision 2:61ad38ea550d:

#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;
			
    //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) * 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;
	*/
    //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) * 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;
	*/
    //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;
}