Naoya Muramatsu / Mbed 2 deprecated LSM9DS0

publish

Dependencies:   mbed

Dependents:   RoboCup_2015

Fork of LSM9DS0 by Taylor Andrews

main.cpp@4:7ede465deae1, 2014-11-23 (annotated)

Committer:
randrews33
Date:
Sun Nov 23 17:46:45 2014 +0000
Revision:
4:7ede465deae1
Parent:
0:1b975a6ae539
Child:
5:bf8f4e7c9905
Fixed some compiler errors that slipped through the initial commit

Who changed what in which revision?

UserRevisionLine numberNew 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 4:7ede465deae1 14 DigitalOut led1(LED1);
randrews33 4:7ede465deae1 15 DigitalOut led2(LED2);
randrews33 4:7ede465deae1 16
randrews33 0:1b975a6ae539 17 Serial pc(USBTX, USBRX); // tx, rx
randrews33 0:1b975a6ae539 18
randrews33 4:7ede465deae1 19 bool printM = true;
randrews33 4:7ede465deae1 20 bool printA = true;
randrews33 4: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 4:7ede465deae1 135 led2 = 1;
randrews33 4:7ede465deae1 136 while (!DReady) {}
randrews33 4:7ede465deae1 137 led2 = 0;
randrews33 0:1b975a6ae539 138
randrews33 0:1b975a6ae539 139 readData();
randrews33 0:1b975a6ae539 140
randrews33 4:7ede465deae1 141 if (printG) printGyro(); // Print "G: gx, gy, gz"
randrews33 4:7ede465deae1 142 if (printA) printAccel(); // Print "A: ax, ay, az"
randrews33 4: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 4:7ede465deae1 158 led1 = !led1;
randrews33 0:1b975a6ae539 159 loop();
randrews33 4:7ede465deae1 160 wait(2);
randrews33 0:1b975a6ae539 161 }
randrews33 0:1b975a6ae539 162 }