ASC格式惯导数据文件转IMR格式文件

   我们使用惯导采集数据之后，如果需要用现有软件进行解算，比如POSMind等等，就会涉及到IMR格式的惯导数据文件。而NovAtel Convert转换软件只能将原始DAT格式的文件转成ASCLL文件，因此我自编程实现了ASC格式文件到IMR格式文件的转换。

ASC格式文件

ASCII数据格式如下：

表 1 ASC文件数据含义

数据内容	含义
%RAWIMUSA	IMU原始文件头，需要提取有该符号标识的这一行数据
42919.530	GPS周内秒
42919.529998647	IMU 时间
-494	加速度计Z轴输出
115	加速度计-Y轴输出
64180	加速度计X轴输出
-36	陀螺仪Z轴输出
8	陀螺仪-Y轴输出
-941	陀螺仪X轴输出

IMR文件格式

Waypoint将所有自定义IMU原始二进制格式转换为通用格式（IMR），该格式在IMU数据转换器中的解码过程后从惯性探测器读取。由于它包含读取和解码数据的重要信息，通用IMU数据格式的前512个字节是一个必须填写、读取和解释的标头。在C/C++结构定义中，通用格式头包含以下字段：

IMR 文件头结构定义

Word	Size (bytes)	Type	Description
szHeader	8	char[8]	“$IMURAW0” – NULL terminated ASCII string
bIsIntelOrMotorola	1	int8_t	0 = Intel (Little Endian), default1 = Motorola (Big Endian)
bDeltaTheta	4	int32_t	0 = Data to follow will be read as scaled angular rates 1 = (default), data to follow will be read as delta thetas, meaning angular increments (i.e. scale and multiply by dDataRateHz to get degrees/second)
bDeltaVelocity	4	int32_t	0 = Data to follow will be read as scaled accelerations 1 = (default), data to follow will be read as delta velocities, meaning velocity increments (i.e. scale and multiply by dDataRateHz to get m/s2)
dDataRateHz	8	double	The data rate of the IMU in Hz. e.g. 0.01 second data rate is 100 Hz
dGyroScaleFactor	8	double	If bDeltaTheta == 0, multiply the gyro measurements by this to get degrees/second If bDeltaTheta == 1, multiply the gyro measurements by this to get degrees, then multiply by dDataRateHz to get degrees/second
dAccelScaleFactor	8	double	If bDeltaVelocity == 0, multiply the accel measurements by this to get m/s2 If bDeltaVelocity == 1, multiply the accel measurements by this to get m/s, then multiply by dDataRateHz to get m/s2
iUtcOrGpsTime	4	int32_t	Defines the time tags as GPS or UTC seconds of the week 0 = Unknown, will default to GPS 1 = Time tags are UTC seconds of week 2 = Time tags are GPS seconds of week
iRcvTimeOrCorrTime	4	int32_t	Defines whether the time tags are on the nominal top of the second or are corrected for receiver time bias
dTimeTagBias	8	double	If you have a known bias between your GPS and IMU time tags enter it here
szImuName	32	char[32]	Name of the IMU being used
reserved1	4	uint8_t[4]	Reserved for future use
szProgramName	32	char[32]	Name of calling program
tCreate	12	time_type	Creation time of file
bLeverArmValid	1	bool	True if lever arms from IMU to primary GNSS antenna are stored in this header
IXoffset	4	int32_t	X value of the lever arm, in millimeters
IYoffset	4	int32_t	Y value of the lever arm, in millimeters

C++源代码
（1） 头文件

#pragma once
#include
#include
#include
#include
#include
#include
#include

（2）函数模块

#include"INS.h"


