forked from logzhan/ORB-SLAM3-UESTC
119 lines
3.6 KiB
C++
119 lines
3.6 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::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;
|
|
} |