Diff: main.cpp

Revision:

5:bf8f4e7c9905

Parent:

4:7ede465deae1

Child:

6:e6a15dcba942

diff -r 7ede465deae1 -r bf8f4e7c9905 main.cpp
--- a/main.cpp	Sun Nov 23 17:46:45 2014 +0000
+++ b/main.cpp	Wed Dec 03 23:08:09 2014 +0000
@@ -1,26 +1,25 @@
+//Most of the Credit goes to jimblom
 #include "LSM9DS0.h"
 #include "mbed.h"
 
 // SDO_XM and SDO_G are both grounded, so our addresses are:
-#define LSM9DS0_XM  0x1E // Would be 0x1E if SDO_XM is LOW
-#define LSM9DS0_G   0x6A // Would be 0x6A if SDO_G is LOW
+#define LSM9DS0_XM  0x1D // Would be 0x1E if SDO_XM is LOW
+#define LSM9DS0_G   0x6B // Would be 0x6A if SDO_G is LOW
 
-// Create an instance of the LSM9DS0 library called `dof` the
-// parameters for this constructor are:
-// pins,[gyro I2C address],[xm I2C add.]
-LSM9DS0 dof(p28, p27, LSM9DS0_G, LSM9DS0_XM);
-DigitalIn DReady(p23);
 
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
-
+LSM9DS0 dof(p9, p10,LSM9DS0_G, LSM9DS0_XM);
 Serial pc(USBTX, USBRX); // tx, rx
 
-bool printM = true;
-bool printA = true;
-bool printG = true;
-bool printHead = true;
+//Uncomment If you want to use interrupts
+/*DigitalIn pinDRDYG(p14);
+DigitalIn pinINTG(p13);
+DigitalIn pinINT1_XM(p14);
+DigitalIn pinINT2_XM(p15);*/
 
+#define PRINT_CALCULATED
+#define PRINT_SPEED 500 // 500 ms between prints
+
+//Init Serial port and LSM9DS0 chip
 void setup()
 {
     pc.baud(115200); // Start serial at 115200 bps
@@ -28,135 +27,140 @@
     // You can either call it with no parameters (the easy way):
     uint16_t status = dof.begin();
     
-    // Or call it with declarations for sensor scales and data rates:
-    //uint16_t status = dof.begin(dof.G_SCALE_2000DPS,
-    //                            dof.A_SCALE_6G, dof.M_SCALE_2GS);
-    
-    // begin() returns a 16-bit value which includes both the gyro
-    // and accelerometers WHO_AM_I response. You can check this to
-    // make sure communication was successful.
+   
+   //Make sure communication is working
     pc.printf("LSM9DS0 WHO_AM_I's returned: 0x");
     pc.printf("%x\n",status);
     pc.printf("Should be 0x49D4\n");
     pc.printf("\n");
 }
 
