import time
import numpy as np
import icm20948
 
# Set up the ICM20948 IMU
imu = icm20948.ICM20948()
 
# Set up calibration variables
num_samples = 1000  # number of samples per orientation
static_calib_data = np.zeros((3, 3))  # matrix to store calibration data
 
# Perform static calibration in 6 different orientations
for i, (axis, orientation) in enumerate(zip(["X", "Y", "Z"], [(1, 0, 0), (0, 1, 0), (0, 0, 1)])):
    print(f"Performing static calibration for axis {axis}...")
 
    # Orient the IMU
    imu.set_accel_scale(2)  # set full-scale range to +/- 2g
    imu.set_accel_rate(1)  # set sample rate to 1kHz
    imu.set_accel_dlpf_cfg(0)  # set low-pass filter to disabled
    imu.set_accel_user_offset(0, 0, 0)
    imu.set_accel_user_offset(axis, 0, 0)
 
    # Collect calibration data
    calib_data = np.zeros((num_samples, 3))
    for j in range(num_samples):
        x, y, z = imu.read_accel_data()
        calib_data[j, :] = [x, y, z]
        time.sleep(0.01)
 
    # Calculate mean and standard deviation for each axis
    mean = np.mean(calib_data, axis=0)
    std_dev = np.std(calib_data, axis=0)
 
    # Store calibration data in matrix
    static_calib_data[i, :] = mean
 
    # Print calibration data for each axis
    print(f"Mean for axis {axis}: {mean}")
    print(f"Standard deviation for axis {axis}: {std_dev}\n")
 
# Calculate bias and sensitivity errors
bias = np.mean(static_calib_data, axis=0)
sensitivity = np.divide(9.81, static_calib_data - bias)
 
# Print calibration results
print("Static calibration complete.")
print(f"Bias: {bias}")
print(f"Sensitivity: {sensitivity}")
 
 
import icm20948
 
# Set up the ICM20948 IMU
imu = icm20948.ICM20948()
 
# Set the full-scale range and sample rate for the accelerometer
imu.set_accel_scale(2)  # +/- 2g
imu.set_accel_rate(1)  # 1kHz
 
# Set the low-pass filter for the accelerometer
imu.set_accel_dlpf_cfg(0)  # disabled
 
# Set the user offsets for the accelerometer
imu.set_accel_user_offset(0, 0, 0)
 
# Get the bias and sensitivity values from static calibration
bias = [0.012, -0.007, 0.025]  # obtained from static calibration
sensitivity = [1.012, 1.005, 1.015]  # obtained from static calibration
 
# Read and compensate for accelerometer errors in real-time
while True:
    # Read raw accelerometer data
    x_raw, y_raw, z_raw = imu.read_accel_data()
 
    # Compensate for bias and sensitivity errors
    x_comp = (x_raw - bias[0]) * sensitivity[0]
    y_comp = (y_raw - bias[1]) * sensitivity[1]
    z_comp = (z_raw - bias[2]) * sensitivity[2]
 
    # Print compensated data
    print(f"X: {x_comp}, Y: {y_comp}, Z: {z_comp}")