File content as of revision 1:ac68f0368a77:

#ifndef GYRO_H
#define GYRO_H
#include "IOMacros.h"
ITG3200 gyro(p9, p10);
DigitalOut looptime2(p18);

SPI spi(p5,p6,p7); 
BMA180 acc1(spi, p8);

Timer dtgyro;
#define PI 3.141592654


int xg= 0;
int yg= 0;
int zg= 0;
float temphoekx=0;
float temp6=0;
float newax = 0;
float neway = 0;
float newax1 = 0;
float neway1 = 0;
float tempx1 = 0;
float tempy1 = 0;
int flag = 0;
float xag = 0;
float xarg = 0;
float yarg = 0;
float zarg = 0;
float yag = 0;
float xagoffset = 0;
float yagoffset = 0;
float zagoffset = -0.75;
float axoffset = 0;
float ayoffset = 0;
float xangle[40] = {0};
float yangle[40] = {0};
float x1 = 0;
float x2 = 0;
float y1 = 0;
float y2 = 0;
float ax1 = 0;
float ay1 = 0;
float ax2 = 0;
  float ax = 0;
  float ay = 0;    
float ay2 = 0;
int dtgyrtemp;
float dtgyr;

void gyrostart(void);
void gyrocal(void);
void gyrosample(void);

void gyrostart(void) {
    gyro.setLpBandwidth(LPFBW_42HZ);
}


void gyrocal(void) {
    for (int i=0; i<100; i++) {
        gyrosample();
        xagoffset += (0.01*xag);
        yagoffset += (0.01*yag);
        //  zagoffset += (0.01*zarg);

    }
}


