更新IMU驱动

Workspace/Application/IMUDriver/CMakeLists.txt

@@ -0,0 +1,22 @@
+# 声明要求的 cmake 最低版本
+cmake_minimum_required(VERSION 2.8)
+
+# 声明一个 cmake 工程
+project(imu)
+
+# 设置编译模式
+set(CMAKE_BUILD_TYPE "Debug")
+
+# 查找 Boost 库
+find_package(Boost REQUIRED)
+
+# 语法：add_executable( 程序名 源代码文件 ）
+
+add_executable(imu main.cpp IMUReader.cpp)
+
+target_include_directories(imu PRIVATE ${CMAKE_SOURCE_DIR})
+
+# 添加线程库
+find_package(Threads REQUIRED)
+target_link_libraries(imu PRIVATE Threads::Threads)
+target_link_libraries(imu ${Boost_LIBRARIES} pthread)

Workspace/Application/IMUDriver/IMUReader.cpp

@@ -0,0 +1,119 @@
+// IMUReader.cpp
+#include <iostream>
+#include <string>
+#include "IMUReader.h"
+
+// 构造函数
+IMUReader::IMUReader() {
+    isRunning = true;
+    // 在构造函数中进行初始化工作
+    IMUInit("COM0");
+    // 创建一个新线程运行数据读取循环
+    dataThread = std::thread(&IMUReader::IMUReadDataThread, this);
+}
+
+// 构造函数
+IMUReader::IMUReader(std::string ComNumber) {
+    isRunning = true;
+    // 在构造函数中进行初始化工作
+    IMUInit(ComNumber);
+    // 创建一个新线程运行数据读取循环
+    dataThread = std::thread(&IMUReader::IMUReadDataThread, this);
+}
+
+// 析构函数
+IMUReader::~IMUReader() {
+    // 在析构函数中通过改变 isExist 变量为 false 退出循环
+    isRunning = false;
+    // 等待数据读取线程结束
+    if (dataThread.joinable()) {
+        dataThread.join();
+    }
+    // realease serial object.
+    if(serial != NULL){
+        delete serial;
+    }
+}
+
+// 获取 IMU 数据状态
+int IMUReader::IMUReadUartData() 
+{
+    uint8_t tmpdata[24];
+
+    // 读取串口数据
+    size_t bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); 
+
+    // check frame head
+    if(tmpdata[0] != 0xfc){
+        return 0;
+    }
+    // check frame type
+    bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); 
+    if(tmpdata[0] != 0x40){
+        return 0;
+    }
+
+    bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); 
+    // check imu data len
+    if(tmpdata[0] != 0x38){
+        return 0;
+    }
+
+    bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); 
+    bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); 
+    bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); 
+    bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); 
+    bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 24)); 
+
+    // 从 tmpdata 中提取数据
+    data.gyroX   = *(float *)(tmpdata+0);
+    data.gyroY   = *(float *)(tmpdata+4);
+    data.gyroZ   = *(float *)(tmpdata+8);
+    data.accelX  = *(float *)(tmpdata+12);
+    data.accelY  = *(float *)(tmpdata+16);
+    data.accelZ  = *(float *)(tmpdata+20);
+
+    // printf("gyro = %f,%f,%f, Acc = %f,%f,%f\n", data.gyroX,data.gyroY,data.gyroZ, 
+    //                                             data.accelX, data.accelY, data.accelZ);
+    // 返回状态，这里简单地返回 0 表示成功
+    return 1;
+}
+
+// 获取 IMU 数据状态
+int IMUReader::ReadData(IMUData& data) {
+    // 模拟获取实际数据的过程，这里使用随机数
+    data.accelX = this->data.accelX;
+    data.accelY = this->data.accelY;
+    data.accelZ = this->data.accelZ;
+    data.gyroX = this->data.gyroX;
+    data.gyroY = this->data.gyroY;
+    data.gyroZ = this->data.gyroZ;
+    // 返回状态，这里简单地返回 0 表示成功
+    return 0;
+}
+
+// 初始化 IMU
+void IMUReader::IMUInit(std::string ComNumber) {
+    // 这里可以进行具体的初始化工作
+    std::cout << "IMU Initialization..." << ComNumber << std::endl;
+    serial = new boost::asio::serial_port(io, ComNumber);
+
+    if(serial == NULL){
+        return;
+    }
+    serial->set_option(boost::asio::serial_port_base::baud_rate(921600)); // 设置波特率
+    serial->set_option(boost::asio::serial_port_base::character_size(8)); // 设置数据位
+}
+
+// 读取 IMU 数据的循环
+void IMUReader::IMUReadDataThread() {
+    while (isRunning) {
+        int status = IMUReadUartData();
+        // 处理获取的数据，这里简单地
+        // Add delay fun.
+    }
+}
+
+float IMUReader::getRandomFloat(){
+    return 1.22f;
+}

Workspace/Application/IMUDriver/IMUReader.h

@@ -0,0 +1,53 @@
+// IMUReader.h
+#pragma once
+
+#include <iostream>
+#include <thread>
+#include <chrono>
+#include <string>
+#include <boost/asio.hpp>
+#include <boost/array.hpp>
+
+// 结构体定义 IMU 数据
+struct IMUData {
+    float accelX;
+    float accelY;
+    float accelZ;
+    float gyroX;
+    float gyroY;
+    float gyroZ;
+};
+
+class IMUReader {
+public:
+    // 构造函数
+    IMUReader(std::string ComNumber);
+
+    // 构造函数
+    IMUReader();
+
+    // 析构函数
+    ~IMUReader();
+
+    // 初始化 IMU
+    void IMUInit(std::string ComNumber);
+
+    // 获取 IMU 数据状态
+    int ReadData(IMUData& data);
+
+private:
+    boost::asio::io_service io;
+    boost::asio::serial_port* serial;
+    // boost::asio::serial_port serial(io, "/dev/ttyUSB0"); // 替换成你的串口设备路径
+
+    bool        isRunning; // 控制是否退出循环的标志变量
+    std::thread dataThread; // 数据读取线程
+    IMUData     data;
+    // 读取 IMU 数据的循环
+    int IMUReadUartData();
+
+    void IMUReadDataThread(void);
+
+    // 辅助函数，生成随机浮点数（模拟实际获取的传感器数据）
+    float getRandomFloat();
+};

Workspace/Application/IMUDriver/main.cpp

@@ -0,0 +1,18 @@
+#include <IMUReader.h>
+#include <stdio.h>
+int main() {
+    // 创建 IMUReader 实例
+    IMUReader imuReader("/dev/ttyUSB0");
+
+    // orb-slam read cam
+    while(1){
+        IMUData data;
+        int status = imuReader.ReadData(data);
+
+        printf("Acc=%f,%f,%f, Gyro=%f,%f,%f\n",data.accelX,data.accelY,data.accelZ,
+                                              data.gyroX,data.gyroY,data.gyroZ);
+        // 30FPS
+        std::this_thread::sleep_for(std::chrono::milliseconds(33));
+    }
+    return 0;
+}