Diff: main.cpp

Revision:

1:ba2d31e3112d

Parent:

0:4b3d36de811a

--- a/main.cpp	Thu Feb 21 00:24:06 2013 +0000
+++ b/main.cpp	Sat Mar 25 16:48:32 2017 +0000
@@ -1,26 +1,42 @@
 #include "mbed.h"
 #include "L3GD20.h"
 #include "LSM303DLHC.h"
- 
-L3GD20 gyro(p28, p27);
+#include "math.h"
+#include "arm_math.h"
+#include "sensfusion9.h"
+L3GD20 gyro(D14, D15);
 Serial debug(USBTX,USBRX);
-LSM303DLHC compass(p28, p27);
+LSM303DLHC compass(D14, D15);
 
+float ax, ay, az;
+float mx, my, mz;
+float gx, gy, gz;
+float roll, pitch, yaw;
+Ticker kalmanTimer;
+float dt = 0.01;
 
+void attitudeUpdate(void) { 
+}
 
-  int main() {
-    float ax, ay, az;
-    float mx, my, mz;
-    float gx, gy, gz;
-    debug.format(8,Serial::None,1);
+int main() {
+    //inizializzo filtro di kalman
+    sensfusion9Init();
+    //kalmanTimer.attach(&attitudeUpdate, dt);
+    
+    debug.format(8, Serial::None, 1);
     debug.baud(115200);
-    debug.printf("miniimu-9 v2 Test");
-
+    
+    //lunghezza asta
+    float l = 1.0f;
     while(1) {
-      
-      compass.read(&ax, &ay, &az, &mx, &my, &mz);
-      gyro.read(&gx, &gy, &gz);
-      debug.printf("a %.4f %.4f %.4f m %.4f %.4f %.4f m %.4f %.4f %.4f\n\r",ax,ay,az,mx,my,mz,gx,gy,gz);
-      wait(0.05);
+        compass.read(&ax, &ay, &az, &mx, &my, &mz);
+        gyro.read(&gx, &gy, &gz);
+        sensfusion9UpdateQ(gx+(ax/l)*dt, gy+(ay/l)*dt, gz+(az/l)*dt, ax, ay, az, mx, my, mz, dt);
+        sensfusion9GetEulerRPY(&roll, &pitch, &yaw);
+        //debug.printf("%.10f %.10f %.10f\n\r", mx, my, mz);
+        //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);
+        //debug.printf("R:%.4f P:%.4f Y:%.4f\n\r", roll, pitch, yaw);
+        debug.printf("%.4f %.4f %.4f\n\r", roll, pitch+3, yaw);
+        wait(dt);
     }
-  }
\ No newline at end of file
+}
\ No newline at end of file