File content as of revision 1:bbfc64bf923d:

/* Testing whether we can get peak detection and how fast we can get data. Stripped out all
    of the data collection stuff and now we just light an led when we get a peak detection*/

// Libraries to include
#include "mbed.h"
#include "MPU6050.h"
#include "SDFileSystem.h"

// Pin Declarations
// Four on-board LED
DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);

// External LED
DigitalOut crashled(p20);
DigitalOut ext_myled2(p12);
DigitalOut ext_myled3(p13);
DigitalOut ext_myled4(p14);

// External Pushbuttons
InterruptIn pb1(p21);
InterruptIn pb2(p22);
InterruptIn crashb(p23);

// Function prototypes
void pb1_irq();
void pb2_irq();
void crashb_irq();
void threshold_detector(int cur_i);


void initMPU6050();
void setup();

// Macros
#define VARS_FILE_NAME "/sd/data_acq/vars.txt"
#define LOG_FILE_NAME "/sd/data_acq/log_%d.txt"

#define PACKET_NOTIFY_COUNT 100000000

#define BUF_LEN 512

// Global Variables
SDFileSystem sd(p5, p6, p7, p8, "sd", p9, SDFileSystem::SWITCH_POS_NC, 18000000); // SD Filesystem
Serial pc(USBTX, USBRX); // Serial Debug
bool serial_toggle = 1;

// Files
FileHandle *fp_data;
//FILE *fp_data;
FILE *fp_log;
FILE *fp_vars;

char data_file[50];
char log_file[50];

int log_file_count = 0;
int data_file_count = 0;
int imu_num = 1;

// IMU
MPU6050 *imu1 = new MPU6050(); // IMU instance
bool connected, calibrated;

Timer timer1; // Timer

// Toggle the LEDs
bool led1_val = false;
bool led2_val = false;

// Data status
bool get_data = false;

// Data sample counter
bool data_write = true;
bool packet_counter = false;
int counting_up = 0;
int counting_up_1 = 0;
float elapsed_time = 0;

// Mag threshold
int mag_threshold = 25;
int mag_window = 10;
float mag_percent = 0.90;
float avg_mag = 0;
float acc_mag = 0;
float acc_running_sum = 0;
int counter_acc = 0;

// Rotation Threshold
int rot_threshold = 129600; // 360 degrees per second
int rot_window = 10;
float rot_percent = 0.90;
float avg_rot = 0;
float gyro_mag = 0;
float gyro_running_sum = 0;
int counter_gyro = 0;

// Data
int i = 0; // Buffer index variable
int j = 0; // Loop index variable
float ax_buf[BUF_LEN], ay_buf[BUF_LEN], az_buf[BUF_LEN];
float avg_samp_mag[BUF_LEN], avg_samp_rot[BUF_LEN];
float gx_buf[BUF_LEN], gy_buf[BUF_LEN], gz_buf[BUF_LEN];
//float yaw_buf[BUF_LEN], pitch_buf[BUF_LEN], roll_buf[BUF_LEN];

float *ax, *ay, *az, *gx, *gy, *gz, *yaw, *pitch, *roll;

// Biases
float gyro_bias[3], accel_bias[3];

// Current State Machine State
enum state{STATE_1, STATE_2A, STATE_2B, STATE_3A, STATE_3B};
enum state cur_state = STATE_1;

