imu_driver.cc
//
// Created by cai on 19-10-8.
//
#include "imu_driver.h"
#include "common.h"
#include <math.h>
#include <iostream>
#include <string>
#include <vector>
using namespace std;
//数据的处理我是放在子线程还是放在主线程呢,雷达人家是放在子线程，所以我也放子线程,主线程就干拷贝的事吧
namespace cartographer {
namespace src {
imuDriver::imuDriver() :
_serial(0) {
//正确数量
head_num = 2;
com_num = 1;
data_num = 14;
IMU_node_buf = new IMU_DATA_PACKAGE();
// vector<IMU_DATA> *testimu = new vector<IMU_DATA>;
vector<IMU_DATA> testimu;
}
imuDriver::~imuDriver() {
if (_serial) {
if (_serial->isOpen()) {
_serial->flush();
_serial->closePort();
}
}
if (_serial) {
delete _serial;
_serial = NULL;
}
if (IMU_node_buf) {
delete IMU_node_buf;
IMU_node_buf = NULL;
}
_thread.join();
}
//处理具体数据
void imuDriver::dealData(IMU_DATA &data, vector<uint8_t> &datapackage) {
//cout << "imuDriver::dealData" << endl;
for (int i = 0; i < data_num / 2; ++i) {
int8_t a = (int8_t) datapackage[(2 * i)];
int8_t b = (int8_t) datapackage[(2 * i) + 1];
int16_t new_ac_int;
//这个强转会出问题
//bc是后面的,所以要加判断
int16_t bc;
if (b < 0) {
uint8_t b8 = (1 << 7) + b;
bc = (int16_t) b8;
bc += (1 << 7);
} else {
bc = (int16_t) b;
}
int16_t ac = (int16_t) a;
new_ac_int = (ac << 8);
new_ac_int += bc;
float new_ac = (float) (new_ac_int);
new_ac /= 100;
// cout << new_ac << "____";
if (i == 0) {
data.XLH = new_ac;
} else if (i == 1) {
data.YLH = new_ac;
} else if (i == 2) {
data.ZLH = new_ac;
} else if (i == 3) {
data.XAH = new_ac;
} else if (i == 4) {
data.YAH = new_ac;
} else if (i == 5) {
data.ZAH = new_ac;
} else if (i == 6) {
data.YawAH = new_ac;
}
}
//cout << "imuDriver::dealData" << endl;
}
void imuDriver::dealAllData(uint8_t *source, size_t t, IMU_DATA_PACKAGE *package) {
//判断是否开始一个帧,数据流失怎么办,只要读到FH就要从头开始,每次读一大段数据,没有校验位
cout << "begin dealAllData" << endl;
int head_isok = is_no;
int com_isok = is_no;
int data_is_right = 0;
int now_head_num = 0;
int now_com_num = 0;
int now_data_num = 0;
//当前包中相应数据的数量
//用长度来校验,数据位是１４个字节,只要命令帧后不是１４个字节,这个数据包就丢掉
vector<uint8_t> datapackage;
for (int i = 0; i < t; ++i) {
if (source[i] == FH && now_data_num == 0) {
datapackage.clear();
if (i >= 1 && source[i - 1] == FH) {
head_isok = is_ok;
} else {
head_isok = is_no;
}
} else if (source[i] == ComNum && now_data_num == 0) {
if (i >= 2 && source[i - 1] == FH) {
com_isok = is_ok;
} else {
com_isok = is_no;
}
} else if (source[i] == Len && now_data_num == 0) {
continue;
} else {
//校验位进这个逻辑
if (now_data_num == 14) {
now_data_num = 0;
continue;
}
if (com_isok == is_ok && head_isok == is_ok) {
now_data_num++;
//装数据的包,默认数据是14个
datapackage.push_back(source[i]);
if (now_data_num == data_num) {
IMU_DATA data;
dealData(data, datapackage);
testimu.push_back(data);
}
}
}
}
};
size_t imuDriver::doProcessSimple(vector<IMU_DATA> *package) {
//清空合宿据
package->clear();
//拷贝数据
for (int j = 0; j < this->testimu.size(); ++j) {
package->push_back(testimu[j]);
}
unsigned long i = package->size();
if (i > 0) {
//拷贝到数据后就赋值
testimu.clear();
return true;
} else {
return false;
}
}
result_t imuDriver::getDatafromSerial(uint8_t *data, size_t size) {
if (!m_isConnected) {
return RESULT_FAIL;
}
size_t r;
//这个循环的逻辑是让_serial把数据读完
cout << " getDatafromSerial size=" << size << endl;
while (size) {
r = _serial->read(data, size);
cout << "r = _serial->read(data, size); r=" << r << endl;
if (r < 1) {
return RESULT_FAIL;
}
size -= r;
data += r;
}
return RESULT_OK;
}
result_t imuDriver::connect(const char *port_path, uint32_t baudrate) {
SerialBaudrate = baudrate;
port = string(port_path);
ScopedLocker lk(_serial_lock);
//看来换了接口，序列没创建成功
cout << "port=" << port << endl;
cout << "SerialBaudrate=" << SerialBaudrate << endl;
if (!_serial) {
_serial = new Serial(port, SerialBaudrate,
Timeout::simpleTimeout(4000));
}
//错误定位在串口没有打开
if (!_serial->open()) {
cout << "serial can not connect" << endl;
return RESULT_FAIL;
} else {
cout << "serial can connect" << endl;
}
trans_delay = _serial->getByteTime();
m_isConnected = true;
// delay(50);
// clearDTR();
return RESULT_OK;
}
bool imuDriver::turnOn() {
cout << "begin turnOn" << endl;
// start scan...
result_t op_result = this->startGet();
if (!IS_OK(op_result)) {
fprintf(stderr, "[imuDriver] Failed to start scan mode: %x\n", op_result);
return false;
}
return true;
}
result_t imuDriver::startGet() {
cout << "begin startGet" << endl;
result_t ans;
ans = this->createThread();
return ans;
}
result_t imuDriver::createThread() {
cout << "begin createThread" << endl;
_thread = CLASS_THREAD(imuDriver, cacheScanData);
if (_thread.getHandle() == 0) {
m_isGetting = false;
return RESULT_FAIL;
}
m_isGetting = true;
return RESULT_OK;
}
int imuDriver::cacheScanData() {
cout << "begin cacheScanData" << endl;
while (true) {
ScopedLocker l(_serial_lock);
flush();
size_t t = 200;
uint8_t recvBuffer[t];
result_t ans = getDatafromSerial(recvBuffer, t);
if (ans < 0) {
cout << "getDatafromSerial Fail" << endl;
return RESULT_FAIL;
}
//cout << "recvBuffer=" << recvBuffer << endl;
dealAllData(recvBuffer, t, IMU_node_buf);
cout << "behind dealAllData then testimu.size()=" << testimu.size() << endl;
}
if (testimu.size() > 0) {
return RESULT_OK;
} else {
return RESULT_FAIL;
}
}
void imuDriver::flush() {
if (!m_isConnected) {
return;
}
if (_serial) {
_serial->flush();
}
delay(30);
}
void imuDriver::setDTR() {
if (!m_isConnected) {
return;
}
if (_serial) {
_serial->flush();
_serial->setDTR(1);
}
}
void imuDriver::clearDTR() {
if (!m_isConnected) {
return;
}
if (_serial) {
_serial->flush();
_serial->setDTR(0);
}
}
}
}
imu_driver.h
//
// Created by cai on 19-10-6.
//
#ifndef SRC_IMU_DRIVER_H
#define SRC_IMU_DRIVER_H
#include <stdlib.h>
#include "serial.h"
#include "locker.h"
#include "thread.h"
#include "imu_protocol.h"
#include <string>
#endif //SRC_IMU_DRIVER_H
using namespace std;
namespace cartographer {
namespace src {
class imuDriver {
public:
/**
* A constructor.
* A more elaborate description of the constructor.
*/
imuDriver();
/**
* A destructor.
* A more elaborate description of the destructor.
*/
virtual ~imuDriver();
/**/
void getRealData(const uint8_t a, const uint8_t b, uint16_t *new_ac);
/**/
void dealData(IMU_DATA &data, vector<uint8_t> &datapackage);
/*
* 将串口读取到的原始数据转换为可以赋给ros节点的数据
* */
void dealAllData(uint8_t *test, size_t t, IMU_DATA_PACKAGE *package);
/*获得ros节点可用的数据包*/
size_t doProcessSimple(vector<IMU_DATA> *ppackage);
/*从串口缓存获取数据*/
result_t getDatafromSerial(uint8_t *data, size_t size);
/*连接串口*/
result_t connect(const char *port_path, uint32_t baudrate);
/*从子线程的串口中获得数据*/
bool turnOn();
/*开始获得接受数据*/
result_t startGet();
/*开启线程获得数据*/
result_t createThread();
/*线程中缓存数据的方法*/
int cacheScanData();
/*清空缓存区的数据*/
void flush();
/*拷贝缓存中的数据*/
result_t copyBuffData(IMU_DATA_PACKAGE *nodebuffer);
/*DTR是data terminal ready*/
void setDTR();
/**/
void clearDTR();
bool m_isGetting; ///< 扫图状态
IMU_DATA_PACKAGE *IMU_node_buf; ///< 激光点信息
vector<IMU_DATA> testimu;
private:
Serial *_serial; ///< 串口
//正确数量
int head_num;
int com_num;
int data_num;
string port;
int SerialBaudrate;
IMU_DATA *data;
Locker _serial_lock; ///< 串口锁
Locker _lock; ///< 线程锁
Thread _thread; ///< 线程id
int trans_delay;
bool m_isConnected = true;
};
}
}
imu_protocol.h
#pragma once
#include "v8stdint.h"
#include <vector>
#define SUNNOISEINTENSITY 0xff
#define GLASSNOISEINTENSITY 0xfe
#if defined(_WIN32)
#pragma pack(1)
#endif
/*
* 指令说明
* */
#define FH 0xAA
#define ComNum 0x02
#define Len 0x0E
#define is_ok 1
#define is_no 0
//imu数据
struct IMU_DATA {
//X轴加速度H
float XLH;
//Y轴加速度H
float YLH;
//Z轴加速度H
float ZLH;
//X轴角速度H
float XAH;
//Y轴角速度H
float YAH;
//Z轴角速度H
float ZAH;
//Yaw偏航角H
float YawAH;
};
struct IMU_DATA_PACKAGE {
//! Array of laser point
std::vector<IMU_DATA> data;
};
struct node_packages {
uint16_t package_Head;
uint16_t package_com;
uint16_t XLH;
uint16_t YLH;
uint16_t ZLH;
uint16_t XAH;
uint16_t YAH;
uint16_t ZAH;
uint16_t YawAH;
} ;
imu_test_main.cc
//
// Created by cai on 19-10-5.
//
#include "serial.h"
#include "cartographer/cartographer_ros/sensor_msg.h"
#include <sstream>
#include "string"
#include "imu_driver.h"
#include "imu_protocol.h"
#include <iostream>
using namespace std;
namespace cartographer {
namespace src {
Serial *ser; //声明串口对象
}
}
int main(int argc, char *argv[]) {
string port = "/dev/ttyUSB0";
int baudrate = 115200;
//初始化节点
printf("imu_nodeimu_nodeimu_nodeimu_nodeimu_node\n");
printf("imu_nodeimu_nodeimu_nodeimu_nodeimu_node\n");
printf("imu_nodeimu_nodeimu_nodeimu_nodeimu_node\n");
printf("imu_nodeimu_nodeimu_nodeimu_nodeimu_node\n");
//声明节点句柄
//发布主题, 消息格式使用sensor_msg::Imu标准格式（topic名称，队列长度）
cartographer::src::imuDriver *imu_driver = new cartographer::src::imuDriver();
result_t connect = imu_driver->connect(port.c_str(), baudrate);
if (connect < 0) {
cout << "IMU can not connect" << endl;
}
//开启子线程从串口获得数据然后存到缓存中
size_t ret = imu_driver->turnOn();
cout << "ret=" << ret << endl;
if (!ret) {
cout << "Failed to start scan mode!!!" << endl;
return 0;
}
//消息发布频率
while (1) {
//cout << "ros::ok()"<<endl;
vector<IMU_DATA> *package = new vector<IMU_DATA>();
imu_driver->doProcessSimple(package);
//cout << "package.size()=" << package->size() << endl;
for (int i = 0; i < package->size(); ++i) {
IMU_DATA imu_data;
//imu_data.header.stamp = ros::Time::now();
//imu_data.header.frame_id = "laser_frame";
//imu_data.header.frame_id = "imu_link";
// float orientation_y = (float)(*package)[i].YawAH;
//
// float angular_velocity_x = (float)(*package)[i].XAH;
// float angular_velocity_y = (float)(*package)[i].YAH;
// float angular_velocity_z = (float)(*package)[i].ZAH;
//
// float linear_acceleration_x = (float)(*package)[i].XLH;
// float linear_acceleration_y = (float)(*package)[i].YLH;
// float linear_acceleration_z = (float)(*package)[i].ZLH;
///////////////////////////////////////////////////////////////////
imu_data.YawAH = (float)(*package)[i].YawAH;
imu_data.XAH = (float)(*package)[i].XAH;
imu_data.YAH = (float)(*package)[i].YAH;
imu_data.ZAH = (float)(*package)[i].ZAH;
imu_data.XLH = (float)(*package)[i].XLH;
imu_data.YLH = (float)(*package)[i].YLH;
imu_data.ZLH = (float)(*package)[i].ZLH;
// imu_data.orientation.y = 0;
//
// imu_data.angular_velocity.x = 0;
// imu_data.angular_velocity.y = 0;
// imu_data.angular_velocity.z = 0;
//
// imu_data.linear_acceleration.x = 2;
// imu_data.linear_acceleration.y = 2;
// imu_data.linear_acceleration.z = 9;
cout << imu_data.YawAH <<"______";
cout << imu_data.XAH <<"______";
cout << imu_data.YAH <<"______";
cout << imu_data.ZAH <<"______";
cout << imu_data.XLH <<"______";
cout << imu_data.YLH <<"______";
cout << imu_data.ZLH <<"______";
cout<< endl;
// cout << "---------------------"<<endl;
//发布topic
}
delete (package);
package = NULL;
//处理ROS的信息，比如订阅消息,并调用回调函数
}
return 0;
}
