Tisham Dhar / Xadow_MPU9150AHRS

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

main.cpp

Committer:
whatnick
Date:
16 months ago
Revision:
10:46b5acee9472
Parent:
9:b7062c55d36c

File content as of revision 10:46b5acee9472:

/* MPU9150 Basic Example Code
 by: Kris Winer
 date: April 1, 2014
 license: Beerware - Use this code however you'd like. If you
 find it useful you can buy me a beer some time.

 Demonstrate basic MPU-9150 functionality including parameterizing the register addresses, initializing the sensor,
 getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to
 allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and
 Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.

 SDA and SCL should have external pull-up resistors (to 3.3V).
 10k resistors are on the EMSENSR-9250 breakout board.

 Hardware setup:
 MPU9150 Breakout --------- Arduino
 VDD ---------------------- 3.3V
 VDDI --------------------- 3.3V
 SDA ----------------------- A4
 SCL ----------------------- A5
 GND ---------------------- GND

 Note: The MPU9150 is an I2C sensor and uses the Arduino Wire library.
 Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
 We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L /twi.h utility file.
 */

//#include "ST_F401_84MHZ.h"
//F401_init84 myinit(0);
#include "mbed.h"
#include "mbed_logo.h"
#include "MPU9150.h"
#include "SSD1308.h"
#include "SDFileSystem.h"
#include "MBed_Adafruit_GPS.h"
#include "SC16IS750.h"
#include "BMP085.h"

//Use Xadow OLED for display
SSD1308 oled = SSD1308(i2c, SSD1308_SA0);

//Use Serial expander for extra UART
SPI spi(P0_9,P0_8,P0_10); //MOSI, MISO, SCK
DigitalOut CS(P0_15); //CS
SC16IS752_SPI serial_spi_nikon(&spi,P0_15,NC,SC16IS750::Channel_B);
SC16IS752_SPI serial_spi_ble(&spi,P0_15,NC,SC16IS750::Channel_A);

//Use BMP085 Temperature,Pressure
BMP085 bmp(i2c);

SDFileSystem sd(P0_21, P0_22, P1_15, P1_19, "sd", P0_20,  SDFileSystem::SWITCH_POS_NC); // Pin-out on Xadow SD board attached to 1st SPI

float sum = 0;
uint32_t sumCount = 0, mcount = 0;
char buffer[64];

MPU9150 MPU9150;

Timer t;

Serial gps(P0_19,P0_18);

//#define DEBUG

#ifdef DEBUG
#include "USBSerial.h"                       // To use USB virtual serial, a driver is needed, check http://mbed.org/handbook/USBSerial
#define LOG(args...)    pc.printf(args)
#else
#define LOG(args...)
#endif

char checkSum(char* theseChars,int checkLen) {
  char check = 0;
  // iterate over the string, XOR each byte with the total sum:
  for (int c = 0; c < checkLen; c++) {
    check = char(check ^ theseChars[c]);
  } 
  // return the result
  return check;
}


