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

# Copyright: 2016-2022 https://www.corvin.cn ROS小课堂
# Description: 从IIC接口中读取6DOF IMU模块的三轴加速度、角度、四元数,
#   然后将其打印输出.这里需要注意IMU模块使用的坐标系为ENU东北天坐标系.
# Author: corvin
# History:
#    20220523:Initial this file.

import math,time
from imu_data_proc import MyIMUClass


# 默认IIC模块的iic地址为0x50
imu_iic_addr = 0x50

# 将度数转换为弧度
deg2rad = math.pi/180.0
accel_factor = 9.806  #sensor accel g convert to m/s^2.


if __name__=='__main__':
    myIMU = MyIMUClass(imu_iic_addr)
    print("Now 6DOF IMU Module is working ...\n")

    while True:
        myIMU.get_YPRAG()

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

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

        # 读取四元数,将其打印输出
        myIMU.get_quatern()
        print("quat_x: ",myIMU.raw_q1, "quat_y: ",myIMU.raw_q2, "quat_z: ",myIMU.raw_q3, "quat_w: ",myIMU.raw_q0)
        
        # 打印输出三轴角速度,弧度值
        print("angular_x: ",float(myIMU.raw_gx)*deg2rad, "angular_y: ",float(myIMU.raw_gy)*deg2rad, "angular_z: ",float(myIMU.raw_gz)*deg2rad)

        # 打印输出三轴线加速度
        print("line_acc_x: ",float(myIMU.raw_ax)*accel_factor ,"line_acc_y: ",float(myIMU.raw_ay)*accel_factor ,"line_acc_z: ",float(myIMU.raw_az)*accel_factor)

        print("------------------------------------------------------------------------------------\n")
        time.sleep(0.1)