#!/usr/bin/env python3
# -*- coding: UTF-8 -*-

# Copyright: 2016-2022 https://www.corvin.cn ROS小课堂
# Description: 从IIC接口中读取6DOF IMU模块的三轴加速度、角度、四元数,
#   然后将其打印输出.
# Author: corvin
# History:
#    20220523:Initial this file.

import math
import time

from imu_data_proc import MyIMUClass

imu_iic_addr = 0x50

degrees2rad = math.pi/180.0
yaw = 0.0

accel_factor = 9.806  #sensor accel g convert to m/s^2.

myIMU = MyIMUClass(imu_iic_addr)

print("Now 6DOF IMU Module is working ...")
while True:
    myIMU.get_YPRAG()

    yaw_deg = float(myIMU.raw_yaw)
    if yaw_deg >= 180.0:
        yaw_deg -= 360.0

    yaw   = yaw_deg*degrees2rad
    pitch = float(myIMU.raw_pitch)*degrees2rad
    roll  = float(myIMU.raw_roll)*degrees2rad
    print("roll: ", roll, "pitch: ", pitch, "yaw: ", yaw)

    myIMU.get_quatern()
    print("q_x: ", myIMU.raw_q1, "q_y: ", myIMU.raw_q2, "q_z: ", myIMU.raw_q3, "q_w: ", myIMU.raw_q0)
    print("a_v_x: ", float(myIMU.raw_gx)*degrees2rad, "a_v_y: ", float(myIMU.raw_gy)*degrees2rad, "a_v_z: ", float(myIMU.raw_gz)*degrees2rad)
    print("l_acc_x: ", -float(myIMU.raw_ax)*accel_factor ,"l_acc_y: ", -float(myIMU.raw_ay)*accel_factor ,"l_acc_z: ", -float(myIMU.raw_az)*accel_factor)

    time.sleep(0.1)