int main()
{

    //Set up I2C
    i2c.frequency(400000);  // use fast (400 kHz) I2C
    
    //Set up SPI
    spi.format(8, 0);;          
//    spi.frequency(100000);    
//    spi.frequency(500000);    
    spi.frequency(1000000);

    //Set up GPS
    Adafruit_GPS myGPS(&gps);
    char c; //when read via Adafruit_GPS::read(), the class returns single character stored here

    LOG("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);

    t.start();

    myGPS.begin(9600);
    //Turn off all sentences except GGA and RMC
    //For MTK GPS
    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
    
    //FOR UBLOX GPS
    myGPS.sendCommand(UBX_DISABLE_ZDA);
    myGPS.sendCommand(UBX_DISABLE_GLL);
    myGPS.sendCommand(UBX_DISABLE_VTG);
    myGPS.sendCommand(UBX_DISABLE_GSV);
    myGPS.sendCommand(UBX_DISABLE_GSA);
    
    wait(1);
    //Set Serial I2C Baudrate for Nikon
    serial_spi_nikon.baud(4800);
    wait(1);
    
    //Set Serial SPI Baudrate for BLE
    serial_spi_ble.baud(2400);
    wait(1);
    
    //Set BLE Baud rate
    //serial_spi_ble.printf("AT+BAUD2");
    //wait(1);
    //serial_spi_ble.baud(19200);
    //wait(1);
    
    //Set output pins for trigggering camera
    serial_spi_nikon.ioSetDirection(0xFF); // All outputs
    serial_spi_nikon.ioSetState(0xFF);     // All On
    
    
    
    oled.fillDisplay(0xAA);
    oled.setDisplayOff();
    wait(1);
    oled.setDisplayOn();

    oled.clearDisplay();
    oled.setDisplayInverse();
    wait(0.5);
    oled.setDisplayNormal();

    oled.writeBitmap((uint8_t*) mbed_logo);

    LOG("OLED test done\r\n");
    wait(10);
    oled.clearDisplay();

    oled.writeString(0, 0, "##AeroGPS-AHRS##");
    // 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
    LOG("I AM 0x%x\n\r", whoami);
    LOG("I SHOULD BE 0x68\n\r");

    if (whoami == 0x68) { // WHO_AM_I should be 0x68
        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);
        //lcd.printString(buffer, 0, 1);
        //lcd.printString("shoud be 0x68", 0, 2);
        wait(1);

        MPU9150.MPU9150SelfTest(SelfTest);
        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
        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();
        LOG("MPU9150 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
        MPU9150.initAK8975A(magCalibration);
        oled.writeString(2,0,"Mag Cal, wave 8");
        MPU9150.magcalMPU9150(magBias);
        oled.writeString(2,0,"Mag Cal done   ");
        wait(1);
        LOG("AK8975 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
    } else {
        LOG("Could not connect to MPU9150: \n\r");
        LOG("%#x \n",  whoami);

        //lcd.clear();
        //lcd.printString("MPU9150", 0, 0);
        //lcd.printString("no connection", 0, 1);
        sprintf(buffer, "WHO_AM_I 0x%x", whoami);
        //lcd.printString(buffer, 0, 2);

        while(1) ; // Loop forever if communication doesn't happen
    }

    uint8_t MagRate = 10; // set magnetometer read rate in Hz; 10 to 100 (max) Hz are reasonable values
    MPU9150.getAres(); // Get accelerometer sensitivity
    MPU9150.getGres(); // Get gyro sensitivity
    mRes = 10.*1229./4096.; // Conversion from 1229 microTesla full scale (4096) to 12.29 Gauss full scale
    // So far, magnetometer bias is calculated and subtracted here manually, should construct an algorithm to do it automatically
    // like the gyro and accelerometer biases
    
    //Wait for GPS to acquire lock
    oled.writeString(2,0,"GPS Waiting...");
    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;
            }
        }
        /*
        Now = t.read_us();
        deltat = (float)((Now - lastUpdate)/1000000.0f);
        
        bool up = false;
        if(deltat > 0.5f)
        {
            bmp.update();
            float temp = bmp.get_temperature();
            float pres = bmp.get_pressure();
            float baroAlt = (float)44330 * (1 - pow((float)(pres/SEA_PRES), 0.190295f));
            
            sprintf(buffer,"ALT:%5.2f",baroAlt);
            oled.writeString(4,0,buffer);
            sprintf(buffer,"TMP:%5.2f",temp);
            oled.writeString(5,0,buffer);
            sprintf(buffer,"PRS:%5.2f",pres);
            oled.writeString(6,0,buffer);
            
            serial_spi_nikon.printf("$GPRMC,000002.000,A,3456.9076,S,13831.2800,E,0.00,65.44,240215,,,D*49\r\n");
            serial_spi_nikon.printf("$PTNTHPR,105.0,N,-21.4,N,0.9,N,A*79\r\n");
            wait(0.1);
            //serial_spi_nikon.printf("$GPVTG,65.44,T,,M,0.00,N,0.00,K,D*0B");
            sprintf(buffer+1,"GPGGA,000002.000,3456.9076,S,13831.2800,E,2,07,0.94,%5.2f,M,-0.5,M,,",baroAlt);
            int checkS = checkSum(buffer+1,strlen(buffer+1));
            buffer[0] = '$';
            serial_spi_nikon.printf(buffer);
            serial_spi_nikon.printf("*%02X\r\n",checkS);
            wait(0.1);
            serial_spi_nikon.printf("$GPGSA,A,3,13,15,24,06,12,02,28,,,,,,1.32,0.94,0.92*09\r\n");
            serial_spi_nikon.printf("$GPGLL,3456.9076,S,13831.2800,E,000002.000,A,D*4D\r\n");
            serial_spi_nikon.printf("$NKGCS,WGS 84*11\r\n");
            
            if(up)
            {
                serial_spi_nikon.ioSetState(0x0);
                up = false;
            }
            else
            {
                serial_spi_nikon.ioSetState(0x3);
                up = true;
            }
            lastUpdate = Now;
        }
        */
    }

    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");
    }

    while(1) {
        c = myGPS.read();   //queries the GPS
        if (c) {
            LOG("%c", c);    //this line will echo the GPS data if not paused
            //serial_spi_nikon.putc(c);
            //serial_spi_nikon.printf("$GPGGA,154850.00,3452.12190,S,13836.65170,E,1,04,1.64,123.5,M,0.0,M,,*7F\r\n");
            //$GPGGA,160202.00,3452.14414,S,13836.63059,E,1,04,2.60,45.4,M,-3.4,M,,*6B
            //serial_spi_nikon.printf("$GPRMC,154850.00,A,3452.12190,S,13836.65170,E,0.510,,110215,,,A*63\r\n");
            //$GPRMC,160203.00,A,3452.14414,S,13836.63079,E,0.332,,110215,,,A*6F
        }
        
        // If intPin goes high, all data registers have new data
        if(MPU9150.readByte(MPU9150_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt

            MPU9150.readAccelData(accelCount);  // Read the x/y/z adc values
            // Now we'll calculate the accleration value into actual g's
            ax = (float)accelCount[0]*aRes; // - accelBias[0];  // get actual g value, this depends on scale being set
            ay = (float)accelCount[1]*aRes; // - accelBias[1];
            az = (float)accelCount[2]*aRes; // - accelBias[2];

            MPU9150.readGyroData(gyroCount);  // Read the x/y/z adc values
            // Calculate the gyro value into actual degrees per second
            gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
            gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
            gz = (float)gyroCount[2]*gRes; // - gyroBias[2];

            mcount++;
            if (mcount > 200/MagRate) {  // this is a poor man's way of setting the magnetometer read rate (see below)
                MPU9150.readMagData(magCount);  // Read the x/y/z adc values
                // Calculate the magnetometer values in milliGauss
                // Include factory calibration per data sheet and user environmental corrections
                mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
                my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
                mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
                mcount = 0;
            }
        }

        //Handle GPS data
        {
            c = myGPS.read();   //queries the GPS
            if (c) {
                LOG("%c", c);    //this line will echo the GPS data if not paused
                //serial_spi_nikon.putc(c);
            }

            //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_spi_nikon.printf(myGPS.lastNMEA());
                    serial_spi_nikon.printf("\n\r");
                    
                    serial_spi_ble.printf(myGPS.lastNMEA());
                    serial_spi_ble.printf("\n\r");
                    
                    if(myGPS.fix && fp!=NULL) {
                        fprintf(fp,myGPS.lastNMEA());
                        fprintf(fp,"\r\n");
                    }
                }
                
            }
        }

        Now = t.read_us();
        deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
        lastUpdate = Now;

        sum += deltat;
        sumCount++;

