97 lines
1.6 KiB
C++
97 lines
1.6 KiB
C++
#ifndef IO__Dm_Imu_HPP
|
|
#define IO__Dm_Imu_HPP
|
|
|
|
#include <math.h>
|
|
#include <serial/serial.h>
|
|
|
|
#include <Eigen/Geometry>
|
|
#include <array>
|
|
#include <fstream>
|
|
#include <initializer_list>
|
|
#include <iostream>
|
|
#include <thread>
|
|
|
|
#include "tools/thread_safe_queue.hpp"
|
|
|
|
namespace io
|
|
{
|
|
|
|
struct __attribute__((packed)) IMU_Receive_Frame
|
|
{
|
|
uint8_t FrameHeader1;
|
|
uint8_t flag1;
|
|
uint8_t slave_id1;
|
|
uint8_t reg_acc;
|
|
uint32_t accx_u32;
|
|
uint32_t accy_u32;
|
|
uint32_t accz_u32;
|
|
uint16_t crc1;
|
|
uint8_t FrameEnd1;
|
|
|
|
uint8_t FrameHeader2;
|
|
uint8_t flag2;
|
|
uint8_t slave_id2;
|
|
uint8_t reg_gyro;
|
|
uint32_t gyrox_u32;
|
|
uint32_t gyroy_u32;
|
|
uint32_t gyroz_u32;
|
|
uint16_t crc2;
|
|
uint8_t FrameEnd2;
|
|
|
|
uint8_t FrameHeader3;
|
|
uint8_t flag3;
|
|
uint8_t slave_id3;
|
|
uint8_t reg_euler; //r-p-y
|
|
uint32_t roll_u32;
|
|
uint32_t pitch_u32;
|
|
uint32_t yaw_u32;
|
|
uint16_t crc3;
|
|
uint8_t FrameEnd3;
|
|
};
|
|
|
|
typedef struct
|
|
{
|
|
float accx;
|
|
float accy;
|
|
float accz;
|
|
float gyrox;
|
|
float gyroy;
|
|
float gyroz;
|
|
float roll;
|
|
float pitch;
|
|
float yaw;
|
|
} IMU_Data;
|
|
|
|
class DM_IMU
|
|
{
|
|
public:
|
|
DM_IMU();
|
|
~DM_IMU();
|
|
|
|
Eigen::Quaterniond imu_at(std::chrono::steady_clock::time_point timestamp);
|
|
|
|
private:
|
|
struct IMUData
|
|
{
|
|
Eigen::Quaterniond q;
|
|
std::chrono::steady_clock::time_point timestamp;
|
|
};
|
|
|
|
void init_serial();
|
|
void get_imu_data_thread();
|
|
|
|
serial::Serial serial_;
|
|
std::thread rec_thread_;
|
|
|
|
tools::ThreadSafeQueue<IMUData> queue_;
|
|
IMUData data_ahead_, data_behind_;
|
|
|
|
std::atomic<bool> stop_thread_{false};
|
|
IMU_Receive_Frame receive_data{}; //receive data frame
|
|
IMU_Data data{};
|
|
};
|
|
|
|
} // namespace io
|
|
|
|
#endif
|