mesure.cpp

00001 #include "Mesure.h"
00002 
00003 tMesure::tMesure(COMPASS_DISCO_L476VG * apCompass, GYRO_DISCO_L476VG * apGyro,AnalogIn * apAIn[NbDAC])
00004 {
00005     pCompass=apCompass;
00006     pGyro=apGyro;
00007     for(int i=0; i<NbDAC;i++)
00008     {
00009         pAIn[i]=apAIn[i];
00010     }
00011     
00012 //Offset and gains to be updated after calibration
00013     
00014     GainMag[0]=1.0;
00015     GainMag[1]=1.0;
00016     GainMag[2]=1.0;
00017     OffsetMag[0]=0.0;
00018     OffsetMag[1]=0.0;
00019     OffsetMag[2]=0.0;
00020     
00021     
00022     GainAcc[0]=0.0005995;
00023     GainAcc[1]=0.0005934;
00024     GainAcc[2]=0.0006065;
00025     OffsetAcc[0]=-0.666;
00026     OffsetAcc[1]=-0.164;
00027     OffsetAcc[2]=0.177;
00028     
00029     GainOmega[0]=1.0;
00030     GainOmega[1]=1.0;
00031     GainOmega[2]=1.0;
00032     OffsetOmega[0]=-316.0;
00033     OffsetOmega[1]=1196.0;
00034     OffsetOmega[2]=385.0;
00035     
00036     for (int i=0;i<NbDAC;i++)
00037     {
00038         GainAIO[i]=3.3;
00039         OffsetAIO[i]=0.0;
00040     }
00041     
00042     
00043 }
00044 
00045 void tMesure::Update()
00046 {
00047     int16_t lMag[3];
00048     int16_t lAcc[3];
00049     float lGyroBuffer[3];
00050     
00051     pCompass->AccGetXYZ(lAcc);
00052     pCompass->MagGetXYZ(lMag);
00053     pGyro->GetXYZ(lGyroBuffer);
00054     
00055  //calibration    
00056     for (int i = 0;i<3;i++)
00057     {
00058          Mag[i]=  (lMag[i]*GainMag[i])+ OffsetMag[i];
00059          Acc[i]=  (lAcc[i]*GainAcc[i])+ OffsetAcc[i];
00060          Omega[i]=  (lGyroBuffer[i]*GainOmega[i])+ OffsetOmega[i];
00061     }
00062     
00063     for(int i=0; i<NbDAC;i++)
00064     {
00065         AIO[i]=(pAIn[i]->read()*GainAIO[i])+OffsetAIO[i];
00066     }
00067 }
00068 
00069 void tMesure::Save(FILE * apFile)
00070 {
00071   
00072   fprintf(apFile,"%f\t",Acc[0]);  
00073   fprintf(apFile,"%f\t",Acc[1]);  
00074   fprintf(apFile,"%f\t",Acc[2]);  
00075   
00076   fprintf(apFile,"%f\t",Omega[0]);  
00077   fprintf(apFile,"%f\t",Omega[1]);  
00078   fprintf(apFile,"%f\t",Omega[2]);
00079   
00080   fprintf(apFile,"%f\t",Mag[0]);  
00081   fprintf(apFile,"%f\t",Mag[1]);  
00082   fprintf(apFile,"%f\t",Mag[2]);
00083   
00084   for (int i=0; i<NbDAC ; i++)
00085   {
00086      fprintf(apFile,"%f\t",AIO[i]); 
00087     }
00088   
00089 
00090   
00091   fprintf(apFile,"\n");
00092   
00093 }