From 7c76e9218bc1582a5729edfafe1b7808f957a753 Mon Sep 17 00:00:00 2001 From: Benjamin Pelletier Date: Sat, 8 Mar 2025 14:34:58 -0800 Subject: [PATCH] Add imu_test sketch (#93) --- utils/imu_test/gravity_estimator.h | 44 ++++++ utils/imu_test/imu_test.ino | 246 +++++++++++++++++++++++++++++ 2 files changed, 290 insertions(+) create mode 100644 utils/imu_test/gravity_estimator.h create mode 100644 utils/imu_test/imu_test.ino diff --git a/utils/imu_test/gravity_estimator.h b/utils/imu_test/gravity_estimator.h new file mode 100644 index 0000000..0028138 --- /dev/null +++ b/utils/imu_test/gravity_estimator.h @@ -0,0 +1,44 @@ +#pragma once + +// Estimator of constant gravity acceleration. +class GravityEstimator { + public: + // Create an estimator incorporating new vertical acceleration measurements at a specified rate. + // newMeasurementWeight: Weight given to new measurements (0, 1). + // afterSeconds: Number of seconds after which newMeasurementWeight is given to new measurements. + GravityEstimator( + double newMeasurementWeight, double afterSeconds) + : kUpdate_(log(1 - newMeasurementWeight) / afterSeconds) + {} + + // Initialize estimator. + // t: Current time (milliseconds). + void init(uint32_t t) { + tPrev_ = t; + value_ = 1.0; + } + + // Provide new vertical acceleration measurement. + // t: Time measurement was taken (milliseconds). + // value: Measured vertical acceleration. + void update(uint32_t t, double value) { + double dt = ((double)t - tPrev_) * 0.001; + double f = exp(kUpdate_ * dt); + tPrev_ = t; + value_ = value_ * f + value * (1 - f); + } + + inline double estimate() { + return value_; + } + + private: + // Last time gravity estimate was updated + uint32_t tPrev_; + + // Rate at which new measurements are incorporated into best guess + double kUpdate_; + + // Best guess for strength of gravity + double value_; +}; diff --git a/utils/imu_test/imu_test.ino b/utils/imu_test/imu_test.ino new file mode 100644 index 0000000..97dc002 --- /dev/null +++ b/utils/imu_test/imu_test.ino @@ -0,0 +1,246 @@ +/* Demonstrates setting up and reading from the IMU to estimate world-frame vertical acceleration. + * + * Control what is printed to Serial by commenting and uncommenting the options under "What to + * display on the serial port" below. When ALIGN_TEXT is commented, the Serial output is suitable + * to be viewed with the Arduino Serial Plotter (squiggly button in upper right). + */ + +#include +#include "gravity_estimator.h" + +// === What to display on the serial port +// #define ALIGN_TEXT 3 // When set, puts values in equal-width columns with this many digits +// #define SHOW_QUATERNION // When set, print the components of the orientation quaternion +// #define SHOW_DEVICE_ACCEL // When set, print the components of the device-frame acceleration +// #define SHOW_WORLD_ACCEL // When set, print the components of the world-frame acceleration +#define SHOW_VERTICAL_ACCEL // When set, print the vertical acceleration (with gravity removed) +#define SHOW_FIXED_BOUNDS 0.2 // When set, print fixed values to keep the Arduino serial plot in a consistent range + +#define AD0_VAL 0 // The value of the last bit of the I2C address. + +ICM_20948_I2C myICM; // Otherwise create an ICM_20948_I2C object + +GravityEstimator gravityEstimator(0.9, 5.0); +double zAvg = 1.0; // Best guess for strength of gravity +uint32_t tPrev; // Last time gravity guess was updated +const double NEW_MEASUREMENT_WEIGHT = 0.9; // Weight given to new measurements... +const double AFTER_SECONDS = 5.0; // ...after this number of seconds +const double K_UPDATE = log(1 - NEW_MEASUREMENT_WEIGHT) / AFTER_SECONDS; + +inline void printFloat(double v) { + #ifdef ALIGN_TEXT + if (v >= 0) { + Serial.print(' '); + } + const int digits = ALIGN_TEXT; + #else + const int digits = 4; + #endif + Serial.print(v, digits); +} + +// https://math.stackexchange.com/a/1721393/921079 +void quaternionMult(double qw, double qx, double qy, double qz, + double rw, double rx, double ry, double rz, + double* pw, double* px, double* py, double* pz) +{ + *pw = rw*qw-rx*qx-ry*qy-rz*qz; + *px = rw*qx+rx*qw-ry*qz+rz*qy; + *py = rw*qy+rx*qz+ry*qw-rz*qx; + *pz = rw*qz-rx*qy+ry*qx+rz*qw; +} + +void rotateByQuaternion(double px, double py, double pz, + double qw, double qx, double qy, double qz, + double* p1x, double* p1y, double* p1z) +{ + double qrw, qrx, qry, qrz, qcw; + quaternionMult(qw, qx, qy, qz, + 0, px, py, pz, + &qrw, &qrx, &qry, &qrz); + quaternionMult(qrw, qrx, qry, qrz, + qw, -qx, -qy, -qz, + &qcw, p1x, p1y, p1z); +} + +void setup() +{ + Serial.begin(115200); + Serial.println(F("leaf imu_test")); + + delay(100); + + while (Serial.available()) + Serial.read(); + + Serial.println(F("Press any key to continue...")); + Serial.flush(); + + while (!Serial.available()) + ; + + Wire.begin(); + Wire.setClock(400000); + + bool initialized = false; + while (!initialized) + { + // Initialize the ICM-20948 + // If the DMP is enabled, .begin performs a minimal startup. We need to configure the sample mode etc. manually. + myICM.begin(Wire, AD0_VAL); + + Serial.print(F("Initialization of the sensor returned: ")); + Serial.println(myICM.statusString()); + if (myICM.status != ICM_20948_Stat_Ok) + { + Serial.println(F("Trying again...")); + delay(500); + } + else + { + initialized = true; + } + } + + Serial.println(F("Device connected!")); + + bool success = true; + + // Initialize the DMP + success &= (myICM.initializeDMP() == ICM_20948_Stat_Ok); + + // Enable the DMP orientation and accelerometer sensors + success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok); + success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_ACCELEROMETER) == ICM_20948_Stat_Ok); + + // Configure the output data rate + success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 0) == ICM_20948_Stat_Ok); // Set to the maximum + success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok); // Set to the maximum + + // Enable the FIFO + success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok); + + // Enable the DMP + success &= (myICM.enableDMP() == ICM_20948_Stat_Ok); + + // Reset DMP + success &= (myICM.resetDMP() == ICM_20948_Stat_Ok); + + // Reset FIFO + success &= (myICM.resetFIFO() == ICM_20948_Stat_Ok); + + // Check success + if (success) + { + Serial.println(F("DMP enabled!")); + } + else + { + Serial.println(F("Enable DMP failed!")); + Serial.println(F("Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h...")); + while (1) + ; + } + + tPrev = millis(); +} + +void loop() +{ + icm_20948_DMP_data_t data; + myICM.readDMPdataFromFIFO(&data); + + if ((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail)) // Was valid data available? + { + if ((data.header & DMP_header_bitmap_Quat9) > 0 && (data.header & DMP_header_bitmap_Accel) > 0) + { + uint32_t t = millis(); + + // Get quaternion that rotates device-frame vectors to world-frame + // Scale to +/- 1 + double qx = ((double)data.Quat9.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30 + double qy = ((double)data.Quat9.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30 + double qz = ((double)data.Quat9.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30 + double xyzMag2 = (qx * qx) + (qy * qy) + (qz * qz); + double qw = xyzMag2 <= 1 ? sqrt(1.0 - xyzMag2) : 0; + + // Get device-frame acceleration + double ax = ((double)data.Raw_Accel.Data.X) / 8192.0; + double ay = ((double)data.Raw_Accel.Data.Y) / 8192.0; + double az = ((double)data.Raw_Accel.Data.Z) / 8192.0; + + // Find world-frame acceleration (awz = acceleration, world frame, z direction = vertical acceleration) + double awx, awy, awz; + rotateByQuaternion(ax, ay, az, qw, qx, qy, qz, &awx, &awy, &awz); + + // Print information to Serial + bool needComma = false; + #ifdef SHOW_QUATERNION + Serial.print(F("Qw:")); + printFloat(qw); + Serial.print(F(",Qx:")); + printFloat(qx); + Serial.print(F(",Qy:")); + printFloat(qy); + Serial.print(F(",Qz:")); + printFloat(qz); + needComma = true; + #endif + + #ifdef SHOW_DEVICE_ACCEL + if (needComma) { + Serial.print(','); + } + Serial.print(F("Ax:")); + printFloat(ax); + Serial.print(F(",Ay:")); + printFloat(ay); + Serial.print(F(",Az:")); + printFloat(az); + needComma = true; + #endif + + #ifdef SHOW_WORLD_ACCEL + if (needComma) { + Serial.print(','); + } + Serial.print("Wx:"); + printFloat(awx); + Serial.print(",Wy:"); + printFloat(awy); + Serial.print(",Wz:"); + printFloat(awz); + needComma = true; + #endif + + #ifdef SHOW_VERTICAL_ACCEL + if (needComma) { + Serial.print(','); + } + Serial.print("dAz:"); + printFloat(awz - gravityEstimator.estimate()); + needComma = true; + #endif + + #ifdef SHOW_FIXED_BOUNDS + if (needComma) { + Serial.print(','); + } + Serial.print("min:"); + printFloat(SHOW_FIXED_BOUNDS); + Serial.print(",max:"); + printFloat(-SHOW_FIXED_BOUNDS); + #endif + + Serial.println(); + + // Update gravity estimator with measured vertical acceleration + gravityEstimator.update(t, awz); + } + } + + if (myICM.status != ICM_20948_Stat_FIFOMoreDataAvail) // If more data is available then we should read it right away - and not delay + { + delay(10); + } +}