Momen Yacoub / Mbed 2 deprecated LSM9DS1_Demo_wCal_4180

imu

Dependencies:   mbed LSM9DS1_Library_cal

main.cpp@1:c769e2f74c1d, 2021-10-04 (annotated)

Committer:
myacoub
Date:
Mon Oct 04 17:23:46 2021 +0000
Revision:
1:c769e2f74c1d
Parent:
0:e693d5bf0a25 
comm

Who changed what in which revision?

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