This program is designed to run on a set of Xadow M0 modules to create a Hotshoe IMU which outputs GPS and Orientation data to Nikon cameras, as well as triggering the camera at set intervals.

Dependencies:   MBed_Adafruit-GPS-Library SC16IS750 SDFileSystem SSD1308_128x64_I2C USBDevice mbed BMP085

Fork of MPU9150AHRS by Kris Winer

/media/uploads/whatnick/20151022_004759.jpg

Files at this revision

API Documentation at this revision

Comitter:
whatnick
Date:
Fri Dec 26 01:02:21 2014 +0000
Parent:
1:9de6ac4b381d
Child:
3:c9267f465464
Commit message:
Added support for serial_i2c for sending data at 4800 baud to camera

Changed in this revision

MBed_Adafruit-GPS-Library.lib Show annotated file Show diff for this revision Revisions of this file
SC16IS750.lib Show annotated file Show diff for this revision Revisions of this file
TinyGPS.lib Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MBed_Adafruit-GPS-Library.lib	Fri Dec 26 01:02:21 2014 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/mlee350/code/MBed_Adafruit-GPS-Library/#a23e3099bb0a
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SC16IS750.lib	Fri Dec 26 01:02:21 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/wim/code/SC16IS750/#ff3e57bebb6a
--- a/TinyGPS.lib	Tue Nov 18 14:21:32 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/shimniok/code/TinyGPS/#f522b8bdf987
--- 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);
+            LOG(" 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("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]);
 
-            /*    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);
-             */
             // 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();