Diff: main.cpp

Revision:

2:f1912528eeaf

Parent:

1:9de6ac4b381d

Child:

3:c9267f465464

--- a/main.cpp	Tue Nov 18 14:21:32 2014 +0000
+++ b/main.cpp	Fri Dec 26 01:02:21 2014 +0000
@@ -33,10 +33,15 @@
 #include "MPU9150.h"
 #include "SSD1308.h"
 #include "SDFileSystem.h"
+#include "MBed_Adafruit_GPS.h"
+#include "SC16IS750.h"
 
 //Use Xadow OLED for display
 SSD1308 oled = SSD1308(i2c, SSD1308_SA0);
 
+//Use Serial expander for extra UART
+SC16IS750_I2C serial_i2c(&i2c, SC16IS750_SA5);
+
 SDFileSystem sd(P0_21, P0_22, P1_15, P1_19, "sd", P0_20,  SDFileSystem::SWITCH_POS_NC); // the pinout on the mbed Cool Components workshop board
 
 float sum = 0;
@@ -48,9 +53,8 @@
 Timer t;
 
 Serial gps(P0_19,P0_18);
-char msg[256];
 
-#define DEBUG
+//#define DEBUG
 
 #ifdef DEBUG
 #include "USBSerial.h"                       // To use USB virtual serial, a driver is needed, check http://mbed.org/handbook/USBSerial
@@ -66,13 +70,16 @@
     //Set up I2C
     i2c.frequency(400000);  // use fast (400 kHz) I2C
 
+    //Set up GPS
+    Adafruit_GPS myGPS(&gps);
+    char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
 
