119 lines
4.3 KiB
C++
119 lines
4.3 KiB
C++
|
//
|
||
|
|
// Created by Wakanda_shaw on 2022/5/4.
|
||
|
|
//
|
||
|
|
#include <bitset>
|
||
|
|
#include "main_window.h"
|
||
|
|
#include "fstream"
|
||
|
|
#include <cmath>
|
||
|
|
#include "iomanip"
|
||
|
|
|
||
|
|
|
||
|
|
using namespace std;
|
||
|
|
int64_t exclusive_or = pow(2, 37) -1 ;
|
||
|
|
void main_window::Read_PVT(QString path, QString outpath) {
|
||
|
|
QFile file(path);
|
||
|
|
file.open(QIODevice::ReadOnly);
|
||
|
|
QByteArray array = file.readAll();
|
||
|
|
ofstream outputfile(outpath.toStdString());
|
||
|
|
if(outputfile){
|
||
|
|
outputfile<<"Tow"<<","<<"ecef_posX"<<","<<"ecef_posY"<<","<<"ecef_posZ"<<","<<"ecef_vX"<<","<<"ecef_vY"<<","<<"ecef_vZ"<<endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
// 将数据类型转换为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);
|
||
|
|
|
||
|
|
while (true) {
|
||
|
|
try {
|
||
|
|
if (sub.substr(6, 5) == "3e704") {
|
||
|
|
uint32_t epoch = uint32_t (stoul(sub.substr(40, 8), nullptr, 16)) >>1;
|
||
|
|
epoch = epoch/1000 + 18; // ms to s and + 闰秒
|
||
|
|
cout<<epoch<<endl;
|
||
|
|
bitset<44> bit_x(stoll(sub.substr(53, 11), nullptr, 16));
|
||
|
|
bitset<40> bit_y(stoll(sub.substr(63, 10), nullptr, 16));
|
||
|
|
bitset<44> bit_z(stoll(sub.substr(72, 11), nullptr, 16));
|
||
|
|
|
||
|
|
int64_t ecef_X, ecef_Y, ecef_Z;
|
||
|
|
string p_x, p_y, p_z;
|
||
|
|
|
||
|
|
p_x = bit_x.to_string().substr(3, 38);
|
||
|
|
p_y = bit_y.to_string().substr(1,38);
|
||
|
|
p_z = bit_z.to_string().substr(3, 38);
|
||
|
|
|
||
|
|
if(p_x.substr(0, 1)=="1"){
|
||
|
|
int64_t data_filed = int64_t(stoll(p_x.substr(1, 37), nullptr, 2));
|
||
|
|
ecef_X = ((data_filed^exclusive_or) + 1) * -1;
|
||
|
|
}else{
|
||
|
|
ecef_X = int64_t (stoll(p_x, nullptr, 2));
|
||
|
|
}
|
||
|
|
if(p_y.substr(0, 1)=="1"){
|
||
|
|
int64_t data_filed = int64_t(stoll(p_y.substr(1, 37), nullptr, 2));
|
||
|
|
ecef_Y = ((data_filed^exclusive_or) + 1) * -1;
|
||
|
|
}else{
|
||
|
|
ecef_Y = int64_t (stoll(p_y, nullptr, 2));
|
||
|
|
}
|
||
|
|
if (p_z.substr(0, 1)=="1"){
|
||
|
|
int64_t data_filed = int64_t (stoll(p_z.substr(1, 37), nullptr, 2));
|
||
|
|
ecef_Z = ((data_filed^exclusive_or) + 1) * -1;
|
||
|
|
}else{
|
||
|
|
ecef_Z = int64_t (stoll(p_z, nullptr, 2));
|
||
|
|
}
|
||
|
|
// 缩放因子
|
||
|
|
double ecef_x, ecef_y, ecef_z;
|
||
|
|
ecef_x=double(ecef_X)/10000;
|
||
|
|
ecef_y=double(ecef_Y)/10000;
|
||
|
|
ecef_z=double(ecef_Z)/10000;
|
||
|
|
|
||
|
|
bitset<36> bit_vx(stoll(sub.substr(82, 9), nullptr, 16));
|
||
|
|
bitset<36> bit_vy(stoll(sub.substr(90, 9), nullptr, 16));
|
||
|
|
bitset<36> bit_vz(stoll(sub.substr(98, 9), nullptr, 16));
|
||
|
|
|
||
|
|
int32_t ecef_vX, ecef_vY, ecef_vZ;
|
||
|
|
string v_x, v_y, v_z;
|
||
|
|
|
||
|
|
v_x = bit_vx.to_string().substr(1, 32);
|
||
|
|
v_y = bit_vy.to_string().substr(1, 32);
|
||
|
|
v_z = bit_vz.to_string().substr(1, 32);
|
||
|
|
ecef_vX = int32_t (stoll(v_x, nullptr, 2));
|
||
|
|
ecef_vY = int32_t (stoll(v_y, nullptr, 2));
|
||
|
|
ecef_vZ = int32_t (stoll(v_z, nullptr, 2));
|
||
|
|
double ecef_vx, ecef_vy, ecef_vz;
|
||
|
|
ecef_vx = double(ecef_vX)/1000000;
|
||
|
|
ecef_vy = double(ecef_vY)/1000000;
|
||
|
|
ecef_vz = double(ecef_vZ)/1000000;
|
||
|
|
|
||
|
|
if(outputfile){
|
||
|
|
outputfile<<fixed<<setprecision(10)<<epoch<<","<<ecef_x<<","<<ecef_y<<","<<ecef_z<<","<<ecef_vx<<","<<ecef_vy<<","<<
|
||
|
|
ecef_vz<<endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
}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);
|
||
|
|
|
||
|
|
}
|
||
|
|
outputfile.close();
|
||
|
|
}
|
||
|
|
|