File content as of revision 4:adc885f4ab75:

 #include "gyroReader.h"
 //#include "math.h"
 
 void gyroReader::gyroUpkeep(){
    //testPin=1;
    int xdiff,ydiff,zdiff;
    checkGyro(xdiff,ydiff,zdiff);
    checkCompass();
    gyroZcount+=zdiff+zcal;
    gyroXcount+=xdiff+xcal;
    polls++;
    //testPin=0;
}
    
int gyroReader::startAccel(){
    char cmd[4];
    char addr=0x32;//0b0011000;
    char gyroaddr=0xD6;
    char magaddr=0x3C;
    cmd[0]=0x20;
    cmd[1]=0x2f;//0b00101111;
    cmd[2]=0;
    cmd[3]=0;
    //printf("%d\r\n",accel.write(addr,cmd,2));
    cmd[1]=0x0f;
    printf("%d\r\n",accel.write(gyroaddr,cmd,2));
    printf("%X,%X,%X,%X\r\n",cmd[0],cmd[1],cmd[2],cmd[3]);
    cmd[0]=0x00;
    cmd[1]=0x04;//75 samples/sec
    cmd[2]=0xE0;//largest possible range (8.1 gauss)
    cmd[3]=0x00;//continuous mode
    //pc.printf("%d\r\n",accel.write(magaddr,cmd,2));
    DBGPRINT("%d\r\n",accel.write(magaddr,cmd,4));
    return 1;
}

float gyroReader::checkCompass(){
    char com[1];
    char cmd[6]={0,0,0,0,0,0};
    char addr=0x3C;//0b0011110;
    com[0]=0x03|0x80;
    accel.write(addr, com, 1,true);
    accel.read(addr, cmd, 6);
    xmag=(signed short)((((unsigned char)cmd[1])<<8)+(unsigned char)cmd[0]);
    ymag=(signed short)((((unsigned char)cmd[3])<<8)+(unsigned char)cmd[2]);
    zmag=(signed short)((((unsigned char)cmd[5])<<8)+(unsigned char)cmd[4]);
    //DBGPRINT("%d\t\t%d\t\t%d\r\n",xmag,ymag,zmag);
    return 0;//atan2((double)xacc,(double)yacc);
}

float gyroReader::checkAccel(){
    char com[1];
    char cmd[6]={0,0,0,0,0,0};
    char addr=0x30;//0b0011000;
    com[0]=0x28|0x80;
    accel.write(addr, com, 1,true);
    accel.read(addr, cmd, 6);
    //int x=cmd
    /*accel.start();
    accel.write(addr);
    accel.write(0x28|0x80);
    accel.start();
    accel.write(addr|1);
    cmd[0]=accel.read(1);
    cmd[1]=accel.read(1);
    cmd[2]=accel.read(1);
    cmd[3]=accel.read(0);
    accel.stop();*/
    //int yacc=(signed char)cmd[1];
    //int xacc=(signed char)cmd[3];
    //int zacc=-(signed char)cmd[5];
    int xacc=(signed short)((((unsigned char)cmd[1])<<8)+(unsigned char)cmd[0]);
    int yacc=(signed short)((((unsigned char)cmd[3])<<8)+(unsigned char)cmd[2]);
    int zacc=(signed short)((((unsigned char)cmd[5])<<8)+(unsigned char)cmd[4]);
    //int yacc=(signed char)cmd[3];
    //int zacc=(signed char)cmd[5];
    float angle=atan(zacc==0?yacc*1000000.0:(float)yacc/(float)zacc)-(zacc>=0?0.0:3.141592);
    //pc.printf("%X\t%X\t%X\t%d\t%X\t%d\t%f\t%f\r\n",cmd[0],cmd[1],cmd[2],yacc,cmd[4],zacc,angle);
    //pc.printf("%X\t%X\t%X\t%X\t%X\t%X\r\n",cmd[0],cmd[1],cmd[2],cmd[3],cmd[4],cmd[5]);
    //pc.printf("%d\t\t%d\t\t%d\r\n",xacc,yacc,zacc);
    
    addr=0x3D;//0b0011000;
    com[0]=0x03|0x80;
    accel.write(addr, com, 1,true);
    accel.read(addr, cmd, 6);
    xacc=(signed short)((((unsigned char)cmd[0])<<8)+(unsigned char)cmd[1]);
    yacc=(signed short)((((unsigned char)cmd[2])<<8)+(unsigned char)cmd[3]);
    zacc=(signed short)((((unsigned char)cmd[4])<<8)+(unsigned char)cmd[5]);
    //pc.printf("%X\t%X\t%X\t%X\t%X\t%X\r\n",cmd[0],cmd[1],cmd[2],cmd[3],cmd[4],cmd[5]);
    //pc.printf("%d\t\t%d\t\t%d\r\n",xacc,yacc,zacc);
    
    return angle;
}

