main.cpp

00001 #include "LSM9DS1.h"
00002 #include "MadgwickUpdate.h"
00003 
00004 Serial pc(USBTX, USBRX);
00005 
00006 Timer t;
00007 int lastPrint = 0;
00008 int printInterval = 500;
00009 
00010 RawSerial dev( p13, p14 );
00011 
00012 int calibrate = 1;
00013 
00014 void dev_recv()
00015 {
00016     while(dev.readable()) {
00017         if ( dev.getc() == 'r' ){
00018                 calibrate = 1;
00019         }
00020     }
00021 }
00022 
00023 int main() {
00024     
00025     LSM9DS1 lol(p28, p27, 0xD6, 0x3C);
00026     
00027     t.start();
00028 
00029     dev.baud(9600);
00030     dev.attach(&dev_recv, Serial::RxIrq);
00031 
00032     lol.begin();
00033     if (!lol.begin()) {
00034         pc.printf("Failed to communicate with LSM9DS1.\n");
00035     }
00036 
00037     while(1) {
00038 
00039         if( calibrate ){
00040             lol.calibrate();
00041             firstUpdate = t.read_us();
00042             lastUpdate = firstUpdate;
00043             q[0] = 1.0f; q[1] = 0.0f; q[2] = 0.0f; q[3] = 0.0f; 
00044             calibrate = 0;
00045         }
00046         
00047         if ( lol.accelAvailable() || lol.gyroAvailable() ) {
00048          
00049             lol.readAccel();
00050             lol.readGyro();
00051             
00052             Now = t.read_us();
00053             deltat = (float)((Now - lastUpdate)/1000000.0f);
00054             lastUpdate = Now;
00055             
00056             float ax = lol.calcAccel(lol.ax);
00057             float ay = lol.calcAccel(lol.ay);
00058             float az = lol.calcAccel(lol.az);
00059             
00060             float gx = lol.calcGyro(lol.gx);
00061             float gy = lol.calcGyro(lol.gy);
00062             float gz = lol.calcGyro(lol.gz);
00063             
00064             
00065             // switch x and y to convert to right handed coordinate system
00066             MadgwickQuaternionUpdate( ay, ax, az, gy*PI/180.0f, gx*PI/180.0f, gz*PI/180.0f );
00067             
00068             int nowMs = t.read_ms();
00069             if( nowMs - lastPrint > printInterval ){
00070                 lastPrint = nowMs;
00071                 pc.printf("gyro: %f %f %f\n\r", gx, gy, gz);
00072                 pc.printf("accel: %f %f %f\n\r", ax, ay, az);
00073                 pc.printf("quat: %f %f %f %f\n\r", q[0], q[1], q[2], q[3]);
00074                 char* str = (char*) &q;
00075                 int i = 0;
00076                 while (i < 16){
00077                     dev.putc( str[i] );
00078                     i += 1;   
00079                 }
00080             }
00081             
00082         }
00083     }
00084 }