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

# Copyright: 2016-2022 https://www.corvin.cn ROS小课堂
# Description: 使用python3从IIC接口中读取IMU模块的三轴线加速度,三轴角速度,四元数原始数据。
# Author: corvin
# History:
#    20220523: Initial this file.

import smbus
import numpy as np


class MyIMUClass(object):
    def __init__(self, dev_iic_addr):
        self.i2c  = smbus.SMBus(1)
        self.addr = dev_iic_addr


    # 读取roll,pitch,yaw数据,三轴角速度,三轴线加速度数据
    def get_YPRAG(self):
        try:
            rpy_data  = self.i2c.read_i2c_block_data(self.addr, 0x3d, 6)
            axyz_data = self.i2c.read_i2c_block_data(self.addr, 0x34, 6)
            gxyz_data = self.i2c.read_i2c_block_data(self.addr, 0x37, 6)
        except IOError:
            print("Read IMU RPYAG date error !")
        else:
            self.raw_roll  = float(np.short(np.short(rpy_data[1]<<8)|rpy_data[0])/32768.0*180.0)
            self.raw_pitch = float(np.short(np.short(rpy_data[3]<<8)|rpy_data[2])/32768.0*180.0)
            self.raw_yaw   = float(np.short(np.short(rpy_data[5]<<8)|rpy_data[4])/32768.0*180.0)

            self.raw_ax = float(np.short(np.short(axyz_data[1]<<8)|axyz_data[0])/32768.0*16.0)
            self.raw_ay = float(np.short(np.short(axyz_data[3]<<8)|axyz_data[2])/32768.0*16.0)
            self.raw_az = float(np.short(np.short(axyz_data[5]<<8)|axyz_data[4])/32768.0*16.0)

            self.raw_gx = float(np.short(np.short(gxyz_data[1]<<8)|gxyz_data[0])/32768.0*2000.0)
            self.raw_gy = float(np.short(np.short(gxyz_data[3]<<8)|gxyz_data[2])/32768.0*2000.0)
            self.raw_gz = float(np.short(np.short(gxyz_data[5]<<8)|gxyz_data[4])/32768.0*2000.0)


    # 读取四元数,从0x51寄存器连续读取8个字节数据
    def get_quatern(self):
        try:
            quat_data = self.i2c.read_i2c_block_data(self.addr, 0x51, 8)
        except IOError:
            print("Read IMU quaternion date error !")
        else:
            self.raw_q0 = float((np.short(np.short(quat_data[1]<<8)|quat_data[0]))/32768.0)
            self.raw_q1 = float((np.short(np.short(quat_data[3]<<8)|quat_data[2]))/32768.0)
            self.raw_q2 = float((np.short(np.short(quat_data[5]<<8)|quat_data[4]))/32768.0)
            self.raw_q3 = float((np.short(np.short(quat_data[7]<<8)|quat_data[6]))/32768.0)