Diff: LSM303D.cpp

Revision:

3:a55edecc96e2

Parent:

2:4bfc36c368c2

--- a/LSM303D.cpp	Tue Feb 09 07:46:40 2016 +0000
+++ b/LSM303D.cpp	Wed Jul 20 14:13:23 2016 +0000
@@ -42,7 +42,7 @@
 #define M_PI 3.14159265358979323846
 #endif
 
-#define FILTER_SHIFT 2      // used in filtering acceleromter readings
+#define FILTER_SHIFT 1      // used in filtering acceleromter readings
 
 const int addr_ls303d = 0x3A;
 
@@ -130,13 +130,17 @@
     reg_v |= 0x00 << 0;     /* Mag Continuous Conversation*/
     write_reg(addr_ls303d,CTRL_REG7_A,reg_v); //26h
 
-    min.x =-4500;
-    min.y =-4500;
-    min.z =-4500;
+    min.x =-2000;
+    min.y =-2000;
+    min.z = 1000;
         
-    max.x = 1200;
-    max.y = 1200;
-    max.z = 100 ;
+    max.x = -1000;
+    max.y = -1000;
+    max.z =  4000;
+    
+    spreed.x = 6000;
+    spreed.y = 6000;
+    spreed.z = 6000;
 }
 
 
@@ -222,8 +226,8 @@
     vector_normalize(&temp_a);
 
     // compute E and N
-    vector E;
-    vector N;
+    vector E;                           //vector East
+    vector N;                           //vector Nord
     vector_cross(&mag,&temp_a,&E);
     vector_normalize(&E);
     vector_cross(&temp_a,&E,&N);
@@ -243,23 +247,34 @@
     //if (hdg > 360) hdg -= 360;
     
     vector_norm_xz(&temp_a);
-    pitch = asin(temp_a.x) * 180/M_PI + 25;
+    pitch = (asin(temp_a.x) * 180/M_PI) + 25;
     
 }
 
-void LSM303D::set_limits(void)
+void LSM303D::set_limits(int mode)
 {
-   if (max.x < mag_raw.x) max.x = mag_raw.x;
-   if (min.x > mag_raw.x) min.x = mag_raw.x;
-   if (max.y < mag_raw.y) max.y = mag_raw.y;
-   if (min.y > mag_raw.y) min.y = mag_raw.y; 
-   if (max.z < mag_raw.z) max.z = mag_raw.z;
-   if (min.z > mag_raw.z) min.z = mag_raw.z;
-   
+   if(mode == 1)                            //first cal.
+    {
+     if (max.x < mag_raw.x) max.x = mag_raw.x;
+     if (min.x > mag_raw.x) min.x = mag_raw.x;
+     if (max.y < mag_raw.y) max.y = mag_raw.y;
+     if (min.y > mag_raw.y) min.y = mag_raw.y; 
+     if (max.z < mag_raw.z) max.z = mag_raw.z;
+     if (min.z > mag_raw.z) min.z = mag_raw.z;
+   }
+   if(mode == 2)                            //autorecal.
+    {   
+     if (max.x < mag_raw.x) max.x += 5;
+     if (min.x > mag_raw.x) min.x -= 5;
+     if (max.y < mag_raw.y) max.y += 5;
+     if (min.y > mag_raw.y) min.y -= 5; 
+     if (max.z < mag_raw.z) max.z += 5;
+     if (min.z > mag_raw.z) min.z -= 5;  
+    }
        
-   if ((max.x - min.x) > 7200) {max.x--; min.x++;};
-   if ((max.y - min.y) > 7200) {max.y--; min.y++;};
-   if ((max.z - min.z) > 6200) {max.z--; min.z++;}; 
+   if ((max.x - min.x) > spreed.x) {max.x--; min.x++;};
+   if ((max.y - min.y) > spreed.y) {max.y--; min.y++;};
+   if ((max.z - min.z) > spreed.z) {max.z--; min.z++;}; 
 }
 
 void LSM303D::frequency(int hz)