#!/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


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

# 将度数转换为弧度
deg2rad = math.pi/180.0
acc_factor = 9.806  #sensor acc 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 = myIMU.raw_yaw
        if yaw_deg >= 180.0:
            yaw_deg -= 360.0

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

        # 读取四元数,将其打印输出
        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:",myIMU.raw_gx*deg2rad, " angular_y:",myIMU.raw_gy*deg2rad, " angular_z:",myIMU.raw_gz*deg2rad)

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

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