Visualize IMU data sent over BLE on a computer
Fork of LSM9DS1_Library by
Diff: main.cpp
- Revision:
- 3:fd549671e512
- Parent:
- 0:e8167f37725c
diff -r e8c2301f7523 -r fd549671e512 main.cpp --- a/main.cpp Wed Feb 03 18:24:29 2016 +0000 +++ b/main.cpp Sun Mar 13 21:40:10 2016 +0000 @@ -1,29 +1,84 @@ #include "LSM9DS1.h" +#include "MadgwickUpdate.h" -DigitalOut myled(LED1); Serial pc(USBTX, USBRX); +Timer t; +int lastPrint = 0; +int printInterval = 500; + +RawSerial dev( p13, p14 ); + +int calibrate = 1; + +void dev_recv() +{ + while(dev.readable()) { + if ( dev.getc() == 'r' ){ + calibrate = 1; + } + } +} + int main() { - //LSM9DS1 lol(p9, p10, 0x6B, 0x1E); - LSM9DS1 lol(p9, p10, 0xD6, 0x3C); + + LSM9DS1 lol(p28, p27, 0xD6, 0x3C); + + t.start(); + + dev.baud(9600); + dev.attach(&dev_recv, Serial::RxIrq); + lol.begin(); if (!lol.begin()) { pc.printf("Failed to communicate with LSM9DS1.\n"); } - lol.calibrate(); + while(1) { - lol.readTemp(); - lol.readMag(); - lol.readGyro(); + + if( calibrate ){ + lol.calibrate(); + firstUpdate = t.read_us(); + lastUpdate = firstUpdate; + q[0] = 1.0f; q[1] = 0.0f; q[2] = 0.0f; q[3] = 0.0f; + calibrate = 0; + } - //pc.printf("%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); - //pc.printf("%d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz)); - pc.printf("gyro: %d %d %d\n\r", lol.gx, lol.gy, lol.gz); - pc.printf("accel: %d %d %d\n\r", lol.ax, lol.ay, lol.az); - pc.printf("mag: %d %d %d\n\n\r", lol.mx, lol.my, lol.mz); - myled = 1; - wait(2); - myled = 0; - wait(2); + if ( lol.accelAvailable() || lol.gyroAvailable() ) { + + lol.readAccel(); + lol.readGyro(); + + Now = t.read_us(); + deltat = (float)((Now - lastUpdate)/1000000.0f); + lastUpdate = Now; + + float ax = lol.calcAccel(lol.ax); + float ay = lol.calcAccel(lol.ay); + float az = lol.calcAccel(lol.az); + + float gx = lol.calcGyro(lol.gx); + float gy = lol.calcGyro(lol.gy); + float gz = lol.calcGyro(lol.gz); + + + // switch x and y to convert to right handed coordinate system + MadgwickQuaternionUpdate( ay, ax, az, gy*PI/180.0f, gx*PI/180.0f, gz*PI/180.0f ); + + int nowMs = t.read_ms(); + if( nowMs - lastPrint > printInterval ){ + lastPrint = nowMs; + pc.printf("gyro: %f %f %f\n\r", gx, gy, gz); + pc.printf("accel: %f %f %f\n\r", ax, ay, az); + pc.printf("quat: %f %f %f %f\n\r", q[0], q[1], q[2], q[3]); + char* str = (char*) &q; + int i = 0; + while (i < 16){ + dev.putc( str[i] ); + i += 1; + } + } + + } } }