LSM9DS1 with EFM32WG
Dependencies: mbed PinDetect LSM9DS1_works
Dependents: WORKS_VERSION_1_sERIAL_PROCESSING LSM9DS1_works
main.cpp@5:b7855a02ddf6, 2020-07-02 (annotated)
- Committer:
- chebbi
- Date:
- Thu Jul 02 08:25:06 2020 +0000
- Revision:
- 5:b7855a02ddf6
- Parent:
- 3:64a8188c5a44
pointeur_version;
Who changed what in which revision?
User | Revision | Line number | New 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 | } |