Brenton Yiu
An mbed app using the MPU6050 for crash detection
Dependencies: MPU6050 SDFileSystem mbed
Diff: main.cpp
- Revision:
- 0:be9547c5b62c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Mar 14 15:46:05 2015 +0000
@@ -0,0 +1,647 @@
+/* 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);
+
+}
\ No newline at end of file