main.cpp

00001 // uLCD-144-G2 demo program for uLCD-4GL LCD driver library
00002 //
00003 #include "mbed.h"
00004 #include "uLCD_4DGL.h"
00005 #include "LSM9DS1.h"
00006 #define PI 3.14159
00007 #define DECLINATION -4.94
00008 
00009 uLCD_4DGL uLCD(p28,p27,p30); // serial tx, serial rx, reset pin;
00010 
00011 int xc = 64;
00012 int yc = 64;
00013 
00014 DigitalOut myled(LED1);
00015 Serial pc(USBTX, USBRX);
00016 // Calculate pitch, roll, and heading.
00017 // Pitch/roll calculations taken from this app note:
00018 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
00019 // Heading calculations taken from this app note:
00020 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
00021 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
00022 {
00023     float roll = atan2(ay, az);
00024     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
00025 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
00026     mx = -mx;
00027     float heading;
00028     if (my == 0.0)
00029         heading = (mx < 0.0) ? 180.0 : 0.0;
00030     else
00031         heading = atan2(mx, my)*360.0/(2.0*PI);
00032     //pc.printf("heading atan=%f \n\r",heading);
00033     heading -= DECLINATION; //correct for geo location
00034     if(heading>180.0) heading = heading - 360.0;
00035     else if(heading<-180.0) heading = 360.0 + heading;
00036     else if(heading<0.0) heading = 360.0  + heading;
00037 
00038 
00039     // Convert everything from radians to degrees:
00040     //heading *= 180.0 / PI;
00041     pitch *= 180.0 / PI;
00042     roll  *= 180.0 / PI;
00043 
00044     pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
00045     pc.printf("Magnetic Heading: %f degress\n\r",heading);
00046     
00047     uLCD.filled_circle(xc, yc, 10, BLACK);
00048     xc = (int) ((roll + 180.0) * 16.0 / 45.0);
00049     if (xc > 110) {
00050         xc = 110;
00051     } else if (xc < 18) {
00052         xc = 18;
00053     }
00054     yc = (int) ((pitch + 180.0) * 16.0 / 45.0);
00055     if (yc > 110) {
00056         yc = 110;
00057     } else if (yc < 18) {
00058         yc = 18;
00059     }
00060     uLCD.filled_circle(xc, yc, 10, RED);
00061     
00062     pc.printf("xc: %d,    yc: %d degress\n\r",xc,yc);
00063 }
00064 
00065 
00066 
00067 
00068 int main()
00069 {
00070     uLCD.circle(64, 64, 60, WHITE);
00071     uLCD.filled_circle(64, 64, 10, RED);
00072     //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
00073     LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
00074     IMU.begin();
00075     if (!IMU.begin()) {
00076         pc.printf("Failed to communicate with LSM9DS1.\n");
00077     }
00078     IMU.calibrate(1);
00079     IMU.calibrateMag(0);
00080     while(1) {
00081         while(!IMU.tempAvailable());
00082         IMU.readTemp();
00083         while(!IMU.magAvailable(X_AXIS));
00084         IMU.readMag();
00085         while(!IMU.accelAvailable());
00086         IMU.readAccel();
00087         while(!IMU.gyroAvailable());
00088         IMU.readGyro();
00089         pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
00090         pc.printf("        X axis    Y axis    Z axis\n\r");
00091         pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
00092         pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
00093         pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00094         printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
00095                       IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00096         myled = 1;
00097         wait(0.1);
00098         myled = 0;
00099         wait(0.1);
00100     }
00101 }