import sys
sys.path.append('.')
import RTIMU
import time

SETTINGS_FILE = "RTIMULib"
s = RTIMU.Settings(SETTINGS_FILE)
imu = RTIMU.RTIMU(s)

if (not imu.IMUInit()):
    print('IMU init failed.')
    sys.exit(1)

imu.setSlerpPower(0.02)
imu.setGyroEnable(True)
imu.setAccelEnable(True)
imu.setCompassEnable(True)

poll_interval = imu.IMUGetPollInterval()
print('poll interval: ', poll_interval)

gyro_meas = []
fusion_meas = []

try:
    while True:

        if imu.IMURead():
            data = imu.getIMUData()
            fusion_pose = data["fusionPose"]
            Gyro = data["gyro"]
            print('\ngyro: {:.4f}, {:.4f}, {:.4f}'.format(Gyro[0], Gyro[1], Gyro[2]))
            print('fusion: {:.4f}, {:.4f}, {:.4f}'.format(fusion_pose[0], fusion_pose[1], fusion_pose[2]))

            time.sleep(poll_interval*1.0/1000.0)

except:
    pass