GNSS单点定位与测速2:二进制格式文件读取(包括电离层延迟参数、卫星星历、原始观测值)
本程序适用于导航工程专业本科生课程作业:单点定位与测速。
本程序中读取的二进制文件以和芯星通UB482板卡协议或诺瓦泰OEM7系列板卡协议中的部分消息格式为准。
文章目录
- GNSS单点定位与测速2:二进制格式文件读取(包括电离层延迟参数、卫星星历、原始观测值)
- 二进制文件读取/binaryread.h
- 写在开头:示例程序针对二进制文件读取的基本方法
- UB482板卡消息协议的几个解释
- 依赖库包含与结构体定义(有更新)
- 从usigned char类型的二进制数据中拷贝出协议对应数据类型的数据函数bit2xxxx
- 辅助函数charreplace:交换字符串中的特定字母
- 单个卫星星历结构体写为RINEX格式文件函数eph2file
- 从消息报告中提取报告对应的GPS时函数breport2GPStime
- CRC校验函数
- 二进制文件读取函数binaryread
- 更新getbinaryreport:从unsigned char类型的二进制数据数组中直接获取消息报告
- 从数据中读取星历函数getsat
- 更新:从数据中读取原始观测值的函数getobs
二进制文件读取/binaryread.h
写在开头:示例程序针对二进制文件读取的基本方法
由于二进制文件/消息的形式第一次出现在现有课程体系中,因此,简要概括一下示例程序针对二进制文件/消息读取的方式。以读取整个文件为例,常见对于ASIIC码文件读取的策略是:根据特定的文件结构书写特定的文件读取函数。然而,这样的策略在需要进行同步帧头和数据不定切分的二进制文件读取中不再具有可操作性。二进制数据的冗余、重复或异常都会导致文件指针的意外丢失,因此我们选择用一个"缓冲区"(fullodata[1024*1024])来记录所有的数据,并根据消息帧头来取得整个文件的"消息报告"(breport),随后利用这个"报告"所记录的关键信息对存储在"缓冲区"的数据进行对应处理。
UB482板卡消息协议的几个解释
本课程中应用到的消息主要有四个:ID为43(与OEM7协议中ID为631的消息具有相同的结构)的原始观测值信息;ID为7的单个卫星星历信息;ID为8的电离层延迟参数信息和ID为47的接收机自行解算的位置信息。应用的头结构主要有两个,一个长度为28,一个长度为24(仅有ID为8的消息执行该头结构)。在读取二进制文档前一定要注意仔细查阅协议。
依赖库包含与结构体定义(有更新)
在这一部分,我们分别定义了二进制消息报告breport,星历eph_t,伪距观测值GPSOBS三种结构体。其中二进制消息报告包括消息ID成员ID、消息长度成员Len、消息在"缓冲区"的数组索引(开始索引start和结束索引end)。需要注意的是,在上一个版本中,观测值结构体只应用了伪距观测值,但是现在已经补全了各种观测值。
#include<iostream>
#include"GNSStime.h"
#define MAXSIZE 1024*1024//最大容忍1M大小的观测文件
#include <fstream>
#include<time.h>
#include<string.h>
#define Frame_IN 0
#define Frame_OUT 1//定义文件指针帧内外
#define EPHYES 1
#define EPHNO 0
using namespace std;
//二进制消息报告
typedef struct {
int start;
int end;
unsigned short ID;
unsigned short Len;
}breport;
//星历:这个行李结构体较上学期原理课上定义的更加全面
typedef struct {
int statu = EPHNO;
double af0, af1, af2;//卫星钟差改正参数
double tgd;//卫星电路群延时改正
bool AS;//AS标识
int svh;//卫星健康状况(svh)
int week; //GPS周
gtime_t toe, toc; //卫星星历的参考时刻,卫星钟时间
double A, e; //轨道长半轴(A),偏心率(e)
double i0; //轨道倾角(i0)
double OMG0; //升交点经度(0MG0)
double omg; //近地点角距(omg)
double M0; //平近点角(M0)
double deln; //平均角速度(deln)
double OMGD; //升交点速率(OMGd)
double idot; //轨道倾角变化率(idot)
double crc, crs; //地心距的摄动改正项(crc,crs)
double cuc, cus; //升交角距的摄动改正项(cuc,cus)
double cic, cis; //倾角的摄动改正项(cic,cis)
double toes; //周内秒(toes)
}eph_t;
//伪距观测值结构体
typedef struct {
int num = 0;//观测到卫星的数量
gtime_t rt;//观测时间
int name[36] = {};//记录卫星编号
double R0[36] = {};//伪距观测值
float psrstd[36] = {};//伪距标准差
double adr[36] = {};//载波相位
float adrstd[36] = {};//载波相位标准差
float dopp[36] = {};//瞬时多普勒Hz
float Cno[36] = {};//载噪比dB-Hz
float loctime[36] = {};//连续跟踪时间(无周跳)
unsigned long trackstatu[36] = {};//连续跟踪状态
}GPSOBS;
从usigned char类型的二进制数据中拷贝出协议对应数据类型的数据函数bit2xxxx
参数:unsigned char型数组或指针。
功能:读取对应返回值类型的数据并返回。
特别说明:这里的几个函数调用时可以使用"缓冲区"头指针+开始读取数据的位置的方式调用,如:bit2ushort(fullodata+12);
unsigned short bit2ushort(unsigned char* p) {
unsigned short r;
memcpy(&r, p, 2);
return r;
}
long bit2long(unsigned char* p) {
long r;
memcpy(&r, p, 4);
return r;
}
double bit2double(unsigned char* p) {
double r;
memcpy(&r, p, 8);
return r;
}
float bit2float(unsigned char* p) {
float r;
memcpy(&r, p, 4);
return r;
}
unsigned long bit2ulong(unsigned char* p) {
unsigned long r;
memcpy(&r, p, 4);
return r;
}
bool bit2bool(unsigned char* p) {
bool r;
memcpy(&r, p, 4);
return r;
}
unsigned int bit2uint(unsigned char* p) {
unsigned int r;
memcpy(&r, p, 4);
return r;
}
辅助函数charreplace:交换字符串中的特定字母
参数:待替换的字符数组str,待替换的字符c,替换后的字符b。
功能:将字符数组str[]中的字符c全部替换为字符b。
特别说明:这个函数是将被用于星历文件写为RINEX格式时,科学计数法的字符E替换为D。
void charreplace(char* str, char c, char b) {
for (int i = 0; i < strlen(str); i++)
if (str[i] == c)
str[i] = b;
}
单个卫星星历结构体写为RINEX格式文件函数eph2file
参数:卫星星历eph,待写入的文件指针*fs,待写入卫星的prn码。
功能:如题。
特别说明:写入过程中,由于示例程序作者缺乏对RINEX协议的完整理解,因此部分参数直接赋0处理,但不影响基于伪距的单点定位结果。
void eph2file(eph_t eph, FILE* fs, int prn) {
COMMONTIME comtoc = time2epoch(eph.toc);
char str[100] = {};
fprintf(fs, "%2d%3d%3d%3d%3d%3d%5.1lf", prn, comtoc.Year % 100, comtoc.Month, comtoc.Day, comtoc.Hour, comtoc.Minute, comtoc.Second);
sprintf(str, "%19.12E%19.12E%19.12E\n", eph.af0, eph.af1, eph.af2); charreplace(str, 'E', 'D'); fprintf(fs, str);//第一行
sprintf(str, " %19.12E%19.12E%19.12E%19.12E\n", 48.0, eph.crs, eph.deln, eph.M0); charreplace(str, 'E', 'D'); fprintf(fs, str);
sprintf(str, " %19.12E%19.12E%19.12E%19.12E\n", eph.cuc, eph.e, eph.cus, sqrt(eph.A)); charreplace(str, 'E', 'D'); fprintf(fs, str);
sprintf(str, " %19.12E%19.12E%19.12E%19.12E\n", eph.toes, eph.cic, eph.OMG0, eph.cis); charreplace(str, 'E', 'D'); fprintf(fs, str);
sprintf(str, " %19.12E%19.12E%19.12E%19.12E\n", eph.i0, eph.crc, eph.omg, eph.OMGD); charreplace(str, 'E', 'D'); fprintf(fs, str);
sprintf(str, " %19.12E%19.12E%19.12E%19.12E\n", eph.idot, 0.0, double(eph.week), 1.0); charreplace(str, 'E', 'D'); fprintf(fs, str);
sprintf(str, " %19.12E%19.12E%19.12E%19.12E\n", 2.0, double(eph.svh), eph.tgd, 48.0); charreplace(str, 'E', 'D'); fprintf(fs, str);
sprintf(str, " %19.12E%19.12E%19.12E%19.12E\n", 0.0, 0.0, 0.0, 0.0); charreplace(str, 'E', 'D'); fprintf(fs, str);
}
从消息报告中提取报告对应的GPS时函数breport2GPStime
参数:单个消息报告r,"缓冲区"数组fullodata。
功能:根据消息报告提取消息对应的GPS时。
特别说明:这里只针对课程中用到的两中头结构书写程序,更新需求请自行添加。如果使用OEM7协议,将无法直接从本函数获得电离层延迟参数的报告时间。
GPSTime breport2GPStime(breport r, unsigned char* data) {
GPSTime result;
if (r.ID == 47||r.ID==43||r.ID==7||r.ID==631) {
unsigned char oweek[2] = { data[r.start + 14],data[r.start + 15] };
unsigned char osec[4] = { data[r.start + 16],data[r.start + 17],data[r.start + 18],data[r.start + 19] };
result.Week = bit2ushort(oweek);
result.Second = double(bit2long(osec)) / 1000.0;
}
if (r.ID == 8) {
unsigned char oweek[2] = { data[r.start + 10],data[r.start + 11] };
unsigned char osec[4] = { data[r.start + 12],data[r.start + 13],data[r.start + 14],data[r.start + 15] };
result.Week = bit2ushort(oweek);
result.Second = double(bit2long(osec)) / 1000.0;
}
return result;
}
CRC校验函数
参数:数据缓冲区数组fullodata[],单条消息(待校验的消息)报告r。
功能:计算CRC校验值并于消息最后四位的CRC校验值比较,若校验成功返回1,否则返回0。
特别说明:在getcrc()函数中,对结构体breport的参数使用仅包括其start,end成员,因此亦可以用作对其成员crc的赋值操作。
//CRC校验函数
unsigned int crc32(const unsigned char* buff, int len) {
int i, j;
unsigned int crc = 0;
for (i = 0; i < len; i++) {
crc ^= buff[i];
for (j = 0; j < 8; j++) {
if (crc & 1) crc = (crc >> 1) ^ POLYCRC32; else crc >>= 1;
}
}
return crc;
}
//检查CRC校验结果
int getcrc(unsigned char* fullodata, breport r) {
return crc32(fullodata + r.start, r.end - r.start - 3) == bit2uint(fullodata + r.end - 3);
}
二进制文件读取函数binaryread
参数:消息报告数组epoch[],缓冲区数组fullodata[],待读文件的文件指针*fp。
功能:先从文件fp中读取所有的二进制数据并存储到缓冲区数组fullodata中,然后针对用数组进行数据解码,生成数据报告并存储在epoch[]中,最后将有效消息的总条数作为返回值返回。
特别说明:本函数只针对课程需要的四种消息进行读取,其中"631"(OEM7板卡协议)与"43"(UB432板卡协议)为相同结构,因此共用考虑。
讨论:由于数据解码过程中,我们使用的自定义的帧指示功能,在数据帧读取过程中不会再进行帧头同步,因此可以不再进行数据crc校验。但更加稳定和保险的方式应当是保持crc校验功能,请有需求自行添加。
int binaryfileread(breport* epoch, unsigned char* fullodata, FILE* fp) {
unsigned char str[100];
int epochnum = 0;
int statu = Frame_OUT;//初始化帧状态为帧外
int fcount=0, count = 0;
//中间关键变量存储
unsigned char headerlen = 0, messagesID[2] = {}, messagesLen[2] = {};
int flag = 0;//帧头查找标志
while (!feof(fp)) {
fread(str, 1, 1, fp);
fullodata[fcount] = str[0];
fcount++;//每次读取一位
}
//同步帧头
for (int i = 0; i < fcount; i++)
{
if (i < fcount - 2 && fullodata[i] == 0xaa && fullodata[i + 1] == 0x44 && fullodata[i + 2] == 0x12)
epoch[epochnum].start = i, statu = Frame_IN, headerlen = 28;
if (i < fcount - 2 && fullodata[i] == 0xaa && fullodata[i + 1] == 0x44 && fullodata[i + 2] == 0xb5)
epoch[epochnum].start = i, statu = Frame_IN, headerlen = 24;
if (statu == Frame_IN && headerlen == 28) {
count++;//记录
if (count == 5) messagesID[0] = fullodata[i];
if (count == 6) messagesID[1] = fullodata[i];
if (count == 9) messagesLen[0] = fullodata[i];
if (count == 10) messagesLen[1] = fullodata[i];
if (count > 28 && count == 28 + int(bit2ushort(messagesLen)) + 4) {
//printf("%d\n",fcount);
epoch[epochnum].end = i;
epoch[epochnum].ID = bit2ushort(messagesID);
epoch[epochnum].Len = bit2ushort(messagesLen);
epochnum++;
statu = Frame_OUT; count = 0;
}
}
if (statu == Frame_IN && headerlen == 24) {
count++;//记录
if (count == 5) messagesID[0] = fullodata[i];
if (count == 6) messagesID[1] = fullodata[i];
if (count == 7) messagesLen[0] = fullodata[i];
if (count == 8) messagesLen[1] = fullodata[i];
if (count > 24 && count == 24 + int(bit2ushort(messagesLen)) + 4) {
//printf("%d\n",fcount);
epoch[epochnum].end = i;
epoch[epochnum].ID = bit2ushort(messagesID);
epoch[epochnum].Len = bit2ushort(messagesLen);
epochnum++;
statu = Frame_OUT; count = 0;
}
}
}
return epochnum;
}
更新getbinaryreport:从unsigned char类型的二进制数据数组中直接获取消息报告
参数:消息报告数组epoch[],缓冲区数组fulldata[],待读数组消息的长度fulllen。
功能:直接从数据数组fulldata中解码数据,生成数据报告并存储在epoch[]中,最后将有效消息的总条数作为返回值返回。
特别说明:本函数只针对课程需要的四种消息进行读取,其中"631"(OEM7板卡协议)与"43"(UB432板卡协议)为相同结构,因此共用考虑。
讨论:由于数据解码过程中,我们使用的自定义的帧指示功能,在数据帧读取过程中不会再进行帧头同步,因此可以大概率可以不再进行数据crc校验。但更加稳定和保险的方式应当是保持crc校验功能,请有需求自行添加。
//从一定长度的二进制数组中获取消息报告
int getbinaryreport(breport* epoch, unsigned char* fullodata, int fulllen) {
int epochnum = 0;
int statu = Frame_OUT;//初始化帧状态为帧外
int fcount = fulllen, count = 0;
//中间关键变量存储
unsigned char headerlen = 0, messagesID[2] = {}, messagesLen[2] = {};
int flag = 0;//帧头查找标志
//同步帧头
for (int i = 0; i < fcount; i++)
{
if (i < fcount - 2 && fullodata[i] == 0xaa && fullodata[i + 1] == 0x44 && fullodata[i + 2] == 0x12)
epoch[epochnum].start = i, statu = Frame_IN, headerlen = 28;
if (i < fcount - 2 && fullodata[i] == 0xaa && fullodata[i + 1] == 0x44 && fullodata[i + 2] == 0xb5)
epoch[epochnum].start = i, statu = Frame_IN, headerlen = 24;
if (statu == Frame_IN && headerlen == 28) {
count++;//记录
if (count == 5) messagesID[0] = fullodata[i];
if (count == 6) messagesID[1] = fullodata[i];
if (count == 9) messagesLen[0] = fullodata[i];
if (count == 10) messagesLen[1] = fullodata[i];
if (count > 28 && count == 28 + int(bit2ushort(messagesLen)) + 4) {
//printf("%d\n",fcount);
epoch[epochnum].end = i;
epoch[epochnum].ID = bit2ushort(messagesID);
epoch[epochnum].Len = bit2ushort(messagesLen);
epoch[epochnum].crc = getcrc(fullodata, epoch[epochnum]);
epochnum++;
statu = Frame_OUT; count = 0;
}
}
if (statu == Frame_IN && headerlen == 24) {
count++;//记录
if (count == 5) messagesID[0] = fullodata[i];
if (count == 6) messagesID[1] = fullodata[i];
if (count == 7) messagesLen[0] = fullodata[i];
if (count == 8) messagesLen[1] = fullodata[i];
if (count > 24 && count == 24 + int(bit2ushort(messagesLen)) + 4) {
//printf("%d\n",fcount);
epoch[epochnum].end = i;
epoch[epochnum].ID = bit2ushort(messagesID);
epoch[epochnum].Len = bit2ushort(messagesLen);
epoch[epochnum].crc = getcrc(fullodata, epoch[epochnum]);
epochnum++;
statu = Frame_OUT; count = 0;
}
}
}
return epochnum;
}
从数据中读取星历函数getsat
参数:星历数组eph[],数据fullodata[],消息报告epoch。
功能:根据报告epoch,从数据fullodata中读取prn号卫星的星历并存储到星历数组eph[prn]中。
特别说明:这里已经应用了星历数组,通过将星历设置为一定大小的数组,让程序通过访问对应下标来访问对应的卫星星历时本示例程序的特点。
unsigned long getsat(eph_t* eph, unsigned char* fullodata, breport epoch) {
int start = epoch.start, end = epoch.end;
unsigned long prn = bit2ulong(fullodata + start + 28);
eph[prn].svh = int(bit2ulong(fullodata + start + 28 + 12));
eph[prn].week = int(bit2ulong(fullodata + start + 28 + 24));
eph[prn].toes = bit2double(fullodata + start + 28 + 32);
eph[prn].A = bit2double(fullodata + start + 28 + 40);
eph[prn].deln = bit2double(fullodata + start + 28 + 48);
eph[prn].M0 = bit2double(fullodata + start + 28 + 56);
eph[prn].e = bit2double(fullodata + start + 28 + 64);
eph[prn].omg = bit2double(fullodata + start + 28 + 72);
eph[prn].cuc = bit2double(fullodata + start + 28 + 80);
eph[prn].cus = bit2double(fullodata + start + 28 + 88);
eph[prn].crc = bit2double(fullodata + start + 28 + 96);
eph[prn].crs = bit2double(fullodata + start + 28 + 104);
eph[prn].cic = bit2double(fullodata + start + 28 + 112);
eph[prn].cis = bit2double(fullodata + start + 28 + 120);
eph[prn].i0 = bit2double(fullodata + start + 28 + 128);
eph[prn].idot = bit2double(fullodata + start + 28 + 136);
eph[prn].OMG0 = bit2double(fullodata + start + 28 + 144);
eph[prn].OMGD = bit2double(fullodata + start + 28 + 152);
double etoc = bit2double(fullodata + start + 28 + 164);//卫星钟时间
eph[prn].tgd = bit2double(fullodata + start + 28 + 172);
eph[prn].af0 = bit2double(fullodata + start + 28 + 180);
eph[prn].af1 = bit2double(fullodata + start + 28 + 188);
eph[prn].af2 = bit2double(fullodata + start + 28 + 196);
eph[prn].AS = bit2ulong(fullodata + start + 28 + 204) ? true : false;
eph[prn].toe = gpst2time(eph[prn].week, eph[prn].toes);//卫星星历的参考时刻
eph[prn].toc = gpst2time(eph[prn].week, etoc);
return prn;//返回卫星prn编号
}
更新:从数据中读取原始观测值的函数getobs
参数:引用观测值结构体R,数据fullodata[],消息报告epoch。
功能:根据报告epoch,从数据fullodata中读取某个历元的原始观测值并存储到观测值结构体R中,并将观测到的卫星数量作为返回值返回。
特别说明:这里只记录观测历元的伪距原始观测值。需要注意的是,在上一个版本中,只使用了伪距观测值,但在这一版本中补齐了其他观测值。
int getobs(GPSOBS& R, unsigned char* fullodata, breport epoch) {
int start = epoch.start, end = epoch.end, snum, name[35];
double R0[35] = {};
//观测时间
COMMONTIME comt = time2epoch(gpst2time(int(breport2GPStime(epoch, fullodata).Week), breport2GPStime(epoch, fullodata).Second));
snum = bit2long(fullodata + start + 28);
R.num = snum;
GPSTime obswas = breport2GPStime(epoch, fullodata);
R.rt = gpst2time(obswas.Week, obswas.Second);
for (int j = 0; j < snum; j++)
{
//PRN码
R.name[j] = int(bit2ushort(fullodata + start + 32 + j * 44));
//观测值
R.R0[j] = bit2double(fullodata + start + 36 + j * 44);
R.psrstd[j] = bit2float(fullodata + start + 28 + 16 + j * 44);
R.adr[j] = bit2double(fullodata + start + 28 + 20 + j * 44);
R.adrstd[j] = bit2float(fullodata + start + 28 + 28 + j * 44);
R.dopp[j] = bit2float(fullodata + start + 28 + 32 + j * 44);
R.Cno[j] = bit2float(fullodata + start + 28 + 36 + j * 44);
R.loctime[j] = bit2float(fullodata + start + 28 + 40 + j * 44);
}
return snum;
}