/*读取IMU解算配置文件------------------------------------------------
输入：配置文件
输出：配置结构体
-------------------------------------------------------------------*/
void INS_Solver::ReadConfig(string filename)
{
	ifstream file(filename);
	if (!file.is_open()) 
	{
		runtime_error("无法打开配置文件: " + filename);
	}

	string line;
	while (getline(file, line))
	{
		// 跳过空行和注释
		if (line.empty() || line[0] == '#') continue;

		// 查找分隔符"："（中文冒号）
		size_t pos = line.find(":");
		if (pos == string::npos)
		{
			cerr << "警告: 格式错误的行 - " << line << std::endl;
			continue;
		}

		// 提取键和值
		string key = line.substr(0, pos);
		string value = line.substr(pos + 1);

		// 去除前后空格
		key.erase(0, key.find_first_not_of(" \t"));
		key.erase(key.find_last_not_of(" \t") + 1);
		value.erase(0, value.find_first_not_of(" \t"));
		value.erase(value.find_last_not_of(" \t") + 1);

		if (key == "Accelerometer conversion factor")
		{
			imuconfig.acc_conversion_factor = stod(value);
		}

		if (key == "Gyroscope conversion factor")
		{
			imuconfig.gyro_conversion_factor = stod(value);
		}

		if (key == "frequency")
		{
			imuconfig.freq = stod(value);
		}
	}
}


// 分割字符串函数
vector split(string &s, char delimiter)
{
	vector tokens;
	string token;
	istringstream tokenStream(s);
	while (getline(tokenStream, token, delimiter))
	{
		if (!token.empty())
		{
			tokens.push_back(token);
		}
	}
	return tokens;
}


/*读取ASCLL格式的RAWIMU文件------------------------------------------
输入：IMU文件名
输出：IMU数据结构体
-------------------------------------------------------------------*/
void INS_Solver::Read_IMU_ASCLL(string filename,int k)
{
	IMUData imudata_current;
	ImuData.reserve(450000);

	ifstream infile(filename);
	if (!infile.is_open())
	{
		cerr << "The file " << filename << " is not exsist!" << endl;
	}

	while (!infile.eof())
	{
		string line;
		getline(infile, line);

		if (line.find("%RAWIMUSXA") == 0)
		{
			 // 去掉校验部分(最后一个*后面的内容)
			int asterisk_pos = line.find_last_of('*');
			string data_part = line.substr(0, asterisk_pos);

			// 处理分号分割的部分
			int semicolon_pos = data_part.find(';');
			string part_before_semicolon = data_part.substr(0, semicolon_pos);
			string part_after_semicolon = data_part.substr(semicolon_pos + 1);

			// 解析分号前的部分
			vector before_semicolon = split(part_before_semicolon, ',');
			if (before_semicolon.size() >= 3) 
			{
				imudata_current.gpsweek = stoi(before_semicolon[1]);
				imudata_current.secofweek = stod(before_semicolon[2]);
			}

			// 解析分号后的部分
			vector after_semicolon = split(part_after_semicolon, ',');

			if (k == 0)
			{
				imudata_current.az = stoi(after_semicolon[5])*imuconfig.acc_conversion_factor*imuconfig.freq;
				imudata_current.ay = (-1)*stoi(after_semicolon[6])*imuconfig.acc_conversion_factor*imuconfig.freq;
				imudata_current.ax = stoi(after_semicolon[7])*imuconfig.acc_conversion_factor*imuconfig.freq;
				imudata_current.gz = stoi(after_semicolon[8])*imuconfig.gyro_conversion_factor*imuconfig.freq;
				imudata_current.gy = (-1)*stoi(after_semicolon[9])*imuconfig.gyro_conversion_factor*imuconfig.freq;
				imudata_current.gx = stoi(after_semicolon[10])*imuconfig.gyro_conversion_factor*imuconfig.freq;
			}
			else
			{
				imudata_current.az = stoi(after_semicolon[5]);
				imudata_current.ay = (-1)*stoi(after_semicolon[6]);
				imudata_current.ax = stoi(after_semicolon[7]);
				imudata_current.gz = stoi(after_semicolon[8]);
				imudata_current.gy = (-1)*stoi(after_semicolon[9]);
				imudata_current.gx = stoi(after_semicolon[10]);
			}
	
			ImuData.push_back(imudata_current);
			cout << ImuData.size() << endl;
		}
		//if (ImuData.size() > 1000)
		//{
		//	break;
		//}
	};
}


