forked from logzhan/ORB-SLAM3-UESTC
138 lines
4.1 KiB
C++
138 lines
4.1 KiB
C++
// 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::IMUReadData()
|
|
{
|
|
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));
|
|
|
|
|
|
// data.vGyro.x = *(float *)(tmpdata+0);
|
|
// data.vGyro.y = *(float *)(tmpdata+4);
|
|
// data.vGyro.z = *(float *)(tmpdata+8);
|
|
// data.vAcc.x = *(float *)(tmpdata+12);
|
|
// data.vAcc.y = *(float *)(tmpdata+16);
|
|
// data.vAcc.z = *(float *)(tmpdata+20);
|
|
// data.timestamp = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch() ).count();
|
|
|
|
IMUData newData;
|
|
newData.vGyro.x = *(float *)(tmpdata+0);
|
|
newData.vGyro.y = *(float *)(tmpdata+4);
|
|
newData.vGyro.z = *(float *)(tmpdata+8);
|
|
newData.vAcc.x = *(float *)(tmpdata+12);
|
|
newData.vAcc.y = *(float *)(tmpdata+16);
|
|
newData.vAcc.z = *(float *)(tmpdata+20);
|
|
newData.timestamp = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
|
std::chrono::system_clock::now().time_since_epoch()).count();
|
|
|
|
std::lock_guard<std::mutex> lock(dataMutex);
|
|
imubuffer.push_back(newData);
|
|
|
|
|
|
|
|
|
|
// 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.vAcc.x= this->data.vAcc.x;
|
|
data.vAcc.y = this->data.vAcc.y;
|
|
data.vAcc.z = this->data.vAcc.z;
|
|
data.vGyro.x= this->data.vGyro.x;
|
|
data.vGyro.y = this->data.vGyro.y;
|
|
data.vGyro.z = this->data.vGyro.z;
|
|
data.timestamp = this->data.timestamp;
|
|
|
|
|
|
|
|
|
|
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 = IMUReadData();
|
|
// 处理获取的数据，这里简单地
|
|
// Add delay fun.
|
|
}
|
|
} |