main.cpp

00001 #include "LSM9DS1.h"
00002 #include "math.h"
00003 //DigitalOut myled(LED1);
00004 Serial pc(USBTX, USBRX);
00005 #define M_PI 3.14 
00006 #define printff_CALCULATED
00007 #define printff_SPEED 250 
00008 #define DECLINATION -8.58
00009 
00010 void printfGyro();
00011 void printfAccel();
00012 void printfMag();
00013 void printfAttitude(float mx, float my, float mz);
00014 
00015 LSM9DS1 lol(PC4, PC5,0xD6, 0x3D);
00016 int main() {
00017     //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
00018     
00019     lol.begin();
00020     if (!lol.begin()) {
00021         pc.printf("Failed to communicate with LSM9DS1.\n");
00022         while(1) ; 
00023     }
00024   
00025     //lol.calibrateMag() ;
00026     while(1) {
00027         
00028         //lol.readTemp();
00029         
00030         if ( lol.magAvailable() )
00031         {
00032         lol.readMag();
00033         }
00034         
00035         
00036         
00037   
00038         printfMag();   // printfff "M: mx, my, mz"
00039         pc.printf("\n") ;
00040         // printff the heading and orientation for fun!
00041         // Call printfff attitude. The LSM9DS1's mag x and y
00042         // axes are opposite to the accelerometer, so my, mx are
00043         // substituted for each other.
00044         printfAttitude(-lol.my, -lol.mx, lol.mz);
00045         pc.printf("\n") ;
00046         wait(1);
00047    } 
00048    }              
00049         
00050 
00051 
00052 
00053 
00054 
00055 void printfMag()
00056 {
00057   // Now we can use the mx, my, and mz variables as we please.
00058   // Either printfff them as raw ADC values, or calculated in Gauss.
00059   pc.printf("M: ");
00060 
00061   // If you want to printffff calculated values, you can use the
00062   // calcMag helper function to convert a raw ADC value to
00063   // Gauss. Give the function the value that you want to convert.
00064   pc.printf("mx= %f" , lol.calcMag(lol.mx), 2);
00065   pc.printf(", ");
00066   pc.printf("my= %f" , lol.calcMag(lol.my), 2);
00067   pc.printf(", ");
00068   pc.printf("mz= %f  " , lol.calcMag(lol.mz), 2);
00069   pc.printf(" gauss \n");
00070    pc.printf(" ***Raw data ****");
00071   pc.printf(" \n");
00072   pc.printf("%d" ,lol.mx);
00073   pc.printf(", ");
00074   pc.printf("%d" ,lol.my);
00075   pc.printf(", ");
00076   pc.printf("%d" ,lol.mz);
00077 
00078 }
00079 
00080 void printfAttitude( float hx, float hy, float mz)
00081 {
00082 
00083   float heading;
00084   
00085   if (hy > 0)
00086   {
00087     heading = 90 - (atan(hx / hy) * (180 / M_PI));
00088   }
00089   else if (hy < 0)
00090   {
00091     heading = - (atan(hx / hy) * (180 / M_PI));
00092   }
00093   else // hy = 0
00094   {
00095     if (hx < 0) heading = 180;
00096     else heading = 0;
00097   }
00098 
00099 
00100   pc.printf("\n") ;
00101   pc.printf("Heading: "); pc.printf("%f" , heading, 2);
00102   pc.printf("\n") ;
00103   
00104 }
00105         
00106       
00107     
00108