int main() {
    
    __disable_irq();    // Disable Interrupts
    
    setup();

    if (connected & !calibrated) {
        fclose(fp_log);

        // Set up the IMU to default values
        initMPU6050();

        // Setup for unique values
        imu1->setFullScaleAccelRange(MPU6050_ACCEL_FS_16);
        imu1->setFullScaleGyroRange(MPU6050_GYRO_FS_2000);

        // Set up the data packets
        ax = (float *) malloc(sizeof(*ax));
        ay = (float *) malloc(sizeof(*ay));
        az = (float *) malloc(sizeof(*az));
        gx = (float *) malloc(sizeof(*gx));
        gy = (float *) malloc(sizeof(*gy));
        gz = (float *) malloc(sizeof(*gz));
        yaw = (float *) malloc(sizeof(*yaw));
        pitch = (float *) malloc(sizeof(*pitch));
        roll = (float *) malloc(sizeof(*roll));
        
        pc.printf("SystemCoreClock = %d Hz\n\r", SystemCoreClock);
        
        pc.printf("Waiting to take data...\n\r");

        __enable_irq();
        
        myled1 = 0;
        myled2 = 1;
        ext_myled2 = 1;

        while(1) {
            if (get_data) {
                counting_up_1++;
                // Run the function to get the data and convert it
                if (imu1->getIntDataReadyStatus()) {
                    counting_up++;
                    // Call the helper function
                    imu1->getAndConvertData(ax, ay, az, yaw, pitch, roll, accel_bias, gyro_bias, gx, gy, gz);
                    //imu1->getMotion6(ax, ay, az, yaw, pitch, roll);
                }
                
                // Store the most recent values
                ax_buf[i % BUF_LEN] = *ax;
                ay_buf[i % BUF_LEN] = *ay;
                az_buf[i % BUF_LEN] = *az;

                gx_buf[i % BUF_LEN] = *gx;
                gy_buf[i % BUF_LEN] = *gy;
                gz_buf[i % BUF_LEN] = *gz;

//                yaw_buf[i % BUF_LEN] = *yaw;
//                pitch_buf[i % BUF_LEN] = *pitch;
//                roll_buf[i % BUF_LEN] = *roll;
                
                threshold_detector(i);

                i++;
                if (i == BUF_LEN && data_write) {
                    // On my mac, it is writing in little-endian
                    
                    // Store the data on the SD card
//                    fp_data->write(ax_buf, sizeof(ax_buf));
//                    fp_data->write("\n", sizeof(char));
//                    fp_data->write(ay_buf, sizeof(ay_buf));
//                    fp_data->write("\n", sizeof(char));
//                    fp_data->write(az_buf, sizeof(az_buf));
//                    fp_data->write("\n", sizeof(char));
//                    fp_data->write(yaw_buf, sizeof(yaw_buf));
//                    fp_data->write("\n", sizeof(char));
//                    fp_data->write(pitch_buf, sizeof(pitch_buf));
//                    fp_data->write("\n", sizeof(char));
//                    fp_data->write(roll_buf, sizeof(roll_buf));
//                    fp_data->write("\n", sizeof(char));
                    
//                    for (j = 0; j < BUF_LEN; j++) {
//                        fprintf(fp_data, "%f, %f, %f, %f, %f, %f\n", ax_buf[j], ay_buf[j], az_buf[j], yaw_buf[j], pitch_buf[j], roll_buf[j]);
//                    }

                    for (j = 0; j < BUF_LEN; j++) {
                        fp_data->write(&(ax_buf[j]), sizeof(ax_buf[j]));
                        fp_data->write(&(ay_buf[j]), sizeof(ay_buf[j]));
                        fp_data->write(&(az_buf[j]), sizeof(az_buf[j]));
                        fp_data->write(&(gx_buf[j]), sizeof(gx_buf[j]));
                        fp_data->write(&(gy_buf[j]), sizeof(gy_buf[j]));
                        fp_data->write(&(gz_buf[j]), sizeof(gz_buf[j]));
//                        fp_data->write(&(yaw_buf[j]), sizeof(yaw_buf[j]));
//                        fp_data->write(&(pitch_buf[j]), sizeof(pitch_buf[j]));
//                        fp_data->write(&(roll_buf[j]), sizeof(roll_buf[j]));
                        fp_data->write(&(avg_samp_mag[j]), sizeof(avg_samp_mag[j]));
                        fp_data->write(&(avg_samp_rot[j]), sizeof(avg_samp_rot[j]));
                        //pc.printf("%f %f %f %f %f %f\n\r", gx_buf[j], gy_buf[j], gz_buf[j], yaw_buf[j], pitch_buf[j], roll_buf[j]);
                    }

                    // Reset i
                    i = 0;
                }
//                
//
//                // Periodically tell we are still taking data
//                if (packet_counter) {
//                    if ((counting_up % PACKET_NOTIFY_COUNT) == 0) {
//                        if (serial_toggle)
//                            pc.printf("%d Packets Received\n\r", counting_up);
//
//                        if ((fp_log = fopen(log_file, "a")) == NULL) {
//                            if (serial_toggle) 
//                                pc.printf("Failed to open log file 0 \n\r");
//                        }
//                        else {
//                            fprintf(fp_log, "%d Packets Received\n\r", counting_up);
//                            fclose(fp_log);
//                        }
//                    }
//                }
            }
        }
    }
    else {
        pc.printf("Conection Fail!\n\r");
        fprintf(fp_log, "Connection Fail!\n\r");

        fclose(fp_log);

        while(1) {
            myled4 = 0;
            ext_myled4 = 0;
            myled1 = 1;
            wait(0.1);
            myled1 = 0;
            myled2 = 1;
            ext_myled2 = 1;
            wait(0.1);
            myled2 = 0;
            ext_myled2 = 0;
            myled3 = 1;
            ext_myled3 = 1;
            wait(0.1);
            myled3 = 0;
            ext_myled3 = 0;
            myled4 = 1;
            ext_myled4 = 1;
            wait(0.1);
        }
    }
}

