Christopher Pollard / Mbed 2 deprecated BLE_IMU_Visualization

Visualize IMU data sent over BLE on a computer

Dependencies:   PinDetect mbed

Fork of LSM9DS1_Library by jim hamblen

main.cpp@3:fd549671e512, 2016-03-13 (annotated)

Committer:
cvmp94
Date:
Sun Mar 13 21:40:10 2016 +0000
Revision:
3:fd549671e512
Parent:
0:e8167f37725c 
initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jmar7 0:e8167f37725c 1 #include "LSM9DS1.h"
cvmp94 3:fd549671e512 2 #include "MadgwickUpdate.h"
jmar7 0:e8167f37725c 3
jmar7 0:e8167f37725c 4 Serial pc(USBTX, USBRX);
jmar7 0:e8167f37725c 5
cvmp94 3:fd549671e512 6 Timer t;
cvmp94 3:fd549671e512 7 int lastPrint = 0;
cvmp94 3:fd549671e512 8 int printInterval = 500;
cvmp94 3:fd549671e512 9
cvmp94 3:fd549671e512 10 RawSerial dev( p13, p14 );
cvmp94 3:fd549671e512 11
cvmp94 3:fd549671e512 12 int calibrate = 1;
cvmp94 3:fd549671e512 13
cvmp94 3:fd549671e512 14 void dev_recv()
cvmp94 3:fd549671e512 15 {
cvmp94 3:fd549671e512 16 while(dev.readable()) {
cvmp94 3:fd549671e512 17 if ( dev.getc() == 'r' ){
cvmp94 3:fd549671e512 18 calibrate = 1;
cvmp94 3:fd549671e512 19 }
cvmp94 3:fd549671e512 20 }
cvmp94 3:fd549671e512 21 }
cvmp94 3:fd549671e512 22
jmar7 0:e8167f37725c 23 int main() {
cvmp94 3:fd549671e512 24
cvmp94 3:fd549671e512 25 LSM9DS1 lol(p28, p27, 0xD6, 0x3C);
cvmp94 3:fd549671e512 26
cvmp94 3:fd549671e512 27 t.start();
cvmp94 3:fd549671e512 28
cvmp94 3:fd549671e512 29 dev.baud(9600);
cvmp94 3:fd549671e512 30 dev.attach(&dev_recv, Serial::RxIrq);
cvmp94 3:fd549671e512 31
jmar7 0:e8167f37725c 32 lol.begin();
jmar7 0:e8167f37725c 33 if (!lol.begin()) {
jmar7 0:e8167f37725c 34 pc.printf("Failed to communicate with LSM9DS1.\n");
jmar7 0:e8167f37725c 35 }
cvmp94 3:fd549671e512 36
jmar7 0:e8167f37725c 37 while(1) {
cvmp94 3:fd549671e512 38
cvmp94 3:fd549671e512 39 if( calibrate ){
cvmp94 3:fd549671e512 40 lol.calibrate();
cvmp94 3:fd549671e512 41 firstUpdate = t.read_us();
cvmp94 3:fd549671e512 42 lastUpdate = firstUpdate;
cvmp94 3:fd549671e512 43 q[0] = 1.0f; q[1] = 0.0f; q[2] = 0.0f; q[3] = 0.0f;
cvmp94 3:fd549671e512 44 calibrate = 0;
cvmp94 3:fd549671e512 45 }
jmar7 0:e8167f37725c 46
cvmp94 3:fd549671e512 47 if ( lol.accelAvailable() || lol.gyroAvailable() ) {
cvmp94 3:fd549671e512 48
cvmp94 3:fd549671e512 49 lol.readAccel();
cvmp94 3:fd549671e512 50 lol.readGyro();
cvmp94 3:fd549671e512 51
cvmp94 3:fd549671e512 52 Now = t.read_us();
cvmp94 3:fd549671e512 53 deltat = (float)((Now - lastUpdate)/1000000.0f);
cvmp94 3:fd549671e512 54 lastUpdate = Now;
cvmp94 3:fd549671e512 55
cvmp94 3:fd549671e512 56 float ax = lol.calcAccel(lol.ax);
cvmp94 3:fd549671e512 57 float ay = lol.calcAccel(lol.ay);
cvmp94 3:fd549671e512 58 float az = lol.calcAccel(lol.az);
cvmp94 3:fd549671e512 59
cvmp94 3:fd549671e512 60 float gx = lol.calcGyro(lol.gx);
cvmp94 3:fd549671e512 61 float gy = lol.calcGyro(lol.gy);
cvmp94 3:fd549671e512 62 float gz = lol.calcGyro(lol.gz);
cvmp94 3:fd549671e512 63
cvmp94 3:fd549671e512 64
cvmp94 3:fd549671e512 65 // switch x and y to convert to right handed coordinate system
cvmp94 3:fd549671e512 66 MadgwickQuaternionUpdate( ay, ax, az, gy*PI/180.0f, gx*PI/180.0f, gz*PI/180.0f );
cvmp94 3:fd549671e512 67
cvmp94 3:fd549671e512 68 int nowMs = t.read_ms();
cvmp94 3:fd549671e512 69 if( nowMs - lastPrint > printInterval ){
cvmp94 3:fd549671e512 70 lastPrint = nowMs;
cvmp94 3:fd549671e512 71 pc.printf("gyro: %f %f %f\n\r", gx, gy, gz);
cvmp94 3:fd549671e512 72 pc.printf("accel: %f %f %f\n\r", ax, ay, az);
cvmp94 3:fd549671e512 73 pc.printf("quat: %f %f %f %f\n\r", q[0], q[1], q[2], q[3]);
cvmp94 3:fd549671e512 74 char* str = (char*) &q;
cvmp94 3:fd549671e512 75 int i = 0;
cvmp94 3:fd549671e512 76 while (i < 16){
cvmp94 3:fd549671e512 77 dev.putc( str[i] );
cvmp94 3:fd549671e512 78 i += 1;
cvmp94 3:fd549671e512 79 }
cvmp94 3:fd549671e512 80 }
cvmp94 3:fd549671e512 81
cvmp94 3:fd549671e512 82 }
jmar7 0:e8167f37725c 83 }
jmar7 0:e8167f37725c 84 }