// // 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; // 将数据类型转换为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)=="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){ IMU.back().epoch = epoch; } } }catch( out_of_range & e ){ cout<time << "," << iter->accx << "," << iter->accy << "," << iter->accz << "," << iter->gyrox << "," << iter->gyroy << "," << iter->gyroz <<","<epoch<