//    if(lastUpdate - firstUpdate > 10000000.0f) {
//     beta = 0.04;  // decrease filter gain after stabilized
//     zeta = 0.015; // increasey bias drift gain after stabilized
//   }

        // Pass gyro rate as rad/s
        MPU9150.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
// MPU9150.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);

        // Serial print and/or display at 0.5 s rate independent of data rates
        delt_t = t.read_ms() - count;
        if (delt_t > 500) { // update LCD once per half-second independent of read rate

            LOG("ax = %f", 1000*ax);
            LOG(" ay = %f", 1000*ay);
            LOG(" az = %f  mg\n\r", 1000*az);

            LOG("gx = %f", gx);
            LOG(" gy = %f", gy);
            LOG(" gz = %f  deg/s\n\r", gz);

            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
            LOG(" temperature = %f  C\n\r", temperature);

            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.
            // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
            // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
            // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
            // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
            // applied in the correct order which for this configuration is yaw, pitch, and then roll.
            // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
            yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
            pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
            roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
            pitch *= 180.0f / PI;
            yaw   *= 180.0f / PI;
            yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
            roll  *= 180.0f / PI;


            LOG("Yaw, Pitch, Roll: %f %f %f\r\n", yaw, pitch, roll);
            LOG("average rate = %f\r\n", (float) sumCount/sum);

            sprintf(buffer, "YPR:%3.0f %3.0f %3.0f", yaw, pitch, roll);
            oled.writeString(1,0,buffer);
            sprintf(buffer+1,"PTNTHPR,%3.1f,N,%3.1f,N,%3.1f,N,A",yaw,pitch,roll);
            int checkS = checkSum(buffer+1,strlen(buffer+1));
            buffer[0] = '$';
            serial_spi_nikon.printf(buffer);
            serial_spi_nikon.printf("*%02X\r\n",checkS);
            
            serial_spi_ble.printf(buffer);
            serial_spi_ble.printf("*%02X\r\n",checkS);
            
            if(fp != NULL) {
                fprintf(fp,"%s%02X\r\n",buffer,checkS);
                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(fp != NULL) {
                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();

            if(count > 1<<21) {
                t.start(); // start the timer over again if ~30 minutes has passed
                count = 0;
                deltat= 0;
                lastUpdate = t.read_us();
            }
            sum = 0;
            sumCount = 0;
            
        }
    }
}