init linux-python3-serial-devel branch

.gitignore

@@ -0,0 +1,60 @@
+# ---> Python
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+env/
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+*.egg-info/
+.installed.cfg
+*.egg
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*,cover
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+

README.md

@@ -0,0 +1,9 @@
+#### 下载代码
+```
+git clone -b linux-python3-iic-devel https://code.corvin.cn:3000/corvin_zhang/rasp_imu_hat_6dof.git
+```
+
+#### 运行
+```
+python3 imu_6dof_iic.py
+```

imu_6dof_iic.py

@@ -0,0 +1,51 @@
+#!/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)
+

imu_data_proc.py

@@ -0,0 +1,61 @@
+#!/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)
+