void pb1_irq() {
    switch (cur_state) {
        case STATE_1:
            cur_state = STATE_2A;
            pc.printf("PB1: STATE_1->STATE_2A\n\r");
            break;
        case STATE_2A:
            pc.printf("PB1: STATE_2A->STATE_2A\n\r");
            break;
        case STATE_3A:
            cur_state = STATE_1;
            if (serial_toggle)
                pc.printf("PB1: STATE_3A->STATE_1\n\r");

            // Create the data file name
            sprintf(data_file, "data_acq/data%d.txt", data_file_count);
            //sprintf(data_file, "/sd/data_acq/data%d.txt", data_file_count);
            data_file_count++; // Increment the data file count

            // Write the new count to the vars file
            fp_vars = fopen(VARS_FILE_NAME, "w");
            fprintf(fp_vars, "log=%d\ndata=%d\nimu=%d\n", log_file_count+1, data_file_count, imu_num);
            fclose(fp_vars);

            // Try to open the data file
            fp_data = sd.open(data_file, O_WRONLY | O_CREAT | O_TRUNC);
            //fp_data = fopen(data_file, "w");
            if (fp_data == NULL) {
                if (serial_toggle)
                    pc.printf("Failed to open data file: %s\n\r", data_file);
                
                if ((fp_log = fopen(log_file, "a")) == NULL)
                    if (serial_toggle)
                        pc.printf("Failed to open log file 1\n\r");
                else {
                    fprintf(fp_log, "Failed to open data file: %s\n\r", data_file);
                    fclose(fp_log);
                }
            }
            else {
                
                if (serial_toggle)
                    pc.printf("Opened data file for writing: %s\n\r", data_file);
        
                if ((fp_log = fopen(log_file, "a")) == NULL)
                    if (serial_toggle)
                        pc.printf("Failed to open log file 2\n\r");
                else {
                    fprintf(fp_log, "Opened data file for writing: %s\n\r", data_file);
                    fclose(fp_log);
                }
            
                if (serial_toggle)
                    pc.printf("Started taking data...\n\r");

                get_data = true;
                timer1.start();
                myled2 = 0; // Taking data
                myled3 = 1;
            }
            break;
        case STATE_2B:
            cur_state = STATE_3B;
            pc.printf("PB1: STATE_2B->STATE_3B\n\r");
            break;
        case STATE_3B:
            cur_state = STATE_2A;
            pc.printf("PB1: STATE_3B->STATE_2A\n\r");
            break;
    }  
}

