LSM9DS1 with EFM32WG
Dependencies: mbed PinDetect LSM9DS1_works
Dependents: WORKS_VERSION_1_sERIAL_PROCESSING LSM9DS1_works
main.cpp
- Committer:
- chebbi
- Date:
- 2020-07-02
- Revision:
- 5:b7855a02ddf6
- Parent:
- 3:64a8188c5a44
File content as of revision 5:b7855a02ddf6:
#include "LSM9DS1.h" //DigitalOut myled(LED1); Serial pc(USBTX, USBRX); #define PI 3.14 #define printff_CALCULATED #define printff_SPEED 250 #define DECLINATION -8.58 #define dt 1/975 #define gey_sensitivity 8.75 void printfGyro(); void printfAccel(); void printfMag(); void printfAttitude(float ax, float ay, float az, float mx, float my, float mz); LSM9DS1 lol(PC4, PC5,0xD6, 0x3C); int main() { //LSM9DS1 lol(p9, p10, 0x6B, 0x1E); lol.begin(); if (!lol.begin()) { pc.printf("Failed to communicate with LSM9DS1.\n"); while(1) ; } lol.calibrate(); lol.calibrateMag() ; while(1) { //lol.readTemp(); if ( lol.magAvailable() ) { lol.readMag(); } if ( lol.accelAvailable() ) { lol.readAccel(); } if ( lol.gyroAvailable() ) { lol.readGyro(); } printfGyro(); // printfff "G: gx, gy, gz" pc.printf("\n") ; printfAccel(); // printfff "A: ax, ay, az" pc.printf("\n") ; printfMag(); // printfff "M: mx, my, mz" pc.printf("\n") ; // printff the heading and orientation fofun! // Call printfff attitude. The LSM9DS1's mag x and y // axes are opposite to the accelerometer, so my, mx are // substituted for each other. printfAttitude(lol.ax, lol.ay, lol.az,lol.gx, lol.gy, lol.gz); pc.printf("\n") ; wait(1); } } void printfGyro() { // Now we can use the gx, gy, and gz variables as we please. // Either printfff them as raw ADC values, or calculated in DPS. pc.printf("G: "); #ifdef printff_CALCULATED // If you want to printffff calculated values, you can use the // calcGyro helper function to convert a raw ADC value to // DPS. Give the function the value that you want to convert. pc.printf("%f" , lol.calcGyro(lol.gx), 2); pc.printf(", "); pc.printf("%f" , lol.calcGyro(lol.gy), 2); pc.printf(", "); pc.printf("%f" , lol.calcGyro(lol.gz), 2); pc.printf(" deg/s "); pc.printf("\n") ; /*#elif defined printfff_RAW pc.printf(lol.gx); pc.printf(", "); pc.printf(lol.gy); pc.printf(", "); pc.printfln(lol.gz);*/ #endif } void printfAccel() { // Now we can use the ax, ay, and az variables as we please. // Either printfff them as raw ADC values, or calculated in g's. pc.printf("A: "); #ifdef printff_CALCULATED // If you want to printffff calculated values, you can use the // calcAccel helper function to convert a raw ADC value to // g's. Give the function the value that you want to convert. pc.printf("%f" , lol.calcAccel(lol.ax), 2); pc.printf(", "); pc.printf("%f" , lol.calcAccel(lol.ay), 2); pc.printf(", "); pc.printf("%f" , lol.calcAccel(lol.az), 2); pc.printf(" g"); /*#elif defined printfff_RAW pc.printf(lol.ax); pc.printf(", "); pc.printf(lol.ay); pc.printf(", "); pc.printfln(lol.az);*/ #endif } void printfMag() { // Now we can use the mx, my, and mz variables as we please. // Either printfff them as raw ADC values, or calculated in Gauss. pc.printf("M: "); #ifdef printff_CALCULATED // If you want to printffff calculated values, you can use the // calcMag helper function to convert a raw ADC value to // Gauss. Give the function the value that you want to convert. pc.printf("%f" , lol.calcMag(lol.mx), 2); pc.printf(", "); pc.printf("%f" , lol.calcMag(lol.my), 2); pc.printf(", "); pc.printf("%f" , lol.calcMag(lol.mz), 2); pc.printf(" gauss"); /*#elif defined printfff_RAW pc.printf(lol.mx); pc.printf(", "); pc.printf(lol.my); pc.printf(", "); pc.printfln(lol.mz);*/ #endif } void printfAttitude(float ax, float ay, float az, float gx, float gy, float gz) { //double roll = atan2(ay, az); //double pitch = atan2(-ax, sqrt(ay * ay + az * az)); double *pitch , *roll ; *pitch +=((float)gx /gey_sensitivity)*dt ; *roll -= ((float)gy /gey_sensitivity)*dt ; double pitchacc = atan2(ay,az)*180/PI ; *pitch = *pitch *0,98+pitchacc*0.02 ; double rollacc =atan2(ax,az) *180/PI; *roll = *roll *0,98+rollacc*0.02 ; /* double heading; if (my == 0) heading = (mx < 0) ? PI : 0; else heading = atan2(-my, mx); heading -= DECLINATION * PI / 180; if (heading > 3.14) heading -= (2 * 3.14); else if (heading < -3.14) heading += (2 * 3.14); // Convert everything from radians to degrees: heading *= 180.0 / PI;*/ *pitch *= 180.0 / PI; *roll *= 180.0 / PI; pc.printf("Pitch, Roll: "); pc.printf("%f" , *pitch, 2); pc.printf(", "); pc.printf("%f" , *roll, 2); }