main.cpp

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