-void printGyro() {
-    dof.readGyro();
+
+void printGyro()
+{
+    // To read from the gyroscope, you must first call the
+    // readGyro() function. When this exits, it'll update the
+    // gx, gy, and gz variables with the most current data.
     
-    pc.printf("G: ");
+    //Uncomment code if you plan on using interrupts
+    /*while(pinDRDYG == 0)
+    {
+        ;
+    }*/
+    
     
-    pc.printf("%2f",dof.calcGyro(dof.gx));
+    dof.readGyro();
+
+    // Now we can use the gx, gy, and gz variables as we please.
+    // Either print them as raw ADC values, or calculated in DPS.
+    pc.printf("G: ");
+#ifdef PRINT_CALCULATED
+    // If you want to print 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("The bias: %2f, %2f, %2f \n", dof.gbias[0], dof.gbias[1], dof.gbias[2]);
+    pc.printf("%2f",dof.calcGyro(dof.gx) - dof.gbias[0]);
     pc.printf(", ");
-    pc.printf("%2f",dof.calcGyro(dof.gy));
+    pc.printf("%2f",dof.calcGyro(dof.gy) - dof.gbias[1]);
     pc.printf(", ");
-    pc.printf("%2f\n",dof.calcGyro(dof.gz));
+    pc.printf("%2f\n",dof.calcGyro(dof.gz) - dof.gbias[2]);
+#elif defined PRINT_RAW
+    pc.printf("%d", dof.gx);
+    pc.printf(", ");
+    pc.printf("%d",dof.gy);
+    pc.printf(", ");
+    pc.printf("%d\n",dof.gz);
+#endif
 }
 
-void printAccel() {
+void printAccel()
+{
+    //Uncomment code if you plan on using interrupts
+    /*while(pinINT1_XM == 0)
+    {
+        ;
+    }*/
+    
+
     dof.readAccel();
     
+    // Now we can use the ax, ay, and az variables as we please.
+    // Either print them as raw ADC values, or calculated in g's.
     pc.printf("A: ");
-    
-    pc.printf("%2f",dof.calcAccel(dof.ax));
+#ifdef PRINT_CALCULATED
+    // If you want to print 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("The bias: %2f, %2f, %2f \n", dof.abias[0], dof.abias[1], dof.abias[2]);
+    pc.printf("%2f",dof.calcAccel(dof.ax) - dof.abias[0]);
     pc.printf(", ");
-    pc.printf("%2f",dof.calcAccel(dof.ay));
+    pc.printf("%2f",dof.calcAccel(dof.ay) - dof.abias[1]);
     pc.printf(", ");
-    pc.printf("%2f\n",dof.calcAccel(dof.az));
+    pc.printf("%2f\n",dof.calcAccel(dof.az) - dof.abias[2]);
+#elif defined PRINT_RAW
+    pc.printf("%d",dof.ax);
+    pc.printf(", ");
+    pc.printf("%d",dof.ay);
+    pc.printf(", ");
+    pc.printf("%d\n",dof.az);
+#endif
     
 }
 
-void printMag() {
+void printMag()
+{
+     //Uncomment code if you plan on using interrupts
+    /*while(pinINT2_XM == 0)
+    {
+        ;
+    }*/
+    
+    // To read from the magnetometer, you must first call the
+    // readMag() function. When this exits, it'll update the
+    // mx, my, and mz variables with the most current data.
     dof.readMag();
     
+    // Now we can use the mx, my, and mz variables as we please.
+    // Either print them as raw ADC values, or calculated in Gauss.
     pc.printf("M: ");
-    
+#ifdef PRINT_CALCULATED
+    // If you want to print 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("%2f",dof.calcMag(dof.mx));
     pc.printf(", ");
     pc.printf("%2f",dof.calcMag(dof.my));
     pc.printf(", ");
     pc.printf("%2f\n",dof.calcMag(dof.mz));
-}
-
-// Here's a fun function to calculate your heading, using Earth's
-// magnetic field.
-// It only works if the sensor is flat (z-axis normal to Earth).
-// Additionally, you may need to add or subtract a declination
-// angle to get the heading normalized to your location.
-// See: http://www.ngdc.noaa.gov/geomag/declination.shtml
-void printHeading(float hx, float hy)
-{
-    float heading;
-    
-    if (hy > 0) heading = 90 - (atan(hx / hy) * (180 / 3.14));
-    else if (hy < 0) heading = - (atan(hx / hy) * (180 / 3.14));
-    else // hy = 0
-    {
-        if (hx < 0) heading = 180;
-        else heading = 0;
-    }
-    
-    pc.printf("Heading: ");
-    pc.printf("%2f\n",heading);
+#elif defined PRINT_RAW
+    pc.printf("%d", dof.mx);
+    pc.printf(", ");
+    pc.printf("%d", dof.my);
+    pc.printf(", ");
+    pc.printf("%d\n", dof.mz);
+#endif
 }
 
-// Another fun function that does calculations based on the
-// acclerometer data. This function will print your LSM9DS0's
-// orientation -- it's roll and pitch angles.
-void printOrientation(float x, float y, float z)
+void loop()
 {
-    float pitch, roll;
-    
-    pitch = atan2(x, sqrt(y * y) + (z * z));
-    roll = atan2(y, sqrt(x * x) + (z * z));
-    pitch *= 180.0 / 3.14;
-    roll *= 180.0 / 3.14;
-    
-    pc.printf("Pitch, Roll: ");
-    pc.printf("%2f",pitch);
-    pc.printf(", ");
-    pc.printf("%2f\n",roll);
-}
-
-void readData() {
-    // To read from the device, you must first call the
-    // readMag(), readAccel(), and readGyro() functions.
-    // When this exits, it'll update the appropriate
-    // variables ([mx, my, mz], [ax, ay, az], [gx, gy, gz])
-    // with the most current data.
-
-    dof.readMag();
-    dof.readAccel();
-    dof.readGyro();
-}
-
-void loop() {
-    // Loop until the Data Ready signal goes high
-    led2 = 1;
-    while (!DReady) {}
-    led2 = 0;
-    
-    readData();
-    
-    if (printG) printGyro();  // Print "G: gx, gy, gz"
-    if (printA) printAccel(); // Print "A: ax, ay, az"
-    if (printM) printMag();   // Print "M: mx, my, mz"
-    
-    // Print the heading and orientation for fun!
-    if (printHead) printHeading((float) dof.mx, (float) dof.my);
-    //printOrientation(dof.calcAccel(dof.ax), dof.calcAccel(dof.ay),
-                    // dof.calcAccel(dof.az));
-    pc.printf("\n");
+    printGyro();  // Print "G: gx, gy, gz" 
+    printAccel(); // Print "A: ax, ay, az"
+    printMag();   // Print "M: mx, my, mz"    
+    wait_ms(PRINT_SPEED);
 }
 
 
 int main()
 {
-    setup();
+    setup();  //Setup sensor and Serial
+    pc.printf("Hello World\n");
+
     while (true)
     {
-      led1 = !led1;
-      loop();
-      wait(2);
-    }
+         //Calculate bias
+         dof.calLSM9DS0(dof.gbias, dof.abias);
+         //pc.printf("Setup Done\n");
+         loop(); //Get sensor values
+   }
 }
+