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@0:2fd46a8b3964, 2016-02-11 (annotated)

Committer:
mikeb
Date:
Thu Feb 11 22:21:06 2016 +0000
Revision:
0:2fd46a8b3964
Child:
1:6c4ba07e0c77
IMU with USB and RS232 Serial

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