ZJUT_Nav/QT/Mac/read_IMU/Source/Read_IMU.cpp

//
// 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);

    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<<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();
}