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?
User | Revision | Line number | New 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 | } |