Diff: Mesure/mesure.cpp

Revision:

1:e1f3b4b8b99b

Parent:

0:0861bf46efe4

diff -r 0861bf46efe4 -r e1f3b4b8b99b Mesure/mesure.cpp
--- a/Mesure/mesure.cpp	Fri Feb 12 20:54:38 2016 +0000
+++ b/Mesure/mesure.cpp	Sat Feb 13 14:29:40 2016 +0000
@@ -1,8 +1,16 @@
 #include "Mesure.h"
 
-tMesure::tMesure(COMPASS_DISCO_L476VG * apCompass)
+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;
@@ -10,12 +18,27 @@
     OffsetMag[1]=0.0;
     OffsetMag[2]=0.0;
     
-    GainAcc[0]=1.0;
-    GainAcc[1]=1.0;
-    GainAcc[2]=1.0;
-    OffsetAcc[0]=0.0;
-    OffsetAcc[1]=0.0;
-    OffsetAcc[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;
+    }
+    
     
 }
 
@@ -23,26 +46,48 @@
 {
     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");
   
 }
\ No newline at end of file