void pb2_irq() {
    
    switch (cur_state) {
        case STATE_1:
            cur_state = STATE_2B;
            pc.printf("PB2: STATE_1->STATE_2B\n\r");
            break;
        case STATE_2A:
            cur_state = STATE_3A;
            pc.printf("PB2: STATE_2A->STATE_3B\n\r");
            break;
        case STATE_3A:
            cur_state = STATE_2B;
            pc.printf("PB2: STATE_3A->STATE_2B\n\r");
            break;
        case STATE_2B:
            pc.printf("PB2: STATE_2B->STATE_2B\n\r");
            break;
        case STATE_3B:
            cur_state = STATE_1;
            timer1.stop();
            
            // Write the remaining values from the buffer
            for (j = 0; j < i; j++) {
                fp_data->write(&(ax_buf[j]), sizeof(ax_buf[j]));
                fp_data->write(&(ay_buf[j]), sizeof(ay_buf[j]));
                fp_data->write(&(az_buf[j]), sizeof(az_buf[j]));
                fp_data->write(&(gx_buf[j]), sizeof(gx_buf[j]));
                fp_data->write(&(gy_buf[j]), sizeof(gy_buf[j]));
                fp_data->write(&(gz_buf[j]), sizeof(gz_buf[j]));
//                fp_data->write(&(yaw_buf[j]), sizeof(yaw_buf[j]));
//                fp_data->write(&(pitch_buf[j]), sizeof(pitch_buf[j]));
//                fp_data->write(&(roll_buf[j]), sizeof(roll_buf[j]));
                fp_data->write(&(avg_samp_mag[j]), sizeof(avg_samp_mag[j]));
                fp_data->write(&(avg_samp_rot[j]), sizeof(avg_samp_rot[j]));
                //pc.printf("%f %f %f %f %f %f\n\r", ax_buf[j], ay_buf[j], az_buf[j], yaw_buf[j], pitch_buf[j], roll_buf[j]);
            }

            if(serial_toggle)
                pc.printf("PB2: STATE_3B->STATE_1\n\r");

            if (fp_data != NULL) {
                if (serial_toggle) {
                    pc.printf("\n\rClosing files: %s\n\r", data_file);
                    pc.printf("Samples Taken: %d, %d\n\r", counting_up, counting_up_1);
                    pc.printf("Time for all samples: %f\n\r", elapsed_time = timer1.read());
                    pc.printf("Sample Rate: %f\n\r", (float) counting_up / elapsed_time);
                }
                
                if (fp_data->close())
                //if (fclose(fp_data))
                    pc.printf("Failed to close file\n\r");
                else
                    fp_data = NULL;

                if ((fp_log = fopen(log_file, "a")) == NULL) {
                    if (serial_toggle) {
                        pc.printf("Failed to open log file\n\r");
                    }
                }
                else {
                    fprintf(fp_log, "\n\rClosing file: %s\n\r", data_file);
                    fprintf(fp_log, "Samples Taken: %d,%d\n\r", counting_up, counting_up_1);
                    fprintf(fp_log, "Time for all samples: %f\n\r", elapsed_time);
                    fprintf(fp_log, "Sample Rate: %f\n\r", (float) counting_up / elapsed_time);
                    fclose(fp_log);
                }
            }
            
            get_data = false;
            counting_up = 0;
            counting_up_1 = 0;
            timer1.reset();
    
            myled3 = 0; // Done taking data
        
            // Flash the fourth led twice
            for (j = 0; j < 4; j++) {
                myled4 = 1;
                wait(0.25);
                myled4 = 0;
                wait(0.25);
            }
                
            myled2 = 1; // Ready to take more data
            break;
    } // Switch
}

void crashb_irq() {
    crashled = 0;
}

void threshold_detector(int cur_i) {
    // The last 10 samples
    for (j = i; j > i-mag_window; j--) {
        acc_mag = 0;
        acc_mag = ax_buf[j % BUF_LEN]*ax_buf[j % BUF_LEN] + ay_buf[j % BUF_LEN]*ay_buf[j % BUF_LEN] + az_buf[j % BUF_LEN]*az_buf[j % BUF_LEN];
        acc_running_sum = acc_running_sum + acc_mag;
        
        gyro_mag = 0;
        gyro_mag = gx_buf[j % BUF_LEN]*gx_buf[j % BUF_LEN] + gy_buf[j % BUF_LEN]*gy_buf[j % BUF_LEN] + gz_buf[j % BUF_LEN]*gz_buf[j % BUF_LEN];
        gyro_running_sum = gyro_running_sum + gyro_mag;
    }
    
    avg_mag = acc_running_sum / mag_window;
    avg_samp_mag[i] = avg_mag;
    
    avg_rot = gyro_running_sum / rot_window;
    avg_samp_rot[i] = avg_rot;
    
    if (avg_mag > mag_threshold*mag_percent)
        crashled = 1;
        
    if (avg_rot > rot_threshold*rot_percent)
        crashled = 1;
        
    acc_running_sum = 0;
    gyro_running_sum = 0;
    acc_mag = 0;
    gyro_mag = 0;
    avg_mag = 0;
    avg_rot = 0;
}