-    pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
+    LOG("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
 
     t.start();
 
-    oled.writeString(0, 0, "##AeroAHRS##");
-
+    myGPS.begin(9600);
+    serial_i2c.baud(4800);
     oled.fillDisplay(0xAA);
     oled.setDisplayOff();
     wait(1);
@@ -85,19 +92,19 @@
 
     oled.writeBitmap((uint8_t*) mbed_logo);
 
-    pc.printf("OLED test done\r\n");
+    LOG("OLED test done\r\n");
     wait(10);
     oled.clearDisplay();
 
-
+    oled.writeString(0, 0, "##AeroAHRS##");
     // Read the WHO_AM_I register, this is a good test of communication
     uint8_t whoami = MPU9150.readByte(MPU9150_ADDRESS, WHO_AM_I_MPU9150);  // Read WHO_AM_I register for MPU-9250
-    pc.printf("I AM 0x%x\n\r", whoami);
-    pc.printf("I SHOULD BE 0x68\n\r");
+    LOG("I AM 0x%x\n\r", whoami);
+    LOG("I SHOULD BE 0x68\n\r");
 
     if (whoami == 0x68) { // WHO_AM_I should be 0x68
-        pc.printf("MPU9150 WHO_AM_I is 0x%x\n\r", whoami);
-        pc.printf("MPU9150 is online...\n\r");
+        LOG("MPU9150 WHO_AM_I is 0x%x\n\r", whoami);
+        LOG("MPU9150 is online...\n\r");
         //lcd.clear();
         //lcd.printString("MPU9150 is", 0, 0);
         //sprintf(buffer, "0x%x", whoami);
@@ -106,29 +113,29 @@
         wait(1);
 
         MPU9150.MPU9150SelfTest(SelfTest);
-        pc.printf("x-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[0]);
-        pc.printf("y-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[1]);
-        pc.printf("z-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[2]);
-        pc.printf("x-axis self test: gyration trim within %f % of factory value\n\r", SelfTest[3]);
-        pc.printf("y-axis self test: gyration trim within %f % of factory value\n\r", SelfTest[4]);
-        pc.printf("z-axis self test: gyration trim within %f % of factory value\n\r", SelfTest[5]);
+        LOG("x-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[0]);
+        LOG("y-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[1]);
+        LOG("z-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[2]);
+        LOG("x-axis self test: gyration trim within %f % of factory value\n\r", SelfTest[3]);
+        LOG("y-axis self test: gyration trim within %f % of factory value\n\r", SelfTest[4]);
+        LOG("z-axis self test: gyration trim within %f % of factory value\n\r", SelfTest[5]);
         wait(1);
         MPU9150.resetMPU9150(); // Reset registers to default in preparation for device calibration
         MPU9150.calibrateMPU9150(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
-        pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
-        pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
-        pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
-        pc.printf("x accel bias = %f\n\r", accelBias[0]);
-        pc.printf("y accel bias = %f\n\r", accelBias[1]);
-        pc.printf("z accel bias = %f\n\r", accelBias[2]);
+        LOG("x gyro bias = %f\n\r", gyroBias[0]);
+        LOG("y gyro bias = %f\n\r", gyroBias[1]);
+        LOG("z gyro bias = %f\n\r", gyroBias[2]);
+        LOG("x accel bias = %f\n\r", accelBias[0]);
+        LOG("y accel bias = %f\n\r", accelBias[1]);
+        LOG("z accel bias = %f\n\r", accelBias[2]);
         wait(1);
         MPU9150.initMPU9150();
-        pc.printf("MPU9150 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+        LOG("MPU9150 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
         MPU9150.initAK8975A(magCalibration);
-        pc.printf("AK8975 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
+        LOG("AK8975 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
     } else {
-        pc.printf("Could not connect to MPU9150: \n\r");
-        pc.printf("%#x \n",  whoami);
+        LOG("Could not connect to MPU9150: \n\r");
+        LOG("%#x \n",  whoami);
 
         //lcd.clear();
         //lcd.printString("MPU9150", 0, 0);
@@ -149,11 +156,31 @@
     magbias[1] = -95.;  // User environmental y-axis correction in milliGauss
     magbias[2] = -260.; // User environmental z-axis correction in milliGauss
 
+    //Wait for GPS to acquire lock
+    oled.writeString(2,0,"GPS Init ");
+    while(!myGPS.fix) {
+        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;
+            }
+        }
+    }
+
     mkdir("/sd/logdir", 0777);
-    FILE *fp = fopen("/sd/logdir/IMULog.txt", "w");
+    sprintf(buffer,"/sd/logdir/%d%d20%d_%d%d%d.txt",myGPS.day, myGPS.month, myGPS.year,
+            myGPS.hour, myGPS.minute, myGPS.seconds);
+    FILE *fp = fopen(buffer, "w");
     if(fp == NULL) {
         LOG("Could not open file for write\n");
         oled.writeString(7,0,"SD Fail");
+    } else {
+        oled.writeString(7,0,"SD OKAY");
     }
 
     while(1) {
@@ -185,6 +212,25 @@
             }
         }
 
+        //Handle GPS data
+        {
+            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;
+                }
+                else
+                {
+                    serial_i2c.printf(myGPS.lastNMEA());
+                }
+            }
+        }
+
         Now = t.read_us();
         deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
         lastUpdate = Now;
@@ -205,37 +251,27 @@
         delt_t = t.read_ms() - count;
         if (delt_t > 500) { // update LCD once per half-second independent of read rate
 
-            pc.printf("ax = %f", 1000*ax);
-            pc.printf(" ay = %f", 1000*ay);
-            pc.printf(" az = %f  mg\n\r", 1000*az);
+            LOG("ax = %f", 1000*ax);
+            LOG(" ay = %f", 1000*ay);
+            LOG(" az = %f  mg\n\r", 1000*az);
 
-            pc.printf("gx = %f", gx);
-            pc.printf(" gy = %f", gy);
-            pc.printf(" gz = %f  deg/s\n\r", gz);
+            LOG("gx = %f", gx);
+            LOG(" gy = %f", gy);
+            LOG(" gz = %f  deg/s\n\r", gz);
 
-            pc.printf("gx = %f", mx);
-            pc.printf(" gy = %f", my);
-            pc.printf(" gz = %f  mG\n\r", mz);
+            LOG("gx = %f", mx);
+            LOG(" gy = %f", my);
+            LOG(" gz = %f  mG\n\r", mz);
 
             tempCount = MPU9150.readTempData();  // Read the adc values
             temperature = ((float) tempCount) / 340.0f + 36.53f; // Temperature in degrees Centigrade
-            pc.printf(" temperature = %f  C\n\r", temperature);
-
-            pc.printf("q0 = %f\n\r", q[0]);
-            pc.printf("q1 = %f\n\r", q[1]);
-            pc.printf("q2 = %f\n\r", q[2]);
-            pc.printf("q3 = %f\n\r", q[3]);
+            LOG(" temperature = %f  C\n\r", temperature);
 
-            /*    lcd.clear();
-                lcd.printString("MPU9150", 0, 0);
-                lcd.printString("x   y   z", 0, 1);
-                sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
-                lcd.printString(buffer, 0, 2);
-                sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
-                lcd.printString(buffer, 0, 3);
-                sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
-                lcd.printString(buffer, 0, 4);
-             */
+            LOG("q0 = %f\n\r", q[0]);
+            LOG("q1 = %f\n\r", q[1]);
+            LOG("q2 = %f\n\r", q[2]);
+            LOG("q3 = %f\n\r", q[3]);
+
             // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
             // In this coordinate system, the positive z-axis is down toward Earth.
             // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
@@ -254,15 +290,76 @@
             roll  *= 180.0f / PI;
 
 
-            pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
-            pc.printf("average rate = %f\n\r", (float) sumCount/sum);
-            
-            sprintf(buffer, "YPR: %.2f %.2f %.2f", yaw, pitch, roll);
-            oled.writeString(0, 0, "##AeroAHRS##");
+            LOG("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
+            LOG("average rate = %f\n\r", (float) sumCount/sum);
+
+            sprintf(buffer, "YPR:%3.0f %3.0f %3.0f", yaw, pitch, roll);
             oled.writeString(1,0,buffer);
-//    lcd.printString(buffer, 0, 4);
-//    sprintf(buffer, "rate = %f", (float) sumCount/sum);
-//    lcd.printString(buffer, 0, 5);
+            if(fp != NULL) {
+                fprintf(fp,"YPR: %f %f %f\n", yaw, pitch, roll);
+                if(fflush(fp)==EOF) {
+                    //SD card removed close file pointer
+                    oled.writeString(7,0,"SD Fail");
+                    fp=NULL;
+                }
+            }
+
+            LOG("Time: %d:%d:%d.%u\n", myGPS.hour, myGPS.minute, myGPS.seconds, myGPS.milliseconds);
+            LOG("Date: %d/%d/20%d\n", myGPS.day, myGPS.month, myGPS.year);
+            LOG("Fix: %d\n", (int) myGPS.fix);
+            LOG("Quality: %d\n", (int) myGPS.fixquality);
+            if (myGPS.fix) {
+                LOG("Location: %5.2f%c, %5.2f%c\n", myGPS.latitude, myGPS.lat, myGPS.longitude, myGPS.lon);
+                LOG("Speed: %5.2f knots\n", myGPS.speed);
+                LOG("Angle: %5.2f\n", myGPS.angle);
+                LOG("Altitude: %5.2f\n", myGPS.altitude);
+                LOG("Satellites: %d\n", myGPS.satellites);
+            }
+
+            if (myGPS.fix) {
+                sprintf(buffer,"LAT:%5.5f%c",myGPS.latitude,myGPS.lat);
+                oled.writeString(2,0,buffer);
+                sprintf(buffer,"LON:%5.5f%c",myGPS.longitude,myGPS.lon);
+                oled.writeString(3,0,buffer);
+                sprintf(buffer,"ALT:%5.2f",myGPS.altitude);
+                oled.writeString(4,0,buffer);
+            } else {
+                oled.writeString(2,0,"                    ");
+                oled.writeString(2,0,"GPS Lost ");
+                oled.writeString(3,0,"                    ");
+                oled.writeString(4,0,"                    ");
+            }
+            sprintf(buffer,"Time: %d:%d:%d.%u\n", myGPS.hour, myGPS.minute, myGPS.seconds, myGPS.milliseconds);
+            oled.writeString(5,0,buffer);
+            sprintf(buffer,"Date: %d/%d/20%d\n", myGPS.day, myGPS.month, myGPS.year);
+            oled.writeString(6,0,buffer);
+
+            if(myGPS.fix && fp!=NULL) {
+                fprintf(fp,"LLA: %5.6f%c, %5.6f%c, %5.2f\n", myGPS.latitude, myGPS.lat, myGPS.longitude, myGPS.lon,myGPS.altitude);
+            }
+
+            if(fp != NULL) {
+                fprintf(fp,"DT: %d/%d/20%d %d:%d:%d.%u\n", myGPS.day, myGPS.month, myGPS.year, myGPS.hour, myGPS.minute, myGPS.seconds, myGPS.milliseconds);
+                if(fflush(fp)==EOF) {
+                    //SD card removed close file pointer
+                    oled.writeString(7,0,"SD Fail");
+                    fp=NULL;
+                }
+            }
+
+            //if FP is null at the end of the loop attempt to open new log with fix
+            if(fp==NULL && myGPS.fix) {
+                mkdir("/sd/logdir", 0777);
+                sprintf(buffer,"/sd/logdir/%d%d20%d_%d%d%d.txt",myGPS.day, myGPS.month, myGPS.year,
+                        myGPS.hour, myGPS.minute, myGPS.seconds);
+                FILE *fp = fopen(buffer, "w");
+                if(fp == NULL) {
+                    LOG("Could not open file for write\n");
+                    oled.writeString(7,0,"SD Fail");
+                } else {
+                    oled.writeString(7,0,"SD OKAY");
+                }
+            }
 
             myled= !myled;
             count = t.read_ms();