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@1:6c4ba07e0c77, 2016-02-11 (annotated)

Committer:
mikeb
Date:
Thu Feb 11 22:46:43 2016 +0000
Revision:
1:6c4ba07e0c77
Parent:
0:2fd46a8b3964
Child:
2:f17a67920767
workin on bubble level, no tests of anything

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