File content as of revision 2:f53340e49cc0:

#include "Mesure.h"

tMesure::tMesure(COMPASS_DISCO_L476VG * apCompass, GYRO_DISCO_L476VG * apGyro,AnalogIn * apAIn[NbDAC])
{
    pCompass=apCompass;
    pGyro=apGyro;
    for(int i=0; i<NbDAC;i++)
    {
        pAIn[i]=apAIn[i];
    }
    
//Offset and gains to be updated after calibration
    
    GainMag[0]=1.0;
    GainMag[1]=1.0;
    GainMag[2]=1.0;
    OffsetMag[0]=0.0;
    OffsetMag[1]=0.0;
    OffsetMag[2]=0.0;
    
    
    GainAcc[0]=0.0005995;
    GainAcc[1]=0.0005934;
    GainAcc[2]=0.0006065;
    OffsetAcc[0]=-0.666;
    OffsetAcc[1]=-0.164;
    OffsetAcc[2]=0.177;
    
    GainOmega[0]=1.0;
    GainOmega[1]=1.0;
    GainOmega[2]=1.0;
    OffsetOmega[0]=-316.0;
    OffsetOmega[1]=1196.0;
    OffsetOmega[2]=385.0;
    
    for (int i=0;i<NbDAC;i++)
    {
        GainAIO[i]=3.3;
        OffsetAIO[i]=0.0;
    }
    
    
}

void tMesure::Update()
{
    int16_t lMag[3];
    int16_t lAcc[3];
    float lGyroBuffer[3];
    
    pCompass->AccGetXYZ(lAcc);
    pCompass->MagGetXYZ(lMag);
    pGyro->GetXYZ(lGyroBuffer);
    
 //calibration    
    for (int i = 0;i<3;i++)
    {
         Mag[i]=  (lMag[i]*GainMag[i])+ OffsetMag[i];
         Acc[i]=  (lAcc[i]*GainAcc[i])+ OffsetAcc[i];
         Omega[i]=  (lGyroBuffer[i]*GainOmega[i])+ OffsetOmega[i];
    }
    
    for(int i=0; i<NbDAC;i++)
    {
        AIO[i]=(pAIn[i]->read()*GainAIO[i])+OffsetAIO[i];
    }
}

void tMesure::Save(FILE * apFile)
{
  
  fprintf(apFile,"%f\t",Acc[0]);  
  fprintf(apFile,"%f\t",Acc[1]);  
  fprintf(apFile,"%f\t",Acc[2]);  
  
  fprintf(apFile,"%f\t",Omega[0]);  
  fprintf(apFile,"%f\t",Omega[1]);  
  fprintf(apFile,"%f\t",Omega[2]);
  
  fprintf(apFile,"%f\t",Mag[0]);  
  fprintf(apFile,"%f\t",Mag[1]);  
  fprintf(apFile,"%f\t",Mag[2]);
  
  for (int i=0; i<NbDAC ; i++)
  {
     fprintf(apFile,"%f\t",AIO[i]); 
    }
  

  
  fprintf(apFile,"\n");
  
}