125 lines
4.8 KiB
C++
125 lines
4.8 KiB
C++
//
|
|
// Created by Wakanda_shaw on 2022/5/4.
|
|
//
|
|
|
|
#include "main_window.h"
|
|
#include "fstream"
|
|
#include "vector"
|
|
#include "string"
|
|
using namespace std;
|
|
|
|
|
|
void main_window::Read_IMU(QString path, QString outpath) {
|
|
QFile file(path);
|
|
file.open(QIODevice::ReadOnly);
|
|
QByteArray array = file.readAll();
|
|
vector<IMU_t> IMU;
|
|
|
|
// 将数据类型转换为string
|
|
string arr = array.toHex().toStdString();
|
|
// 设定分隔符
|
|
string split = "d300";
|
|
// 从头开始
|
|
int start_pos = arr.find(split,0);
|
|
int end_pos = arr.find(split, start_pos+4);
|
|
//子字符串个数
|
|
int delta = end_pos - start_pos;
|
|
// 截取以d300开头的子字符串
|
|
string sub = arr.substr(start_pos, delta);
|
|
uint32_t last_epoch= -1;
|
|
while(true){
|
|
try {
|
|
if(sub.substr(6,5)=="3e740"){
|
|
double accx,accy,accz,gyrox,gyroy,gyroz;
|
|
uint32_t time;
|
|
/* accelerator */
|
|
if(sub.substr(13, 2)=="1e"){
|
|
time = uint32_t(stoul(sub.substr(15, 8), nullptr, 16));
|
|
int16_t accX = int16_t (stoi(sub.substr(23, 4), nullptr, 16)) ;
|
|
int16_t accY = int16_t (stoi(sub.substr(27, 4), nullptr, 16)) ;
|
|
int16_t accZ = int16_t (stoi(sub.substr(31, 4), nullptr, 16)) ;
|
|
accx = accX * 0.000061 * 9.8;
|
|
accy = accY * 0.000061 * 9.8;
|
|
accz = accZ * 0.000061 * 9.8;
|
|
if(sub.substr(11,2)=="03"){
|
|
int16_t gyroX = int16_t (stoi(sub.substr(45, 4), nullptr, 16));
|
|
int16_t gyroY = int16_t (stoi(sub.substr(49, 4), nullptr, 16));
|
|
int16_t gyroZ = int16_t (stoi(sub.substr(53, 4), nullptr, 16));
|
|
gyrox = gyroX * 0.00437 * DEG2RAD;
|
|
gyroy = gyroY * 0.00437 * DEG2RAD;
|
|
gyroz = gyroZ * 0.00437 * DEG2RAD;
|
|
}
|
|
}else if(sub.substr(13, 2)=="1f" ){/* gyro */
|
|
time = uint32_t(stoul(sub.substr(15, 8), nullptr, 16));
|
|
int16_t gyroX = int16_t (stoi(sub.substr(23, 4), nullptr, 16));
|
|
int16_t gyroY = int16_t (stoi(sub.substr(27, 4), nullptr, 16));
|
|
int16_t gyroZ = int16_t (stoi(sub.substr(31, 4), nullptr, 16));
|
|
gyrox = gyroX * 0.00437 * DEG2RAD;
|
|
gyroy = gyroY * 0.00437 * DEG2RAD;
|
|
gyroz = gyroZ * 0.00437 * DEG2RAD;
|
|
}
|
|
|
|
/* load */
|
|
if(IMU.empty() || IMU.back().time!=time){
|
|
IMU_t temp;
|
|
temp.time = time;
|
|
temp.accx = accx;
|
|
temp.accy = accy;
|
|
temp.accz = accz;
|
|
temp.gyrox = gyrox;
|
|
temp.gyroy = gyroy;
|
|
temp.gyroz = gyroz;
|
|
temp.epoch = 0;
|
|
IMU.push_back(temp);
|
|
}else if(IMU.back().time==time){
|
|
IMU.back().accx = accx;
|
|
IMU.back().accy = accy;
|
|
IMU.back().accz = accz;
|
|
IMU.back().gyrox = gyrox;
|
|
IMU.back().gyroy = gyroy;
|
|
IMU.back().gyroz = gyroz;
|
|
}
|
|
}else if(sub.substr(6,3)==GPS_MSM7 || sub.substr(6,3)==GAL_MSM7 || sub.substr(6,3)==BDS_MSM7){
|
|
uint32_t epoch = uint32_t(stoul(sub.substr(12, 8), nullptr, 16))>>2;
|
|
epoch = epoch/1000; //ms to s
|
|
epoch+=18;
|
|
if(sub.substr(6,3)==BDS_MSM7){
|
|
epoch+=14;
|
|
}
|
|
if(IMU.back().epoch !=epoch && epoch!=last_epoch){
|
|
IMU.back().epoch = epoch;
|
|
}
|
|
last_epoch = epoch;
|
|
|
|
}
|
|
|
|
}catch( out_of_range & e ){
|
|
cout<<e.what()<<endl;
|
|
}catch( invalid_argument & e2){
|
|
cout<<e2.what()<<endl;
|
|
}
|
|
|
|
start_pos = end_pos;
|
|
// 循环跳出
|
|
if(start_pos == std::string::npos){
|
|
break;
|
|
}
|
|
end_pos = arr.find(split, start_pos+4);
|
|
delta = end_pos - start_pos;
|
|
sub = arr.substr(start_pos, delta);
|
|
}
|
|
// 得到输出路径
|
|
ofstream outputfile(outpath.toStdString());
|
|
if(outputfile){
|
|
outputfile<<"Obssec"<<","<<"AccX"<<","<<"AccY"<<","<<"AccZ"<<","<<"gyroX"<<","<<"gyroY"
|
|
<<","<<"gyroZ"<<","<<"Tow"<<endl;
|
|
}
|
|
for (auto iter = IMU.begin(); iter != IMU.end(); iter++){
|
|
if(outputfile) {
|
|
outputfile << iter->time << "," << iter->accx << "," << iter->accy << "," << iter->accz << ","
|
|
<< iter->gyrox << ","
|
|
<< iter->gyroy << "," << iter->gyroz <<","<<iter->epoch<<endl;
|
|
}
|
|
}
|
|
outputfile.close();
|
|
} |