From 15a8da071e994590f137af46cb9b372cb0f3bfad Mon Sep 17 00:00:00 2001 From: Mike Bernard Date: Tue, 10 Nov 2020 23:01:22 -0500 Subject: [PATCH] add calibrate_imu_linear_acceleration function --- src/main.ino | 128 +++++++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 120 insertions(+), 8 deletions(-) diff --git a/src/main.ino b/src/main.ino index ad8edd1..db169cb 100644 --- a/src/main.ino +++ b/src/main.ino @@ -1,10 +1,19 @@ #include // C STD Int library, might not be needed +// #include // TODO: what's this and why is it commented-out? +#include // TODO: what's this? +#include "I2Cdev.h" +#include // TODO: what's this? #include "mission_constants.hh" #include "moding.hh" #include "MPU6050.h" // MPU 6050 IMU Library +#include // TODO: what's this? +#include // TODO: what's this? #include "Wire.h" // Arduino library #include "workspace.hh" // variable storage + +using namespace Eigen; + // (--) instance of workspace class storing all the variables used in the loop Workspace ws; @@ -23,6 +32,43 @@ void print_measurements(float a[3], float w[3]) 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_ACC_BIAS) +{ + int num_loops = 2500; // number of cycles to check measurements for bias + long a_sums[3] = {0, 0, 0}; // sum of 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); + a_sums[0] += ax; + a_sums[1] += ay; + a_sums[2] += az; + + // TODO: do we need to zero the values of ax, ay, and az every loop? + // TODO: why aren't we also calculating an agular velocity bias? + } + + // find average bias + a_sums[0] /= num_loops; + a_sums[1] /= num_loops; + a_sums[2] /= num_loops; + + // store biases for later use + IMU_ACC_BIAS[0] = a_sums[0]; + IMU_ACC_BIAS[1] = a_sums[1]; // TODO: why does AGON1a have a_sums[1] - imuRange/2? + IMU_ACC_BIAS[2] = a_sums[2]; +} + /* get_latest_measurements * * Wrapper around MPU6050::getMotion6 that converts the sensor counts into engineering values. @@ -42,25 +88,39 @@ void print_measurements(float a[3], float w[3]) * Outputs: * - return 0 */ -int get_latest_measurements(MPU6050 imu, float *a[3], float *w[3], bool debug=false) +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*LSB_LINEAR*1000*G_2_MS2; // (m/s^2) x-axis acceleration - *a[1] = ay*LSB_LINEAR*1000*G_2_MS2; // (m/s^2) y-axis acceleration - *a[2] = az*LSB_LINEAR*1000*G_2_MS2; // (m/s^2) z-axis acceleration - *w[0] = wx*LSB_ANGULAR*DEG_2_RAD; // (rad/s) x-axis angular velocity - *w[1] = wy*LSB_ANGULAR*DEG_2_RAD; // (rad/s) y-axis angular velocity - *w[2] = wz*LSB_ANGULAR*DEG_2_RAD; // (rad/s) z-axis angular velocity + *a[0] = (ax - IMU_ACC_BIAS[0])*LSB_LINEAR*1000*G_2_MS2; // (m/s^2) x-axis acceleration + *a[1] = (ay - IMU_ACC_BIAS[1])*LSB_LINEAR*1000*G_2_MS2; // (m/s^2) y-axis acceleration + *a[2] = (az - IMU_ACC_BIAS[2])*LSB_LINEAR*1000*G_2_MS2; // (m/s^2) z-axis acceleration + *w[0] = wx*LSB_ANGULAR*DEG_2_RAD; // (rad/s) x-axis angular velocity + *w[1] = wy*LSB_ANGULAR*DEG_2_RAD; // (rad/s) y-axis angular velocity + *w[2] = wz*LSB_ANGULAR*DEG_2_RAD; // (rad/s) z-axis angular velocity if (debug) { print_measurements(a, w); } +} - return 0; + +/* tvc_abs + * + * TODO: add docstring + */ +void tvc_abs(int x, int y, double gamma, int d) +{ + // TODO: why are x and y in parentheses? + // TODO: where are cos and sin defined? + double u = (x)*cos(gamma) + (y)*sin(gamma); + double v = (y)*cos(gamma) - (x)*sin(gamma); + ws.tvc_y.write(v + TVC_Y_OFFSET); + ws.tvc_x.write(u + TVC_X_OFFSET); + delay(d); // TODO: why is this delay here? } @@ -70,6 +130,58 @@ int get_latest_measurements(MPU6050 imu, float *a[3], float *w[3], bool debug=fa */ void setup() { + // set up pins (OUTPUT and LOW are defined in Arduino.h) + pinMode(B_LED_1, OUTPUT); + pinMode(G_LED_1, OUTPUT); + pinMode(B_LED_2, OUTPUT); + pinMode(G_LED_2, OUTPUT); + + digitalWrite(B_LED_1, LOW); + digitalWrite(G_LED_1, LOW); + digitalWrite(B_LED_2, LOW); + digitalWrite(G_LED_2, LOW); + + // join I2C bus (I2Cdev library doesn't do this automatically) + // TODO: do these need to be #if's, i.e. could they be regular C++ if's? + #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + Wire.begin(); + #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + Fastwire::setup(400, true); + #endif + + flash.begin(9600); // TODO: what does this do? + // flash.eraseChip(); // TODO: what does this do and why is it commented-out? + + // set initial mission mode + ws.mode = STARTUP_STABLE; + + // set up Thrust-Vector Controller (TVC) + ws.tvc_x.attach(TVC_X_PIN); // TODO: add description + ws.tvc_y.attach(TVC_Y_PIN); // TODO: add description + ws.tvc_top.attach(TVC_TOP_PIN); // TODO: add description + + ws.tvc_top.write(10); delay(1000); // TODO: what is the "10" doing and can it be a mission constant? + + // TODO: what's going on in this block? + tvc_abs( 0, 0, BETA, TVC_DELAY*100); + tvc_abs( 30, 0, BETA, TVC_DELAY*100); + tvc_abs(-30, 0, BETA, TVC_DELAY*100); + tvc_abs( 0, 0, BETA, TVC_DELAY*100); + tvc_abs( 0, 30, BETA, TVC_DELAY*100); + tvc_abs( 0, -30, BETA, TVC_DELAY*100); + tvc_abs( 0, 0, BETA, TVC_DELAY*100); + + // set up IMU + ws.imu.initialize(); + ws.imu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8); + ws.imu.setFullScaleGyroRange(MPU6050_GYRO_FS_250); + ws.imu.CalibrateAccel(30); // TODO: What is "30"? Can this be made a mission constant? + ws.imu.CalibrateGyro(30); // TODO: What is "30"? Can this be made a mission constant? + + // calibrate the IMU linear acceleration sensors + calibrate_imu_linear_acceleration(*IMU_ACC_BIAS); + + // TODO: finish pulling over the rest of AGON1a setup code below this point }