Diff: Accelerometer.cpp

Revision:

3:de79cb7b645b

Parent:

0:ab4465742afd

--- a/Accelerometer.cpp	Tue Feb 09 23:58:49 2021 +0000
+++ b/Accelerometer.cpp	Tue Feb 16 17:59:49 2021 +0000
@@ -16,7 +16,12 @@
     i2c.frequency(freq);
     
     //Set Range
-    acc.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);  
+    acc.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
+    
+    //Calibration
+    AOffset[S_PANEL] = getAngleCal(S_PANEL,N_CAL);
+    AOffset[S_R1] = getAngleCal(S_R1,N_CAL);
+    AOffset[S_R2] = getAngleCal(S_R2,N_CAL);
 }
 
 //setSource
@@ -28,16 +33,19 @@
             Panel = 0;
             R1 = 1;
             R2 = 1;
+            curOffset = AOffset[S_PANEL];
             break;
         case S_R1: //Reflector 1
             Panel = 1;
             R1 = 0;
             R2 = 1;
+            curOffset = AOffset[S_R1];
             break;
         case S_R2: //Reflector 2
             Panel = 1;
             R1 = 1;
             R2 = 0;
+            curOffset = AOffset[S_R2];
             break;
     }
 }
@@ -48,21 +56,39 @@
     return acc.testConnection();
 }
 
+//getAngleCal
+float Accelerometer::getAngleCal(int s, int n)
+{
+    float ang[3];
+    float ang_out = 0;
+    float a[n];
+    Accelerometer::setSource(s);
+    
+    for(int i = 0;i < n;i++)
+    {
+        acc.getAcceleroAngle(ang);
+        a[i] = ang[MEAS_AXIS];
+    }
+    return a[(int)n/2];
+}
+
 //getAngle
 float Accelerometer::getAngle(int s)
 {
     float ang[3];
     float ang_out = 0;
+    float a[N_AVG];
     Accelerometer::setSource(s);
     
     for(int i = 0;i < N_AVG;i++)
     {
         acc.getAcceleroAngle(ang);
-        ang_out += ang[MEAS_AXIS];
+        a[i] = ang[MEAS_AXIS];
     }
-    return ang_out/N_AVG;
+    return a[(int)N_AVG/2]-curOffset;
 }
 
+
 //checkAngle
 bool Accelerometer::checkAngle(float ref, float cur)
 {