树莓派: OLED显示MPU6050姿态角

拿铁不拿铁

于 2024-08-14 04:02:35 发布

阅读量300

点赞数 8

文章标签： python 机器学习开发语言

本文链接：https://blog.csdn.net/weixin_74923758/article/details/141178799

版权

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转换成欧拉角，链接如下：树莓派：MPU6050 欧拉角算法程序_积分增益控制陀螺仪偏差的收敛速度-CSDN博客

实测效果：