Terrabots / Mbed 2 deprecated IMU_LSM9DS1

IMU demo

Dependencies:   LSM9DS1_Library_cal mbed

main.cpp@0:9a033a3d8481, 2017-02-09 (annotated)

Committer:
gkang34
Date:
Thu Feb 09 19:46:20 2017 +0000
Revision:
0:9a033a3d8481
IMU calibrates and outputs angle

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gkang34 0:9a033a3d8481 1 #include "mbed.h"
gkang34 0:9a033a3d8481 2 #include "LSM9DS1.h"
gkang34 0:9a033a3d8481 3 #define PI 3.14159
gkang34 0:9a033a3d8481 4 // Earth's magnetic field varies by location. Add or subtract
gkang34 0:9a033a3d8481 5 // a declination to get a more accurate heading. Calculate
gkang34 0:9a033a3d8481 6 // your's here:
gkang34 0:9a033a3d8481 7 // http://www.ngdc.noaa.gov/geomag-web/#declination
gkang34 0:9a033a3d8481 8 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
gkang34 0:9a033a3d8481 9
gkang34 0:9a033a3d8481 10 DigitalOut myled(LED1);
gkang34 0:9a033a3d8481 11 Serial pc(USBTX, USBRX);
gkang34 0:9a033a3d8481 12 // Calculate pitch, roll, and heading.
gkang34 0:9a033a3d8481 13 // Pitch/roll calculations taken from this app note:
gkang34 0:9a033a3d8481 14 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
gkang34 0:9a033a3d8481 15 // Heading calculations taken from this app note:
gkang34 0:9a033a3d8481 16 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
gkang34 0:9a033a3d8481 17 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
gkang34 0:9a033a3d8481 18 {
gkang34 0:9a033a3d8481 19 float roll = atan2(ay, az);
gkang34 0:9a033a3d8481 20 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
gkang34 0:9a033a3d8481 21 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
gkang34 0:9a033a3d8481 22 mx = -mx;
gkang34 0:9a033a3d8481 23 float heading;
gkang34 0:9a033a3d8481 24 if (my == 0.0)
gkang34 0:9a033a3d8481 25 heading = (mx < 0.0) ? 180.0 : 0.0;
gkang34 0:9a033a3d8481 26 else
gkang34 0:9a033a3d8481 27 heading = atan2(mx, my)*360.0/(2.0*PI);
gkang34 0:9a033a3d8481 28 //pc.printf("heading atan=%f \n\r",heading);
gkang34 0:9a033a3d8481 29 heading -= DECLINATION; //correct for geo location
gkang34 0:9a033a3d8481 30 if(heading>180.0) heading = heading - 360.0;
gkang34 0:9a033a3d8481 31 else if(heading<-180.0) heading = 360.0 + heading;
gkang34 0:9a033a3d8481 32 else if(heading<0.0) heading = 360.0 + heading;
gkang34 0:9a033a3d8481 33
gkang34 0:9a033a3d8481 34
gkang34 0:9a033a3d8481 35 // Convert everything from radians to degrees:
gkang34 0:9a033a3d8481 36 //heading *= 180.0 / PI;
gkang34 0:9a033a3d8481 37 pitch *= 180.0 / PI;
gkang34 0:9a033a3d8481 38 roll *= 180.0 / PI;
gkang34 0:9a033a3d8481 39
gkang34 0:9a033a3d8481 40 pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
gkang34 0:9a033a3d8481 41 pc.printf("Magnetic Heading: %f degress\n\r",heading);
gkang34 0:9a033a3d8481 42 }
gkang34 0:9a033a3d8481 43
gkang34 0:9a033a3d8481 44
gkang34 0:9a033a3d8481 45
gkang34 0:9a033a3d8481 46
gkang34 0:9a033a3d8481 47 int main()
gkang34 0:9a033a3d8481 48 {
gkang34 0:9a033a3d8481 49 //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
gkang34 0:9a033a3d8481 50 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
gkang34 0:9a033a3d8481 51 IMU.begin();
gkang34 0:9a033a3d8481 52 if (!IMU.begin()) {
gkang34 0:9a033a3d8481 53 pc.printf("Failed to communicate with LSM9DS1.\n");
gkang34 0:9a033a3d8481 54 }
gkang34 0:9a033a3d8481 55 IMU.calibrate(1);
gkang34 0:9a033a3d8481 56
gkang34 0:9a033a3d8481 57 // init a0
gkang34 0:9a033a3d8481 58 float ax0 = IMU.calcAccel(IMU.ax);
gkang34 0:9a033a3d8481 59 float ay0 = IMU.calcAccel(IMU.ay);
gkang34 0:9a033a3d8481 60 float az0 = IMU.calcAccel(IMU.az);
gkang34 0:9a033a3d8481 61
gkang34 0:9a033a3d8481 62 //IMU.calibrateMag(0);
gkang34 0:9a033a3d8481 63 while(1) {
gkang34 0:9a033a3d8481 64
gkang34 0:9a033a3d8481 65 while(!IMU.accelAvailable());
gkang34 0:9a033a3d8481 66 IMU.readAccel();
gkang34 0:9a033a3d8481 67
gkang34 0:9a033a3d8481 68 // current a = <ax,ay,az>
gkang34 0:9a033a3d8481 69 float ax = IMU.calcAccel(IMU.ax);
gkang34 0:9a033a3d8481 70 float ay = IMU.calcAccel(IMU.ay);
gkang34 0:9a033a3d8481 71 float az = IMU.calcAccel(IMU.az);
gkang34 0:9a033a3d8481 72 // theta in degrees between a0 and a
gkang34 0:9a033a3d8481 73 float theta = acos((ax0*ax + ay0*ay + az0*az)/(sqrt(ax0*ax0+ay0*ay0+az0*az0)*sqrt(ax*ax+ay*ay+az*az)))*180/PI;
gkang34 0:9a033a3d8481 74
gkang34 0:9a033a3d8481 75 //while(!IMU.tempAvailable());
gkang34 0:9a033a3d8481 76 //IMU.readTemp();
gkang34 0:9a033a3d8481 77 //while(!IMU.magAvailable(X_AXIS));
gkang34 0:9a033a3d8481 78 //IMU.readMag();
gkang34 0:9a033a3d8481 79 //while(!IMU.accelAvailable());
gkang34 0:9a033a3d8481 80 //IMU.readAccel();
gkang34 0:9a033a3d8481 81 //while(!IMU.gyroAvailable());
gkang34 0:9a033a3d8481 82 //IMU.readGyro();
gkang34 0:9a033a3d8481 83 //pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
gkang34 0:9a033a3d8481 84 pc.printf(" X axis Y axis Z axis\n\r");
gkang34 0:9a033a3d8481 85 //pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
gkang34 0:9a033a3d8481 86 pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
gkang34 0:9a033a3d8481 87 //pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
gkang34 0:9a033a3d8481 88 //printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
gkang34 0:9a033a3d8481 89 // IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
gkang34 0:9a033a3d8481 90 pc.printf("angle: %9f in degrees\n\r", theta);
gkang34 0:9a033a3d8481 91 myled = 1;
gkang34 0:9a033a3d8481 92 wait(0.5);
gkang34 0:9a033a3d8481 93 myled = 0;
gkang34 0:9a033a3d8481 94 wait(0.5);
gkang34 0:9a033a3d8481 95 }
gkang34 0:9a033a3d8481 96 }
gkang34 0:9a033a3d8481 97