ECE 4180 / Mbed 2 deprecated 4180-L2-P3

Lab 2 Part 3

Dependencies:   mbed 4DGL-uLCD-SE LSM9DS1_Library_cal

main.cpp@2:3d9e0b74f24d, 2021-02-11 (annotated)

Committer:
glanier9
Date:
Thu Feb 11 16:03:03 2021 +0000
Revision:
2:3d9e0b74f24d
Parent:
1:de9832e45f61 
Final version

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