main.cpp

00001 #include "mbed.h"
00002 #include "L3GD20.h"
00003 #include "LSM303DLHC.h"
00004 #include "math.h"
00005 #include "arm_math.h"
00006 #include "sensfusion9.h"
00007 L3GD20 gyro(D14, D15);
00008 Serial debug(USBTX,USBRX);
00009 LSM303DLHC compass(D14, D15);
00010 
00011 float ax, ay, az;
00012 float mx, my, mz;
00013 float gx, gy, gz;
00014 float roll, pitch, yaw;
00015 Ticker kalmanTimer;
00016 float dt = 0.01;
00017 
00018 void attitudeUpdate(void) { 
00019 }
00020 
00021 int main() {
00022     //inizializzo filtro di kalman
00023     sensfusion9Init();
00024     //kalmanTimer.attach(&attitudeUpdate, dt);
00025     
00026     debug.format(8, Serial::None, 1);
00027     debug.baud(115200);
00028     
00029     //lunghezza asta
00030     float l = 1.0f;
00031     while(1) {
00032         compass.read(&ax, &ay, &az, &mx, &my, &mz);
00033         gyro.read(&gx, &gy, &gz);
00034         sensfusion9UpdateQ(gx+(ax/l)*dt, gy+(ay/l)*dt, gz+(az/l)*dt, ax, ay, az, mx, my, mz, dt);
00035         sensfusion9GetEulerRPY(&roll, &pitch, &yaw);
00036         //debug.printf("%.10f %.10f %.10f\n\r", mx, my, mz);
00037         //debug.printf("a %.4f %.4f %.4f m %.4f %.4f %.4f m %.4f %.4f %.4f R:%.4f P:%.4f Y:%.4f\n\r",ax,ay,az,mx,my,mz,gx,gy,gz,roll, pitch, yaw);
00038         //debug.printf("R:%.4f P:%.4f Y:%.4f\n\r", roll, pitch, yaw);
00039         debug.printf("%.4f %.4f %.4f\n\r", roll, pitch+3, yaw);
00040         wait(dt);
00041     }
00042 }