ece4180_team / Mbed 2 deprecated lab2_part3

more code

Dependencies:   4DGL-uLCD-SE LSM9DS1_Library_cal mbed

main.cpp@11:36209d0b82ea, 2022-09-16 (annotated)

Committer:
mus3
Date:
Fri Sep 16 20:27:40 2022 +0000
Revision:
11:36209d0b82ea
Parent:
10:22f453bd8038 
sort of working code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
4180_1 8:31e63caf37e2 1 // uLCD-144-G2 demo program for uLCD-4GL LCD driver library
4180_1 0:cfcf73272647 2 //
4180_1 0:cfcf73272647 3 #include "mbed.h"
4180_1 2:75727e89a717 4 #include "uLCD_4DGL.h"
mus3 10:22f453bd8038 5 #include "LSM9DS1.h"
mus3 10:22f453bd8038 6 #define PI 3.14159
mus3 10:22f453bd8038 7 #define DECLINATION -4.94
4180_1 0:cfcf73272647 8
mus3 10:22f453bd8038 9 uLCD_4DGL uLCD(p28,p27,p30); // serial tx, serial rx, reset pin;
4180_1 7:7bd7397ab89f 10
mus3 11:36209d0b82ea 11 int xc = 64;
mus3 11:36209d0b82ea 12 int yc = 64;
mus3 11:36209d0b82ea 13
mus3 10:22f453bd8038 14 DigitalOut myled(LED1);
mus3 10:22f453bd8038 15 Serial pc(USBTX, USBRX);
mus3 10:22f453bd8038 16 // Calculate pitch, roll, and heading.
mus3 10:22f453bd8038 17 // Pitch/roll calculations taken from this app note:
mus3 10:22f453bd8038 18 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
mus3 10:22f453bd8038 19 // Heading calculations taken from this app note:
mus3 10:22f453bd8038 20 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
mus3 11:36209d0b82ea 21 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
mus3 10:22f453bd8038 22 {
mus3 11:36209d0b82ea 23 float roll = atan2(ay, az);
mus3 11:36209d0b82ea 24 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
mus3 10:22f453bd8038 25 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
mus3 10:22f453bd8038 26 mx = -mx;
mus3 10:22f453bd8038 27 float heading;
mus3 10:22f453bd8038 28 if (my == 0.0)
mus3 10:22f453bd8038 29 heading = (mx < 0.0) ? 180.0 : 0.0;
mus3 10:22f453bd8038 30 else
mus3 10:22f453bd8038 31 heading = atan2(mx, my)*360.0/(2.0*PI);
mus3 10:22f453bd8038 32 //pc.printf("heading atan=%f \n\r",heading);
mus3 10:22f453bd8038 33 heading -= DECLINATION; //correct for geo location
mus3 10:22f453bd8038 34 if(heading>180.0) heading = heading - 360.0;
mus3 10:22f453bd8038 35 else if(heading<-180.0) heading = 360.0 + heading;
mus3 10:22f453bd8038 36 else if(heading<0.0) heading = 360.0 + heading;
mus3 10:22f453bd8038 37
mus3 10:22f453bd8038 38
mus3 10:22f453bd8038 39 // Convert everything from radians to degrees:
mus3 10:22f453bd8038 40 //heading *= 180.0 / PI;
mus3 10:22f453bd8038 41 pitch *= 180.0 / PI;
mus3 10:22f453bd8038 42 roll *= 180.0 / PI;
mus3 10:22f453bd8038 43
mus3 10:22f453bd8038 44 pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
mus3 10:22f453bd8038 45 pc.printf("Magnetic Heading: %f degress\n\r",heading);
mus3 11:36209d0b82ea 46
mus3 11:36209d0b82ea 47 uLCD.filled_circle(xc, yc, 10, BLACK);
mus3 11:36209d0b82ea 48 xc = (int) ((roll + 180.0) * 16.0 / 45.0);
mus3 11:36209d0b82ea 49 if (xc > 110) {
mus3 11:36209d0b82ea 50 xc = 110;
mus3 11:36209d0b82ea 51 } else if (xc < 18) {
mus3 11:36209d0b82ea 52 xc = 18;
mus3 11:36209d0b82ea 53 }
mus3 11:36209d0b82ea 54 yc = (int) ((pitch + 180.0) * 16.0 / 45.0);
mus3 11:36209d0b82ea 55 if (yc > 110) {
mus3 11:36209d0b82ea 56 yc = 110;
mus3 11:36209d0b82ea 57 } else if (yc < 18) {
mus3 11:36209d0b82ea 58 yc = 18;
mus3 11:36209d0b82ea 59 }
mus3 11:36209d0b82ea 60 uLCD.filled_circle(xc, yc, 10, RED);
mus3 11:36209d0b82ea 61
mus3 11:36209d0b82ea 62 pc.printf("xc: %d, yc: %d degress\n\r",xc,yc);
4180_1 6:f752accd632c 63 }
4180_1 7:7bd7397ab89f 64
4180_1 7:7bd7397ab89f 65
4180_1 8:31e63caf37e2 66
mus3 10:22f453bd8038 67
mus3 10:22f453bd8038 68 int main()
mus3 10:22f453bd8038 69 {
mus3 10:22f453bd8038 70 uLCD.circle(64, 64, 60, WHITE);
mus3 10:22f453bd8038 71 uLCD.filled_circle(64, 64, 10, RED);
mus3 10:22f453bd8038 72 //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
mus3 10:22f453bd8038 73 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
mus3 10:22f453bd8038 74 IMU.begin();
mus3 10:22f453bd8038 75 if (!IMU.begin()) {
mus3 10:22f453bd8038 76 pc.printf("Failed to communicate with LSM9DS1.\n");
mus3 10:22f453bd8038 77 }
mus3 10:22f453bd8038 78 IMU.calibrate(1);
mus3 10:22f453bd8038 79 IMU.calibrateMag(0);
mus3 10:22f453bd8038 80 while(1) {
mus3 10:22f453bd8038 81 while(!IMU.tempAvailable());
mus3 10:22f453bd8038 82 IMU.readTemp();
mus3 10:22f453bd8038 83 while(!IMU.magAvailable(X_AXIS));
mus3 10:22f453bd8038 84 IMU.readMag();
mus3 10:22f453bd8038 85 while(!IMU.accelAvailable());
mus3 10:22f453bd8038 86 IMU.readAccel();
mus3 10:22f453bd8038 87 while(!IMU.gyroAvailable());
mus3 10:22f453bd8038 88 IMU.readGyro();
mus3 10:22f453bd8038 89 pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
mus3 10:22f453bd8038 90 pc.printf(" X axis Y axis Z axis\n\r");
mus3 10:22f453bd8038 91 pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
mus3 10:22f453bd8038 92 pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
mus3 10:22f453bd8038 93 pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
mus3 10:22f453bd8038 94 printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
mus3 10:22f453bd8038 95 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
mus3 10:22f453bd8038 96 myled = 1;
mus3 10:22f453bd8038 97 wait(0.1);
mus3 10:22f453bd8038 98 myled = 0;
mus3 10:22f453bd8038 99 wait(0.1);
mus3 10:22f453bd8038 100 }
mus3 10:22f453bd8038 101 }