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

# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
# Author: corvin
# Description: 从IIC接口中读取IMU模块的三轴加速度、三轴角速度、四元数。
#    这里注意self.RPY_CONST表示的是180.0/32768.0,self.ACC_CONST
#    表示16.0/32768.0，self.ANG_CONST表示2000.0/32768.0.
# History:
#    20211122:Initial this file.
#    20211202:init函数增加iic_bus,iic_addr参数，同时优化get_YPRAG函数.
import smbus,rospy
import numpy as np

class MyIMU(object):
    def __init__(self, iic_bus, iic_addr):
        self.i2c  = smbus.SMBus(iic_bus)
        self.addr = iic_addr
        self.RPY_CONST = 0.00549316
        self.ACC_CONST = 0.00048828 
        self.ANG_CONST = 0.06103516

    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:
            rospy.logerr("Read IMU RPYAG date error !")
        else:
            self.raw_roll  = float(np.short(np.short(rpy_data[1]<<8)|rpy_data[0])*self.RPY_CONST)
            self.raw_pitch = float(np.short(np.short(rpy_data[3]<<8)|rpy_data[2])*self.RPY_CONST)
            self.raw_yaw   = float(np.short(np.short(rpy_data[5]<<8)|rpy_data[4])*self.RPY_CONST)

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

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

    def get_quatern(self):
        try:
            quat_data = self.i2c.read_i2c_block_data(self.addr, 0x51, 8)
        except IOError:
            rospy.logerr("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)