void gyrosample(void) {
    //Timer for filtervar
    dtgyro.stop();
    dtgyrtemp = dtgyro.read_us();
    dtgyr = dtgyrtemp;
    dtgyro.reset();
    dtgyro.start();
    
    //Get gyro & accelero data
    __disable_irq(); //For some reason deadlocks without this
    xg = gyro.getGyroX();
    yg = gyro.getGyroY();
    zg = gyro.getGyroZ();
    __enable_irq();
    //accsample();
    acc1.readRaw();
    
    //Cast int to float for better precision
    xarg = xg;
    yarg = yg;
    zarg = zg;

    //Calc angular rate in degrees/second
    xarg = (xarg/14.375);
    yarg = (yarg/14.375);
    zarg = (zarg/14.375);

    zarg -= zagoffset;
    xag = xarg;
    yag = yarg;

    y1 = acc1.y;
    y2 = acc1.y;
    x1 = acc1.x;
    x2 = acc1.x;



    //Calc y angle
    y1 = y1*y1;
    y1 = 1-y1;
    y1 = sqrt(y1);
    y1 = y2/y1;

    ay = atan(y1);
    ay = ay*(180/PI);

    //Calc x angle
    x1 = x1*x1;
    x1 = 1-x1;
    x1 = sqrt(x1);
    x1 = x2/x1;

    ax = atan(x1);
    ax = ax*(180/PI);


    //if [angle] is NAN use previous value
    if (ax != ax) ax = ax2;
    if (ay != ay) ay = ay2;
    if (ax == ax) ax2 = ax;
    if (ay == ay) ay2 = ay;
    
    
    //Moving avarage filter
    xangle[0] = (ax + 6);
    ax1 = ((0.025*xangle[0]) + (0.025*xangle[1]) + (0.025*xangle[2]) + (0.025*xangle[3]) + (0.025*xangle[4]) + (0.025*xangle[5]) + (0.025*xangle[6]) + (0.025*xangle[7]) + (0.025*xangle[8]) + (0.025*xangle[9]) + (0.025*xangle[10]) + (0.025*xangle[11]) + (0.025*xangle[12]) + (0.025*xangle[13]) + (0.025*xangle[14]) + (0.025*xangle[15]) + (0.025*xangle[16]) + (0.025*xangle[17]) + (0.025*xangle[18]) + (0.025*xangle[19]) + (0.025*xangle[20]) + (0.025*xangle[21]) + (0.025*xangle[22]) + (0.025*xangle[23]) + (0.025*xangle[24]) + (0.025*xangle[25]) + (0.025*xangle[26]) + (0.025*xangle[27]) + (0.025*xangle[28]) + (0.025*xangle[29]) + (0.025*xangle[30]) + (0.025*xangle[31]) + (0.025*xangle[32]) + (0.025*xangle[33]) + (0.025*xangle[34]) + (0.025*xangle[35]) + (0.025*xangle[36]) + (0.025*xangle[37]) + (0.025*xangle[38]) + (0.025*xangle[39]));
    xangle[39] = xangle[38]; 
    xangle[38] = xangle[37];
    xangle[37] = xangle[36];
    xangle[36] = xangle[35];
    xangle[35] = xangle[34];
    xangle[34] = xangle[33];
    xangle[33] = xangle[32];
    xangle[32] = xangle[31];
    xangle[31] = xangle[30];
    xangle[30] = xangle[29]; 
    xangle[29] = xangle[28]; 
    xangle[28] = xangle[27];
    xangle[27] = xangle[26];
    xangle[26] = xangle[25];
    xangle[25] = xangle[24];
    xangle[24] = xangle[23];
    xangle[23] = xangle[22];
    xangle[22] = xangle[21];
    xangle[21] = xangle[20];
    xangle[20] = xangle[19]; 
    xangle[19] = xangle[18];
    xangle[18] = xangle[17];
    xangle[17] = xangle[16];
    xangle[16] = xangle[15];
    xangle[15] = xangle[14];
    xangle[14] = xangle[13];
    xangle[13] = xangle[12];
    xangle[12] = xangle[11];
    xangle[11] = xangle[10];
    xangle[10] = xangle[9]; 
    xangle[9] = xangle[8];
    xangle[8] = xangle[7];
    xangle[7] = xangle[6];
    xangle[6] = xangle[5];
    xangle[5] = xangle[4];
    xangle[4] = xangle[3];
    xangle[3] = xangle[2];
    xangle[2] = xangle[1];
    xangle[1] = xangle[0];

    
    yangle[0] = ay;
    ay1 = ((0.025*yangle[0]) + (0.025*yangle[1]) + (0.025*yangle[2]) + (0.025*yangle[3]) + (0.025*yangle[4]) + (0.025*yangle[5]) + (0.025*yangle[6]) + (0.025*yangle[7]) + (0.025*yangle[8]) + (0.025*yangle[9]) + (0.025*yangle[10]) + (0.025*yangle[11]) + (0.025*yangle[12]) + (0.025*yangle[13]) + (0.025*yangle[14]) + (0.025*yangle[15]) + (0.025*yangle[16]) + (0.025*yangle[17]) + (0.025*yangle[18]) + (0.025*yangle[19]) + (0.025*yangle[20]) + (0.025*yangle[21]) + (0.025*yangle[22]) + (0.025*yangle[23]) + (0.025*yangle[24]) + (0.025*yangle[25]) + (0.025*yangle[26]) + (0.025*yangle[27]) + (0.025*yangle[28]) + (0.025*yangle[29]) + (0.025*yangle[30]) + (0.025*yangle[31]) + (0.025*yangle[32]) + (0.025*yangle[33]) + (0.025*yangle[34]) + (0.025*yangle[35]) + (0.025*yangle[36]) + (0.025*yangle[37]) + (0.025*yangle[38]) + (0.025*yangle[39]));
    yangle[39] = yangle[38]; 
    yangle[38] = yangle[37];
    yangle[37] = yangle[36];
    yangle[36] = yangle[35];
    yangle[35] = yangle[34];
    yangle[34] = yangle[33];
    yangle[33] = yangle[32];
    yangle[32] = yangle[31];
    yangle[31] = yangle[30];
    yangle[30] = yangle[29]; 
    yangle[29] = yangle[28]; 
    yangle[28] = yangle[27];
    yangle[27] = yangle[26];
    yangle[26] = yangle[25];
    yangle[25] = yangle[24];
    yangle[24] = yangle[23];
    yangle[23] = yangle[22];
    yangle[22] = yangle[21];
    yangle[21] = yangle[20];
    yangle[20] = yangle[19]; 
    yangle[19] = yangle[18];
    yangle[18] = yangle[17];
    yangle[17] = yangle[16];
    yangle[16] = yangle[15];
    yangle[15] = yangle[14];
    yangle[14] = yangle[13];
    yangle[13] = yangle[12];
    yangle[12] = yangle[11];
    yangle[11] = yangle[10];
    yangle[10] = yangle[9]; 
    yangle[9] = yangle[8];
    yangle[8] = yangle[7];
    yangle[7] = yangle[6];
    yangle[6] = yangle[5];
    yangle[5] = yangle[4];
    yangle[4] = yangle[3];
    yangle[3] = yangle[2];
    yangle[2] = yangle[1];
    yangle[1] = yangle[0];



    
    //Substract angular rate offsets
    xag += xagoffset;
    yag += yagoffset;
 //   xag = (xag*-1);
    yag = (yag*-1);
    

    //calc filtervar
    temp6 = 5000000;                      //time constant [us]
    temp6 = temp6/(temp6+dtgyr);
    

    //complementary filter
    newax = (temp6*(newax + (yag*(dtgyr/1000000)))) + ((1-temp6)*ax1);
    neway = (temp6*(neway + (xag*(dtgyr/1000000)))) + ((1-temp6)*ay1);
 //   neway = neway*-1;

    //if [angle] is NAN use previous value
    if (newax != newax) newax = tempx1;
    if (neway != neway) neway = tempy1;
    if (newax == newax) tempx1 = newax;
    if (neway == neway) tempy1 = neway;
}



#endif