Diff: main.cpp

Revision:

4:6c3cdc717a1f

Parent:

3:64a8188c5a44

--- a/main.cpp	Mon Mar 02 10:08:57 2020 +0000
+++ b/main.cpp	Mon Jun 01 20:09:14 2020 +0000
@@ -1,8 +1,8 @@
 #include "LSM9DS1.h"
-
+#include "math.h"
 //DigitalOut myled(LED1);
 Serial pc(USBTX, USBRX);
-#define PI 3,14 
+#define M_PI 3.14 
 #define printff_CALCULATED
 #define printff_SPEED 250 
 #define DECLINATION -8.58
@@ -10,9 +10,9 @@
 void printfGyro();
 void printfAccel();
 void printfMag();
-void printfAttitude(float ax, float ay, float az, float mx, float my, float mz);
+void printfAttitude(float mx, float my, float mz);
 
-LSM9DS1 lol(PC4, PC5,0xD6, 0x3C);
+LSM9DS1 lol(PC4, PC5,0xD6, 0x3D);
 int main() {
     //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
     
@@ -21,7 +21,8 @@
         pc.printf("Failed to communicate with LSM9DS1.\n");
         while(1) ; 
     }
-    lol.calibrate();
+  
+    //lol.calibrateMag() ;
     while(1) {
         
         //lol.readTemp();
@@ -30,145 +31,78 @@
         {
         lol.readMag();
         }
-        if ( lol.accelAvailable() )
-        {
-        lol.readAccel();
-        }
-        if ( lol.gyroAvailable() )
-        {
-        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") ;
         // 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);
+        printfAttitude(-lol.my, -lol.mx, lol.mz);
         pc.printf("\n") ;
         wait(1);
    } 
    }              
         
-        void printfGyro()
-     {
-  // Now we can use the gx, gy, and gz variables as we please.
-  // Either printfff them as raw ADC values, or calculated in DPS.
-        pc.printf("G: ");
-        #ifdef printff_CALCULATED 
-  // If you want to printffff calculated values, you can use the
-  // calcGyro helper function to convert a raw ADC value to
-  // DPS. Give the function the value that you want to convert.
-  pc.printf("%f" , lol.calcGyro(lol.gx), 2);
-  pc.printf(", ");
-  pc.printf("%f" , lol.calcGyro(lol.gy), 2);
-  pc.printf(", ");
-  pc.printf("%f" , lol.calcGyro(lol.gz), 2);
-  pc.printf(" deg/s ");
-  pc.printf("\n") ;
-/*#elif defined printfff_RAW
-  pc.printf(lol.gx);
-  pc.printf(", ");
-  pc.printf(lol.gy);
-  pc.printf(", ");
-  pc.printfln(lol.gz);*/
-    #endif
-}
+
 
-void printfAccel()
-{
-  // 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.
-  pc.printf("%f" , lol.calcAccel(lol.ax), 2);
-  pc.printf(", ");
-  pc.printf("%f" , lol.calcAccel(lol.ay), 2);
-  pc.printf(", ");
-  pc.printf("%f" , lol.calcAccel(lol.az), 2);
-  pc.printf(" g");
-/*#elif defined printfff_RAW
-  pc.printf(lol.ax);
-  pc.printf(", ");
-  pc.printf(lol.ay);
-  pc.printf(", ");
-  pc.printfln(lol.az);*/
-  
-#endif
 
-}
+
 
 void printfMag()
 {
   // Now we can use the mx, my, and mz variables as we please.
   // Either printfff them as raw ADC values, or calculated in Gauss.
   pc.printf("M: ");
-#ifdef printff_CALCULATED
+
   // If you want to printffff calculated values, you can use the
   // calcMag helper function to convert a raw ADC value to
   // Gauss. Give the function the value that you want to convert.
-  pc.printf("%f" , lol.calcMag(lol.mx), 2);
+  pc.printf("mx= %f" , lol.calcMag(lol.mx), 2);
   pc.printf(", ");
-  pc.printf("%f" , lol.calcMag(lol.my), 2);
+  pc.printf("my= %f" , lol.calcMag(lol.my), 2);
   pc.printf(", ");
-  pc.printf("%f" , lol.calcMag(lol.mz), 2);
-  pc.printf(" gauss");
-  
-/*#elif defined printfff_RAW
-  pc.printf(lol.mx);
+  pc.printf("mz= %f  " , lol.calcMag(lol.mz), 2);
+  pc.printf(" gauss \n");
+   pc.printf(" ***Raw data ****");
+  pc.printf(" \n");
+  pc.printf("%d" ,lol.mx);
   pc.printf(", ");
-  pc.printf(lol.my);
+  pc.printf("%d" ,lol.my);
   pc.printf(", ");
-  pc.printfln(lol.mz);*/
-    #endif
+  pc.printf("%d" ,lol.mz);
+
 }
 
-void printfAttitude(float ax, float ay, float az, float mx, float my, float mz)
+void printfAttitude( float hx, float hy, float mz)
 {
-  float roll = atan2(ay, az);
-  float pitch = atan2(-ax, sqrt(ay * ay + az * az));
 
   float heading;
-  if (my == 0)
-    heading = (mx < 0) ? PI : 0;
-  else
-    heading = atan2(mx, my);
-
-  heading -= DECLINATION * PI / 180;
-
-  if (heading > PI) heading -= (2 * PI);
-  else if (heading < -PI) heading += (2 * PI);
+  
+  if (hy > 0)
+  {
+    heading = 90 - (atan(hx / hy) * (180 / M_PI));
+  }
+  else if (hy < 0)
+  {
+    heading = - (atan(hx / hy) * (180 / M_PI));
+  }
+  else // hy = 0
+  {
+    if (hx < 0) heading = 180;
+    else heading = 0;
+  }
 
-  // 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" , pitch, 2);
-  pc.printf(", ");
-  pc.printf("%f" , roll, 2);
   pc.printf("\n") ;
   pc.printf("Heading: "); pc.printf("%f" , heading, 2);
+  pc.printf("\n") ;
   
 }
         
-        //pc.printffff("%d %d %d %d %d %d %d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz), lol.ax, lol.ay, lol.az, lol.mx, lol.my, lol.mz);
-        //pc.printffff("%d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz));
-        //pc.printffff("gyro: %d %d %d\n\r", lol.gx, lol.gy, lol.gz);
-        //pc.printffff("accel: %d %d %d\n\r", lol.ax, lol.ay, lol.az);
-        //pc.printffff("mag: %d %d %d\n\n\r", lol.mx, lol.my, lol.mz);
-        //myled = 1;
-        //wait_ms(500);
-        //myled = 0;
-        //wait_ms(500);
+