m b / Mbed 2 deprecated 4180_Lab2_Mike_SD_IMU_I2C_RS232

4180 IMU parts (I2C, uLCD, RS232, USB Serial, SD)

Dependencies:   4DGL-uLCD-SE LSM9DS1_Library_cal SDFileSystem mbed

main.cpp@2:f17a67920767, 2016-02-12 (annotated)

Committer:
mikeb
Date:
Fri Feb 12 16:25:04 2016 +0000
Revision:
2:f17a67920767
Parent:
1:6c4ba07e0c77 
Added SDFile System, all untested

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikeb 0:2fd46a8b3964 1 #include "mbed.h"
mikeb 0:2fd46a8b3964 2 #include "LSM9DS1.h"
mikeb 1:6c4ba07e0c77 3 #include "uLCD_4DGL.h"
mikeb 2:f17a67920767 4 #include "SDFileSystem.h"
mikeb 0:2fd46a8b3964 5 #define PI 3.14159
mikeb 0:2fd46a8b3964 6 // Earth's magnetic field varies by location. Add or subtract
mikeb 0:2fd46a8b3964 7 // a declination to get a more accurate heading. Calculate
mikeb 0:2fd46a8b3964 8 // your's here:
mikeb 0:2fd46a8b3964 9 // http://www.ngdc.noaa.gov/geomag-web/#declination
mikeb 0:2fd46a8b3964 10 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
mikeb 0:2fd46a8b3964 11
mikeb 0:2fd46a8b3964 12 DigitalOut myled(LED1);
mikeb 0:2fd46a8b3964 13 DigitalIn sel(p21);
mikeb 0:2fd46a8b3964 14 Serial pc(USBTX, USBRX);
mikeb 0:2fd46a8b3964 15 Serial device(p13, p14);
mikeb 1:6c4ba07e0c77 16 uLCD_4DGL uLCD(p27,p28,p30); // serial tx, serial rx, reset pin;
mikeb 2:f17a67920767 17 SDFileSystem sd(p5, p6, p7, p8, "sd");
mikeb 2:f17a67920767 18 //Global variables. Whatever. Sue me.
mikeb 2:f17a67920767 19 float oldx = 0;
mikeb 2:f17a67920767 20 float oldy = 0;
mikeb 0:2fd46a8b3964 21
mikeb 0:2fd46a8b3964 22
mikeb 0:2fd46a8b3964 23 // Calculate pitch, roll, and heading.
mikeb 0:2fd46a8b3964 24 // Pitch/roll calculations taken from this app note:
mikeb 0:2fd46a8b3964 25 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
mikeb 0:2fd46a8b3964 26 // Heading calculations taken from this app note:
mikeb 0:2fd46a8b3964 27 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
mikeb 0:2fd46a8b3964 28 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
mikeb 0:2fd46a8b3964 29 {
mikeb 0:2fd46a8b3964 30 float roll = atan2(ay, az);
mikeb 0:2fd46a8b3964 31 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
mikeb 0:2fd46a8b3964 32 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
mikeb 0:2fd46a8b3964 33 mx = -mx;
mikeb 0:2fd46a8b3964 34 float heading;
mikeb 0:2fd46a8b3964 35 if (my == 0.0)
mikeb 0:2fd46a8b3964 36 heading = (mx < 0.0) ? 180.0 : 0.0;
mikeb 0:2fd46a8b3964 37 else
mikeb 0:2fd46a8b3964 38 heading = atan2(mx, my)*360.0/(2.0*PI);
mikeb 0:2fd46a8b3964 39 //pc.printf("heading atan=%f \n\r",heading);
mikeb 0:2fd46a8b3964 40 heading -= DECLINATION; //correct for geo location
mikeb 0:2fd46a8b3964 41 if(heading>180.0) heading = heading - 360.0;
mikeb 0:2fd46a8b3964 42 else if(heading<-180.0) heading = 360.0 + heading;
mikeb 0:2fd46a8b3964 43 else if(heading<0.0) heading = 360.0 + heading;
mikeb 0:2fd46a8b3964 44
mikeb 0:2fd46a8b3964 45
mikeb 0:2fd46a8b3964 46 // Convert everything from radians to degrees:
mikeb 0:2fd46a8b3964 47 //heading *= 180.0 / PI;
mikeb 0:2fd46a8b3964 48 pitch *= 180.0 / PI;
mikeb 0:2fd46a8b3964 49 roll *= 180.0 / PI;
mikeb 0:2fd46a8b3964 50 if (sel){
mikeb 0:2fd46a8b3964 51 pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
mikeb 0:2fd46a8b3964 52 pc.printf("Magnetic Heading: %f degress\n\r",heading);
mikeb 0:2fd46a8b3964 53 }
mikeb 0:2fd46a8b3964 54 else{
mikeb 0:2fd46a8b3964 55 device.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
mikeb 0:2fd46a8b3964 56 device.printf("Magnetic Heading: %f degress\n\r",heading);
mikeb 0:2fd46a8b3964 57 }
mikeb 0:2fd46a8b3964 58
mikeb 0:2fd46a8b3964 59 }
mikeb 0:2fd46a8b3964 60
mikeb 1:6c4ba07e0c77 61 void bubLev(float ax, float ay, float az, float oldx, float oldy){
mikeb 2:f17a67920767 62 uLCD.filled_circle(oldx, oldy, 5, BLACK);
mikeb 2:f17a67920767 63 uLCD.filled_circle(floor(-ax/az*40.0)+63, floor(-ay/az*40.0)+63, 5, 0xFF00FF);
mikeb 2:f17a67920767 64 oldx = floor(ax/az*40)+63;
mikeb 2:f17a67920767 65 oldy = floor(ay/az*40)+63;
mikeb 1:6c4ba07e0c77 66 }
mikeb 1:6c4ba07e0c77 67
mikeb 0:2fd46a8b3964 68
mikeb 0:2fd46a8b3964 69
mikeb 0:2fd46a8b3964 70
mikeb 0:2fd46a8b3964 71 int main()
mikeb 0:2fd46a8b3964 72 {
mikeb 0:2fd46a8b3964 73 //device.baud(19200);
mikeb 0:2fd46a8b3964 74 //device.format(8N1);
mikeb 2:f17a67920767 75
mikeb 2:f17a67920767 76 mkdir("/sd/mydir", 0777);
mikeb 2:f17a67920767 77 FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
mikeb 2:f17a67920767 78 if(fp == NULL) {
mikeb 2:f17a67920767 79 error("Could not open file for write\n");
mikeb 2:f17a67920767 80 }
mikeb 2:f17a67920767 81 fprintf(fp, "Hello SD File World!\n Mike and Gedeon Lab 2.\n");
mikeb 2:f17a67920767 82 fclose(fp);
mikeb 2:f17a67920767 83
mikeb 2:f17a67920767 84 char myString [100];
mikeb 2:f17a67920767 85
mikeb 2:f17a67920767 86 FILE *fr = fopen("/sd/mydir/sdtest.txt", "r");
mikeb 2:f17a67920767 87 if (fgets(myString, 100, fr) != NULL){
mikeb 2:f17a67920767 88 pc.printf("%s\n", myString);
mikeb 2:f17a67920767 89 }
mikeb 2:f17a67920767 90 fclose(fr);
mikeb 2:f17a67920767 91 wait(2);
mikeb 2:f17a67920767 92
mikeb 0:2fd46a8b3964 93 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
mikeb 0:2fd46a8b3964 94 sel.mode(PullUp);
mikeb 1:6c4ba07e0c77 95 uLCD.baudrate(3000000);
mikeb 0:2fd46a8b3964 96 IMU.begin();
mikeb 0:2fd46a8b3964 97 if (!IMU.begin()) {
mikeb 0:2fd46a8b3964 98 pc.printf("Failed to communicate with LSM9DS1.\n");
mikeb 0:2fd46a8b3964 99 }
mikeb 0:2fd46a8b3964 100 IMU.calibrate(1);
mikeb 0:2fd46a8b3964 101 IMU.calibrateMag(0);
mikeb 2:f17a67920767 102 uLCD.filled_circle(63, 63, 40, 0x0000FF);
mikeb 0:2fd46a8b3964 103 while(1) {
mikeb 0:2fd46a8b3964 104 while(!IMU.tempAvailable());
mikeb 0:2fd46a8b3964 105 IMU.readTemp();
mikeb 0:2fd46a8b3964 106 while(!IMU.magAvailable(X_AXIS));
mikeb 0:2fd46a8b3964 107 IMU.readMag();
mikeb 0:2fd46a8b3964 108 while(!IMU.accelAvailable());
mikeb 0:2fd46a8b3964 109 IMU.readAccel();
mikeb 0:2fd46a8b3964 110 while(!IMU.gyroAvailable());
mikeb 0:2fd46a8b3964 111 IMU.readGyro();
mikeb 0:2fd46a8b3964 112 if (sel){
mikeb 0:2fd46a8b3964 113 pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
mikeb 0:2fd46a8b3964 114 pc.printf(" X axis Y axis Z axis\n\r");
mikeb 0:2fd46a8b3964 115 pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
mikeb 0:2fd46a8b3964 116 pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
mikeb 0:2fd46a8b3964 117 pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
mikeb 0:2fd46a8b3964 118 printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
mikeb 0:2fd46a8b3964 119 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
mikeb 0:2fd46a8b3964 120 }
mikeb 0:2fd46a8b3964 121 else{
mikeb 0:2fd46a8b3964 122 device.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
mikeb 0:2fd46a8b3964 123 device.printf(" X axis Y axis Z axis\n\r");
mikeb 0:2fd46a8b3964 124 device.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
mikeb 0:2fd46a8b3964 125 device.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
mikeb 0:2fd46a8b3964 126 device.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
mikeb 0:2fd46a8b3964 127 printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
mikeb 0:2fd46a8b3964 128 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
mikeb 0:2fd46a8b3964 129 }
mikeb 2:f17a67920767 130
mikeb 2:f17a67920767 131 bubLev(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), oldx, oldy);
mikeb 0:2fd46a8b3964 132 myled = 1;
mikeb 0:2fd46a8b3964 133 wait(0.5);
mikeb 0:2fd46a8b3964 134 myled = 0;
mikeb 0:2fd46a8b3964 135 wait(0.5);
mikeb 0:2fd46a8b3964 136 }
mikeb 0:2fd46a8b3964 137 }