![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
An mbed app using the MPU6050 for crash detection
Dependencies: MPU6050 SDFileSystem mbed
main.cpp
- Committer:
- byiu3
- Date:
- 2015-03-14
- Revision:
- 1:bbfc64bf923d
- Parent:
- 0:be9547c5b62c
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); }