Diff: main.cpp

Revision:

24:cbdf0f7d33bd

Parent:

23:f170a1d72a84

Child:

25:6e43bbe76aec

--- a/main.cpp	Mon Sep 19 16:27:18 2016 +0000
+++ b/main.cpp	Tue Sep 20 04:18:34 2016 +0000
@@ -3,9 +3,12 @@
 #include "FXAS21000.h"
 #include "MBed_Adafruit_GPS.h"
 #include "Hexi_OLED_SSD1351.h"
+#include "sensor_fusion.h"
 #include "images.h"
+
 #include "rtos.h"
 
+
 DigitalOut myled(LED1);
 
 Serial gps(PTD3,PTD2);
@@ -47,18 +50,16 @@
 float gyro_data[3];
 
 Timer t;
-int lastUpdate;
 
 void gps_thread(void const *args)
 {
     while (true) {
         c = myGPS.read();   //queries the GPS
-        if (c) {
-            LOG("%c", c);    //this line will echo the GPS data if not paused
-        }
+        
         //check if we recieved a new message from GPS, if so, attempt to parse it,
         if ( myGPS.newNMEAreceived() ) {
             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
+                LOG(myGPS.lastNMEA());
                 continue;
             }
         }
@@ -72,37 +73,37 @@
         acc.getAxis(acc_data);
         mag.getAxis(mag_data);
         gyro.ReadXYZ(gyro_data);
-        /*
+        
         LOG("FXOS8700Q ACC: X=%1.4f Y=%1.4f Z=%1.4f  ", acc_data.x, acc_data.y, acc_data.z);
         LOG("    MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", mag_data.x, mag_data.y, mag_data.z);
         LOG("FXAS21000      X=%4.2f Y=%4.2f Z=%4.1f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
-        */
+        
         acc.getX(&faX);
         acc.getY(&faY);
         acc.getZ(&faZ);
         mag.getX(&fmX);
         mag.getY(&fmY);
         mag.getZ(&fmZ);
-        /*
+        
         LOG("FXOS8700Q ACC: X=%1.4f Y=%1.4f Z=%1.4f  ", faX, faY, faZ);
         LOG("    MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", fmX, fmY, fmZ);
-        */
+        
         acc.getAxis(acc_raw);
         mag.getAxis(mag_raw);
-        /*
+        
         LOG("FXOS8700Q ACC: X=%d Y=%d Z=%d  ", acc_raw.x, acc_raw.y, acc_raw.z);
         LOG("    MAG: X=%d Y=%d Z=%d\r\n", mag_raw.x, mag_raw.y, mag_raw.z);
-        */
+        
         acc.getX(&raX);
         acc.getY(&raY);
         acc.getZ(&raZ);
         mag.getX(&rmX);
         mag.getY(&rmY);
         mag.getZ(&rmZ);
-        /*
+        
         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);
     }
 }
@@ -131,9 +132,6 @@
     pc.baud(115200);
 #endif
 
-    Thread gps_t(gps_thread);
-    Thread imu_t(imu_thread);
-
     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());
@@ -192,6 +190,9 @@
     textProperties.alignParam = OLED_TEXT_ALIGN_RIGHT;
     oled.SetTextProperties(&textProperties);
 
+    //Start read and update threads
+    Thread gps_t(gps_thread);
+    Thread imu_t(imu_thread);
 
     while (true) {     
         /* Format the MAG reading */