Diff: main.cpp

Revision:

23:f170a1d72a84

Parent:

22:e69bc54ca4c0

Child:

24:cbdf0f7d33bd

--- a/main.cpp	Mon Sep 19 04:59:45 2016 +0000
+++ b/main.cpp	Mon Sep 19 16:27:18 2016 +0000
@@ -3,12 +3,16 @@
 #include "FXAS21000.h"
 #include "MBed_Adafruit_GPS.h"
 #include "Hexi_OLED_SSD1351.h"
+#include "images.h"
+#include "rtos.h"
 
 DigitalOut myled(LED1);
 
 Serial gps(PTD3,PTD2);
+Adafruit_GPS myGPS(&gps);
+char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
 
-//#define DEBUG
+#define DEBUG
 
 #ifdef DEBUG
 Serial pc(USBTX, USBRX);
@@ -36,19 +40,75 @@
 
 FXAS21000 gyro( PTC11, PTC10); // Proper Ports for Hexiwear
 
+float faX, faY, faZ;
+float fmX, fmY, fmZ;
+int16_t raX, raY, raZ;
+int16_t rmX, rmY, rmZ;
+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()) ) {
+                continue;
+            }
+        }
+        Thread::wait(1);
+    }
+}
+
+void imu_thread(void const *args)
+{
+    while(true) {
+        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);
+    }
+}
+
 int main()
 {
-    float faX, faY, faZ;
-    float fmX, fmY, fmZ;
-    int16_t raX, raY, raZ;
-    int16_t rmX, rmY, rmZ;
-    float gyro_data[3];
-
-    Adafruit_GPS myGPS(&gps);
-    char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
     char text[30];  /* Text Buffer */
 
 //Start timer
@@ -71,6 +131,9 @@
     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());
@@ -85,6 +148,16 @@
     /* Fills the screen with solid black */
     oled.FillScreen(COLOR_BLACK);
 
+    /* Adding full screen splash */
+    const uint8_t *image = NXP_whole_bmp;
+    /* Draws the image on the Screen starting at (x=0,y=0) */
+    oled.DrawImage(image,0,0);
+
+    Thread::wait(1000);
+
+    /* Fills the screen with solid black */
+    oled.FillScreen(COLOR_BLACK);
+
     /* Display Text at (x=7,y=0) */
     strcpy((char *) text,"AERO GPS-AHRS");
     oled.Label((uint8_t *)text,7,0);
@@ -120,81 +193,38 @@
     oled.SetTextProperties(&textProperties);
 
 
-    while (true) {
+    while (true) {     
+        /* Format the MAG reading */
+        sprintf(text,"%.1f %.1f %.1f",fmX,fmY,fmZ);
+        /* Display Mag readings to 1 decimal */
+        oled.TextBox((uint8_t *)text,15,15,80,15); /*Expand textbox for more digits*/
 
-        c = myGPS.read();   //queries the GPS
+        /* Format the ACC reading */
+        sprintf(text,"%.2f %.2f %.2f",faX,faY,faZ);
+        /* Display acceleration to 2 decimal */
+        oled.TextBox((uint8_t *)text,15,30,80,15); /*Expand textbox for more digits*/
 
-        //check if we recieved a new message from GPS, if so, attempt to parse it,
-        if ( myGPS.newNMEAreceived() ) {
-            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
-                continue;
-            }
+        /* Format the GYRO reading */
+        sprintf(text,"%.2f %.2f %.2f",gyro_data[0],gyro_data[1],gyro_data[2]);
+        /* Display gyro to 2 decimal */
+        oled.TextBox((uint8_t *)text,15,45,80,15); /*Expand textbox for more digits*/
+
+
+        /* Format the time reading */
+        sprintf(text,"%.2f",t.read());
+
+        /* Display time reading */
+        oled.TextBox((uint8_t *)text,55,60,35,15); /*Expand textbox for more digits*/
+
+        if(myGPS.fix) {
+            LOG(myGPS.lastNMEA());
+            sprintf(text,"%.2f%c %.2f%c",myGPS.longitude/100.0f,myGPS.lat,myGPS.latitude/100.0f,myGPS.lon);
+        } else {
+            sprintf(text,"No GPS");
         }
 
-        int Now = t.read_ms();
-
-        if(Now - lastUpdate > 500) {
-
-            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);
-
-            /* Format the MAG reading */
-            sprintf(text,"%.1f %.1f %.1f",fmX,fmY,fmZ);
-            /* Display Mag readings to 1 decimal */
-            oled.TextBox((uint8_t *)text,15,15,80,15); /*Expand textbox for more digits*/
-
-            /* Format the ACC reading */
-            sprintf(text,"%.2f %.2f %.2f",faX,faY,faZ);
-            /* Display acceleration to 2 decimal */
-            oled.TextBox((uint8_t *)text,15,30,80,15); /*Expand textbox for more digits*/
-
-            /* Format the GYRO reading */
-            sprintf(text,"%.2f %.2f %.2f",gyro_data[0],gyro_data[1],gyro_data[2]);
-            /* Display gyro to 2 decimal */
-            oled.TextBox((uint8_t *)text,15,45,80,15); /*Expand textbox for more digits*/
-
-
-            /* Format the time reading */
-            sprintf(text,"%.2f",t.read());
-
-            /* Display time reading */
-            oled.TextBox((uint8_t *)text,55,60,35,15); /*Expand textbox for more digits*/
-
-            if(myGPS.fix) {
-                LOG(myGPS.lastNMEA());
-                sprintf(text,"%.2f %.2f %.2f",myGPS.longitude,myGPS.longitude,myGPS.altitude);
-            } else {
-                sprintf(text,"No GPS");
-            }
-            
-            /* Display GPS location or lock status */
-            oled.TextBox((uint8_t *)text,15,75,80,15); /*Expand textbox for more digits*/
-
-            lastUpdate = t.read_ms();
-        }
+        /* Display GPS location or lock status */
+        oled.TextBox((uint8_t *)text,15,75,80,15); /*Expand textbox for more digits*/
+        Thread::wait(1000);
     }
 }