Jetson_MPU6050_DMP_Python讀取

zeroSignal發表於2024-10-26
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()

相關文章