File content as of revision 8:981f7e2365b6:

// Coded by: Erik van de Coevering

#include "mbed.h"
#include "MAfilter.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;
float KPx, KPy, KIx, KIy, KDx, KDy, KPz, KIz, KDz, error_x1, error_x, error_y1, error_y;
MAfilter10 ma_errorx, ma_errory;
static signed int m1 = 0;
static signed int m2 = 0;
static signed int m3 = 0;
static signed int m4 = 0;

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 bool armed = false;
    float xcntrl = 0;
    float ycntrl = 0;
    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: 2.7, I: 2.0, D: 0.4
    // Calc PID values and prevent integral windup on KIx
    error_x = angles[0] - xcntrl;
    KPx = error_x * KP_y;
    KIx = KIx + (error_x * 0.001f * KI_y); // PID gains mixed up, will fix soon.

    if(KIx > 10) KIx = 10;
    if(KIx < -10) KIx = -10;

    KDx = (angles[3] + (error_x)) * KD_y; // Should be a derative of the error, but this way feels much more responsive

    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
    error_y = angles[1] + ycntrl;
    KPy = error_y * KP_x;
    KIy = KIy + (error_y * 0.001f * KI_x);

    if(KIy > 10) KIy = 10;
    if(KIy < -10) KIy = -10;
    KDy = (angles[4] + (error_y)) * 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 > 20) KIz = 20;
    if(KIz < -20) KIz = -20;

    KDz = (error_z - error_z1) * KD_z;

    error_z1 = error_z;

    zcomp = (KPz + 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;
}