Diff: main.cpp

Revision:

4:7faa8147cabb

Parent:

3:64a8188c5a44

--- a/main.cpp	Mon Mar 02 10:08:57 2020 +0000
+++ b/main.cpp	Fri Jun 19 10:22:18 2020 +0000
@@ -2,7 +2,7 @@
 
 //DigitalOut myled(LED1);
 Serial pc(USBTX, USBRX);
-#define PI 3,14 
+#define PI 3.14 
 #define printff_CALCULATED
 #define printff_SPEED 250 
 #define DECLINATION -8.58
@@ -22,6 +22,7 @@
         while(1) ; 
     }
     lol.calibrate();
+    lol.calibrateMag() ;
     while(1) {
         
         //lol.readTemp();
@@ -39,19 +40,19 @@
         lol.readGyro();
         }
         
-        printfGyro();  // printfff "G: gx, gy, gz"
-        pc.printf("\n") ;
-        printfAccel(); // printfff "A: ax, ay, az"
-        pc.printf("\n") ;
-        printfMag();   // printfff "M: mx, my, mz"
-        pc.printf("\n") ;
+      //  printfGyro();  // printfff "G: gx, gy, gz"
+        //pc.printf("\n") ;
+       // printfAccel(); // printfff "A: ax, ay, az"
+        //pc.printf("\n") ;
+       // printfMag();   // printfff "M: mx, my, mz"
+        //pc.printf("\n") ;
         // printff the heading and orientation for fun!
         // Call printfff attitude. The LSM9DS1's mag x and y
         // axes are opposite to the accelerometer, so my, mx are
         // substituted for each other.
         printfAttitude(lol.ax, lol.ay, lol.az,-lol.my, -lol.mx, lol.mz);
-        pc.printf("\n") ;
-        wait(1);
+       pc.printf("\n") ;
+        wait(0.5);
    } 
    }              
         
@@ -85,7 +86,7 @@
   // Now we can use the ax, ay, and az variables as we please.
   // Either printfff them as raw ADC values, or calculated in g's.
   pc.printf("A: ");
-#ifdef printff_CALCULATED
+
   // If you want to printffff calculated values, you can use the
   // calcAccel helper function to convert a raw ADC value to
   // g's. Give the function the value that you want to convert.
@@ -102,7 +103,7 @@
   pc.printf(", ");
   pc.printfln(lol.az);*/
   
-#endif
+
 
 }
 
@@ -133,31 +134,36 @@
 
 void printfAttitude(float ax, float ay, float az, float mx, float my, float mz)
 {
-  float roll = atan2(ay, az);
-  float pitch = atan2(-ax, sqrt(ay * ay + az * az));
+  double  roll = atan2(ay, az);
+  double  pitch = atan2(-ax, sqrt(ay * ay + az * az));
 
-  float heading;
-  if (my == 0)
+  double heading;
+  /*if (my == 0)
     heading = (mx < 0) ? PI : 0;
-  else
-    heading = atan2(mx, my);
+  else*/
+    heading = atan2(my, mx);
 
   heading -= DECLINATION * PI / 180;
 
-  if (heading > PI) heading -= (2 * PI);
-  else if (heading < -PI) heading += (2 * PI);
+  if (heading > 3.14) heading -= (2 * 3.14);
+  else if (heading < -3.14) heading += (2 * 3.14);
 
   // Convert everything from radians to degrees:
   heading *= 180.0 / PI;
   pitch *= 180.0 / PI;
   roll  *= 180.0 / PI;
 
-  pc.printf("Pitch, Roll: ");
+
+
+ 
+  pc.printf("%f" , roll, 2);
+  pc.printf("/") ;
   pc.printf("%f" , pitch, 2);
-  pc.printf(", ");
-  pc.printf("%f" , roll, 2);
-  pc.printf("\n") ;
-  pc.printf("Heading: "); pc.printf("%f" , heading, 2);
+  pc.printf("/") ;
+  pc.printf("%f" , heading, 2);
+
+  
+
   
 }