Brenton Yiu / Mbed 2 deprecated buffer_algorithm

An mbed app using the MPU6050 for crash detection

Dependencies:   MPU6050 SDFileSystem mbed

main.cpp@1:bbfc64bf923d, 2015-03-14 (annotated)

Committer:
byiu3
Date:
Sat Mar 14 15:49:32 2015 +0000
Revision:
1:bbfc64bf923d
Parent:
0:be9547c5b62c 
Published some libraries

Who changed what in which revision?

UserRevisionLine numberNew contents of line
byiu3 0:be9547c5b62c 1 /* Testing whether we can get peak detection and how fast we can get data. Stripped out all
byiu3 0:be9547c5b62c 2 of the data collection stuff and now we just light an led when we get a peak detection*/
byiu3 0:be9547c5b62c 3
byiu3 0:be9547c5b62c 4 // Libraries to include
byiu3 0:be9547c5b62c 5 #include "mbed.h"
byiu3 0:be9547c5b62c 6 #include "MPU6050.h"
byiu3 0:be9547c5b62c 7 #include "SDFileSystem.h"
byiu3 0:be9547c5b62c 8
byiu3 0:be9547c5b62c 9 // Pin Declarations
byiu3 0:be9547c5b62c 10 // Four on-board LED
byiu3 0:be9547c5b62c 11 DigitalOut myled1(LED1);
byiu3 0:be9547c5b62c 12 DigitalOut myled2(LED2);
byiu3 0:be9547c5b62c 13 DigitalOut myled3(LED3);
byiu3 0:be9547c5b62c 14 DigitalOut myled4(LED4);
byiu3 0:be9547c5b62c 15
byiu3 0:be9547c5b62c 16 // External LED
byiu3 0:be9547c5b62c 17 DigitalOut crashled(p20);
byiu3 0:be9547c5b62c 18 DigitalOut ext_myled2(p12);
byiu3 0:be9547c5b62c 19 DigitalOut ext_myled3(p13);
byiu3 0:be9547c5b62c 20 DigitalOut ext_myled4(p14);
byiu3 0:be9547c5b62c 21
byiu3 0:be9547c5b62c 22 // External Pushbuttons
byiu3 0:be9547c5b62c 23 InterruptIn pb1(p21);
byiu3 0:be9547c5b62c 24 InterruptIn pb2(p22);
byiu3 0:be9547c5b62c 25 InterruptIn crashb(p23);
byiu3 0:be9547c5b62c 26
byiu3 0:be9547c5b62c 27 // Function prototypes
byiu3 0:be9547c5b62c 28 void pb1_irq();
byiu3 0:be9547c5b62c 29 void pb2_irq();
byiu3 0:be9547c5b62c 30 void crashb_irq();
byiu3 0:be9547c5b62c 31 void threshold_detector(int cur_i);
byiu3 0:be9547c5b62c 32
byiu3 0:be9547c5b62c 33
byiu3 0:be9547c5b62c 34 void initMPU6050();
byiu3 0:be9547c5b62c 35 void setup();
byiu3 0:be9547c5b62c 36
byiu3 0:be9547c5b62c 37 // Macros
byiu3 0:be9547c5b62c 38 #define VARS_FILE_NAME "/sd/data_acq/vars.txt"
byiu3 0:be9547c5b62c 39 #define LOG_FILE_NAME "/sd/data_acq/log_%d.txt"
byiu3 0:be9547c5b62c 40
byiu3 0:be9547c5b62c 41 #define PACKET_NOTIFY_COUNT 100000000
byiu3 0:be9547c5b62c 42
byiu3 0:be9547c5b62c 43 #define BUF_LEN 512
byiu3 0:be9547c5b62c 44
byiu3 0:be9547c5b62c 45 // Global Variables
byiu3 0:be9547c5b62c 46 SDFileSystem sd(p5, p6, p7, p8, "sd", p9, SDFileSystem::SWITCH_POS_NC, 18000000); // SD Filesystem
byiu3 0:be9547c5b62c 47 Serial pc(USBTX, USBRX); // Serial Debug
byiu3 0:be9547c5b62c 48 bool serial_toggle = 1;
byiu3 0:be9547c5b62c 49
byiu3 0:be9547c5b62c 50 // Files
byiu3 0:be9547c5b62c 51 FileHandle *fp_data;
byiu3 0:be9547c5b62c 52 //FILE *fp_data;
byiu3 0:be9547c5b62c 53 FILE *fp_log;
byiu3 0:be9547c5b62c 54 FILE *fp_vars;
byiu3 0:be9547c5b62c 55
byiu3 0:be9547c5b62c 56 char data_file[50];
byiu3 0:be9547c5b62c 57 char log_file[50];
byiu3 0:be9547c5b62c 58
byiu3 0:be9547c5b62c 59 int log_file_count = 0;
byiu3 0:be9547c5b62c 60 int data_file_count = 0;
byiu3 0:be9547c5b62c 61 int imu_num = 1;
byiu3 0:be9547c5b62c 62
byiu3 0:be9547c5b62c 63 // IMU
byiu3 0:be9547c5b62c 64 MPU6050 *imu1 = new MPU6050(); // IMU instance
byiu3 0:be9547c5b62c 65 bool connected, calibrated;
byiu3 0:be9547c5b62c 66
byiu3 0:be9547c5b62c 67 Timer timer1; // Timer
byiu3 0:be9547c5b62c 68
byiu3 0:be9547c5b62c 69 // Toggle the LEDs
byiu3 0:be9547c5b62c 70 bool led1_val = false;
byiu3 0:be9547c5b62c 71 bool led2_val = false;
byiu3 0:be9547c5b62c 72
byiu3 0:be9547c5b62c 73 // Data status
byiu3 0:be9547c5b62c 74 bool get_data = false;
byiu3 0:be9547c5b62c 75
byiu3 0:be9547c5b62c 76 // Data sample counter
byiu3 0:be9547c5b62c 77 bool data_write = true;
byiu3 0:be9547c5b62c 78 bool packet_counter = false;
byiu3 0:be9547c5b62c 79 int counting_up = 0;
byiu3 0:be9547c5b62c 80 int counting_up_1 = 0;
byiu3 0:be9547c5b62c 81 float elapsed_time = 0;
byiu3 0:be9547c5b62c 82
byiu3 0:be9547c5b62c 83 // Mag threshold
byiu3 0:be9547c5b62c 84 int mag_threshold = 25;
byiu3 0:be9547c5b62c 85 int mag_window = 10;
byiu3 0:be9547c5b62c 86 float mag_percent = 0.90;
byiu3 0:be9547c5b62c 87 float avg_mag = 0;
byiu3 0:be9547c5b62c 88 float acc_mag = 0;
byiu3 0:be9547c5b62c 89 float acc_running_sum = 0;
byiu3 0:be9547c5b62c 90 int counter_acc = 0;
byiu3 0:be9547c5b62c 91
byiu3 0:be9547c5b62c 92 // Rotation Threshold
byiu3 0:be9547c5b62c 93 int rot_threshold = 129600; // 360 degrees per second
byiu3 0:be9547c5b62c 94 int rot_window = 10;
byiu3 0:be9547c5b62c 95 float rot_percent = 0.90;
byiu3 0:be9547c5b62c 96 float avg_rot = 0;
byiu3 0:be9547c5b62c 97 float gyro_mag = 0;
byiu3 0:be9547c5b62c 98 float gyro_running_sum = 0;
byiu3 0:be9547c5b62c 99 int counter_gyro = 0;
byiu3 0:be9547c5b62c 100
byiu3 0:be9547c5b62c 101 // Data
byiu3 0:be9547c5b62c 102 int i = 0; // Buffer index variable
byiu3 0:be9547c5b62c 103 int j = 0; // Loop index variable
byiu3 0:be9547c5b62c 104 float ax_buf[BUF_LEN], ay_buf[BUF_LEN], az_buf[BUF_LEN];
byiu3 0:be9547c5b62c 105 float avg_samp_mag[BUF_LEN], avg_samp_rot[BUF_LEN];
byiu3 0:be9547c5b62c 106 float gx_buf[BUF_LEN], gy_buf[BUF_LEN], gz_buf[BUF_LEN];
byiu3 0:be9547c5b62c 107 //float yaw_buf[BUF_LEN], pitch_buf[BUF_LEN], roll_buf[BUF_LEN];
byiu3 0:be9547c5b62c 108
byiu3 0:be9547c5b62c 109 float *ax, *ay, *az, *gx, *gy, *gz, *yaw, *pitch, *roll;
byiu3 0:be9547c5b62c 110
byiu3 0:be9547c5b62c 111 // Biases
byiu3 0:be9547c5b62c 112 float gyro_bias[3], accel_bias[3];
byiu3 0:be9547c5b62c 113
byiu3 0:be9547c5b62c 114 // Current State Machine State
byiu3 0:be9547c5b62c 115 enum state{STATE_1, STATE_2A, STATE_2B, STATE_3A, STATE_3B};
byiu3 0:be9547c5b62c 116 enum state cur_state = STATE_1;
byiu3 0:be9547c5b62c 117
byiu3 0:be9547c5b62c 118 int main() {
byiu3 0:be9547c5b62c 119
byiu3 0:be9547c5b62c 120 __disable_irq(); // Disable Interrupts
byiu3 0:be9547c5b62c 121
byiu3 0:be9547c5b62c 122 setup();
byiu3 0:be9547c5b62c 123
byiu3 0:be9547c5b62c 124 if (connected & !calibrated) {
byiu3 0:be9547c5b62c 125 fclose(fp_log);
byiu3 0:be9547c5b62c 126
byiu3 0:be9547c5b62c 127 // Set up the IMU to default values
byiu3 0:be9547c5b62c 128 initMPU6050();
byiu3 0:be9547c5b62c 129
byiu3 0:be9547c5b62c 130 // Setup for unique values
byiu3 0:be9547c5b62c 131 imu1->setFullScaleAccelRange(MPU6050_ACCEL_FS_16);
byiu3 0:be9547c5b62c 132 imu1->setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
byiu3 0:be9547c5b62c 133
byiu3 0:be9547c5b62c 134 // Set up the data packets
byiu3 0:be9547c5b62c 135 ax = (float *) malloc(sizeof(*ax));
byiu3 0:be9547c5b62c 136 ay = (float *) malloc(sizeof(*ay));
byiu3 0:be9547c5b62c 137 az = (float *) malloc(sizeof(*az));
byiu3 0:be9547c5b62c 138 gx = (float *) malloc(sizeof(*gx));
byiu3 0:be9547c5b62c 139 gy = (float *) malloc(sizeof(*gy));
byiu3 0:be9547c5b62c 140 gz = (float *) malloc(sizeof(*gz));
byiu3 0:be9547c5b62c 141 yaw = (float *) malloc(sizeof(*yaw));
byiu3 0:be9547c5b62c 142 pitch = (float *) malloc(sizeof(*pitch));
byiu3 0:be9547c5b62c 143 roll = (float *) malloc(sizeof(*roll));
byiu3 0:be9547c5b62c 144
byiu3 0:be9547c5b62c 145 pc.printf("SystemCoreClock = %d Hz\n\r", SystemCoreClock);
byiu3 0:be9547c5b62c 146
byiu3 0:be9547c5b62c 147 pc.printf("Waiting to take data...\n\r");
byiu3 0:be9547c5b62c 148
byiu3 0:be9547c5b62c 149 __enable_irq();
byiu3 0:be9547c5b62c 150
byiu3 0:be9547c5b62c 151 myled1 = 0;
byiu3 0:be9547c5b62c 152 myled2 = 1;
byiu3 0:be9547c5b62c 153 ext_myled2 = 1;
byiu3 0:be9547c5b62c 154
byiu3 0:be9547c5b62c 155 while(1) {
byiu3 0:be9547c5b62c 156 if (get_data) {
byiu3 0:be9547c5b62c 157 counting_up_1++;
byiu3 0:be9547c5b62c 158 // Run the function to get the data and convert it
byiu3 0:be9547c5b62c 159 if (imu1->getIntDataReadyStatus()) {
byiu3 0:be9547c5b62c 160 counting_up++;
byiu3 0:be9547c5b62c 161 // Call the helper function
byiu3 0:be9547c5b62c 162 imu1->getAndConvertData(ax, ay, az, yaw, pitch, roll, accel_bias, gyro_bias, gx, gy, gz);
byiu3 0:be9547c5b62c 163 //imu1->getMotion6(ax, ay, az, yaw, pitch, roll);
byiu3 0:be9547c5b62c 164 }
byiu3 0:be9547c5b62c 165
byiu3 0:be9547c5b62c 166 // Store the most recent values
byiu3 0:be9547c5b62c 167 ax_buf[i % BUF_LEN] = *ax;
byiu3 0:be9547c5b62c 168 ay_buf[i % BUF_LEN] = *ay;
byiu3 0:be9547c5b62c 169 az_buf[i % BUF_LEN] = *az;
byiu3 0:be9547c5b62c 170
byiu3 0:be9547c5b62c 171 gx_buf[i % BUF_LEN] = *gx;
byiu3 0:be9547c5b62c 172 gy_buf[i % BUF_LEN] = *gy;
byiu3 0:be9547c5b62c 173 gz_buf[i % BUF_LEN] = *gz;
byiu3 0:be9547c5b62c 174
byiu3 0:be9547c5b62c 175 // yaw_buf[i % BUF_LEN] = *yaw;
byiu3 0:be9547c5b62c 176 // pitch_buf[i % BUF_LEN] = *pitch;
byiu3 0:be9547c5b62c 177 // roll_buf[i % BUF_LEN] = *roll;
byiu3 0:be9547c5b62c 178
byiu3 0:be9547c5b62c 179 threshold_detector(i);
byiu3 0:be9547c5b62c 180
byiu3 0:be9547c5b62c 181 i++;
byiu3 0:be9547c5b62c 182 if (i == BUF_LEN && data_write) {
byiu3 0:be9547c5b62c 183 // On my mac, it is writing in little-endian
byiu3 0:be9547c5b62c 184
byiu3 0:be9547c5b62c 185 // Store the data on the SD card
byiu3 0:be9547c5b62c 186 // fp_data->write(ax_buf, sizeof(ax_buf));
byiu3 0:be9547c5b62c 187 // fp_data->write("\n", sizeof(char));
byiu3 0:be9547c5b62c 188 // fp_data->write(ay_buf, sizeof(ay_buf));
byiu3 0:be9547c5b62c 189 // fp_data->write("\n", sizeof(char));
byiu3 0:be9547c5b62c 190 // fp_data->write(az_buf, sizeof(az_buf));
byiu3 0:be9547c5b62c 191 // fp_data->write("\n", sizeof(char));
byiu3 0:be9547c5b62c 192 // fp_data->write(yaw_buf, sizeof(yaw_buf));
byiu3 0:be9547c5b62c 193 // fp_data->write("\n", sizeof(char));
byiu3 0:be9547c5b62c 194 // fp_data->write(pitch_buf, sizeof(pitch_buf));
byiu3 0:be9547c5b62c 195 // fp_data->write("\n", sizeof(char));
byiu3 0:be9547c5b62c 196 // fp_data->write(roll_buf, sizeof(roll_buf));
byiu3 0:be9547c5b62c 197 // fp_data->write("\n", sizeof(char));
byiu3 0:be9547c5b62c 198
byiu3 0:be9547c5b62c 199 // for (j = 0; j < BUF_LEN; j++) {
byiu3 0:be9547c5b62c 200 // 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]);
byiu3 0:be9547c5b62c 201 // }
byiu3 0:be9547c5b62c 202
byiu3 0:be9547c5b62c 203 for (j = 0; j < BUF_LEN; j++) {
byiu3 0:be9547c5b62c 204 fp_data->write(&(ax_buf[j]), sizeof(ax_buf[j]));
byiu3 0:be9547c5b62c 205 fp_data->write(&(ay_buf[j]), sizeof(ay_buf[j]));
byiu3 0:be9547c5b62c 206 fp_data->write(&(az_buf[j]), sizeof(az_buf[j]));
byiu3 0:be9547c5b62c 207 fp_data->write(&(gx_buf[j]), sizeof(gx_buf[j]));
byiu3 0:be9547c5b62c 208 fp_data->write(&(gy_buf[j]), sizeof(gy_buf[j]));
byiu3 0:be9547c5b62c 209 fp_data->write(&(gz_buf[j]), sizeof(gz_buf[j]));
byiu3 0:be9547c5b62c 210 // fp_data->write(&(yaw_buf[j]), sizeof(yaw_buf[j]));
byiu3 0:be9547c5b62c 211 // fp_data->write(&(pitch_buf[j]), sizeof(pitch_buf[j]));
byiu3 0:be9547c5b62c 212 // fp_data->write(&(roll_buf[j]), sizeof(roll_buf[j]));
byiu3 0:be9547c5b62c 213 fp_data->write(&(avg_samp_mag[j]), sizeof(avg_samp_mag[j]));
byiu3 0:be9547c5b62c 214 fp_data->write(&(avg_samp_rot[j]), sizeof(avg_samp_rot[j]));
byiu3 0:be9547c5b62c 215 //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]);
byiu3 0:be9547c5b62c 216 }
byiu3 0:be9547c5b62c 217
byiu3 0:be9547c5b62c 218 // Reset i
byiu3 0:be9547c5b62c 219 i = 0;
byiu3 0:be9547c5b62c 220 }
byiu3 0:be9547c5b62c 221 //
byiu3 0:be9547c5b62c 222 //
byiu3 0:be9547c5b62c 223 // // Periodically tell we are still taking data
byiu3 0:be9547c5b62c 224 // if (packet_counter) {
byiu3 0:be9547c5b62c 225 // if ((counting_up % PACKET_NOTIFY_COUNT) == 0) {
byiu3 0:be9547c5b62c 226 // if (serial_toggle)
byiu3 0:be9547c5b62c 227 // pc.printf("%d Packets Received\n\r", counting_up);
byiu3 0:be9547c5b62c 228 //
byiu3 0:be9547c5b62c 229 // if ((fp_log = fopen(log_file, "a")) == NULL) {
byiu3 0:be9547c5b62c 230 // if (serial_toggle)
byiu3 0:be9547c5b62c 231 // pc.printf("Failed to open log file 0 \n\r");
byiu3 0:be9547c5b62c 232 // }
byiu3 0:be9547c5b62c 233 // else {
byiu3 0:be9547c5b62c 234 // fprintf(fp_log, "%d Packets Received\n\r", counting_up);
byiu3 0:be9547c5b62c 235 // fclose(fp_log);
byiu3 0:be9547c5b62c 236 // }
byiu3 0:be9547c5b62c 237 // }
byiu3 0:be9547c5b62c 238 // }
byiu3 0:be9547c5b62c 239 }
byiu3 0:be9547c5b62c 240 }
byiu3 0:be9547c5b62c 241 }
byiu3 0:be9547c5b62c 242 else {
byiu3 0:be9547c5b62c 243 pc.printf("Conection Fail!\n\r");
byiu3 0:be9547c5b62c 244 fprintf(fp_log, "Connection Fail!\n\r");
byiu3 0:be9547c5b62c 245
byiu3 0:be9547c5b62c 246 fclose(fp_log);
byiu3 0:be9547c5b62c 247
byiu3 0:be9547c5b62c 248 while(1) {
byiu3 0:be9547c5b62c 249 myled4 = 0;
byiu3 0:be9547c5b62c 250 ext_myled4 = 0;
byiu3 0:be9547c5b62c 251 myled1 = 1;
byiu3 0:be9547c5b62c 252 wait(0.1);
byiu3 0:be9547c5b62c 253 myled1 = 0;
byiu3 0:be9547c5b62c 254 myled2 = 1;
byiu3 0:be9547c5b62c 255 ext_myled2 = 1;
byiu3 0:be9547c5b62c 256 wait(0.1);
byiu3 0:be9547c5b62c 257 myled2 = 0;
byiu3 0:be9547c5b62c 258 ext_myled2 = 0;
byiu3 0:be9547c5b62c 259 myled3 = 1;
byiu3 0:be9547c5b62c 260 ext_myled3 = 1;
byiu3 0:be9547c5b62c 261 wait(0.1);
byiu3 0:be9547c5b62c 262 myled3 = 0;
byiu3 0:be9547c5b62c 263 ext_myled3 = 0;
byiu3 0:be9547c5b62c 264 myled4 = 1;
byiu3 0:be9547c5b62c 265 ext_myled4 = 1;
byiu3 0:be9547c5b62c 266 wait(0.1);
byiu3 0:be9547c5b62c 267 }
byiu3 0:be9547c5b62c 268 }
byiu3 0:be9547c5b62c 269 }
byiu3 0:be9547c5b62c 270
byiu3 0:be9547c5b62c 271 void pb1_irq() {
byiu3 0:be9547c5b62c 272 switch (cur_state) {
byiu3 0:be9547c5b62c 273 case STATE_1:
byiu3 0:be9547c5b62c 274 cur_state = STATE_2A;
byiu3 0:be9547c5b62c 275 pc.printf("PB1: STATE_1->STATE_2A\n\r");
byiu3 0:be9547c5b62c 276 break;
byiu3 0:be9547c5b62c 277 case STATE_2A:
byiu3 0:be9547c5b62c 278 pc.printf("PB1: STATE_2A->STATE_2A\n\r");
byiu3 0:be9547c5b62c 279 break;
byiu3 0:be9547c5b62c 280 case STATE_3A:
byiu3 0:be9547c5b62c 281 cur_state = STATE_1;
byiu3 0:be9547c5b62c 282 if (serial_toggle)
byiu3 0:be9547c5b62c 283 pc.printf("PB1: STATE_3A->STATE_1\n\r");
byiu3 0:be9547c5b62c 284
byiu3 0:be9547c5b62c 285 // Create the data file name
byiu3 0:be9547c5b62c 286 sprintf(data_file, "data_acq/data%d.txt", data_file_count);
byiu3 0:be9547c5b62c 287 //sprintf(data_file, "/sd/data_acq/data%d.txt", data_file_count);
byiu3 0:be9547c5b62c 288 data_file_count++; // Increment the data file count
byiu3 0:be9547c5b62c 289
byiu3 0:be9547c5b62c 290 // Write the new count to the vars file
byiu3 0:be9547c5b62c 291 fp_vars = fopen(VARS_FILE_NAME, "w");
byiu3 0:be9547c5b62c 292 fprintf(fp_vars, "log=%d\ndata=%d\nimu=%d\n", log_file_count+1, data_file_count, imu_num);
byiu3 0:be9547c5b62c 293 fclose(fp_vars);
byiu3 0:be9547c5b62c 294
byiu3 0:be9547c5b62c 295 // Try to open the data file
byiu3 0:be9547c5b62c 296 fp_data = sd.open(data_file, O_WRONLY | O_CREAT | O_TRUNC);
byiu3 0:be9547c5b62c 297 //fp_data = fopen(data_file, "w");
byiu3 0:be9547c5b62c 298 if (fp_data == NULL) {
byiu3 0:be9547c5b62c 299 if (serial_toggle)
byiu3 0:be9547c5b62c 300 pc.printf("Failed to open data file: %s\n\r", data_file);
byiu3 0:be9547c5b62c 301
byiu3 0:be9547c5b62c 302 if ((fp_log = fopen(log_file, "a")) == NULL)
byiu3 0:be9547c5b62c 303 if (serial_toggle)
byiu3 0:be9547c5b62c 304 pc.printf("Failed to open log file 1\n\r");
byiu3 0:be9547c5b62c 305 else {
byiu3 0:be9547c5b62c 306 fprintf(fp_log, "Failed to open data file: %s\n\r", data_file);
byiu3 0:be9547c5b62c 307 fclose(fp_log);
byiu3 0:be9547c5b62c 308 }
byiu3 0:be9547c5b62c 309 }
byiu3 0:be9547c5b62c 310 else {
byiu3 0:be9547c5b62c 311
byiu3 0:be9547c5b62c 312 if (serial_toggle)
byiu3 0:be9547c5b62c 313 pc.printf("Opened data file for writing: %s\n\r", data_file);
byiu3 0:be9547c5b62c 314
byiu3 0:be9547c5b62c 315 if ((fp_log = fopen(log_file, "a")) == NULL)
byiu3 0:be9547c5b62c 316 if (serial_toggle)
byiu3 0:be9547c5b62c 317 pc.printf("Failed to open log file 2\n\r");
byiu3 0:be9547c5b62c 318 else {
byiu3 0:be9547c5b62c 319 fprintf(fp_log, "Opened data file for writing: %s\n\r", data_file);
byiu3 0:be9547c5b62c 320 fclose(fp_log);
byiu3 0:be9547c5b62c 321 }
byiu3 0:be9547c5b62c 322
byiu3 0:be9547c5b62c 323 if (serial_toggle)
byiu3 0:be9547c5b62c 324 pc.printf("Started taking data...\n\r");
byiu3 0:be9547c5b62c 325
byiu3 0:be9547c5b62c 326 get_data = true;
byiu3 0:be9547c5b62c 327 timer1.start();
byiu3 0:be9547c5b62c 328 myled2 = 0; // Taking data
byiu3 0:be9547c5b62c 329 myled3 = 1;
byiu3 0:be9547c5b62c 330 }
byiu3 0:be9547c5b62c 331 break;
byiu3 0:be9547c5b62c 332 case STATE_2B:
byiu3 0:be9547c5b62c 333 cur_state = STATE_3B;
byiu3 0:be9547c5b62c 334 pc.printf("PB1: STATE_2B->STATE_3B\n\r");
byiu3 0:be9547c5b62c 335 break;
byiu3 0:be9547c5b62c 336 case STATE_3B:
byiu3 0:be9547c5b62c 337 cur_state = STATE_2A;
byiu3 0:be9547c5b62c 338 pc.printf("PB1: STATE_3B->STATE_2A\n\r");
byiu3 0:be9547c5b62c 339 break;
byiu3 0:be9547c5b62c 340 }
byiu3 0:be9547c5b62c 341 }
byiu3 0:be9547c5b62c 342
byiu3 0:be9547c5b62c 343 void pb2_irq() {
byiu3 0:be9547c5b62c 344
byiu3 0:be9547c5b62c 345 switch (cur_state) {
byiu3 0:be9547c5b62c 346 case STATE_1:
byiu3 0:be9547c5b62c 347 cur_state = STATE_2B;
byiu3 0:be9547c5b62c 348 pc.printf("PB2: STATE_1->STATE_2B\n\r");
byiu3 0:be9547c5b62c 349 break;
byiu3 0:be9547c5b62c 350 case STATE_2A:
byiu3 0:be9547c5b62c 351 cur_state = STATE_3A;
byiu3 0:be9547c5b62c 352 pc.printf("PB2: STATE_2A->STATE_3B\n\r");
byiu3 0:be9547c5b62c 353 break;
byiu3 0:be9547c5b62c 354 case STATE_3A:
byiu3 0:be9547c5b62c 355 cur_state = STATE_2B;
byiu3 0:be9547c5b62c 356 pc.printf("PB2: STATE_3A->STATE_2B\n\r");
byiu3 0:be9547c5b62c 357 break;
byiu3 0:be9547c5b62c 358 case STATE_2B:
byiu3 0:be9547c5b62c 359 pc.printf("PB2: STATE_2B->STATE_2B\n\r");
byiu3 0:be9547c5b62c 360 break;
byiu3 0:be9547c5b62c 361 case STATE_3B:
byiu3 0:be9547c5b62c 362 cur_state = STATE_1;
byiu3 0:be9547c5b62c 363 timer1.stop();
byiu3 0:be9547c5b62c 364
byiu3 0:be9547c5b62c 365 // Write the remaining values from the buffer
byiu3 0:be9547c5b62c 366 for (j = 0; j < i; j++) {
byiu3 0:be9547c5b62c 367 fp_data->write(&(ax_buf[j]), sizeof(ax_buf[j]));
byiu3 0:be9547c5b62c 368 fp_data->write(&(ay_buf[j]), sizeof(ay_buf[j]));
byiu3 0:be9547c5b62c 369 fp_data->write(&(az_buf[j]), sizeof(az_buf[j]));
byiu3 0:be9547c5b62c 370 fp_data->write(&(gx_buf[j]), sizeof(gx_buf[j]));
byiu3 0:be9547c5b62c 371 fp_data->write(&(gy_buf[j]), sizeof(gy_buf[j]));
byiu3 0:be9547c5b62c 372 fp_data->write(&(gz_buf[j]), sizeof(gz_buf[j]));
byiu3 0:be9547c5b62c 373 // fp_data->write(&(yaw_buf[j]), sizeof(yaw_buf[j]));
byiu3 0:be9547c5b62c 374 // fp_data->write(&(pitch_buf[j]), sizeof(pitch_buf[j]));
byiu3 0:be9547c5b62c 375 // fp_data->write(&(roll_buf[j]), sizeof(roll_buf[j]));
byiu3 0:be9547c5b62c 376 fp_data->write(&(avg_samp_mag[j]), sizeof(avg_samp_mag[j]));
byiu3 0:be9547c5b62c 377 fp_data->write(&(avg_samp_rot[j]), sizeof(avg_samp_rot[j]));
byiu3 0:be9547c5b62c 378 //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]);
byiu3 0:be9547c5b62c 379 }
byiu3 0:be9547c5b62c 380
byiu3 0:be9547c5b62c 381 if(serial_toggle)
byiu3 0:be9547c5b62c 382 pc.printf("PB2: STATE_3B->STATE_1\n\r");
byiu3 0:be9547c5b62c 383
byiu3 0:be9547c5b62c 384 if (fp_data != NULL) {
byiu3 0:be9547c5b62c 385 if (serial_toggle) {
byiu3 0:be9547c5b62c 386 pc.printf("\n\rClosing files: %s\n\r", data_file);
byiu3 0:be9547c5b62c 387 pc.printf("Samples Taken: %d, %d\n\r", counting_up, counting_up_1);
byiu3 0:be9547c5b62c 388 pc.printf("Time for all samples: %f\n\r", elapsed_time = timer1.read());
byiu3 0:be9547c5b62c 389 pc.printf("Sample Rate: %f\n\r", (float) counting_up / elapsed_time);
byiu3 0:be9547c5b62c 390 }
byiu3 0:be9547c5b62c 391
byiu3 0:be9547c5b62c 392 if (fp_data->close())
byiu3 0:be9547c5b62c 393 //if (fclose(fp_data))
byiu3 0:be9547c5b62c 394 pc.printf("Failed to close file\n\r");
byiu3 0:be9547c5b62c 395 else
byiu3 0:be9547c5b62c 396 fp_data = NULL;
byiu3 0:be9547c5b62c 397
byiu3 0:be9547c5b62c 398 if ((fp_log = fopen(log_file, "a")) == NULL) {
byiu3 0:be9547c5b62c 399 if (serial_toggle) {
byiu3 0:be9547c5b62c 400 pc.printf("Failed to open log file\n\r");
byiu3 0:be9547c5b62c 401 }
byiu3 0:be9547c5b62c 402 }
byiu3 0:be9547c5b62c 403 else {
byiu3 0:be9547c5b62c 404 fprintf(fp_log, "\n\rClosing file: %s\n\r", data_file);
byiu3 0:be9547c5b62c 405 fprintf(fp_log, "Samples Taken: %d,%d\n\r", counting_up, counting_up_1);
byiu3 0:be9547c5b62c 406 fprintf(fp_log, "Time for all samples: %f\n\r", elapsed_time);
byiu3 0:be9547c5b62c 407 fprintf(fp_log, "Sample Rate: %f\n\r", (float) counting_up / elapsed_time);
byiu3 0:be9547c5b62c 408 fclose(fp_log);
byiu3 0:be9547c5b62c 409 }
byiu3 0:be9547c5b62c 410 }
byiu3 0:be9547c5b62c 411
byiu3 0:be9547c5b62c 412 get_data = false;
byiu3 0:be9547c5b62c 413 counting_up = 0;
byiu3 0:be9547c5b62c 414 counting_up_1 = 0;
byiu3 0:be9547c5b62c 415 timer1.reset();
byiu3 0:be9547c5b62c 416
byiu3 0:be9547c5b62c 417 myled3 = 0; // Done taking data
byiu3 0:be9547c5b62c 418
byiu3 0:be9547c5b62c 419 // Flash the fourth led twice
byiu3 0:be9547c5b62c 420 for (j = 0; j < 4; j++) {
byiu3 0:be9547c5b62c 421 myled4 = 1;
byiu3 0:be9547c5b62c 422 wait(0.25);
byiu3 0:be9547c5b62c 423 myled4 = 0;
byiu3 0:be9547c5b62c 424 wait(0.25);
byiu3 0:be9547c5b62c 425 }
byiu3 0:be9547c5b62c 426
byiu3 0:be9547c5b62c 427 myled2 = 1; // Ready to take more data
byiu3 0:be9547c5b62c 428 break;
byiu3 0:be9547c5b62c 429 } // Switch
byiu3 0:be9547c5b62c 430 }
byiu3 0:be9547c5b62c 431
byiu3 0:be9547c5b62c 432 void crashb_irq() {
byiu3 0:be9547c5b62c 433 crashled = 0;
byiu3 0:be9547c5b62c 434 }
byiu3 0:be9547c5b62c 435
byiu3 0:be9547c5b62c 436 void threshold_detector(int cur_i) {
byiu3 0:be9547c5b62c 437 // The last 10 samples
byiu3 0:be9547c5b62c 438 for (j = i; j > i-mag_window; j--) {
byiu3 0:be9547c5b62c 439 acc_mag = 0;
byiu3 0:be9547c5b62c 440 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];
byiu3 0:be9547c5b62c 441 acc_running_sum = acc_running_sum + acc_mag;
byiu3 0:be9547c5b62c 442
byiu3 0:be9547c5b62c 443 gyro_mag = 0;
byiu3 0:be9547c5b62c 444 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];
byiu3 0:be9547c5b62c 445 gyro_running_sum = gyro_running_sum + gyro_mag;
byiu3 0:be9547c5b62c 446 }
byiu3 0:be9547c5b62c 447
byiu3 0:be9547c5b62c 448 avg_mag = acc_running_sum / mag_window;
byiu3 0:be9547c5b62c 449 avg_samp_mag[i] = avg_mag;
byiu3 0:be9547c5b62c 450
byiu3 0:be9547c5b62c 451 avg_rot = gyro_running_sum / rot_window;
byiu3 0:be9547c5b62c 452 avg_samp_rot[i] = avg_rot;
byiu3 0:be9547c5b62c 453
byiu3 0:be9547c5b62c 454 if (avg_mag > mag_threshold*mag_percent)
byiu3 0:be9547c5b62c 455 crashled = 1;
byiu3 0:be9547c5b62c 456
byiu3 0:be9547c5b62c 457 if (avg_rot > rot_threshold*rot_percent)
byiu3 0:be9547c5b62c 458 crashled = 1;
byiu3 0:be9547c5b62c 459
byiu3 0:be9547c5b62c 460 acc_running_sum = 0;
byiu3 0:be9547c5b62c 461 gyro_running_sum = 0;
byiu3 0:be9547c5b62c 462 acc_mag = 0;
byiu3 0:be9547c5b62c 463 gyro_mag = 0;
byiu3 0:be9547c5b62c 464 avg_mag = 0;
byiu3 0:be9547c5b62c 465 avg_rot = 0;
byiu3 0:be9547c5b62c 466 }
byiu3 0:be9547c5b62c 467
byiu3 0:be9547c5b62c 468 void setup() {
byiu3 0:be9547c5b62c 469 // Setting up
byiu3 0:be9547c5b62c 470 myled1 = 1;
byiu3 0:be9547c5b62c 471
byiu3 0:be9547c5b62c 472 // Pin setup
byiu3 0:be9547c5b62c 473 pb1.mode(PullUp);
byiu3 0:be9547c5b62c 474 pb1.rise(&pb1_irq);
byiu3 0:be9547c5b62c 475
byiu3 0:be9547c5b62c 476 pb2.mode(PullUp);
byiu3 0:be9547c5b62c 477 pb2.rise(&pb2_irq);
byiu3 0:be9547c5b62c 478
byiu3 0:be9547c5b62c 479 crashb.mode(PullUp);
byiu3 0:be9547c5b62c 480 crashb.rise(&crashb_irq);
byiu3 0:be9547c5b62c 481
byiu3 0:be9547c5b62c 482 // Serial debug baud rate
byiu3 0:be9547c5b62c 483 pc.baud(9600);
byiu3 0:be9547c5b62c 484 pc.printf("\n\r");
byiu3 0:be9547c5b62c 485
byiu3 0:be9547c5b62c 486 //Display the card type and capacity
byiu3 0:be9547c5b62c 487 pc.printf("\nCard type: ");
byiu3 0:be9547c5b62c 488 if (sd.card_type() == SDFileSystem::CARD_NONE)
byiu3 0:be9547c5b62c 489 pc.printf("None\n\r");
byiu3 0:be9547c5b62c 490 else if (sd.card_type() == SDFileSystem::CARD_MMC)
byiu3 0:be9547c5b62c 491 pc.printf("MMC\n\r");
byiu3 0:be9547c5b62c 492 else if (sd.card_type() == SDFileSystem::CARD_SD)
byiu3 0:be9547c5b62c 493 pc.printf("SD\n\r");
byiu3 0:be9547c5b62c 494 else if (sd.card_type() == SDFileSystem::CARD_SDHC)
byiu3 0:be9547c5b62c 495 pc.printf("SDHC\n\r");
byiu3 0:be9547c5b62c 496 else
byiu3 0:be9547c5b62c 497 pc.printf("Unknown\n\r");
byiu3 0:be9547c5b62c 498 pc.printf("Sectors: %llu\n\r", sd.disk_sectors());
byiu3 0:be9547c5b62c 499 pc.printf("Capacity: %.1fMB\n\r", (sd.disk_sectors() * 512) / 1048576.0);
byiu3 0:be9547c5b62c 500
byiu3 0:be9547c5b62c 501 // Open the setup file, if possible, if not, create one
byiu3 0:be9547c5b62c 502 // Contains the last log file number
byiu3 0:be9547c5b62c 503 fp_vars = fopen(VARS_FILE_NAME, "r");
byiu3 0:be9547c5b62c 504 if (fp_vars == NULL) {
byiu3 0:be9547c5b62c 505 pc.printf("No Vars File; Creating One...\n\r");
byiu3 0:be9547c5b62c 506 fp_vars = fopen(VARS_FILE_NAME, "w");
byiu3 0:be9547c5b62c 507 fprintf(fp_vars, "log=%d\ndata=%d\nimu=%d\n", log_file_count, data_file_count, imu_num);
byiu3 0:be9547c5b62c 508 fprintf(fp_vars, "mag_threshold=%d\nmag_window=%d\nmag_percent=%f\n", mag_threshold, mag_window, mag_percent);
byiu3 0:be9547c5b62c 509 fprintf(fp_vars, "rot_threshold=%d\nrot_window=%d\nrot_percent=%f\n", rot_threshold, rot_window, rot_percent);
byiu3 0:be9547c5b62c 510 fprintf(fp_vars, "serial=%d\n", serial_toggle);
byiu3 0:be9547c5b62c 511 fclose(fp_vars);
byiu3 0:be9547c5b62c 512 }
byiu3 0:be9547c5b62c 513 else {
byiu3 0:be9547c5b62c 514 fscanf(fp_vars, "log=%d\ndata=%d\nimu=%d\n", &log_file_count, &data_file_count, &imu_num);
byiu3 0:be9547c5b62c 515 fscanf(fp_vars, "mag_threshold=%d\nmag_window=%d\nmag_percent=%f\n", &mag_threshold, &mag_window, &mag_percent);
byiu3 0:be9547c5b62c 516 fscanf(fp_vars, "rot_threshold=%d\nrot_window=%d\nrot_percent=%f\n", &rot_threshold, &rot_window, &rot_percent);
byiu3 0:be9547c5b62c 517 fscanf(fp_vars, "serial=%d\n", &serial_toggle);
byiu3 0:be9547c5b62c 518 rewind(fp_vars);
byiu3 0:be9547c5b62c 519 fprintf(fp_vars, "log=%d\ndata=%d\nimu=%d\n", log_file+1, data_file+1, imu_num);
byiu3 0:be9547c5b62c 520 pc.printf("%d,%d,%d\n\r", log_file_count, data_file_count, imu_num);
byiu3 0:be9547c5b62c 521 }
byiu3 0:be9547c5b62c 522
byiu3 0:be9547c5b62c 523 // Open the log file to write
byiu3 0:be9547c5b62c 524 sprintf(log_file, LOG_FILE_NAME, log_file_count);
byiu3 0:be9547c5b62c 525 if ((fp_log = fopen(log_file, "w")) == NULL)
byiu3 0:be9547c5b62c 526 pc.printf("Failed to open log file: %s\n\r", log_file);
byiu3 0:be9547c5b62c 527
byiu3 0:be9547c5b62c 528 // Get the last log file number
byiu3 0:be9547c5b62c 529 pc.printf("Log File Starting at %d\n\r", log_file_count);
byiu3 0:be9547c5b62c 530 pc.printf("Data File Starting at %d\n\r", data_file_count);
byiu3 0:be9547c5b62c 531 pc.printf("Using IMU %d\n\r", imu_num);
byiu3 0:be9547c5b62c 532
byiu3 0:be9547c5b62c 533 fprintf(fp_log, "Log File Starting at %d\n\r", log_file_count);
byiu3 0:be9547c5b62c 534 fprintf(fp_log, "Data File Starting at %d\n\r", data_file_count);
byiu3 0:be9547c5b62c 535 fprintf(fp_log, "Using IMU %d\n\r", imu_num);
byiu3 0:be9547c5b62c 536
byiu3 0:be9547c5b62c 537 // An empty line
byiu3 0:be9547c5b62c 538 fprintf(fp_log, "\n\r");
byiu3 0:be9547c5b62c 539
byiu3 0:be9547c5b62c 540 // Test the connection
byiu3 0:be9547c5b62c 541 connected = imu1->testConnection();
byiu3 0:be9547c5b62c 542 if (connected) {
byiu3 0:be9547c5b62c 543 pc.printf("Connection Success!\n\r");
byiu3 0:be9547c5b62c 544 fprintf(fp_log, "Connection Success!\n\r");
byiu3 0:be9547c5b62c 545 }
byiu3 0:be9547c5b62c 546 else {
byiu3 0:be9547c5b62c 547 pc.printf("Connection Failed!\n\r");
byiu3 0:be9547c5b62c 548 fprintf(fp_log, "Connection Failed!\n\r");
byiu3 0:be9547c5b62c 549 }
byiu3 0:be9547c5b62c 550
byiu3 0:be9547c5b62c 551 if (connected) {
byiu3 0:be9547c5b62c 552 float destination[6];
byiu3 0:be9547c5b62c 553 calibrated = imu1->selfTest(destination);
byiu3 0:be9547c5b62c 554
byiu3 0:be9547c5b62c 555 if (!calibrated) {
byiu3 0:be9547c5b62c 556 pc.printf("Passed Self Test\n\r");
byiu3 0:be9547c5b62c 557 fprintf(fp_log, "Passed Self Test\n\r");
byiu3 0:be9547c5b62c 558 }
byiu3 0:be9547c5b62c 559 else {
byiu3 0:be9547c5b62c 560 pc.printf("Failed Self Test\n\r");
byiu3 0:be9547c5b62c 561 fprintf(fp_log, "Failed Self Test\n\r");
byiu3 0:be9547c5b62c 562 }
byiu3 0:be9547c5b62c 563
byiu3 0:be9547c5b62c 564 // Set the biases
byiu3 0:be9547c5b62c 565 accel_bias[0] = 0;
byiu3 0:be9547c5b62c 566 accel_bias[1] = 0;
byiu3 0:be9547c5b62c 567 accel_bias[2] = 0;
byiu3 0:be9547c5b62c 568
byiu3 0:be9547c5b62c 569 gyro_bias[0] = 0;
byiu3 0:be9547c5b62c 570 gyro_bias[1] = 0;
byiu3 0:be9547c5b62c 571 gyro_bias[2] = 0;
byiu3 0:be9547c5b62c 572 }
byiu3 0:be9547c5b62c 573 }
byiu3 0:be9547c5b62c 574
byiu3 0:be9547c5b62c 575 /* Notes on sampling rates and output rates
byiu3 0:be9547c5b62c 576 * The accelerometer ALWAYS outputs data at 1kHz.
byiu3 0:be9547c5b62c 577 * The gyroscope has a variable output rate at either 1 kHz or 8 kHz
byiu3 0:be9547c5b62c 578 * set by DLPF_CFG
byiu3 0:be9547c5b62c 579 * The MPU6050 will sample data at the rate specified by setting the
byiu3 0:be9547c5b62c 580 * SMPLRT_DIV value and put these values into the sensor output
byiu3 0:be9547c5b62c 581 * registers, fifo output, dmp sampling, and motion detection.
byiu3 0:be9547c5b62c 582 * What this means is that even though the accelerometer and gyroscope
byiu3 0:be9547c5b62c 583 * are outputting at 1 kHz and 1 or 8 kHz, the MPU6050 may be
byiu3 0:be9547c5b62c 584 * ignoring data if the sampling rate is set lower than these. It
byiu3 0:be9547c5b62c 585 * also means that it can get multiple accelerometer outputs that
byiu3 0:be9547c5b62c 586 * are duplicates if the sampling rate is set higher than 1 kHz.
byiu3 0:be9547c5b62c 587 * The bandpass filter causes a delay in the data getting outputted so
byiu3 0:be9547c5b62c 588 * this also needs to be factored into the equation. From my
byiu3 0:be9547c5b62c 589 * understanding, the delay will set the maximum rate that the
byiu3 0:be9547c5b62c 590 * sensor can output at. So a delay of 4.9 ms means that the max
byiu3 0:be9547c5b62c 591 * rate that the sensor can output at is 1/0.0049s, about 200 Hz.
byiu3 0:be9547c5b62c 592 * In this example we set DLPF_CFG = 0x3 which will set a 1 kHz
byiu3 0:be9547c5b62c 593 * output rate for both the gyro and acelerometer. But there
byiu3 0:be9547c5b62c 594 * is a delay of 4.9 ms for the accelerometer and 4.8 ms for the
byiu3 0:be9547c5b62c 595 * gyro so the output rate is 200 Hz. We set the MPU6050 sampling
byiu3 0:be9547c5b62c 596 * rate to be 200 Hz so that we don't have duplicates and don't
byiu3 0:be9547c5b62c 597 * miss any data point.
byiu3 0:be9547c5b62c 598 */
byiu3 0:be9547c5b62c 599
byiu3 0:be9547c5b62c 600 void initMPU6050() {
byiu3 0:be9547c5b62c 601 // Clear sleep mode bit, enable all sensors
byiu3 0:be9547c5b62c 602 imu1->setSleepEnabled(false);
byiu3 0:be9547c5b62c 603
byiu3 0:be9547c5b62c 604 // Wait for PLL to lock
byiu3 0:be9547c5b62c 605 wait(0.1);
byiu3 0:be9547c5b62c 606
byiu3 0:be9547c5b62c 607 // Set clock source to have x-axis gyro reference
byiu3 0:be9547c5b62c 608 imu1->setClockSource(MPU6050_CLOCK_PLL_XGYRO);
byiu3 0:be9547c5b62c 609
byiu3 0:be9547c5b62c 610 // Disable FSYNC and set accle and gyro to 44 and 42 Hz, respectively
byiu3 0:be9547c5b62c 611 // Set DLPF_CFG to 0 for a sample rate of 1 kHz for the acceleromter and
byiu3 0:be9547c5b62c 612 // 8 kHz for the gyroscope.
byiu3 0:be9547c5b62c 613 imu1->setDLPFMode(MPU6050_DLPF_BW_256);
byiu3 0:be9547c5b62c 614
byiu3 0:be9547c5b62c 615 // Set the sampling rate of the device. This is the rate at which the registers
byiu3 0:be9547c5b62c 616 // get occupied and it is Gyroscope_Rate/x where x is the argument sent into
byiu3 0:be9547c5b62c 617 // setRate(x)
byiu3 0:be9547c5b62c 618 imu1->setRate(0x3);
byiu3 0:be9547c5b62c 619
byiu3 0:be9547c5b62c 620 // Set the gyroscope configuration
byiu3 0:be9547c5b62c 621 imu1->setGyroXSelfTest(false); // Clear the self-test bits
byiu3 0:be9547c5b62c 622 imu1->setGyroYSelfTest(false);
byiu3 0:be9547c5b62c 623 imu1->setGyroZSelfTest(false);
byiu3 0:be9547c5b62c 624 imu1->setFullScaleGyroRange(MPU6050_GYRO_FS_250); // Clear the FS bits
byiu3 0:be9547c5b62c 625 imu1->setFullScaleGyroRange(MPU6050_GYRO_FS_2000); // Set the full-scale range
byiu3 0:be9547c5b62c 626
byiu3 0:be9547c5b62c 627 // Set the accelerometer configuration
byiu3 0:be9547c5b62c 628 imu1->setAccelXSelfTest(false); // Clear the self-test bits
byiu3 0:be9547c5b62c 629 imu1->setAccelYSelfTest(false);
byiu3 0:be9547c5b62c 630 imu1->setAccelZSelfTest(false);
byiu3 0:be9547c5b62c 631 imu1->setFullScaleAccelRange(MPU6050_ACCEL_FS_2); // Clear the AFS bits
byiu3 0:be9547c5b62c 632 imu1->setFullScaleAccelRange(MPU6050_ACCEL_FS_16); // Set the full-scale range
byiu3 0:be9547c5b62c 633
byiu3 0:be9547c5b62c 634 // Configure interrupts and bypass enable
byiu3 0:be9547c5b62c 635 // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS
byiu3 0:be9547c5b62c 636 // Enable I2C_BYPASS_EN so addition chips can join the I2C bus and all can be
byiu3 0:be9547c5b62c 637 // controlled by the microcontroller as master
byiu3 0:be9547c5b62c 638 imu1->setInterruptMode(MPU6050_INTMODE_ACTIVEHIGH);
byiu3 0:be9547c5b62c 639 imu1->setInterruptDrive(MPU6050_INTDRV_PUSHPULL);
byiu3 0:be9547c5b62c 640 imu1->setInterruptLatch(MPU6050_INTLATCH_WAITCLEAR);
byiu3 0:be9547c5b62c 641 imu1->setInterruptLatchClear(MPU6050_INTCLEAR_STATUSREAD);
byiu3 0:be9547c5b62c 642 imu1->setFSyncInterruptLevel(false);
byiu3 0:be9547c5b62c 643 imu1->setFSyncInterruptEnabled(false);
byiu3 0:be9547c5b62c 644 imu1->setI2CBypassEnabled(true);
byiu3 0:be9547c5b62c 645 imu1->setIntDataReadyEnabled(true);
byiu3 0:be9547c5b62c 646
byiu3 0:be9547c5b62c 647 }