Brenton Yiu / Mbed 2 deprecated buffer_algorithm

An mbed app using the MPU6050 for crash detection

Dependencies:   MPU6050 SDFileSystem mbed

main.cpp@0:be9547c5b62c, 2015-03-14 (annotated)

Committer:
byiu3
Date:
Sat Mar 14 15:46:05 2015 +0000
Revision:
0:be9547c5b62c
The most recent stable revision of my crash app. Contains support for two of the three crash algorithms (mag, rotation)

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 }