diff --git a/Workspace/Application/IMUDriver/CMakeLists.txt b/Workspace/Application/IMUDriver/CMakeLists.txt new file mode 100644 index 0000000..1b09c21 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/Workspace/Application/IMUDriver/IMUReader.cpp b/Workspace/Application/IMUDriver/IMUReader.cpp new file mode 100644 index 0000000..5888b68 --- /dev/null +++ b/Workspace/Application/IMUDriver/IMUReader.cpp @@ -0,0 +1,119 @@ +// IMUReader.cpp +#include +#include +#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; +} \ No newline at end of file diff --git a/Workspace/Application/IMUDriver/IMUReader.h b/Workspace/Application/IMUDriver/IMUReader.h new file mode 100644 index 0000000..2a48db1 --- /dev/null +++ b/Workspace/Application/IMUDriver/IMUReader.h @@ -0,0 +1,53 @@ +// IMUReader.h +#pragma once + +#include +#include +#include +#include +#include +#include + +// 结构体定义 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(); +}; diff --git a/Workspace/Application/IMUDriver/main.cpp b/Workspace/Application/IMUDriver/main.cpp new file mode 100644 index 0000000..72ebaca --- /dev/null +++ b/Workspace/Application/IMUDriver/main.cpp @@ -0,0 +1,18 @@ +#include +#include +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; +} \ No newline at end of file diff --git a/Workspace/Examples/Monocular/mono_euroc_cap b/Workspace/Examples/Monocular/mono_euroc_cap deleted file mode 100755 index 91e5d11..0000000 Binary files a/Workspace/Examples/Monocular/mono_euroc_cap and /dev/null differ