Diff: LSM303D.cpp

Revision:

2:4bfc36c368c2

Parent:

1:5d0acf280330

Child:

3:a55edecc96e2

diff -r 5d0acf280330 -r 4bfc36c368c2 LSM303D.cpp
--- a/LSM303D.cpp	Sat Oct 31 16:28:12 2015 +0000
+++ b/LSM303D.cpp	Tue Feb 09 07:46:40 2016 +0000
@@ -129,6 +129,14 @@
     reg_v = 0x00;          
     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;
+        
+    max.x = 1200;
+    max.y = 1200;
+    max.z = 100 ;
 }
 
 
@@ -161,20 +169,24 @@
     acc.z = (float) (_filt_az >> FILTER_SHIFT);
     
     // offset and scale
-    mag.x = (3 * mag.x + ((mag_raw.x + _offset_x) * _scale_x))/4;
-    mag.y = (3 * mag.y + ((mag_raw.y + _offset_y) * _scale_y))/4;
-    mag.z = (3 * mag.z + ((mag_raw.z + _offset_z) * _scale_z))/4; 
-    
+    //mag.x = (3 * mag.x + ((mag_raw.x + _offset_x) * _scale_x))/4;
+    //mag.y = (3 * mag.y + ((mag_raw.y + _offset_y) * _scale_y))/4;
+    //mag.z = (3 * mag.z + ((mag_raw.z + _offset_z) * _scale_z))/4; 
+    mag.x = (mag.x + ((mag_raw.x + (((max.x - min.x)/2) - max.x)) * (10000/(max.x - min.x))) )/2;
+    mag.y = (mag.y + ((mag_raw.y + (((max.y - min.y)/2) - max.y)) * (10000/(max.y - min.y))) )/2;
+    mag.z = (mag.z + ((mag_raw.z + (((max.z - min.z)/2) - max.z)) * (10000/(max.z - min.z))) )/2;
+   
     vector_normalize(&mag);  
 }
 
 void LSM303D::read()
 {
     char data[1] = { OUT_X_A | (1<<7)}; //Page 23 In order to read multiple bytes, it is necessary to assert the most significant bit of the subaddress field. In other words, SUB(7) must be equal to ‘1’ while SUB(6-0) represents the address of the first register to be read.
-    char out[6] = {0,0,0,0,0,0};
+    char out[6] = {0,0,0,0,0,0};    
+    
     _compass.write( addr_ls303d, data,1);
     _compass.read( addr_ls303d, out, 6);
-
+    
     acc_raw.x= short( (((short)out[1]) << 8) | out[0] );
     acc_raw.y= short( (((short)out[3]) << 8) | out[2] );
     acc_raw.z= short( (((short)out[5]) << 8) | out[4] );   
@@ -187,7 +199,7 @@
     mag_raw.x= short( (((short)out[1]) << 8)| out[0]);
     mag_raw.y= short( (((short)out[3]) << 8)| out[2]);
     mag_raw.z= short( (((short)out[5]) << 8)| out[4]); 
-    
+  
     set_vectors();
     
     calc_pos();
@@ -197,6 +209,9 @@
 
 void LSM303D::calc_pos(void)
 {   
+  float x;
+  float y;
+  float c;
     ////////////////////////////////////////////////
     // compute heading       
     ////////////////////////////////////////////////
@@ -214,14 +229,39 @@
     vector_cross(&temp_a,&E,&N);
     
     // compute heading
-    hdg = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
-    if (hdg < 0) hdg += 360;
+    x = vector_dot(&E,&from);
+    y = vector_dot(&N,&from);
+    c = sqrt(x*x + y*y);
+    //hdg = atan2(x, y)) * 180/M_PI;
+    hdg = atan2(x, y); // * 180/M_PI;
+    hdg = ((asin(fabs(x)/c) * (1 - fabs(sin(hdg)))) + (acos(fabs(y)/c) * fabs(sin(hdg))))* 180 / M_PI;
+    if ((x > 0) && (y < 0)) hdg = 180 - hdg;
+    if ((x < 0) && (y < 0)) hdg = 180 + hdg;
+    if ((x < 0) && (y > 0)) hdg = 360 - hdg;
+    
+    //if (hdg <   0) hdg += 360;
+    //if (hdg > 360) hdg -= 360;
     
     vector_norm_xz(&temp_a);
-    pitch = asin(-temp_a.x) * 180/M_PI + 30;
+    pitch = asin(temp_a.x) * 180/M_PI + 25;
     
 }
 
+void LSM303D::set_limits(void)
+{
+   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 ((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++;}; 
+}
+
 void LSM303D::frequency(int hz)
 {
     _compass.frequency(hz);