Diff: Mesure/mesure.cpp

Revision:

0:0861bf46efe4

Child:

1:e1f3b4b8b99b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mesure/mesure.cpp	Fri Feb 12 20:54:38 2016 +0000
@@ -0,0 +1,48 @@
+#include "Mesure.h"
+
+tMesure::tMesure(COMPASS_DISCO_L476VG * apCompass)
+{
+    pCompass=apCompass;
+    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]=1.0;
+    GainAcc[1]=1.0;
+    GainAcc[2]=1.0;
+    OffsetAcc[0]=0.0;
+    OffsetAcc[1]=0.0;
+    OffsetAcc[2]=0.0;
+    
+}
+
+void tMesure::Update()
+{
+    int16_t lMag[3];
+    int16_t lAcc[3];
+    pCompass->AccGetXYZ(lAcc);
+    pCompass->MagGetXYZ(lMag);
+    
+    for (int i = 0;i<3;i++)
+    {
+         Mag[i]=  (lMag[i]*GainMag[i])+ OffsetMag[i];
+         Acc[i]=  (lAcc[i]*GainAcc[i])+ OffsetAcc[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",Mag[0]);  
+  fprintf(apFile,"%f\t",Mag[1]);  
+  fprintf(apFile,"%f\t",Mag[2]);
+  
+  fprintf(apFile,"\n");
+  
+}
\ No newline at end of file