# Импортируем необходимые модули
from amperka_pico_imu_10dof import icm20948
import time
import math

# Печатаем тестовую строку в консоль
print("IMU-sensor Test Program")
# Создаём объект для работы с IMU-сенсором
imu=icm20948.ICM20948()


while True:
    # Считываем данных с акселерометра
    accelerometer = imu.read_accelerometer_g()
    # Считываем данных с гироскопа
    gyroscope = imu.read_gyroscope_rad()
    # Считываем данных с магнитометра
    magnetometer = imu.read_magnetometer_uT()
    ax = accelerometer[0]
    ay = accelerometer[1]
    az = accelerometer[2]
    gx = gyroscope[0]
    gy = gyroscope[1]
    gz = gyroscope[2]
    mx = magnetometer[0]
    my = magnetometer[1]
    mz = magnetometer[2]
    # Обновляем входные данные в фильтр Маджвика
    imu.AHRS_update(gx, gy, gz, ax, ay, az, mx, my, mz)
    # Получаем из фильтра углы Эйлера: yaw, pitch и roll
    yaw = imu.get_yaw();
    pitch = imu.get_pitch();
    roll = imu.get_roll();
    print(f"Yaw:\t{yaw}")
    print(f"Pitch:\t{pitch }")
    print(f"Roll:\t{roll}")
    print()
    time.sleep(0.1)