bool INS_Solver::readFFSHead(FILE* m_fp, INS_HEAD& m_FFSHead)
{
	// Read IMR File Head Part
	fread(&m_FFSHead.szHeader, sizeof(char), 8, m_fp);
	fread(&m_FFSHead.bIsIntelOrMotorola, sizeof(char), 1, m_fp);
	fread(&m_FFSHead.dVersionNumber, sizeof(double), 1, m_fp);
	fread(&m_FFSHead.bDeltaTheta, sizeof(int), 1, m_fp);
	fread(&m_FFSHead.bDeltaVelocity, sizeof(int), 1, m_fp);
	fread(&m_FFSHead.dDataRateHz, sizeof(double), 1, m_fp);
	fread(&m_FFSHead.dGyrosScaleFactor, sizeof(double), 1, m_fp);
	fread(&m_FFSHead.dAccelScaleFactor, sizeof(double), 1, m_fp);
	fread(&m_FFSHead.iUtcOrGpsTime, sizeof(int), 1, m_fp);
	fread(&m_FFSHead.iRcvTimeOrCorrTime, sizeof(int), 1, m_fp);
	fread(&m_FFSHead.dTimeTagBias, sizeof(double), 1, m_fp);
	fread(&m_FFSHead.szImuName, sizeof(char), 32, m_fp);
	fread(&m_FFSHead.bDirValid, sizeof(bool), 1, m_fp);
	fread(&m_FFSHead.ucX, sizeof(unsigned char), 1, m_fp);
	fread(&m_FFSHead.ucY, sizeof(unsigned char), 1, m_fp);
	fread(&m_FFSHead.ucZ, sizeof(unsigned char), 1, m_fp);
	fread(&m_FFSHead.szProgramName, sizeof(char), 32, m_fp);
	fread(&m_FFSHead.tCreate, sizeof(time_type), 1, m_fp);
	if (m_FFSHead.tCreate.year < 50)
		m_FFSHead.tCreate.year += 2000;
	else
		m_FFSHead.tCreate.year += 1900;
	fread(&m_FFSHead.bLeverArmValid, sizeof(bool), 1, m_fp);
	fread(&m_FFSHead.lXoffset, sizeof(int), 1, m_fp);
	fread(&m_FFSHead.lYoffset, sizeof(int), 1, m_fp);
	fread(&m_FFSHead.lZoffset, sizeof(int), 1, m_fp);
	fread(&m_FFSHead.Reserved, sizeof(bool), 354, m_fp);

	return true;
}


bool INS_Solver::readFFSBody(FILE *m_fp, IMUData& insData, INS_HEAD &m_FFSHead, list& m_AllDataRecords)
{
	double dt = 0.0;
	map dts;

	double tow_pre = 0.0;
	double tow = 0.0;
	double gtPre = 0.0;

	while (!feof(m_fp))
	{
		INS_DATA imrData;
		fread(&tow, sizeof(double), 1, m_fp);
		fread(&imrData.gx, sizeof(int), 1, m_fp);
		fread(&imrData.gy, sizeof(int), 1, m_fp);
		fread(&imrData.gz, sizeof(int), 1, m_fp);
		fread(&imrData.ax, sizeof(int), 1, m_fp);
		fread(&imrData.ay, sizeof(int), 1, m_fp);
		fread(&imrData.az, sizeof(int), 1, m_fp);

		if (tow > 604800) tow -= 604800;

		// Cross GPS week
		if (tow_pre != 0.0 && fabs(tow - tow_pre) > 604700)
		{
			string msg("IMU data time exceeds one weeek.\n");
			printf(msg.c_str());
		}
		tow_pre = tow;

		// The unit of outputs (gx, gy, gz) for imr files is deg/s
		insData.secofweek = tow;
		insData.gx = imrData.gx * m_FFSHead.dGyrosScaleFactor * m_FFSHead.dDataRateHz;
		insData.gy = imrData.gy * m_FFSHead.dGyrosScaleFactor * m_FFSHead.dDataRateHz;
		insData.gz = imrData.gz * m_FFSHead.dGyrosScaleFactor * m_FFSHead.dDataRateHz;
		insData.ax = imrData.ax * m_FFSHead.dAccelScaleFactor * m_FFSHead.dDataRateHz;
		insData.ay = imrData.ay * m_FFSHead.dAccelScaleFactor * m_FFSHead.dDataRateHz;
		insData.az = imrData.az * m_FFSHead.dAccelScaleFactor * m_FFSHead.dDataRateHz;

		// Correct the time offset
		insData.secofweek -= (m_FFSHead.dTimeTagBias * 0.001);

		if (insData.secofweek > gtPre)
		{
			m_AllDataRecords.push_back(insData);

			gtPre = insData.secofweek;
		}
		else if (isEqual(insData.secofweek, gtPre))
		{
			char msg[1024] = { '\0' };
			snprintf(msg, sizeof(msg), "IMU data: data duplication --- %.4f.\n", insData.secofweek);
			printf(msg);
		}
		else
		{
			char msg[1024] = { '\0' };
			snprintf(msg, sizeof(msg), "IMU data: previous %.4f is greater than current %.4f.\n", gtPre, insData.secofweek);
			printf(msg);
		}
	}

	return true;
}


