File content as of revision 5:5f74d3265c84:

#include "mbed.h"
#include "SDFileSystem.h"
#include "Rtc_Ds1307.h"
#include <string>
#include "debug.h"
#include "MPU6050.h"
#ifndef DEBUG
#define DEBUG
#endif

float sum = 0;
uint32_t sumCount = 0;

SDFileSystem sd(D11, D12, D13, D10, "sd");
MPU6050 mpu6050;
Serial pc(D1,D0); // tx, rx
Rtc_Ds1307 rtc(D14, D15);
Serial device(D8, D2);

Timer t;

bool storeData(float, float, float);
bool sendDataTime(int);
bool clearData();
bool setTimeData(Rtc_Ds1307::Time_rtc);

char buffer[128];
int readptr = 0;
char j[] = "strongaaa";
int k = 0;
int x,numcount = 0;
int l[13] = {0};
int m[13] = {0};
int main()
{
    pc.baud(9600);
    //Set up I2C
    i2c.frequency(400000);  // use fast (400 kHz) I2C
    t.start();
    pc.printf("DogPremetter\n");        
                                                 //First
    Rtc_Ds1307::Time_rtc tm;
    rtc.startClock();                                                                 //startClock
    // setTimeData(tm);                                                               //setTimeData
    mkdir("/sd/dataimu", 0777);
    rtc.getTime(tm);
    pc.printf("Hello\n");
    pc.printf("The current time is : %02d:%02d:%02d\n", tm.hour, tm.min, tm.sec);
    pc.printf("The current date is : %02d/%02d/%04d\n", tm.mon, tm.date, tm.year);
    mkdir("/sd/dataimu", 0777);

    //  ##############################IMU############################################################################################################
    // Read the WHO_AM_I register, this is a good test of communication
    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
    pc.printf("I AM 0x%x\n\r", whoami);
    pc.printf("I SHOULD BE 0x68\n\r");
    if (whoami == 0x68) { // WHO_AM_I should always be 0x68
        pc.printf("MPU6050 is online...");
        wait(1);
        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
            mpu6050.initMPU6050();
            pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature

            wait(2);
        } else {
            pc.printf("Device did not the pass self-test!\n\r");
        }
    } else {
        pc.printf("Could not connect to MPU6050: \n\r");
        pc.printf("%#x \n",  whoami);

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

        // If data ready bit set, all data registers have new data
        if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
            mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
            mpu6050.getAres();

            // 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];

            mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
            mpu6050.getGres();

            // 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];

            tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
            temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
        }

        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
        mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);

        // 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

            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;
            roll  *= 180.0f / PI;

            rtc.getTime(tm);
            //  ##############################IMU+BLUETOOTH+DATALOGGER#######################################################################################
            pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
            storeData(yaw,pitch,roll);


            if((yaw > -100 && yaw < 79) && (pitch > -88 && pitch < 45) && (roll > -178 && roll < 178)) {
                numcount++;
                sendDataTime(tm.hour);
                rtc.getTime(tm);
                pc.printf("num = %d\n",numcount);


            }
            storeData(yaw,pitch,roll);
            if(device.readable()) {
                if( device.getc() == '#' ) {

                    for(x=1; x<13; x++) {
                        device.printf("%d\n",m[x]);
                        pc.printf("%d\n",m[x]);
                        wait(0.2);
                    }
                    k = l[1]+l[2]+l[3]+l[4]+l[5]+l[6]+l[7]+l[8]+l[9]+l[10]+l[11]+l[12]+130000;
                    device.printf("%d\n",k);
                    pc.printf("%d\n",k);
                } else if( device.getc() == '$' ) {
                    clearData();
                    m[13] = 0;
                    l[13] = 0;
                    numcount = 0;
                    k = 0;
                }
            }

            //   pc.printf("average rate = %f\n\r", (float) sumCount/sum);
            //  #############################################################################################################################################
            myled= !myled;
            count = t.read_ms();
            sum = 0;
            sumCount = 0;
        }
    }

}



bool storeData(float x, float y, float z)
{
    FILE *fp = fopen("/sd/dataimu/logger.txt", "a");
    if(fp == NULL) {
        error("Could not open file for write\n");
        return false;
    } else {
        //sprintf(&input[0],"x=%f, y=%f, z=%f\n ",x,y,z);
        pc.printf("InreturnData x,y,z\n");
        fprintf(fp, "yaw=%f , pitch=%f , roll=%f\n ",x,y,z);
        fprintf(fp, "l[1]= %d l[2]=%d l[3]= %d l[4]=%d l[5]= %d l[6]=%d l[7]= %d l[8]=%d  l[9]= %d l[10]=%d l[11]= %d l[12]=%d ",l[1],l[2],l[3],l[4],l[5],l[6],l[7],l[8],l[9],l[10],l[11],l[12]);
        fprintf(fp, "Sum = %d\n",k);
        pc.printf("yaw=%f , pitch=%f , roll=%f\n ",x,y,z);

    }
    pc.printf("OutreturnData\n");
    fclose(fp);
    return true;
}




bool sendDataTime(int data)
{
    if(data == 00 || data == 01) {
        l[1]++;
        m[1] = l[1]+10000 ;
        wait(0.5);


    } else if(data == 02 || data == 03) {
        l[2]++;
        m[2] = l[2]+20000 ;
        wait(0.5);

    } else if(data == 04 || data == 05) {
        l[3]++;
        m[3] = l[3]+30000 ;
        wait(0.5);

    } else if(data == 06 || data == 07) {
        l[4]++;
        m[4] = l[4]+40000 ;
        wait(0.5);

    } else if(data > 07 && data < 10) {
        l[5]++;
        m[5] = l[5]+50000 ;
        wait(0.5);

    } else if(data == 10 || data == 11) {
        l[5]++;
        m[6] = l[6]+60000 ;
        wait(0.5);

    } else if(data == 12 || data == 13) {
        l[7]++;
        m[7] = l[7]+70000 ;
        wait(0.5);

    } else if(data == 14 || data == 15) {
        l[8]++;
        m[8] = l[8]+80000 ;
        wait(0.5);

    } else if(data == 16 || data == 17) {
        l[9]++;
        m[9] = l[9]+90000 ;
        wait(0.5);

    } else if(data == 18 || data == 19) {
        l[10]++;
        m[10] = l[10]+100000 ;
        wait(0.5);

    } else if(data == 20 || data == 21) {
        l[11]++;
        m[11] = l[11]+110000 ;
        wait(0.5);

    } else if(data == 22 || data == 23) {
        l[12]++;
        m[12] = l[12]+120000 ;
        wait(0.5);

    }
    return true;
}


bool clearData()
{
    //device.printf("clear Data \n");
    pc.printf("clear Data \n ");
    remove("/sd/dataimu/logger.txt");
    return true;
}

bool setTimeData(Rtc_Ds1307::Time_rtc tm)
{
    tm.hour=14;
    tm.min=50;
    tm.sec=00;
    tm.mon=12;
    tm.date=6;
    tm.year=2015;
    rtc.setTime(tm,true,false);
}