From e3ccebeb5ea6b2980e2685350ea6f6f10dfa97a7 Mon Sep 17 00:00:00 2001 From: LiamSmego Date: Sun, 14 Mar 2021 14:13:35 -0400 Subject: [PATCH] delete outdated imu code --- main/main.ino | 102 -------------------------------------------------- 1 file changed, 102 deletions(-) diff --git a/main/main.ino b/main/main.ino index 89242cf..11c00f6 100644 --- a/main/main.ino +++ b/main/main.ino @@ -16,108 +16,6 @@ SPIFlash flash(1); // (--) instance of workspace class storing all the variables used in the loop Workspace ws; -/* print_measurements - * - * Print out the latest IMU measurements, formatted nicely. - */ -void print_measurements(float a[3], float w[3]) -{ - Serial.print('a (m/s^2): '); Serial.print(a[0]); - Serial.print('\t'); Serial.print(a[1]); - Serial.print('\t'); Serial.println(a[2]); - - Serial.print('w (rad/s): '); Serial.print(w[0]); - Serial.print('\t'); Serial.print(w[1]); - Serial.print('\t'); Serial.println(w[2]); -} - - -/* calibrate_imu_linear_acceleration - * - * Checks a sample of IMU measurements over a few seconds while the rocket - * is ASSUMED to be still. Averages the sum of those measurements to get a - * linear acceleration bias along each axis. This bias is stored and assumed - * to be constant throughout the flight. Bias is used in get_latest_measurements - * when converting sensor readings to engineering values. - */ -void calibrate_imu_linear_acceleration(int16_t *imu_lin_acc_bias[3], int16_t *imu_ang_vel_bias[3]) -{ - int num_loops = 2500; // number of cycles to check measurements for bias - long a_sums[3] = {0, 0, 0}; // sum of linear acceleration measurements each loop to be averaged - long w_sums[3] = {0, 0, 0}; // sum of angular velocity measurements each loop to be averaged - - int16_t ax, ay, az, wx, wy, wz; // temp loop vars - - for (int i = 0; i < num_loops; i++) - { - imu.getMotion6(&ax, &ay, &az, &wx, &wy, &wz); //we want to label the vertical axis as z. the MPUs are orientated with y as vertical - a_sums[0] += ax; - a_sums[1] += ay; - a_sums[2] += az; - - w_sums[0] += wx; - w_sums[1] += wy; - w_sums[2] += wz; - } - - // find average bias - a_sums[0] /= num_loops; - a_sums[1] /= num_loops; - a_sums[2] /= num_loops; - - w_sums[0] /= num_loops; - w_sums[1] /= num_loops; - w_sums[2] /= num_loops; - - // store biases for later use - *imu_lin_acc_bias[0] = a_sums[0]; - *imu_lin_acc_bias[1] = a_sums[1]; - *imu_lin_acc_bias[2] = a_sums[2]; - - *imu_ang_vel_bias[0] = w_sums[0]; - *imu_ang_vel_bias[1] = w_sums[1]; - *imu_ang_vel_bias[2] = w_sums[2]; -} - -/* get_latest_measurements - * - * Wrapper around MPU6050::getMotion6 that converts the sensor counts into engineering values. - * - * Inputs: - * - MPU6050 imu: instance of an MPU 6050 IMU connection to read measurements from - * - float *a[3]: address of 3-array to store converted linear acceleration measurements - * - float *w[3]: address of 3-array to store converted angular velocity measurements - * - * Modifies: - * - Sets values of passed arrays based on converted sensor measurement values - * - Linear Acceleration conversion: $a_i = a_{m,i}\cdot\text{LSB}_{\text{linear}}\cdot1000\frac{\text{milli-gee}}{\text{gee}}\cdot9.80665\frac{\text{m/s}^2}{gee}$ - * - Angular Velocity conversion: $\omega_i = \omega_{m,i}\cdot\text{LSB}_{\text{angular}}\cdot0.0174...\frac{\text{rad}}{\text{deg}}$ - * where $a_{m,i}$ denotes the linear acceleration measurement in direction $i$ - * and $\omega_{m,i}$ denotes the angular velocity measurement about axis $i$. - * - * Outputs: - * - return 0 - */ -void get_latest_measurements(MPU6050 imu, float *a[3], float *w[3], bool debug=false) -{ - // READ DATA - int16_t ax, ay, az, wx, wy, wz; // temp vars to store raw measurements - imu.getMotion6(&ax, &ay, &az, &wx, &wy, &wz); // stores measurements in temp vars - - // CONVERT AND STORE DATA - *a[0] = (ax - IMU_LIN_ACC_BIAS[0])*LSB_LINEAR*1000*G_2_MS2; // (m/s^2) x-axis acceleration - *a[1] = (ay - IMU_LIN_ACC_BIAS[1])*LSB_LINEAR*1000*G_2_MS2; // (m/s^2) y-axis acceleration - *a[2] = (az - IMU_LIN_ACC_BIAS[2])*LSB_LINEAR*1000*G_2_MS2; // (m/s^2) z-axis acceleration - - *w[0] = (wx - IMU_ANG_VEL_BIAS)*LSB_ANGULAR*DEG_2_RAD; // (rad/s) x-axis angular velocity - *w[1] = (wy - IMU_ANG_VEL_BIAS)*LSB_ANGULAR*DEG_2_RAD; // (rad/s) y-axis angular velocity - *w[2] = (wz - IMU_ANG_VEL_BIAS)*LSB_ANGULAR*DEG_2_RAD; // (rad/s) z-axis angular velocity - - if (debug) { - print_measurements(a, w); - } -} - /* tvc_abs *