mirror of
https://github.com/deadcxap/balancing-cube.git
synced 2026-01-09 12:01:45 +03:00
119 lines
3.2 KiB
C++
119 lines
3.2 KiB
C++
// This program can be used to get the offsets and scaling factors for the gyro and accelerometer. This cannot be done
|
|
// automatically. The cube must be placed in six different orientations, corresponding to each axis facing up and down.
|
|
// Calibration offsets should initially be set to zero. Use the methodology as described in Analog Devices AN-1057,
|
|
// which may be found here: https://www.analog.com/en/app-notes/an-1057.html. In particular, equations 17 and 18.
|
|
|
|
#include <Wire.h>
|
|
#include <ICM20948_WE.h>
|
|
#define ICM20948_ADDR 0x68
|
|
|
|
TwoWire wirePort = TwoWire(0);
|
|
ICM20948_WE myIMU = ICM20948_WE(&wirePort);
|
|
|
|
void setup() {
|
|
// Set up serial
|
|
Serial.begin(921600);
|
|
|
|
// Set up I2C on pins 47 (SDA) and 21 (SCL) at 400 kHz
|
|
wirePort.begin(47, 21, 400000);
|
|
|
|
// Initialize IMU
|
|
if(!myIMU.init()){
|
|
Serial.println("ICM20948 does not respond");
|
|
}
|
|
else{
|
|
Serial.println("ICM20948 is connected");
|
|
}
|
|
|
|
// Set accelerometer and gyroscope ranges
|
|
myIMU.setAccRange(ICM20948_ACC_RANGE_2G);
|
|
myIMU.setGyrRange(ICM20948_GYRO_RANGE_2000);
|
|
|
|
// Set digital low pass filter setting such that noise is minimized (lowest cut-off frequency)
|
|
myIMU.setAccDLPF(ICM20948_DLPF_6);
|
|
myIMU.setGyrDLPF(ICM20948_DLPF_6);
|
|
|
|
// Disable sample rate divider
|
|
myIMU.setAccSampleRateDivider(0);
|
|
myIMU.setGyrSampleRateDivider(0);
|
|
}
|
|
|
|
unsigned long timer;
|
|
xyzFloat acc, gyr;
|
|
float ax, ay, az, gx, gy, gz;
|
|
|
|
// Calibration offsets
|
|
const float b_gyr_x = 0.74;
|
|
const float b_gyr_y = 0.66;
|
|
const float b_gyr_z = -0.45;
|
|
|
|
// IMU accelerator offsets and gain
|
|
const float b_acc_x = -6;
|
|
const float b_acc_y = -215;
|
|
const float b_acc_z = 161;
|
|
const float f_acc_x = 1672.7;
|
|
const float f_acc_y = 1673.3;
|
|
const float f_acc_z = 1691.5;
|
|
|
|
void loop() {
|
|
// Start a timer
|
|
timer = millis();
|
|
|
|
// Measure 1000 samples and average them
|
|
for(int i = 0; i < 1000; i++) {
|
|
myIMU.readSensor();
|
|
acc = myIMU.getAccRawValues();
|
|
gyr = myIMU.getGyrValues();
|
|
|
|
ax += acc.x / 1000;
|
|
ay += acc.y / 1000;
|
|
az += acc.z / 1000;
|
|
gx += gyr.x / 1000;
|
|
gy += gyr.y / 1000;
|
|
gz += gyr.z / 1000;
|
|
|
|
// Print raw and calibrated values in last cycle
|
|
if(i == 999) {
|
|
Serial.print("Raw: Acc: ");
|
|
Serial.print(ax);
|
|
Serial.print(", ");
|
|
Serial.print(ay);
|
|
Serial.print(", ");
|
|
Serial.print(az);
|
|
Serial.print(", ");
|
|
Serial.print("gyr: ");
|
|
Serial.print(gx);
|
|
Serial.print(", ");
|
|
Serial.print(gy);
|
|
Serial.print(", ");
|
|
Serial.println(gz);
|
|
|
|
// Equation 19 in AD AN-1057 is used here
|
|
Serial.print("Calibrated: Acc: ");
|
|
Serial.print((ax - b_acc_x) / f_acc_x);
|
|
Serial.print(", ");
|
|
Serial.print((ay - b_acc_y) / f_acc_y);
|
|
Serial.print(", ");
|
|
Serial.print((az - b_acc_z) / f_acc_z);
|
|
Serial.print(", ");
|
|
Serial.print("gyr: ");
|
|
Serial.print(gx - b_gyr_x);
|
|
Serial.print(", ");
|
|
Serial.print(gy - b_gyr_y);
|
|
Serial.print(", ");
|
|
Serial.println(gz - b_gyr_z);
|
|
}
|
|
}
|
|
|
|
// Print how long it took to get 1000 measurements
|
|
Serial.print("Took "); Serial.print(millis() - timer); Serial.println(" ms.");
|
|
|
|
// Reset averages
|
|
ax = 0;
|
|
ay = 0;
|
|
az = 0;
|
|
gx = 0;
|
|
gy = 0;
|
|
gz = 0;
|
|
}
|