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