main.cpp

00001 #include "LSM9DS1.h"
00002 
00003 //DigitalOut myled(LED1);
00004 Serial pc(USBTX, USBRX);
00005 #define PI 3.14 
00006 #define printff_CALCULATED
00007 #define printff_SPEED 250 
00008 #define DECLINATION -8.58
00009 
00010 void printfGyro();
00011 void printfAccel();
00012 void printfMag();
00013 void printfAttitude(float ax, float ay, float az, float mx, float my, float mz);
00014 
00015 LSM9DS1 lol(PC4, PC5,0xD6, 0x3C);
00016 int main() {
00017     //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
00018     
00019     lol.begin();
00020     if (!lol.begin()) {
00021         pc.printf("Failed to communicate with LSM9DS1.\n");
00022         while(1) ; 
00023     }
00024     lol.calibrate();
00025     lol.calibrateMag() ;
00026     while(1) {
00027         
00028         //lol.readTemp();
00029         
00030         if ( lol.magAvailable() )
00031         {
00032         lol.readMag();
00033         }
00034         if ( lol.accelAvailable() )
00035         {
00036         lol.readAccel();
00037         }
00038         if ( lol.gyroAvailable() )
00039         {
00040         lol.readGyro();
00041         }
00042         
00043       //  printfGyro();  // printfff "G: gx, gy, gz"
00044         //pc.printf("\n") ;
00045        // printfAccel(); // printfff "A: ax, ay, az"
00046         //pc.printf("\n") ;
00047        // printfMag();   // printfff "M: mx, my, mz"
00048         //pc.printf("\n") ;
00049         // printff the heading and orientation for fun!
00050         // Call printfff attitude. The LSM9DS1's mag x and y
00051         // axes are opposite to the accelerometer, so my, mx are
00052         // substituted for each other.
00053         printfAttitude(lol.ax, lol.ay, lol.az,-lol.my, -lol.mx, lol.mz);
00054        pc.printf("\n") ;
00055         wait(0.5);
00056    } 
00057    }              
00058         
00059         void printfGyro()
00060      {
00061   // Now we can use the gx, gy, and gz variables as we please.
00062   // Either printfff them as raw ADC values, or calculated in DPS.
00063         pc.printf("G: ");
00064         #ifdef printff_CALCULATED 
00065   // If you want to printffff calculated values, you can use the
00066   // calcGyro helper function to convert a raw ADC value to
00067   // DPS. Give the function the value that you want to convert.
00068   pc.printf("%f" , lol.calcGyro(lol.gx), 2);
00069   pc.printf(", ");
00070   pc.printf("%f" , lol.calcGyro(lol.gy), 2);
00071   pc.printf(", ");
00072   pc.printf("%f" , lol.calcGyro(lol.gz), 2);
00073   pc.printf(" deg/s ");
00074   pc.printf("\n") ;
00075 /*#elif defined printfff_RAW
00076   pc.printf(lol.gx);
00077   pc.printf(", ");
00078   pc.printf(lol.gy);
00079   pc.printf(", ");
00080   pc.printfln(lol.gz);*/
00081     #endif
00082 }
00083 
00084 void printfAccel()
00085 {
00086   // Now we can use the ax, ay, and az variables as we please.
00087   // Either printfff them as raw ADC values, or calculated in g's.
00088   pc.printf("A: ");
00089 
00090   // If you want to printffff calculated values, you can use the
00091   // calcAccel helper function to convert a raw ADC value to
00092   // g's. Give the function the value that you want to convert.
00093   pc.printf("%f" , lol.calcAccel(lol.ax), 2);
00094   pc.printf(", ");
00095   pc.printf("%f" , lol.calcAccel(lol.ay), 2);
00096   pc.printf(", ");
00097   pc.printf("%f" , lol.calcAccel(lol.az), 2);
00098   pc.printf(" g");
00099 /*#elif defined printfff_RAW
00100   pc.printf(lol.ax);
00101   pc.printf(", ");
00102   pc.printf(lol.ay);
00103   pc.printf(", ");
00104   pc.printfln(lol.az);*/
00105   
00106 
00107 
00108 }
00109 
00110 void printfMag()
00111 {
00112   // Now we can use the mx, my, and mz variables as we please.
00113   // Either printfff them as raw ADC values, or calculated in Gauss.
00114   pc.printf("M: ");
00115 #ifdef printff_CALCULATED
00116   // If you want to printffff calculated values, you can use the
00117   // calcMag helper function to convert a raw ADC value to
00118   // Gauss. Give the function the value that you want to convert.
00119   pc.printf("%f" , lol.calcMag(lol.mx), 2);
00120   pc.printf(", ");
00121   pc.printf("%f" , lol.calcMag(lol.my), 2);
00122   pc.printf(", ");
00123   pc.printf("%f" , lol.calcMag(lol.mz), 2);
00124   pc.printf(" gauss");
00125   
00126 /*#elif defined printfff_RAW
00127   pc.printf(lol.mx);
00128   pc.printf(", ");
00129   pc.printf(lol.my);
00130   pc.printf(", ");
00131   pc.printfln(lol.mz);*/
00132     #endif
00133 }
00134 
00135 void printfAttitude(float ax, float ay, float az, float mx, float my, float mz)
00136 {
00137   double  roll = atan2(ay, az);
00138   double  pitch = atan2(-ax, sqrt(ay * ay + az * az));
00139 
00140   double heading;
00141   /*if (my == 0)
00142     heading = (mx < 0) ? PI : 0;
00143   else*/
00144     heading = atan2(my, mx);
00145 
00146   heading -= DECLINATION * PI / 180;
00147 
00148   if (heading > 3.14) heading -= (2 * 3.14);
00149   else if (heading < -3.14) heading += (2 * 3.14);
00150 
00151   // Convert everything from radians to degrees:
00152   heading *= 180.0 / PI;
00153   pitch *= 180.0 / PI;
00154   roll  *= 180.0 / PI;
00155 
00156 
00157 
00158  
00159   pc.printf("%f" , roll, 2);
00160   pc.printf("/") ;
00161   pc.printf("%f" , pitch, 2);
00162   pc.printf("/") ;
00163   pc.printf("%f" , heading, 2);
00164 
00165   
00166 
00167   
00168 }
00169         
00170         //pc.printffff("%d %d %d %d %d %d %d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz), lol.ax, lol.ay, lol.az, lol.mx, lol.my, lol.mz);
00171         //pc.printffff("%d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz));
00172         //pc.printffff("gyro: %d %d %d\n\r", lol.gx, lol.gy, lol.gz);
00173         //pc.printffff("accel: %d %d %d\n\r", lol.ax, lol.ay, lol.az);
00174         //pc.printffff("mag: %d %d %d\n\n\r", lol.mx, lol.my, lol.mz);
00175         //myled = 1;
00176         //wait_ms(500);
00177         //myled = 0;
00178         //wait_ms(500);
00179     
00180