File content as of revision 10:605b0bfadc2e:

/*
2014_12_15
Author: Michał Szewc
Free to use
*/

#include "Offsets.h"


Offsets::Offsets()
{
    reset();
}

Offsets::~Offsets(void)
{
}

void Offsets::reset()
{
    i=0;
}


void Offsets::setOffsets(float *o,float *katmagneto, Serial &serial, IMU &imu)
{
    float offsetGyr[3];
    char znak;
    char buff[120];
    int i;
    float d[9];
    float yaw;
    
    sprintf(buff, "Hello!\n\rPlace Qudrocopter motionlessly and press o\n\r");
    serial.printf(buff);
    do{
        if(serial.readable()){
        znak=serial.getc();
        }
        sprintf(buff, "Still waiting for a ...\n\r");
        serial.printf(buff);
        wait(3);
    }while(znak!='a');
    znak=0;
    sprintf(buff, "Thanks! Gyroscope offsets are being set\n\r");
    serial.printf(buff);
    for (i=0;i<1000;i++){
        imu.readData(d);
        offsetGyr[0]+=d[0];
        offsetGyr[1]+=d[1];
        offsetGyr[2]+=d[2];
        yaw = yaw + atan2(d[7],d[6])+4.98333*M_PI/180;
        }
        offsetGyr[0]/=1000;
        offsetGyr[1]/=1000;
        offsetGyr[2]/=1000;
        yaw = yaw/1000;
        
        //o[0] = offsetGyr[0];
        //o[1] = offsetGyr[1];
        //o[2] = offsetGyr[2];
        // lub tak:
        *(o+0) = offsetGyr[0];
        *(o+1) = offsetGyr[1];
        *(o+2) = offsetGyr[2];
        *(katmagneto+0) = yaw;
    sprintf(buff, "Gyroscope offsets in DPS:\n\r");
    serial.printf(buff);
    sprintf(buff, "offsetGyro[0]: %f,offsetGyro[1]: %f,offsetGyro[2]: %f,offsetMagneto: %f\n\r",(offsetGyr[0]*180/M_PI),(offsetGyr[1]*180/M_PI),(offsetGyr[2]*180/M_PI),yaw*180/M_PI);
    serial.printf(buff);
    sprintf(buff, "Press b to accept\n\r");
    serial.printf(buff);
     do{
        if(serial.readable()){
        znak=serial.getc();
        }
        sprintf(buff, "Press b ...\n\r");
        serial.printf(buff);
        wait(3); 
    }while(znak!='b');
    znak=0;
    return;
    
    
}


void Offsets::offsetData(float *d, float *D, float *O) 
{
    /*for (int i=0; i<3; i++) {
         O[i] = d[i]-D[i]; 
    }*/
    O[0] = d[0]-D[0];
    O[1] = d[1]-D[1];
    O[2] = d[2]-D[2];
}

void Offsets::offsetData2(double *d, float *D, float *O) 
{
    /*for (int i=0; i<3; i++) {
         O[i] = d[i]-D[i]; 
    }*/
    O[0] = d[0]-D[0];
    O[1] = d[1]-D[1];
    O[2] = d[2]-D[2];
}

void Offsets::offsetMagneto(float *d, float *D, float *O) 
{
    /*for (int i=0; i<3; i++) {
         O[i] = d[i]-D[i]; 
    }*/
    O[0] = d[0]-D[0];
}