/*读取IMR文件格式的惯导原始数据---------------------------------------
输入：IMR文件名
输出：IMR文件头和惯导数据
--------------------------------------------------------------------*/
void INS_Solver::Read_IMU_IMR(string IMRFile, INS_HEAD m_FFSHead, list m_AllDataRecords)
{
	FILE* m_fp = nullptr;
	errno_t err = fopen_s(&m_fp, IMRFile.c_str(), "rb");
	if (err != 0 || m_fp == nullptr)
	{
		string msg = "IMR file wrong! File path --- " + IMRFile;
		printf(msg.c_str());
		return;
	}

	IMUData insData;

	readFFSHead(m_fp, m_FFSHead);
	readFFSBody(m_fp, insData, m_FFSHead, m_AllDataRecords);
	cout << "IMR file is read!" << endl;
}


/*将ASCLL文件格式的惯导原始数据转换为imr格式--------------------------
输入：ascll文件
输出：imr文件
--------------------------------------------------------------------*/
void INS_Solver::imufile_ascll2imr(string file_ascll)
{
	Read_IMU_ASCLL(file_ascll, 1); // 读取ASCII数据

	if (ImuData.size() < 2)
	{
		throw runtime_error("Insufficient IMU data points");
	}

	// 计算平均采样率
	double freq = imuconfig.freq;

	// 生成输出文件名
	size_t last_dot = file_ascll.find_last_of(".");
	string file_imr = (last_dot != string::npos) ?
		file_ascll.substr(0, last_dot) + ".imr" :
		file_ascll + ".imr";

	FILE* outfile = nullptr;
	if (fopen_s(&outfile, file_imr.c_str(), "wb") != 0 || !outfile)
	{
		throw runtime_error("Cannot open output file: " + file_imr);
	}

	// 逐个字段写入文件头
	// 确保字符串正确终止并精确写入指定长度
	const char header_id[] = "$IMURAM0";
	fwrite(header_id, sizeof(char), 8, outfile);  // 精确写入8字节

	char endian = 0;  // 小端序
	fwrite(&endian, sizeof(char), 1, outfile);

	double version = 8.80;
	fwrite(&version, sizeof(double), 1, outfile);

	int delta_theta = 1;
	fwrite(&delta_theta, sizeof(int), 1, outfile);

	int delta_velocity = 1;
	fwrite(&delta_velocity, sizeof(int), 1, outfile);

	fwrite(&freq, sizeof(double), 1, outfile);

	fwrite(&imuconfig.gyro_conversion_factor, sizeof(double), 1, outfile);
	fwrite(&imuconfig.acc_conversion_factor, sizeof(double), 1, outfile);

	int time_type = 2;  // GPS时间
	fwrite(&time_type, sizeof(int), 1, outfile);

	int time_corr = 2;  // 校正时间
	fwrite(&time_corr, sizeof(int), 1, outfile);

	double time_bias = 0.0;
	fwrite(&time_bias, sizeof(double), 1, outfile);

	// 写入IMU名称（精确32字节）
	const char imu_name[32] = "IMU_NAME";
	fwrite(imu_name, sizeof(char), 32, outfile);

	bool dir_valid = false;
	fwrite(&dir_valid, sizeof(bool), 1, outfile);

	unsigned char axis_x = 'X';
	fwrite(&axis_x, sizeof(unsigned char), 1, outfile);

	unsigned char axis_y = 'Y';
	fwrite(&axis_y, sizeof(unsigned char), 1, outfile);

	unsigned char axis_z = 'Z';
	fwrite(&axis_z, sizeof(unsigned char), 1, outfile);

	// 写入程序名称（精确32字节）
	const char program_name[32] = "ASCLL_to_IMR_Converter";
	fwrite(program_name, sizeof(char), 32, outfile);

	// 写入创建时间
	time_t now = time(nullptr);
	struct tm tm_now;
	if (gmtime_s(&tm_now, &now) == 0)
	{
		short int year = static_cast(tm_now.tm_year + 1900);
		short int month = static_cast(tm_now.tm_mon + 1);
		short int day = static_cast(tm_now.tm_mday);
		short int hour = static_cast(tm_now.tm_hour);
		short int minute = static_cast(tm_now.tm_min);
		short int second = static_cast(tm_now.tm_sec);

		fwrite(&year, sizeof(short int), 1, outfile);
		fwrite(&month, sizeof(short int), 1, outfile);
		fwrite(&day, sizeof(short int), 1, outfile);
		fwrite(&hour, sizeof(short int), 1, outfile);
		fwrite(&minute, sizeof(short int), 1, outfile);
		fwrite(&second, sizeof(short int), 1, outfile);
	}
	else
	{
		// 如果获取时间失败，写入默认值
		short int zero = 0;
		for (int i = 0; i < 6; i++)
			fwrite(&zero, sizeof(short int), 1, outfile);
	}

	bool lever_valid = false;
	fwrite(&lever_valid, sizeof(bool), 1, outfile);

	int offset_zero = 0;
	fwrite(&offset_zero, sizeof(int), 1, outfile);  // X偏移
	fwrite(&offset_zero, sizeof(int), 1, outfile);  // Y偏移
	fwrite(&offset_zero, sizeof(int), 1, outfile);  // Z偏移

	// 写入保留字段（354字节全零）
	char reserved[354] = { 0 };
	fwrite(reserved, sizeof(char), 354, outfile);

	// 逐个字段写入数据记录
	const int MAX_INT = numeric_limits::max();
	const int MIN_INT = numeric_limits::min();

	for (const auto& imu : ImuData)
	{
		// 写入时间
		double time_val = imu.secofweek;
		fwrite(&time_val, sizeof(double), 1, outfile);

		// 确保数据在int范围内并写入
		auto write_clamped = [&](double value)
		{
			int clamped = (value > MAX_INT) ? MAX_INT :
				(value < MIN_INT) ? MIN_INT :
				static_cast(value);
			fwrite(&clamped, sizeof(int), 1, outfile);
		};

		// 写入传感器数据
		write_clamped(imu.gx);
		write_clamped(imu.gy);
		write_clamped(imu.gz);
		write_clamped(imu.ax);
		write_clamped(imu.ay);
		write_clamped(imu.az);
	}

	fclose(outfile);
	cout << "Successfully converted to: " << file_imr
		<< " with " << ImuData.size() << " records" << endl;
	ImuData.clear();
}



void INS_Solver::ascll2type2(string file_ascll)
{
	Read_IMU_ASCLL(file_ascll, 1); // 读取ASCII数据
	
	// 生成输出文件名
	size_t last_dot = file_ascll.find_last_of(".");
	string file_imr = (last_dot != string::npos) ?
		file_ascll.substr(0, last_dot) + ".txt" :
		file_ascll + ".txt";

	ofstream outfile(file_imr);

	for (const auto& imu : ImuData)
	{
		outfile << imu.secofweek << " " << imu.ax << " " << imu.ay << " " << imu.az << " " << imu.gx << " " << imu.gy << " " << imu.gz << endl;
	}

	outfile.close();
}

欢迎大家和我交流！