Allows for reading accelerometer, gyroscope, and magnetometer data from an LSM9DS0 IMU device
Dependents: uVGA_4180 uLCD_4180_mini ECE4781_Project
main.cpp@3:7ede465deae1, 2014-11-23 (annotated)
- Committer:
- randrews33
- Date:
- Sun Nov 23 17:46:45 2014 +0000
- Revision:
- 3:7ede465deae1
- Parent:
- 0:1b975a6ae539
- Child:
- 4:bf8f4e7c9905
Fixed some compiler errors that slipped through the initial commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
randrews33 | 0:1b975a6ae539 | 1 | #include "LSM9DS0.h" |
randrews33 | 0:1b975a6ae539 | 2 | #include "mbed.h" |
randrews33 | 0:1b975a6ae539 | 3 | |
randrews33 | 0:1b975a6ae539 | 4 | // SDO_XM and SDO_G are both grounded, so our addresses are: |
randrews33 | 0:1b975a6ae539 | 5 | #define LSM9DS0_XM 0x1E // Would be 0x1E if SDO_XM is LOW |
randrews33 | 0:1b975a6ae539 | 6 | #define LSM9DS0_G 0x6A // Would be 0x6A if SDO_G is LOW |
randrews33 | 0:1b975a6ae539 | 7 | |
randrews33 | 0:1b975a6ae539 | 8 | // Create an instance of the LSM9DS0 library called `dof` the |
randrews33 | 0:1b975a6ae539 | 9 | // parameters for this constructor are: |
randrews33 | 0:1b975a6ae539 | 10 | // pins,[gyro I2C address],[xm I2C add.] |
randrews33 | 0:1b975a6ae539 | 11 | LSM9DS0 dof(p28, p27, LSM9DS0_G, LSM9DS0_XM); |
randrews33 | 0:1b975a6ae539 | 12 | DigitalIn DReady(p23); |
randrews33 | 0:1b975a6ae539 | 13 | |
randrews33 | 3:7ede465deae1 | 14 | DigitalOut led1(LED1); |
randrews33 | 3:7ede465deae1 | 15 | DigitalOut led2(LED2); |
randrews33 | 3:7ede465deae1 | 16 | |
randrews33 | 0:1b975a6ae539 | 17 | Serial pc(USBTX, USBRX); // tx, rx |
randrews33 | 0:1b975a6ae539 | 18 | |
randrews33 | 3:7ede465deae1 | 19 | bool printM = true; |
randrews33 | 3:7ede465deae1 | 20 | bool printA = true; |
randrews33 | 3:7ede465deae1 | 21 | bool printG = true; |
randrews33 | 0:1b975a6ae539 | 22 | bool printHead = true; |
randrews33 | 0:1b975a6ae539 | 23 | |
randrews33 | 0:1b975a6ae539 | 24 | void setup() |
randrews33 | 0:1b975a6ae539 | 25 | { |
randrews33 | 0:1b975a6ae539 | 26 | pc.baud(115200); // Start serial at 115200 bps |
randrews33 | 0:1b975a6ae539 | 27 | // Use the begin() function to initialize the LSM9DS0 library. |
randrews33 | 0:1b975a6ae539 | 28 | // You can either call it with no parameters (the easy way): |
randrews33 | 0:1b975a6ae539 | 29 | uint16_t status = dof.begin(); |
randrews33 | 0:1b975a6ae539 | 30 | |
randrews33 | 0:1b975a6ae539 | 31 | // Or call it with declarations for sensor scales and data rates: |
randrews33 | 0:1b975a6ae539 | 32 | //uint16_t status = dof.begin(dof.G_SCALE_2000DPS, |
randrews33 | 0:1b975a6ae539 | 33 | // dof.A_SCALE_6G, dof.M_SCALE_2GS); |
randrews33 | 0:1b975a6ae539 | 34 | |
randrews33 | 0:1b975a6ae539 | 35 | // begin() returns a 16-bit value which includes both the gyro |
randrews33 | 0:1b975a6ae539 | 36 | // and accelerometers WHO_AM_I response. You can check this to |
randrews33 | 0:1b975a6ae539 | 37 | // make sure communication was successful. |
randrews33 | 0:1b975a6ae539 | 38 | pc.printf("LSM9DS0 WHO_AM_I's returned: 0x"); |
randrews33 | 0:1b975a6ae539 | 39 | pc.printf("%x\n",status); |
randrews33 | 0:1b975a6ae539 | 40 | pc.printf("Should be 0x49D4\n"); |
randrews33 | 0:1b975a6ae539 | 41 | pc.printf("\n"); |
randrews33 | 0:1b975a6ae539 | 42 | } |
randrews33 | 0:1b975a6ae539 | 43 | |
randrews33 | 0:1b975a6ae539 | 44 | void printGyro() { |
randrews33 | 0:1b975a6ae539 | 45 | dof.readGyro(); |
randrews33 | 0:1b975a6ae539 | 46 | |
randrews33 | 0:1b975a6ae539 | 47 | pc.printf("G: "); |
randrews33 | 0:1b975a6ae539 | 48 | |
randrews33 | 0:1b975a6ae539 | 49 | pc.printf("%2f",dof.calcGyro(dof.gx)); |
randrews33 | 0:1b975a6ae539 | 50 | pc.printf(", "); |
randrews33 | 0:1b975a6ae539 | 51 | pc.printf("%2f",dof.calcGyro(dof.gy)); |
randrews33 | 0:1b975a6ae539 | 52 | pc.printf(", "); |
randrews33 | 0:1b975a6ae539 | 53 | pc.printf("%2f\n",dof.calcGyro(dof.gz)); |
randrews33 | 0:1b975a6ae539 | 54 | } |
randrews33 | 0:1b975a6ae539 | 55 | |
randrews33 | 0:1b975a6ae539 | 56 | void printAccel() { |
randrews33 | 0:1b975a6ae539 | 57 | dof.readAccel(); |
randrews33 | 0:1b975a6ae539 | 58 | |
randrews33 | 0:1b975a6ae539 | 59 | pc.printf("A: "); |
randrews33 | 0:1b975a6ae539 | 60 | |
randrews33 | 0:1b975a6ae539 | 61 | pc.printf("%2f",dof.calcAccel(dof.ax)); |
randrews33 | 0:1b975a6ae539 | 62 | pc.printf(", "); |
randrews33 | 0:1b975a6ae539 | 63 | pc.printf("%2f",dof.calcAccel(dof.ay)); |
randrews33 | 0:1b975a6ae539 | 64 | pc.printf(", "); |
randrews33 | 0:1b975a6ae539 | 65 | pc.printf("%2f\n",dof.calcAccel(dof.az)); |
randrews33 | 0:1b975a6ae539 | 66 | |
randrews33 | 0:1b975a6ae539 | 67 | } |
randrews33 | 0:1b975a6ae539 | 68 | |
randrews33 | 0:1b975a6ae539 | 69 | void printMag() { |
randrews33 | 0:1b975a6ae539 | 70 | dof.readMag(); |
randrews33 | 0:1b975a6ae539 | 71 | |
randrews33 | 0:1b975a6ae539 | 72 | pc.printf("M: "); |
randrews33 | 0:1b975a6ae539 | 73 | |
randrews33 | 0:1b975a6ae539 | 74 | pc.printf("%2f",dof.calcMag(dof.mx)); |
randrews33 | 0:1b975a6ae539 | 75 | pc.printf(", "); |
randrews33 | 0:1b975a6ae539 | 76 | pc.printf("%2f",dof.calcMag(dof.my)); |
randrews33 | 0:1b975a6ae539 | 77 | pc.printf(", "); |
randrews33 | 0:1b975a6ae539 | 78 | pc.printf("%2f\n",dof.calcMag(dof.mz)); |
randrews33 | 0:1b975a6ae539 | 79 | } |
randrews33 | 0:1b975a6ae539 | 80 | |
randrews33 | 0:1b975a6ae539 | 81 | // Here's a fun function to calculate your heading, using Earth's |
randrews33 | 0:1b975a6ae539 | 82 | // magnetic field. |
randrews33 | 0:1b975a6ae539 | 83 | // It only works if the sensor is flat (z-axis normal to Earth). |
randrews33 | 0:1b975a6ae539 | 84 | // Additionally, you may need to add or subtract a declination |
randrews33 | 0:1b975a6ae539 | 85 | // angle to get the heading normalized to your location. |
randrews33 | 0:1b975a6ae539 | 86 | // See: http://www.ngdc.noaa.gov/geomag/declination.shtml |
randrews33 | 0:1b975a6ae539 | 87 | void printHeading(float hx, float hy) |
randrews33 | 0:1b975a6ae539 | 88 | { |
randrews33 | 0:1b975a6ae539 | 89 | float heading; |
randrews33 | 0:1b975a6ae539 | 90 | |
randrews33 | 0:1b975a6ae539 | 91 | if (hy > 0) heading = 90 - (atan(hx / hy) * (180 / 3.14)); |
randrews33 | 0:1b975a6ae539 | 92 | else if (hy < 0) heading = - (atan(hx / hy) * (180 / 3.14)); |
randrews33 | 0:1b975a6ae539 | 93 | else // hy = 0 |
randrews33 | 0:1b975a6ae539 | 94 | { |
randrews33 | 0:1b975a6ae539 | 95 | if (hx < 0) heading = 180; |
randrews33 | 0:1b975a6ae539 | 96 | else heading = 0; |
randrews33 | 0:1b975a6ae539 | 97 | } |
randrews33 | 0:1b975a6ae539 | 98 | |
randrews33 | 0:1b975a6ae539 | 99 | pc.printf("Heading: "); |
randrews33 | 0:1b975a6ae539 | 100 | pc.printf("%2f\n",heading); |
randrews33 | 0:1b975a6ae539 | 101 | } |
randrews33 | 0:1b975a6ae539 | 102 | |
randrews33 | 0:1b975a6ae539 | 103 | // Another fun function that does calculations based on the |
randrews33 | 0:1b975a6ae539 | 104 | // acclerometer data. This function will print your LSM9DS0's |
randrews33 | 0:1b975a6ae539 | 105 | // orientation -- it's roll and pitch angles. |
randrews33 | 0:1b975a6ae539 | 106 | void printOrientation(float x, float y, float z) |
randrews33 | 0:1b975a6ae539 | 107 | { |
randrews33 | 0:1b975a6ae539 | 108 | float pitch, roll; |
randrews33 | 0:1b975a6ae539 | 109 | |
randrews33 | 0:1b975a6ae539 | 110 | pitch = atan2(x, sqrt(y * y) + (z * z)); |
randrews33 | 0:1b975a6ae539 | 111 | roll = atan2(y, sqrt(x * x) + (z * z)); |
randrews33 | 0:1b975a6ae539 | 112 | pitch *= 180.0 / 3.14; |
randrews33 | 0:1b975a6ae539 | 113 | roll *= 180.0 / 3.14; |
randrews33 | 0:1b975a6ae539 | 114 | |
randrews33 | 0:1b975a6ae539 | 115 | pc.printf("Pitch, Roll: "); |
randrews33 | 0:1b975a6ae539 | 116 | pc.printf("%2f",pitch); |
randrews33 | 0:1b975a6ae539 | 117 | pc.printf(", "); |
randrews33 | 0:1b975a6ae539 | 118 | pc.printf("%2f\n",roll); |
randrews33 | 0:1b975a6ae539 | 119 | } |
randrews33 | 0:1b975a6ae539 | 120 | |
randrews33 | 0:1b975a6ae539 | 121 | void readData() { |
randrews33 | 0:1b975a6ae539 | 122 | // To read from the device, you must first call the |
randrews33 | 0:1b975a6ae539 | 123 | // readMag(), readAccel(), and readGyro() functions. |
randrews33 | 0:1b975a6ae539 | 124 | // When this exits, it'll update the appropriate |
randrews33 | 0:1b975a6ae539 | 125 | // variables ([mx, my, mz], [ax, ay, az], [gx, gy, gz]) |
randrews33 | 0:1b975a6ae539 | 126 | // with the most current data. |
randrews33 | 0:1b975a6ae539 | 127 | |
randrews33 | 0:1b975a6ae539 | 128 | dof.readMag(); |
randrews33 | 0:1b975a6ae539 | 129 | dof.readAccel(); |
randrews33 | 0:1b975a6ae539 | 130 | dof.readGyro(); |
randrews33 | 0:1b975a6ae539 | 131 | } |
randrews33 | 0:1b975a6ae539 | 132 | |
randrews33 | 0:1b975a6ae539 | 133 | void loop() { |
randrews33 | 0:1b975a6ae539 | 134 | // Loop until the Data Ready signal goes high |
randrews33 | 3:7ede465deae1 | 135 | led2 = 1; |
randrews33 | 3:7ede465deae1 | 136 | while (!DReady) {} |
randrews33 | 3:7ede465deae1 | 137 | led2 = 0; |
randrews33 | 0:1b975a6ae539 | 138 | |
randrews33 | 0:1b975a6ae539 | 139 | readData(); |
randrews33 | 0:1b975a6ae539 | 140 | |
randrews33 | 3:7ede465deae1 | 141 | if (printG) printGyro(); // Print "G: gx, gy, gz" |
randrews33 | 3:7ede465deae1 | 142 | if (printA) printAccel(); // Print "A: ax, ay, az" |
randrews33 | 3:7ede465deae1 | 143 | if (printM) printMag(); // Print "M: mx, my, mz" |
randrews33 | 0:1b975a6ae539 | 144 | |
randrews33 | 0:1b975a6ae539 | 145 | // Print the heading and orientation for fun! |
randrews33 | 0:1b975a6ae539 | 146 | if (printHead) printHeading((float) dof.mx, (float) dof.my); |
randrews33 | 0:1b975a6ae539 | 147 | //printOrientation(dof.calcAccel(dof.ax), dof.calcAccel(dof.ay), |
randrews33 | 0:1b975a6ae539 | 148 | // dof.calcAccel(dof.az)); |
randrews33 | 0:1b975a6ae539 | 149 | pc.printf("\n"); |
randrews33 | 0:1b975a6ae539 | 150 | } |
randrews33 | 0:1b975a6ae539 | 151 | |
randrews33 | 0:1b975a6ae539 | 152 | |
randrews33 | 0:1b975a6ae539 | 153 | int main() |
randrews33 | 0:1b975a6ae539 | 154 | { |
randrews33 | 0:1b975a6ae539 | 155 | setup(); |
randrews33 | 0:1b975a6ae539 | 156 | while (true) |
randrews33 | 0:1b975a6ae539 | 157 | { |
randrews33 | 3:7ede465deae1 | 158 | led1 = !led1; |
randrews33 | 0:1b975a6ae539 | 159 | loop(); |
randrews33 | 3:7ede465deae1 | 160 | wait(2); |
randrews33 | 0:1b975a6ae539 | 161 | } |
randrews33 | 0:1b975a6ae539 | 162 | } |