树莓派: OLED显示MPU6050姿态角

from PIL import ImageDraw
from PIL import Image
from PIL import ImageFont
import Adafruit_SSD1306

from mpu6050 import mpu6050
import threading
import time
import queue
import  MPU6050filter

myQueue = queue.Queue() # 创建一个线程队列

def t_mpu6050():
    sensor = mpu6050(address=0x68)
    sensor.set_gyro_range(mpu6050.ACCEL_RANGE_16G)  # 选择量程
    sensor.set_accel_range(mpu6050.GYRO_RANGE_2000DEG)
    time.sleep(1)
    while 1:
        accel_data = sensor.get_accel_data() # 读取加速度的值
        gyro_data = sensor.get_gyro_data()   # 读取陀螺仪的值

        # 转换成姿态角
        rotation = MPU6050filter.IMUupdate(accel_data['x'],accel_data['y'],accel_data['z'],
                                           gyro_data['x'], gyro_data['y'], gyro_data['z'])

        if myQueue.empty():
            myQueue.put(rotation['Pitch'])
            myQueue.put(rotation['Roll'])
            myQueue.put(rotation['Yaw'])
        time.sleep(0.2)

def t_OLED():
    RST = 24
    disp = Adafruit_SSD1306.SSD1306_128_64(rst=RST)  # 创建OLED
    disp.begin()    # 初始化OLED库
    disp.clear()    # 清屏
    disp.display()  # 显示
    width = disp.width  # 读取宽度
    height = disp.height # 读取高度

    font = ImageFont.truetype('/usr/share/fonts/truetype/wqy/wqy-zenhei.ttc', 15)

    get_rotation={}
    while 1:
        if not myQueue.empty():
            get_rotation['Pitch'] = myQueue.get()
            get_rotation['Roll'] = myQueue.get()
            get_rotation['Yaw'] = myQueue.get()

            print(get_rotation['Pitch'])

            # 新建一个图片，然后绘制字体
            image = Image.new('1', (width, height))
            draw = ImageDraw.Draw(image)
            draw.text((0, 5), "Pitch: ", font=font, fill=255)
            draw.text((0, 25), "Roll: ", font=font, fill=255)
            draw.text((0, 45), "Yaw: ", font=font, fill=255)
            draw.text((39, 5), str(int(get_rotation['Pitch'])), font=font, fill=255)
            draw.text((32, 25), str(int(get_rotation['Roll'])),font=font, fill=255)
            draw.text((32, 45), str(int(get_rotation['Yaw'])), font=font, fill=255)
            # OLED加载图片，然后显示
            disp.image(image)
            disp.display()

def main():
    # 创建OELD和MPU6050线程
    t1 = threading.Thread(target=t_mpu6050)
    t2 = threading.Thread(target=t_OLED)

    t1.start()
    t2.start()

    t1.join()
    t2.join()


if __name__ == '__main__':
    main()

硬件部分：

0.96OLED屏幕和MPU6050 的 I2C接在树莓派的SDA1和SCL1

MPU6050转换成欧拉角，链接如下：https://blog.csdn.net/weixin_38956024/article/details/94757023

 

实测效果：