Diff: main.cpp

Revision:

3:64a8188c5a44

Parent:

0:e8167f37725c

Child:

4:6c3cdc717a1f

Child:

5:b7855a02ddf6

--- a/main.cpp	Wed Feb 03 18:24:29 2016 +0000
+++ b/main.cpp	Mon Mar 02 10:08:57 2020 +0000
@@ -1,29 +1,174 @@
 #include "LSM9DS1.h"
 
-DigitalOut myled(LED1);
+//DigitalOut myled(LED1);
 Serial pc(USBTX, USBRX);
+#define PI 3,14 
+#define printff_CALCULATED
+#define printff_SPEED 250 
+#define DECLINATION -8.58
 
+void printfGyro();
+void printfAccel();
+void printfMag();
+void printfAttitude(float ax, float ay, float az, float mx, float my, float mz);
+
+LSM9DS1 lol(PC4, PC5,0xD6, 0x3C);
 int main() {
     //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
-    LSM9DS1 lol(p9, p10, 0xD6, 0x3C);
+    
     lol.begin();
     if (!lol.begin()) {
         pc.printf("Failed to communicate with LSM9DS1.\n");
+        while(1) ; 
     }
     lol.calibrate();
     while(1) {
-        lol.readTemp();
+        
+        //lol.readTemp();
+        
+        if ( lol.magAvailable() )
+        {
         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);
+        pc.printf("\n") ;
+        wait(1);
+   } 
+   }              
         
-        //pc.printf("%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.printf("%d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz));
-        pc.printf("gyro: %d %d %d\n\r", lol.gx, lol.gy, lol.gz);
-        pc.printf("accel: %d %d %d\n\r", lol.ax, lol.ay, lol.az);
-        pc.printf("mag: %d %d %d\n\n\r", lol.mx, lol.my, lol.mz);
-        myled = 1;
-        wait(2);
-        myled = 0;
-        wait(2);
-    }
+        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(", ");
+  pc.printf("%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(", ");
+  pc.printf(lol.my);
+  pc.printf(", ");
+  pc.printfln(lol.mz);*/
+    #endif
+}
+
+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));
+
+  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);
+
+  // 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.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);
+    
+