main.cpp

00001 #include "mbed.h"
00002 #include "LSM9DS1.h"
00003 #define PI 3.14159
00004 // Earth's magnetic field varies by location. Add or subtract
00005 // a declination to get a more accurate heading. Calculate
00006 // your's here:
00007 // http://www.ngdc.noaa.gov/geomag-web/#declination
00008 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
00009 
00010 DigitalOut myled(LED1);
00011 Serial pc(p9, p10);
00012 // Calculate pitch, roll, and heading.
00013 // Pitch/roll calculations taken from this app note:
00014 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
00015 // Heading calculations taken from this app note:
00016 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
00017 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
00018 {
00019     float roll = atan2(ay, az);
00020     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
00021 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
00022     mx = -mx;
00023     float heading;
00024     if (my == 0.0)
00025         heading = (mx < 0.0) ? 180.0 : 0.0;
00026     else
00027         heading = atan2(mx, my)*360.0/(2.0*PI);
00028     //pc.printf("heading atan=%f \n\r",heading);
00029     heading -= DECLINATION; //correct for geo location
00030     if(heading>180.0) heading = heading - 360.0;
00031     else if(heading<-180.0) heading = 360.0 + heading;
00032     else if(heading<0.0) heading = 360.0  + heading;
00033 
00034 
00035     // Convert everything from radians to degrees:
00036     //heading *= 180.0 / PI;
00037     pitch *= 180.0 / PI;
00038     roll  *= 180.0 / PI;
00039 
00040     pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
00041     pc.printf("Magnetic Heading: %f degress\n\r",heading);
00042 }
00043 
00044 
00045 
00046 
00047 int main()
00048 {
00049     //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
00050     LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
00051     IMU.begin();
00052     if (!IMU.begin()) {
00053         pc.printf("Failed to communicate with LSM9DS1.\n");
00054     }
00055     IMU.calibrate(1);
00056     IMU.calibrateMag(0);
00057     while(1) {
00058         while(!IMU.tempAvailable());
00059         IMU.readTemp();
00060         while(!IMU.magAvailable(X_AXIS));
00061         IMU.readMag();
00062         while(!IMU.accelAvailable());
00063         IMU.readAccel();
00064         while(!IMU.gyroAvailable());
00065         IMU.readGyro();
00066         pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
00067         pc.printf("        X axis    Y axis    Z axis\n\r");
00068         pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
00069         pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
00070         pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00071         printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
00072                       IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00073         myled = 1;
00074         wait(0.5);
00075         myled = 0;
00076         wait(0.5);
00077     }
00078 }
00079