###### tags: `控制組`
# IMU on rpi

參考 https://medium.com/@niru5/hands-on-with-rpi-and-mpu9250-part-3-232378fa6dbc

import os
import sys
import time
import smbus

from imusensor.MPU9250 import MPU9250

address = 0x68
bus = smbus.SMBus(1)
imu = MPU9250.MPU9250(bus, address)
imu.begin()

while True:
	imu.readSensor()
	imu.computeOrientation()

	print ("Accel x: {0} ; Accel y : {1} ; Accel z : {2}".format(imu.AccelVals[0], imu.AccelVals[1], imu.AccelVals[2]))
	print ("Gyro x: {0} ; Gyro y : {1} ; Gyro z : {2}".format(imu.GyroVals[0], imu.GyroVals[1], imu.GyroVals[2]))
	print ("Mag x: {0} ; Mag y : {1} ; Mag z : {2}".format(imu.MagVals[0], imu.MagVals[1], imu.MagVals[2]))
	print ("roll: {0} ; pitch : {1} ; yaw : {2}".format(imu.roll, imu.pitch, imu.yaw))
	time.sleep(0.1)

加上濾波器

import os
import sys
import time
import smbus


from imusensor.MPU9250 import MPU9250
from imusensor.filters import madgwick

sensorfusion = madgwick.Madgwick(0.5)

address = 0x68
bus = smbus.SMBus(1)
imu = MPU9250.MPU9250(bus, address)
imu.begin()

imu.loadCalibDataFromFile("place_your_code_here.json")

currTime = time.time()
print_count = 0
while True:
	imu.readSensor()
	for i in range(10):
		newTime = time.time()
		dt = newTime - currTime
		currTime = newTime

		sensorfusion.updateRollPitchYaw(imu.AccelVals[0], imu.AccelVals[1], imu.AccelVals[2], imu.GyroVals[0], \
									imu.GyroVals[1], imu.GyroVals[2], imu.MagVals[0], imu.MagVals[1], imu.MagVals[2], dt)

	if print_count == 2:
		print ("mad roll: {0} ; mad pitch : {1} ; mad yaw : {2}".format(sensorfusion.roll, sensorfusion.pitch, sensorfusion.yaw))
		print_count = 0

	print_count = print_count + 1
	time.sleep(0.01)

包成ros

#!/usr/bin/env python3
import os
import sys
import time
import smbus


from imusensor.MPU9250 import MPU9250
from imusensor.filters import madgwick
import rospy
from std_msgs.msg import Float64MultiArray

def talker():
    #initialization
    sensorfusion = madgwick.Madgwick(0.5)

    address = 0x68
    bus = smbus.SMBus(1)
    imu = MPU9250.MPU9250(bus, address)
    imu.begin()

    imu.caliberateGyro()
    imu.caliberateAccelerometer()

    currTime = time.time()
    print_count = 0
    #ros
    pub = rospy.Publisher('imu_angle', Float64MultiArray, queue_size = 10)
    rospy.init_node('talker', anonymous = True)
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        imu.readSensor()
        for i in range(10):
            newTime = time.time()
            dt = newTime - currTime
            currTime = newTime
            sensorfusion.updateRollPitchYaw(imu.AccelVals[0], imu.AccelVals[1], imu.AccelVals[2], imu.GyroVals[0], \
                                                                        imu.GyroVals[1], imu.GyroVals[2], imu.MagVals[0], imu.MagVals[1], imu.MagVals[2], dt)
        if print_count == 2:
            arr = [sensorfusion.roll, sensorfusion.pitch, sensorfusion.yaw, imu.AccelVals[0], imu.AccelVals[1], imu.AccelVals[2]]
            pub_imu = Float64MultiArray(data = arr)
            pub.publish(pub_imu)
            print_count = 0
        print_count = print_count + 1
        time.sleep(0.01)
        rate.sleep()
if __name__ == '__main__':
    try: 
        talker()
    except rospy.ROSInterruptException:
        pass