Diff: main.cpp

Revision:

25:6e43bbe76aec

Parent:

24:cbdf0f7d33bd

Child:

26:e35dd673fd13

--- a/main.cpp	Tue Sep 20 04:18:34 2016 +0000
+++ b/main.cpp	Sat Sep 24 06:10:54 2016 +0000
@@ -12,6 +12,8 @@
 DigitalOut myled(LED1);
 
 Serial gps(PTD3,PTD2);
+Serial camera(PTC17,PTC16);
+
 Adafruit_GPS myGPS(&gps);
 char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
 
@@ -51,7 +53,12 @@
 
 Timer t;
 
-void gps_thread(void const *args)
+//Worker threads
+Thread gps_t,imu_t;
+
+
+//GPS Reader worker task
+void gps_thread(void)
 {
     while (true) {
         c = myGPS.read();   //queries the GPS
@@ -67,7 +74,8 @@
     }
 }
 
-void imu_thread(void const *args)
+//IMU reader and sensor fusion worker task
+void imu_thread(void)
 {
     while(true) {
         acc.getAxis(acc_data);
@@ -104,7 +112,12 @@
         LOG("FXOS8700Q ACC: X=%d Y=%d Z=%d  ", raX, raY, raZ);
         LOG("    MAG: X=%d Y=%d Z=%d\r\n\n", rmX, rmY, rmZ);
         
-        Thread::wait(100);
+        MadgwickQuaternionUpdate(faX,faY,faZ,gyro_data[0],gyro_data[1],gyro_data[2],fmX,fmY,fmZ);
+        calcEuler();
+        
+        LOG("EULER: Y=%f R=%f P=%f\n", yaw,roll,pitch);
+        
+        Thread::wait(10);
     }
 }
 
@@ -132,6 +145,9 @@
     pc.baud(115200);
 #endif
 
+//Connect to Nikon D800 UART at 4800baud
+    camera.baud(4800);
+
     acc.enable();
     LOG("\r\n\nFXOS8700Q Who Am I= %X\r\n", acc.whoAmI());
     LOG("\r\n\nFXAS21000 Who Am I= %X\r\n", gyro.getWhoAmI());
@@ -191,8 +207,8 @@
     oled.SetTextProperties(&textProperties);
 
     //Start read and update threads
-    Thread gps_t(gps_thread);
-    Thread imu_t(imu_thread);
+    gps_t.start(gps_thread);
+    imu_t.start(imu_thread);
 
     while (true) {     
         /* Format the MAG reading */