main.cpp

00001 #include "LSM9DS1.h"
00002 #include "MadgwickUpdate.h"
00003 
00004 #include "USBSerial.h"
00005 USBSerial pc(0x1f00, 0x2012, 0x0001, false);
00006 Timer t;
00007 int lastPrint = 0;
00008 int printInterval = 3;
00009 // https://github.com/kriswiner/LSM9DS1/blob/master/LSM9DS1_MS5611_BasicAHRS_t3.ino
00010 int main()
00011 {
00012 
00013     wait(1);
00014     pc.printf("HOLLA ! .\n");
00015     LSM9DS1 lol(PB_9, PB_8, 0xD4, 0x38);
00016     t.start();
00017     lol.begin();
00018     if (!lol.begin()) {
00019         pc.printf("Failed to communicate with LSM9DS1.\n");
00020     }
00021     lol.calibrate();
00022     while(1) {
00023         firstUpdate = t.read_ms();
00024         lastUpdate = firstUpdate;
00025         q[0] = 1.0f;q[1] = 0.0f;q[2] = 0.0f;q[3] = 0.0f;
00026         if ( lol.accelAvailable() || lol.gyroAvailable() ) {
00027 
00028             lol.readAccel();
00029             lol.readGyro();
00030 
00031             Now = t.read_ms();
00032             deltat = (float)((Now - lastUpdate)/1000000.0f);
00033             lastUpdate = Now;
00034 
00035             float ax = lol.calcAccel(lol.ax);float ay = lol.calcAccel(lol.ay);float az = lol.calcAccel(lol.az);
00036             float gx = lol.calcGyro(lol.gx);float gy = lol.calcGyro(lol.gy);float gz = lol.calcGyro(lol.gz);
00037             float mx = lol.calcMag(lol.mx);float my = lol.calcMag(lol.my);float mz = lol.calcMag(lol.mz);
00038             // switch x and y to convert to right handed coordinate system
00039             //MadgwickQuaternionUpdate( ay, ax, az, gy*PI/180.0f, gx*PI/180.0f, gz*PI/180.0f );
00040 
00041             int nowMs = t.read_ms();
00042             if( nowMs - lastPrint > printInterval ) {
00043                 lastPrint = nowMs;
00044                 //pc.printf("gyro: %f %f %f\n\r", gx, gy, gz);
00045                 //pc.printf("accel: %f %f %f\n\r", ax, ay, az);
00046                 //pc.printf("quat: %f %f %f %f\n\r", q[0], q[1], q[2], q[3]);
00047                 pc.printf("%d %f,%f,%f,%f,%f,%f\n",nowMs,ax,ay,az,gx,gy,gz);
00048 
00049             }
00050         }
00051     }
00052 }
00053