walid chebbi / Mbed 2 deprecated LSM9DS1_works_copy

test

Dependencies:   mbed PinDetect

main.cpp@3:35ac8fecbd90, 2020-03-02 (annotated)

Committer:
chebbi
Date:
Mon Mar 02 10:05:21 2020 +0000
Revision:
3:35ac8fecbd90
Parent:
0:e8167f37725c 
test

Who changed what in which revision?

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