Terrabots / Mbed 2 deprecated IMU

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed LSM9DS1_Library

IMU.cpp@2:ae1bfb236387, 2017-02-16 (annotated)

Committer:
jcallahan1
Date:
Thu Feb 16 18:25:00 2017 +0000
Revision:
2:ae1bfb236387
Parent:
1:419c1baddb68
Child:
4:ab08a70e0d7d
Error in constructor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jcallahan1 2:ae1bfb236387 1 #include "mbed.h"
jcallahan1 2:ae1bfb236387 2 #include "LSM9DS1.h"
jcallahan1 2:ae1bfb236387 3 #include "IMU.h"
jcallahan1 2:ae1bfb236387 4
jcallahan1 2:ae1bfb236387 5 #define PI 3.1514
jcallahan1 2:ae1bfb236387 6
jcallahan1 2:ae1bfb236387 7 IMU::IMU() {
jcallahan1 2:ae1bfb236387 8
jcallahan1 2:ae1bfb236387 9 LSM9DS1 I(p9, p10, 0xD6, 0x3C);
jcallahan1 2:ae1bfb236387 10 I.begin();
jcallahan1 2:ae1bfb236387 11 if (!I.begin()) {
jcallahan1 2:ae1bfb236387 12 printf("Failed to communicate with LSM9DS1.\n");
jcallahan1 2:ae1bfb236387 13 }
jcallahan1 2:ae1bfb236387 14 I.calibrate(1);
jcallahan1 2:ae1bfb236387 15 bool state;
jcallahan1 2:ae1bfb236387 16 float ax01 = I.calcAccel(I.ax);
jcallahan1 2:ae1bfb236387 17 float ay01 = I.calcAccel(I.ay);
jcallahan1 2:ae1bfb236387 18 float az01 = I.calcAccel(I.az);
jcallahan1 2:ae1bfb236387 19 float ax0 = ax01;
jcallahan1 2:ae1bfb236387 20 float ay0 = ay01;
jcallahan1 2:ae1bfb236387 21 float az0 = az01;
jcallahan1 2:ae1bfb236387 22 float ax;
jcallahan1 2:ae1bfb236387 23 float ay;
jcallahan1 2:ae1bfb236387 24 float az;
jcallahan1 2:ae1bfb236387 25 float theta;
jcallahan1 2:ae1bfb236387 26 }
jcallahan1 2:ae1bfb236387 27
jcallahan1 2:ae1bfb236387 28 void IMU::trackBed()
jcallahan1 2:ae1bfb236387 29 {
jcallahan1 2:ae1bfb236387 30 if (state != 0) {
jcallahan1 2:ae1bfb236387 31 return;
jcallahan1 2:ae1bfb236387 32 }
jcallahan1 2:ae1bfb236387 33 else {
jcallahan1 2:ae1bfb236387 34 while(state == 0) {
jcallahan1 2:ae1bfb236387 35
jcallahan1 2:ae1bfb236387 36 while(!I.accelAvailable());
jcallahan1 2:ae1bfb236387 37 I.readAccel();
jcallahan1 2:ae1bfb236387 38
jcallahan1 2:ae1bfb236387 39 // current a = <ax,ay,az>
jcallahan1 2:ae1bfb236387 40 ax = I.calcAccel(I.ax);
jcallahan1 2:ae1bfb236387 41 ay = I.calcAccel(I.ay);
jcallahan1 2:ae1bfb236387 42 az = I.calcAccel(I.az);
jcallahan1 2:ae1bfb236387 43 // theta in degrees between a0 and a
jcallahan1 2:ae1bfb236387 44 theta = acos((ax0*ax + ay0*ay + az0*az)/(sqrt(ax0*ax0+ay0*ay0+az0*az0)*sqrt(ax*ax+ay*ay+az*az)))*180/PI;
jcallahan1 2:ae1bfb236387 45 }
jcallahan1 2:ae1bfb236387 46 }
jcallahan1 2:ae1bfb236387 47 }
jcallahan1 2:ae1bfb236387 48
jcallahan1 2:ae1bfb236387 49 void IMU::trackHeading()
jcallahan1 2:ae1bfb236387 50 {
jcallahan1 2:ae1bfb236387 51 if (state != 1) {
jcallahan1 2:ae1bfb236387 52 return;
jcallahan1 2:ae1bfb236387 53 }
jcallahan1 2:ae1bfb236387 54 else {
jcallahan1 2:ae1bfb236387 55 while (state == 1) {
jcallahan1 2:ae1bfb236387 56
jcallahan1 2:ae1bfb236387 57 while(!I.accelAvailable());
jcallahan1 2:ae1bfb236387 58
jcallahan1 2:ae1bfb236387 59 I.readAccel();
jcallahan1 2:ae1bfb236387 60 ax0 = I.calcAccel(I.ax);
jcallahan1 2:ae1bfb236387 61 ay0 = I.calcAccel(I.ay);
jcallahan1 2:ae1bfb236387 62 az0 = I.calcAccel(I.az);
jcallahan1 2:ae1bfb236387 63 }
jcallahan1 2:ae1bfb236387 64 }
jcallahan1 2:ae1bfb236387 65 }
jcallahan1 2:ae1bfb236387 66