void setup() {
    // Setting up
    myled1 = 1;
    
    // Pin setup
    pb1.mode(PullUp);
    pb1.rise(&pb1_irq);

    pb2.mode(PullUp);
    pb2.rise(&pb2_irq);
    
    crashb.mode(PullUp);
    crashb.rise(&crashb_irq);
    
    // Serial debug baud rate 
    pc.baud(9600);
    pc.printf("\n\r");
    
    //Display the card type and capacity
    pc.printf("\nCard type: ");
    if (sd.card_type() == SDFileSystem::CARD_NONE)
        pc.printf("None\n\r");
    else if (sd.card_type() == SDFileSystem::CARD_MMC)
        pc.printf("MMC\n\r");
    else if (sd.card_type() == SDFileSystem::CARD_SD)
        pc.printf("SD\n\r");
    else if (sd.card_type() == SDFileSystem::CARD_SDHC)
        pc.printf("SDHC\n\r");
    else
        pc.printf("Unknown\n\r");
    pc.printf("Sectors: %llu\n\r", sd.disk_sectors());
    pc.printf("Capacity: %.1fMB\n\r", (sd.disk_sectors() * 512) / 1048576.0);

    // Open the setup file, if possible, if not, create one
    // Contains the last log file number
    fp_vars = fopen(VARS_FILE_NAME, "r");
    if (fp_vars == NULL) {
        pc.printf("No Vars File; Creating One...\n\r");
        fp_vars = fopen(VARS_FILE_NAME, "w");
        fprintf(fp_vars, "log=%d\ndata=%d\nimu=%d\n", log_file_count, data_file_count, imu_num);
        fprintf(fp_vars, "mag_threshold=%d\nmag_window=%d\nmag_percent=%f\n", mag_threshold, mag_window, mag_percent);
        fprintf(fp_vars, "rot_threshold=%d\nrot_window=%d\nrot_percent=%f\n", rot_threshold, rot_window, rot_percent);
        fprintf(fp_vars, "serial=%d\n", serial_toggle);
        fclose(fp_vars);
    }
    else {
        fscanf(fp_vars, "log=%d\ndata=%d\nimu=%d\n", &log_file_count, &data_file_count, &imu_num);
        fscanf(fp_vars, "mag_threshold=%d\nmag_window=%d\nmag_percent=%f\n", &mag_threshold, &mag_window, &mag_percent);
        fscanf(fp_vars, "rot_threshold=%d\nrot_window=%d\nrot_percent=%f\n", &rot_threshold, &rot_window, &rot_percent);
        fscanf(fp_vars, "serial=%d\n", &serial_toggle);
        rewind(fp_vars);
        fprintf(fp_vars, "log=%d\ndata=%d\nimu=%d\n", log_file+1, data_file+1, imu_num);
        pc.printf("%d,%d,%d\n\r", log_file_count, data_file_count, imu_num);
    }
    
    // Open the log file to write
    sprintf(log_file, LOG_FILE_NAME, log_file_count);
    if ((fp_log = fopen(log_file, "w")) == NULL)
        pc.printf("Failed to open log file: %s\n\r", log_file);
        
    // Get the last log file number
    pc.printf("Log File Starting at %d\n\r", log_file_count);
    pc.printf("Data File Starting at %d\n\r", data_file_count);
    pc.printf("Using IMU %d\n\r", imu_num);
    
    fprintf(fp_log, "Log File Starting at %d\n\r", log_file_count);
    fprintf(fp_log, "Data File Starting at %d\n\r", data_file_count);
    fprintf(fp_log, "Using IMU %d\n\r", imu_num);
    
    // An empty line
    fprintf(fp_log, "\n\r");
    
    // Test the connection
    connected = imu1->testConnection();
    if (connected) {
        pc.printf("Connection Success!\n\r");
        fprintf(fp_log, "Connection Success!\n\r");
    }
    else {
        pc.printf("Connection Failed!\n\r");
        fprintf(fp_log, "Connection Failed!\n\r");
    }

    if (connected) {
        float destination[6];
        calibrated = imu1->selfTest(destination);

        if (!calibrated) {
            pc.printf("Passed Self Test\n\r");
            fprintf(fp_log, "Passed Self Test\n\r");
        }
        else {
            pc.printf("Failed Self Test\n\r");
            fprintf(fp_log, "Failed Self Test\n\r");
        }

        // Set the biases
        accel_bias[0] = 0;
        accel_bias[1] = 0;
        accel_bias[2] = 0;
        
        gyro_bias[0] = 0;
        gyro_bias[1] = 0;
        gyro_bias[2] = 0;
    }
}