int gyroReader::checkGyro(int& xacc, int& yacc, int& zacc){
    char com[1];
    char cmd[6]={0,0,0,0,0,0};
    char addr=0xD6;//0b0011000;
    com[0]=0x28|0x80;
    accel.write(addr, com, 1,true);
    accel.read(addr, cmd, 6);
    //int x=cmd
    /*accel.start();
    accel.write(addr);
    accel.write(0x28|0x80);
    accel.start();
    accel.write(addr|1);
    cmd[0]=accel.read(1);
    cmd[1]=accel.read(1);
    cmd[2]=accel.read(1);
    cmd[3]=accel.read(0);
    accel.stop();*/
    //int xacc=((int)cmd[3]<<8)|cmd[2];
    //int xacc=((int)cmd[1]<<8)|cmd[0];
    //xacc=-((cmd[1]&0x80)==0)?xacc:-((xacc-1)^0xFFFF);
    //xacc=-((cmd[3]&0x80)==0)?xacc:-((xacc-1)^0xFFFF);
    //int yacc=(signed char)cmd[3];
    //int zacc=(signed char)cmd[5];
    xacc=(signed short)((((unsigned char)cmd[1])<<8)+(unsigned char)cmd[0]);
    yacc=(signed short)((((unsigned char)cmd[3])<<8)+(unsigned char)cmd[2]);
    zacc=(signed short)((((unsigned char)cmd[5])<<8)+(unsigned char)cmd[4]);
    //float angle=atan(zacc==0?yacc*1000000.0:(float)yacc/(float)zacc)-(zacc>=0?0.1:3.141592);
    //pc.printf("%d\t%d\t%d\t%d\t%d\t%d\t%f\r\n",cmd[1],xacc,cmd[3],yacc,cmd[5],zacc);
    return xacc;
}

gyroReader::gyroReader(PinName pinA, PinName pinB) : accel(pinA,pinB)/*, upkeepTimer(gyroReader::gyroUpkeep, osTimerPeriodic, (void *)0), testPin(test)*/{
    gyroZcount=0;
    gyroXcount=0;
    zcal=-54;
    xcal=120;
    polls=0;
    startAccel();
    xmag=ymag=zmag=0;
    //testPin=0;
    //upkeepTimer.start(10);
    gyroUpkeepTicker.attach_us(this,&gyroReader::gyroUpkeep,10000);
}
void gyroReader::setLevel(){
    gyroXcount=0;
}

void gyroReader::resetZ(){
    gyroZcount=0;
}

int gyroReader::getZ(){
    return gyroZcount;
}
int gyroReader::getX(){
    return gyroXcount;
}
void gyroReader::reset(){
    gyroZcount=gyroXcount=polls=0;
}

void gyroReader::calibrate(){
    zcal=-(gyroZcount/polls)+zcal;
    xcal=-(gyroXcount/polls)+xcal;
    DBGPRINT("cal: %d, %d (%d)\r\n",zcal,xcal,polls);
}

int gyroReader::compZ(int compval){
    int midval= (compval-gyroZcount)%4050000;
    if(midval > 2025000){
        midval-=4050000;
    }
    return midval;
}

double gyroReader::getZDegrees(){
    return gyroZcount*360.0/4050000.0;
}