Kevin Abraham / Mbed 2 deprecated 4180Lab2-3E

ECE 4180 Lab 2 Part 3 Extra Credit

Dependencies:   4DGL-uLCD-SE LSM9DS1_Library_cal mbed

Fork of uLCD144G2_demo by jim hamblen

main.cpp@10:a35b7bd001ba, 2018-10-09 (annotated)

Committer:
abraha2d
Date:
Tue Oct 09 00:47:47 2018 +0000
Revision:
10:a35b7bd001ba
Parent:
8:31e63caf37e2 
Save point

Who changed what in which revision?

UserRevisionLine numberNew contents of line
4180_1 0:cfcf73272647 1 #include "mbed.h"
abraha2d 10:a35b7bd001ba 2 #include "LSM9DS1.h"
4180_1 2:75727e89a717 3 #include "uLCD_4DGL.h"
4180_1 0:cfcf73272647 4
abraha2d 10:a35b7bd001ba 5 #define PI 3.14159
abraha2d 10:a35b7bd001ba 6
abraha2d 10:a35b7bd001ba 7 // Earth's magnetic field varies by location. Add or subtract
abraha2d 10:a35b7bd001ba 8 // a declination to get a more accurate heading. Calculate
abraha2d 10:a35b7bd001ba 9 // your's here:
abraha2d 10:a35b7bd001ba 10 // http://www.ngdc.noaa.gov/geomag-web/#declination
abraha2d 10:a35b7bd001ba 11 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
abraha2d 10:a35b7bd001ba 12
abraha2d 10:a35b7bd001ba 13 Serial pc(USBTX, USBRX);
4180_1 2:75727e89a717 14 uLCD_4DGL uLCD(p9,p10,p11); // serial tx, serial rx, reset pin;
4180_1 0:cfcf73272647 15
abraha2d 10:a35b7bd001ba 16 // Calculate heading.
abraha2d 10:a35b7bd001ba 17 // Heading calculations taken from this app note:
abraha2d 10:a35b7bd001ba 18 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
abraha2d 10:a35b7bd001ba 19 float calcHeading(float mx, float my, float mz)
abraha2d 10:a35b7bd001ba 20 {
abraha2d 10:a35b7bd001ba 21 mx = -mx;
abraha2d 10:a35b7bd001ba 22 float heading;
abraha2d 10:a35b7bd001ba 23 if (my == 0.0)
abraha2d 10:a35b7bd001ba 24 heading = (mx < 0.0) ? 180.0 : 0.0;
abraha2d 10:a35b7bd001ba 25 else
abraha2d 10:a35b7bd001ba 26 heading = atan2(mx, my) * 360.0 / (2.0 * PI);
abraha2d 10:a35b7bd001ba 27 heading -= DECLINATION;
abraha2d 10:a35b7bd001ba 28 if (heading > 180.0) heading = heading - 360.0;
abraha2d 10:a35b7bd001ba 29 else if (heading < -180.0) heading = 360.0 + heading;
abraha2d 10:a35b7bd001ba 30 else if (heading < 0.0) heading = 360.0 + heading;
abraha2d 10:a35b7bd001ba 31 return heading;
abraha2d 10:a35b7bd001ba 32 }
abraha2d 10:a35b7bd001ba 33
abraha2d 10:a35b7bd001ba 34 float rotate_point_x(float x, float y, float a)
abraha2d 10:a35b7bd001ba 35 {
abraha2d 10:a35b7bd001ba 36 return x * cos(a * PI/180.0) - y * sin(a * PI/180.0);
abraha2d 10:a35b7bd001ba 37 }
abraha2d 10:a35b7bd001ba 38
abraha2d 10:a35b7bd001ba 39 float rotate_point_y(float x, float y, float a)
abraha2d 10:a35b7bd001ba 40 {
abraha2d 10:a35b7bd001ba 41 return y * cos(a * PI/180.0) + x * sin(a * PI/180.0);
abraha2d 10:a35b7bd001ba 42 }
abraha2d 10:a35b7bd001ba 43
4180_1 2:75727e89a717 44 int main()
4180_1 2:75727e89a717 45 {
4180_1 3:454d1f4c8fd7 46 uLCD.cls();
abraha2d 10:a35b7bd001ba 47 uLCD.printf("Baud rate: 3000000");
abraha2d 10:a35b7bd001ba 48 uLCD.baudrate(3000000);
abraha2d 10:a35b7bd001ba 49
abraha2d 10:a35b7bd001ba 50 uLCD.printf("Calibrating IMU...\n");
abraha2d 10:a35b7bd001ba 51 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
abraha2d 10:a35b7bd001ba 52 bool success = IMU.begin();
abraha2d 10:a35b7bd001ba 53 if (!success) {
abraha2d 10:a35b7bd001ba 54 uLCD.printf("Failed to communicate with LSM9DS1.\n");
4180_1 7:7bd7397ab89f 55 }
abraha2d 10:a35b7bd001ba 56 IMU.calibrate(1);
abraha2d 10:a35b7bd001ba 57 IMU.calibrateMag(0);
4180_1 7:7bd7397ab89f 58
4180_1 7:7bd7397ab89f 59 wait(0.5);
4180_1 3:454d1f4c8fd7 60 uLCD.cls();
4180_1 7:7bd7397ab89f 61 while(1) {
abraha2d 10:a35b7bd001ba 62 while(!IMU.magAvailable(X_AXIS));
abraha2d 10:a35b7bd001ba 63 IMU.readMag();
abraha2d 10:a35b7bd001ba 64 float heading = calcHeading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
abraha2d 10:a35b7bd001ba 65 pc.printf("%f ", heading);
abraha2d 10:a35b7bd001ba 66 uLCD.circle(64, 64, 48, WHITE);
abraha2d 10:a35b7bd001ba 67 uLCD.triangle(
abraha2d 10:a35b7bd001ba 68 64 + rotate_point_x(-5, 10, heading), 64 + rotate_point_y(-5, 10, heading),
abraha2d 10:a35b7bd001ba 69 64 + rotate_point_x(5, 10, heading), 64 + rotate_point_y(5, 10, heading),
abraha2d 10:a35b7bd001ba 70 64 + rotate_point_x(0, -15, heading), 64 + rotate_point_y(0, -15, heading),
abraha2d 10:a35b7bd001ba 71 RED);
abraha2d 10:a35b7bd001ba 72 wait(0.05);
abraha2d 10:a35b7bd001ba 73 uLCD.triangle(
abraha2d 10:a35b7bd001ba 74 64 + rotate_point_x(-5, 10, heading), 64 + rotate_point_y(-5, 10, heading),
abraha2d 10:a35b7bd001ba 75 64 + rotate_point_x(5, 10, heading), 64 + rotate_point_y(5, 10, heading),
abraha2d 10:a35b7bd001ba 76 64 + rotate_point_x(0, -15, heading), 64 + rotate_point_y(0, -15, heading),
abraha2d 10:a35b7bd001ba 77 BLACK);
4180_1 5:a1ef40ff0f78 78 }
4180_1 6:f752accd632c 79 }