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

        # 三个数据常量,依次分别代表180.0/32768, 16.0/32768.0, 2000.0/32768.0
        self.RPY_CONST = 0.005493164
        self.ACC_CONST = 0.000488281
        self.ANG_CONST = 0.061035156


    # 读取roll,pitch,yaw数据,三轴角速度,三轴线加速度数据
    def get_YPRAG(self):
        try:
            # 读取三轴角度,共计6个字节
            rpy_data = self.i2c.read_i2c_block_data(self.addr, 0x3d, 6)
            # 读取三轴加速度,共计6个字节
            acc_data = self.i2c.read_i2c_block_data(self.addr, 0x34, 6)
            # 读取三轴角速度,共计6个字节
            ang_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])*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(acc_data[1]<<8)|acc_data[0])*self.ACC_CONST)
            self.raw_ay = float(np.short(np.short(acc_data[3]<<8)|acc_data[2])*self.ACC_CONST)
            self.raw_az = float(np.short(np.short(acc_data[5]<<8)|acc_data[4])*self.ACC_CONST)

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


    # 读取四元数,从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)