/* Notes on sampling rates and output rates
 * The accelerometer ALWAYS outputs data at 1kHz.
 * The gyroscope has a variable output rate at either 1 kHz or 8 kHz
 *      set by DLPF_CFG
 * The MPU6050 will sample data at the rate specified by setting the
 *      SMPLRT_DIV value and put these values into the sensor output
 *      registers, fifo output, dmp sampling, and motion detection.
 * What this means is that even though the accelerometer and gyroscope
 *      are outputting at 1 kHz and 1 or 8 kHz, the MPU6050 may be
 *      ignoring data if the sampling rate is set lower than these. It
 *      also means that it can get multiple accelerometer outputs that
 *      are duplicates if the sampling rate is set higher than 1 kHz.
 * The bandpass filter causes a delay in the data getting outputted so
 *      this also needs to be factored into the equation. From my
 *      understanding, the delay will set the maximum rate that the
 *      sensor can output at. So a delay of 4.9 ms means that the max
 *      rate that the sensor can output at is 1/0.0049s, about 200 Hz. 
 * In this example we set DLPF_CFG = 0x3 which will set a 1 kHz 
 *      output rate for both the gyro and acelerometer. But there
 *      is a delay of 4.9 ms for the accelerometer and 4.8 ms for the
 *      gyro so the output rate is 200 Hz. We set the MPU6050 sampling
 *      rate to be 200 Hz so that we don't have duplicates and don't
 *      miss any data point. 
*/

void initMPU6050() {
    // Clear sleep mode bit, enable all sensors
    imu1->setSleepEnabled(false);

    // Wait for PLL to lock
    wait(0.1);

    // Set clock source to have x-axis gyro reference
    imu1->setClockSource(MPU6050_CLOCK_PLL_XGYRO);

    // Disable FSYNC and set accle and gyro to 44 and 42 Hz, respectively
    // Set DLPF_CFG to 0 for a sample rate of 1 kHz for the acceleromter and
    // 8 kHz for the gyroscope. 
    imu1->setDLPFMode(MPU6050_DLPF_BW_256);

    // Set the sampling rate of the device. This is the rate at which the registers
    // get occupied and it is Gyroscope_Rate/x where x is the argument sent into 
    // setRate(x)
    imu1->setRate(0x3);

    // Set the gyroscope configuration
    imu1->setGyroXSelfTest(false); // Clear the self-test bits
    imu1->setGyroYSelfTest(false);
    imu1->setGyroZSelfTest(false);
    imu1->setFullScaleGyroRange(MPU6050_GYRO_FS_250); // Clear the FS bits
    imu1->setFullScaleGyroRange(MPU6050_GYRO_FS_2000); // Set the full-scale range

    // Set the accelerometer configuration
    imu1->setAccelXSelfTest(false); // Clear the self-test bits
    imu1->setAccelYSelfTest(false); 
    imu1->setAccelZSelfTest(false);
    imu1->setFullScaleAccelRange(MPU6050_ACCEL_FS_2); // Clear the AFS bits
    imu1->setFullScaleAccelRange(MPU6050_ACCEL_FS_16); // Set the full-scale range

    // Configure interrupts and bypass enable
    // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS
    // Enable I2C_BYPASS_EN so addition chips can join the I2C bus and all can be
    // controlled by the microcontroller as master
    imu1->setInterruptMode(MPU6050_INTMODE_ACTIVEHIGH);
    imu1->setInterruptDrive(MPU6050_INTDRV_PUSHPULL);
    imu1->setInterruptLatch(MPU6050_INTLATCH_WAITCLEAR);
    imu1->setInterruptLatchClear(MPU6050_INTCLEAR_STATUSREAD);
    imu1->setFSyncInterruptLevel(false);
    imu1->setFSyncInterruptEnabled(false);
    imu1->setI2CBypassEnabled(true);
    imu1->setIntDataReadyEnabled(true);

}