walid chebbi / Mbed 2 deprecated LSM9DS1_works

LSM9DS1 with EFM32WG

Dependencies:   mbed PinDetect LSM9DS1_works

Dependents:   WORKS_VERSION_1_sERIAL_PROCESSING LSM9DS1_works

main.cpp@6:8c9ce2a4afbe, 2020-07-02 (annotated)

Committer:
chebbi
Date:
Thu Jul 02 08:47:55 2020 +0000
Revision:
6:8c9ce2a4afbe
Parent:
5:b7855a02ddf6 
pointeur_with_mbedos2;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jmar7 0:e8167f37725c 1 #include "LSM9DS1.h"
jmar7 0:e8167f37725c 2
chebbi 3:64a8188c5a44 3 //DigitalOut myled(LED1);
jmar7 0:e8167f37725c 4 Serial pc(USBTX, USBRX);
chebbi 5:b7855a02ddf6 5 #define PI 3.14
chebbi 3:64a8188c5a44 6 #define printff_CALCULATED
chebbi 3:64a8188c5a44 7 #define printff_SPEED 250
chebbi 3:64a8188c5a44 8 #define DECLINATION -8.58
jmar7 0:e8167f37725c 9
chebbi 5:b7855a02ddf6 10
chebbi 5:b7855a02ddf6 11 #define dt 1/975
chebbi 5:b7855a02ddf6 12 #define gey_sensitivity 8.75
chebbi 5:b7855a02ddf6 13
chebbi 5:b7855a02ddf6 14
chebbi 3:64a8188c5a44 15 void printfGyro();
chebbi 3:64a8188c5a44 16 void printfAccel();
chebbi 3:64a8188c5a44 17 void printfMag();
chebbi 3:64a8188c5a44 18 void printfAttitude(float ax, float ay, float az, float mx, float my, float mz);
chebbi 3:64a8188c5a44 19
chebbi 3:64a8188c5a44 20 LSM9DS1 lol(PC4, PC5,0xD6, 0x3C);
jmar7 0:e8167f37725c 21 int main() {
jmar7 0:e8167f37725c 22 //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
chebbi 3:64a8188c5a44 23
jmar7 0:e8167f37725c 24 lol.begin();
jmar7 0:e8167f37725c 25 if (!lol.begin()) {
jmar7 0:e8167f37725c 26 pc.printf("Failed to communicate with LSM9DS1.\n");
chebbi 3:64a8188c5a44 27 while(1) ;
jmar7 0:e8167f37725c 28 }
jmar7 0:e8167f37725c 29 lol.calibrate();
chebbi 5:b7855a02ddf6 30 lol.calibrateMag() ;
jmar7 0:e8167f37725c 31 while(1) {
chebbi 3:64a8188c5a44 32
chebbi 3:64a8188c5a44 33 //lol.readTemp();
chebbi 3:64a8188c5a44 34
chebbi 3:64a8188c5a44 35 if ( lol.magAvailable() )
chebbi 3:64a8188c5a44 36 {
jmar7 0:e8167f37725c 37 lol.readMag();
chebbi 3:64a8188c5a44 38 }
chebbi 3:64a8188c5a44 39 if ( lol.accelAvailable() )
chebbi 3:64a8188c5a44 40 {
chebbi 3:64a8188c5a44 41 lol.readAccel();
chebbi 3:64a8188c5a44 42 }
chebbi 3:64a8188c5a44 43 if ( lol.gyroAvailable() )
chebbi 3:64a8188c5a44 44 {
jmar7 0:e8167f37725c 45 lol.readGyro();
chebbi 3:64a8188c5a44 46 }
chebbi 3:64a8188c5a44 47
chebbi 3:64a8188c5a44 48 printfGyro(); // printfff "G: gx, gy, gz"
chebbi 3:64a8188c5a44 49 pc.printf("\n") ;
chebbi 3:64a8188c5a44 50 printfAccel(); // printfff "A: ax, ay, az"
chebbi 3:64a8188c5a44 51 pc.printf("\n") ;
chebbi 3:64a8188c5a44 52 printfMag(); // printfff "M: mx, my, mz"
chebbi 3:64a8188c5a44 53 pc.printf("\n") ;
chebbi 5:b7855a02ddf6 54 // printff the heading and orientation fofun!
chebbi 3:64a8188c5a44 55 // Call printfff attitude. The LSM9DS1's mag x and y
chebbi 3:64a8188c5a44 56 // axes are opposite to the accelerometer, so my, mx are
chebbi 3:64a8188c5a44 57 // substituted for each other.
chebbi 5:b7855a02ddf6 58 printfAttitude(lol.ax, lol.ay, lol.az,lol.gx, lol.gy, lol.gz);
chebbi 3:64a8188c5a44 59 pc.printf("\n") ;
chebbi 3:64a8188c5a44 60 wait(1);
chebbi 3:64a8188c5a44 61 }
chebbi 3:64a8188c5a44 62 }
jmar7 0:e8167f37725c 63
chebbi 3:64a8188c5a44 64 void printfGyro()
chebbi 3:64a8188c5a44 65 {
chebbi 3:64a8188c5a44 66 // Now we can use the gx, gy, and gz variables as we please.
chebbi 3:64a8188c5a44 67 // Either printfff them as raw ADC values, or calculated in DPS.
chebbi 3:64a8188c5a44 68 pc.printf("G: ");
chebbi 3:64a8188c5a44 69 #ifdef printff_CALCULATED
chebbi 3:64a8188c5a44 70 // If you want to printffff calculated values, you can use the
chebbi 3:64a8188c5a44 71 // calcGyro helper function to convert a raw ADC value to
chebbi 3:64a8188c5a44 72 // DPS. Give the function the value that you want to convert.
chebbi 3:64a8188c5a44 73 pc.printf("%f" , lol.calcGyro(lol.gx), 2);
chebbi 3:64a8188c5a44 74 pc.printf(", ");
chebbi 3:64a8188c5a44 75 pc.printf("%f" , lol.calcGyro(lol.gy), 2);
chebbi 3:64a8188c5a44 76 pc.printf(", ");
chebbi 3:64a8188c5a44 77 pc.printf("%f" , lol.calcGyro(lol.gz), 2);
chebbi 3:64a8188c5a44 78 pc.printf(" deg/s ");
chebbi 3:64a8188c5a44 79 pc.printf("\n") ;
chebbi 3:64a8188c5a44 80 /*#elif defined printfff_RAW
chebbi 3:64a8188c5a44 81 pc.printf(lol.gx);
chebbi 3:64a8188c5a44 82 pc.printf(", ");
chebbi 3:64a8188c5a44 83 pc.printf(lol.gy);
chebbi 3:64a8188c5a44 84 pc.printf(", ");
chebbi 3:64a8188c5a44 85 pc.printfln(lol.gz);*/
chebbi 3:64a8188c5a44 86 #endif
chebbi 3:64a8188c5a44 87 }
chebbi 3:64a8188c5a44 88
chebbi 3:64a8188c5a44 89 void printfAccel()
chebbi 3:64a8188c5a44 90 {
chebbi 3:64a8188c5a44 91 // Now we can use the ax, ay, and az variables as we please.
chebbi 3:64a8188c5a44 92 // Either printfff them as raw ADC values, or calculated in g's.
chebbi 3:64a8188c5a44 93 pc.printf("A: ");
chebbi 3:64a8188c5a44 94 #ifdef printff_CALCULATED
chebbi 3:64a8188c5a44 95 // If you want to printffff calculated values, you can use the
chebbi 3:64a8188c5a44 96 // calcAccel helper function to convert a raw ADC value to
chebbi 3:64a8188c5a44 97 // g's. Give the function the value that you want to convert.
chebbi 3:64a8188c5a44 98 pc.printf("%f" , lol.calcAccel(lol.ax), 2);
chebbi 3:64a8188c5a44 99 pc.printf(", ");
chebbi 3:64a8188c5a44 100 pc.printf("%f" , lol.calcAccel(lol.ay), 2);
chebbi 3:64a8188c5a44 101 pc.printf(", ");
chebbi 3:64a8188c5a44 102 pc.printf("%f" , lol.calcAccel(lol.az), 2);
chebbi 3:64a8188c5a44 103 pc.printf(" g");
chebbi 3:64a8188c5a44 104 /*#elif defined printfff_RAW
chebbi 3:64a8188c5a44 105 pc.printf(lol.ax);
chebbi 3:64a8188c5a44 106 pc.printf(", ");
chebbi 3:64a8188c5a44 107 pc.printf(lol.ay);
chebbi 3:64a8188c5a44 108 pc.printf(", ");
chebbi 3:64a8188c5a44 109 pc.printfln(lol.az);*/
chebbi 3:64a8188c5a44 110
chebbi 3:64a8188c5a44 111 #endif
chebbi 3:64a8188c5a44 112
jmar7 0:e8167f37725c 113 }
chebbi 3:64a8188c5a44 114
chebbi 3:64a8188c5a44 115 void printfMag()
chebbi 3:64a8188c5a44 116 {
chebbi 3:64a8188c5a44 117 // Now we can use the mx, my, and mz variables as we please.
chebbi 3:64a8188c5a44 118 // Either printfff them as raw ADC values, or calculated in Gauss.
chebbi 3:64a8188c5a44 119 pc.printf("M: ");
chebbi 3:64a8188c5a44 120 #ifdef printff_CALCULATED
chebbi 3:64a8188c5a44 121 // If you want to printffff calculated values, you can use the
chebbi 3:64a8188c5a44 122 // calcMag helper function to convert a raw ADC value to
chebbi 3:64a8188c5a44 123 // Gauss. Give the function the value that you want to convert.
chebbi 3:64a8188c5a44 124 pc.printf("%f" , lol.calcMag(lol.mx), 2);
chebbi 3:64a8188c5a44 125 pc.printf(", ");
chebbi 3:64a8188c5a44 126 pc.printf("%f" , lol.calcMag(lol.my), 2);
chebbi 3:64a8188c5a44 127 pc.printf(", ");
chebbi 3:64a8188c5a44 128 pc.printf("%f" , lol.calcMag(lol.mz), 2);
chebbi 3:64a8188c5a44 129 pc.printf(" gauss");
chebbi 3:64a8188c5a44 130
chebbi 3:64a8188c5a44 131 /*#elif defined printfff_RAW
chebbi 3:64a8188c5a44 132 pc.printf(lol.mx);
chebbi 3:64a8188c5a44 133 pc.printf(", ");
chebbi 3:64a8188c5a44 134 pc.printf(lol.my);
chebbi 3:64a8188c5a44 135 pc.printf(", ");
chebbi 3:64a8188c5a44 136 pc.printfln(lol.mz);*/
chebbi 3:64a8188c5a44 137 #endif
chebbi 3:64a8188c5a44 138 }
chebbi 3:64a8188c5a44 139
chebbi 5:b7855a02ddf6 140 void printfAttitude(float ax, float ay, float az, float gx, float gy, float gz)
chebbi 3:64a8188c5a44 141 {
chebbi 5:b7855a02ddf6 142 //double roll = atan2(ay, az);
chebbi 5:b7855a02ddf6 143 //double pitch = atan2(-ax, sqrt(ay * ay + az * az));
chebbi 5:b7855a02ddf6 144
chebbi 5:b7855a02ddf6 145
chebbi 5:b7855a02ddf6 146 double *pitch , *roll ;
chebbi 5:b7855a02ddf6 147
chebbi 5:b7855a02ddf6 148 *pitch +=((float)gx /gey_sensitivity)*dt ;
chebbi 5:b7855a02ddf6 149 *roll -= ((float)gy /gey_sensitivity)*dt ;
chebbi 3:64a8188c5a44 150
chebbi 5:b7855a02ddf6 151 double pitchacc = atan2(ay,az)*180/PI ;
chebbi 5:b7855a02ddf6 152 *pitch = *pitch *0,98+pitchacc*0.02 ;
chebbi 5:b7855a02ddf6 153
chebbi 5:b7855a02ddf6 154 double rollacc =atan2(ax,az) *180/PI;
chebbi 5:b7855a02ddf6 155 *roll = *roll *0,98+rollacc*0.02 ;
chebbi 5:b7855a02ddf6 156
chebbi 5:b7855a02ddf6 157 /* double heading;
chebbi 3:64a8188c5a44 158 if (my == 0)
chebbi 3:64a8188c5a44 159 heading = (mx < 0) ? PI : 0;
chebbi 3:64a8188c5a44 160 else
chebbi 5:b7855a02ddf6 161 heading = atan2(-my, mx);
chebbi 3:64a8188c5a44 162
chebbi 3:64a8188c5a44 163 heading -= DECLINATION * PI / 180;
chebbi 3:64a8188c5a44 164
chebbi 5:b7855a02ddf6 165 if (heading > 3.14) heading -= (2 * 3.14);
chebbi 5:b7855a02ddf6 166 else if (heading < -3.14) heading += (2 * 3.14);
chebbi 3:64a8188c5a44 167
chebbi 3:64a8188c5a44 168 // Convert everything from radians to degrees:
chebbi 5:b7855a02ddf6 169 heading *= 180.0 / PI;*/
chebbi 5:b7855a02ddf6 170 *pitch *= 180.0 / PI;
chebbi 5:b7855a02ddf6 171 *roll *= 180.0 / PI;
chebbi 3:64a8188c5a44 172
chebbi 3:64a8188c5a44 173 pc.printf("Pitch, Roll: ");
chebbi 5:b7855a02ddf6 174 pc.printf("%f" , *pitch, 2);
chebbi 3:64a8188c5a44 175 pc.printf(", ");
chebbi 5:b7855a02ddf6 176 pc.printf("%f" , *roll, 2);
chebbi 3:64a8188c5a44 177
chebbi 5:b7855a02ddf6 178 }