Jetson_MPU6050_DMP_Python读取

编译动态链接库

I2CDevLib仓库
选用Linux上驱动I2C和MPU6050的代码，克隆LinuxI2CDev文件夹到本地，然后进入到文件夹中，创建一个main.cpp用来创建与Python的函数接口，可以自定义。这里的代码没有考虑零偏，只是从DMP取出四元数换算得到结果的，实际用的时候有不小的零偏，可以添加上初始化时的零偏纠正过程。

#include <stdio.h>  
#include "MPU6050/MPU6050_6Axis_MotionApps20.h"  
  
extern "C" {  
    MPU6050 mpu;  
    bool dmp_initialized = false;  
  
    void initialize_dmp() {  
        // 初始化 MPU6050  
        mpu.initialize();  
  
        // 检查设备连接  
        if (!mpu.testConnection()) {  
            printf("MPU6050 connection failed\n");  
            return;  
        }  
  
        // 初始化 DMP  
        if (mpu.dmpInitialize() != 0) {  
            printf("DMP initialization failed\n");  
            return;  
        }  
  
        // 启用 DMP  
        mpu.setDMPEnabled(true);  
  
        dmp_initialized = true;  
    }  
  
    void get_yaw_pitch_roll(float *yaw, float *pitch, float *roll) {  
        if (!dmp_initialized) {  
            printf("DMP not initialized\n");  
            return;  
        }  
  
        // 获取 DMP 数据包大小  
        uint16_t packetSize = mpu.dmpGetFIFOPacketSize();  
        uint8_t fifoBuffer[64];  
  
        // 检查是否有可用的 DMP 数据包  
        if (mpu.dmpPacketAvailable()) {  
            // 读取 DMP 数据包  
            mpu.dmpGetCurrentFIFOPacket(fifoBuffer);  
  
            // 获取四元数  
            Quaternion q;  
            mpu.dmpGetQuaternion(&q, fifoBuffer);  
  
            // 获取重力向量  
            VectorFloat gravity;  
            mpu.dmpGetGravity(&gravity, &q);  
  
            // 计算 yaw, pitch, roll  
            float ypr[3];  
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);  
  
            // 将结果赋值给输出参数  
            *yaw = ypr[0];  
            *pitch = ypr[1];  
            *roll = ypr[2];  
        }  
    }  
}

配置CmakeLists.txt，当然，在配置成动态链接库之前，可以编译成可执行程序验证正确性。

cmake_minimum_required(VERSION 3.10)  
  
# 项目名称  
project(MPU6050_Project)  
  
# 设置 C++ 标准  
set(CMAKE_CXX_STANDARD 11)  
set(CMAKE_CXX_STANDARD_REQUIRED True)  
  
# 设置 -fPIC 选项  
set(CMAKE_POSITION_INDEPENDENT_CODE ON)  
  
# 添加 I2Cdev 库  
add_library(I2Cdev I2Cdev/I2Cdev.cpp)  
target_include_directories(I2Cdev PUBLIC I2Cdev)  
  
# 添加 MPU6050 库  
add_library(MPU6050 MPU6050/MPU6050.cpp MPU6050/MPU6050_6Axis_MotionApps20.cpp)  
target_include_directories(MPU6050 PUBLIC MPU6050)  
  
# 链接 I2Cdev 库到 MPU6050 库  
target_link_libraries(MPU6050 I2Cdev)  
  
# 添加 main 可执行文件  
add_library(mpu6050_lib SHARED main.cpp)  
target_include_directories(mpu6050_lib PUBLIC .)  
  
# 链接 MPU6050 库到 main 可执行文件  
target_link_libraries(mpu6050_lib MPU6050)

创建build文件夹，进入build文件夹，执行cmake ..，然后make，即可看到libmpu6050_lib.so，记住路径，或者移动到python代码同目录下

调用和获取数据

使用如下代码，可以加载并自动获取yaw, pitch, roll角度。这里使用单独的线程，因为不连续读取时会出现错误数据，原因未知。

# 加载动态链接库
lib = ctypes.CDLL('./cpp/libmpu6050_lib.so')

# 定义初始化函数原型
lib.initialize_dmp.argtypes = []
lib.initialize_dmp.restype = None

# 定义获取角度函数原型
lib.get_yaw_pitch_roll.argtypes = [ctypes.POINTER(ctypes.c_float), ctypes.POINTER(ctypes.c_float), ctypes.POINTER(ctypes.c_float)]
lib.get_yaw_pitch_roll.restype = None

# 初始化 DMP
lib.initialize_dmp()

# 调用获取角度函数
yaw = ctypes.c_float()
pitch = ctypes.c_float()
roll = ctypes.c_float()


lock = threading.Lock()  # 线程锁，用于并发安全
stop_event = threading.Event()

def yaw_read_thread():
    global stop_event
    # 读取角度
    try:
        while True:
            with lock:
                lib.get_yaw_pitch_roll(ctypes.byref(yaw), ctypes.byref(pitch), ctypes.byref(roll))
    finally:
        stop_event.set()
    # print(f"Yaw: {yaw.value}, Pitch: {pitch.value}, Roll: {roll.value}")

# 启动yaw read
threading.Thread(target=yaw_read_thread, daemon=True).start()