/* decode binex mesaage 0x01-02: decoded glonass ephmemeris ------------------*/
static int decode_bnx_01_02(raw_t *raw, unsigned char *buff, int len)
{
geph_t geph={0};
unsigned char *p=buff;
double tod,tof,tau_gps;
int prn,day,leap;
trace(4,"binex 0x01-02: len=%d\n",len);
if (len>=119) {
prn =U1(p)+1; p+=1;
day =U2(p); p+=2;
tod =U4(p); p+=4;
geph.taun =-R8(p); p+=8;
geph.gamn =R8(p); p+=8;
tof =U4(p); p+=4;
geph.pos[0]=R8(p)*1E3; p+=8;
geph.vel[0]=R8(p)*1E3; p+=8;
geph.acc[0]=R8(p)*1E3; p+=8;
geph.pos[1]=R8(p)*1E3; p+=8;
geph.vel[1]=R8(p)*1E3; p+=8;
geph.acc[1]=R8(p)*1E3; p+=8;
geph.pos[2]=R8(p)*1E3; p+=8;
geph.vel[2]=R8(p)*1E3; p+=8;
geph.acc[2]=R8(p)*1E3; p+=8;
geph.svh =U1(p)&0x1; p+=1;
geph.frq =I1(p); p+=1;
geph.age =U1(p); p+=1;
leap =U1(p); p+=1;
tau_gps =R8(p); p+=8;
geph.dtaun =R8(p);
}
else {
trace(2,"binex 0x01-02: length error len=%d\n",len);
return -1;
}
if (!(geph.sat=satno(SYS_GLO,prn))) {
trace(2,"binex 0x01-02: satellite error prn=%d\n",prn);
return -1;
}
if (raw->time.time==0) return 0;
geph.toe=utc2gpst(adjday(raw->time,tod-10800.0));
geph.tof=utc2gpst(adjday(raw->time,tof-10800.0));
geph.iode=(int)(fmod(tod+10800.0,86400.0)/900.0+0.5);
if (!strstr(raw->opt,"-EPHALL")) {
if (fabs(timediff(geph.toe,raw->nav.geph[prn-MINPRNGLO].toe))<1.0&&
geph.svh==raw->nav.geph[prn-MINPRNGLO].svh) return 0; /* unchanged */
}
raw->nav.geph[prn-1]=geph;
raw->ephsat=geph.sat;
return 2;
}
/* decode binex mesaage 0x01-03: decoded sbas ephmemeris ---------------------*/
static int decode_bnx_01_03(raw_t *raw, unsigned char *buff, int len)
{
seph_t seph={0};
unsigned char *p=buff;
double tow,tod,tof;
int prn,week,iodn;
trace(4,"binex 0x01-03: len=%d\n",len);
if (len>=98) {
prn =U1(p); p+=1;
week =U2(p); p+=2;
tow =U4(p); p+=4;
seph.af0 =R8(p); p+=8;
tod =R4(p); p+=4;
tof =U4(p); p+=4;
seph.pos[0]=R8(p)*1E3; p+=8;
seph.vel[0]=R8(p)*1E3; p+=8;
seph.acc[0]=R8(p)*1E3; p+=8;
seph.pos[1]=R8(p)*1E3; p+=8;
seph.vel[1]=R8(p)*1E3; p+=8;
seph.acc[1]=R8(p)*1E3; p+=8;
seph.pos[2]=R8(p)*1E3; p+=8;
seph.vel[2]=R8(p)*1E3; p+=8;
seph.acc[2]=R8(p)*1E3; p+=8;
seph.svh =U1(p); p+=1;
seph.sva =U1(p); p+=1;
iodn =U1(p);
}
else {
trace(2,"binex 0x01-03 length error: len=%d\n",len);
return -1;
}
if (!(seph.sat=satno(SYS_SBS,prn))) {
trace(2,"binex 0x01-03 satellite error: prn=%d\n",prn);
return -1;
}
seph.t0=gpst2time(week,tow);
seph.tof=adjweek(seph.t0,tof);
if (!strstr(raw->opt,"-EPHALL")) {
if (fabs(timediff(seph.t0,raw->nav.seph[prn-MINPRNSBS].t0))<1.0&&
seph.sva==raw->nav.seph[prn-MINPRNSBS].sva) return 0; /* unchanged */
}
raw->nav.seph[prn-MINPRNSBS]=seph;
raw->ephsat=seph.sat;
return 2;
}
/* decode ublox rxm-sfrb: subframe buffer ------------------------------------*/
static int decode_rxmsfrb(raw_t *raw)
{
unsigned int words[10];
int i,prn,sat,sys,id;
unsigned char *p=raw->buff+6;
trace(4,"decode_rxmsfrb: len=%d\n",raw->len);
if (raw->len<42) {
trace(2,"ubx rxmsfrb length error: len=%d\n",raw->len);
return -1;
}
prn=U1(p+1);
if (!(sat=satno(MINPRNSBS<=prn?SYS_SBS:SYS_GPS,prn))) {
trace(2,"ubx rxmsfrb satellite number error: prn=%d\n",prn);
return -1;
}
sys=satsys(sat,&prn);
if (sys==SYS_GPS) {
id=save_subfrm(sat,raw);
if (id==3) return decode_ephem(sat,raw);
if (id==4) return decode_alm1 (sat,raw);
if (id==5) return decode_alm2 (sat,raw);
return 0;
}
else if (sys==SYS_SBS) {
for (i=0,p+=2;i<10;i++,p+=4) words[i]=U4(p);
return sbsdecodemsg(raw->time,prn,words,&raw->sbsmsg)?3:0;
}
return 0;
}
bool TTetrahedron::IsInner(const TPoint& P) const {
TVector N, U1(X1, X2), U2(X1, X3), U3(X1, X4), U4(X2, X3), U5(X2, X4), U6(X2, X1);
TVector Up1(X1, P), Up2(X2, P);
N = Cross(U1, U2); //X4 is on opposite side of plain X1,X2,X3 than P
if (Dot(N, U3) * Dot(N, Up1) < 0) {
return false;
}
N = Cross(U1, U3); //X3 x P
if (Dot(N, U2) * Dot(N, Up1) < 0) {
return false;
}
N = Cross(U2, U3); //X2 x P
if (Dot(N, U1) * Dot(N, Up1) < 0) {
return false;
}
N = Cross(U4, U5); //X1 x P
if (Dot(N, U6) * Dot(N, Up2) < 0) {
return false;
}
return true;
}
/* decode waas messages ------------------------------------------------------*/
static int decode_gw10sbs(raw_t *raw)
{
double tow;
int i,prn;
unsigned char *p=raw->buff+2;
trace(4,"decode_gw10sbs : len=%d\n",raw->len);
tow=U4(p)/1000.0;
prn=U1(p+4);
if (prn<MINPRNSBS||MAXPRNSBS<prn) {
trace(2,"gw10 sbs satellite number error: prn=%d\n",prn);
return -1;
}
raw->sbsmsg.prn=prn;
raw->sbsmsg.tow=(int)tow;
tow=time2gpst(raw->time,&raw->sbsmsg.week);
if (raw->sbsmsg.tow<tow-302400.0) raw->sbsmsg.week++;
else if (raw->sbsmsg.tow>tow+302400.0) raw->sbsmsg.week--;
for (i=0;i<29;i++) {
raw->sbsmsg.msg[i]=*(p+5+i);
}
raw->sbsmsg.msg[28]&=0xC0;
return 3;
}
/* decode solution -----------------------------------------------------------*/
static int decode_gw10sol(raw_t *raw)
{
gtime_t time;
double ep[6]={0},sec;
unsigned char *p=raw->buff+6;
trace(4,"decode_gw10sol : len=%d\n",raw->len);
if (U2(p+42)&0xC00) { /* time valid? */
trace(2,"gw10 sol time/day invalid\n");
return 0;
}
sec=U4(p+27)/16384.0;
sec=floor(sec*1000.0+0.5)/1000.0;
ep[2]=bcd2num(p[31]);
ep[1]=bcd2num(p[32]);
ep[0]=bcd2num(p[33])*100+bcd2num(p[34]);
time=utc2gpst(timeadd(epoch2time(ep),sec));
/* set time if no time available */
if (raw->time.time==0) {
raw->time=time;
}
return 0;
}
/* decode ubx-nav-cov: navigation covariance matrix -----------------------------------*/
int decode_navcov(tUbxRawData *raw)
{
UbxMsg01_36_COV cov;
unsigned char *p = raw->buff + UBX_MSGSTART_SHIFT;
memset(&cov, 0, sizeof(cov));
cov.iTOW = U4(p);
cov.version = U1(p + 4);
cov.posCovValid = U1(p + 5);
cov.velCovValid = U1(p + 6);
memcpy(cov.reserved, p + 7, 9 * sizeof(unsigned char));
cov.covP11 = MAXUBX(0, R4(p + 16));
cov.covP12 = MAXUBX(0, R4(p + 20));
cov.covP13 = MAXUBX(0, R4(p + 24));
cov.covP22 = MAXUBX(0, R4(p + 28));
cov.covP23 = MAXUBX(0, R4(p + 32));
cov.covP33 = MAXUBX(0, R4(p + 36));
cov.covV11 = MAXUBX(0, R4(p + 40));
cov.covV12 = MAXUBX(0, R4(p + 44));
cov.covV13 = MAXUBX(0, R4(p + 48));
cov.covV22 = MAXUBX(0, R4(p + 52));
cov.covV23 = MAXUBX(0, R4(p + 56));
cov.covV33 = MAXUBX(0, R4(p + 60));
/* fill up interface for real time processing */
pb_Set_ExternalUbxGNSS_COV(&cov);
return 0;
}
/* decode binex mesaage 0x7f: gnss data prototyping --------------------------*/
static int decode_bnx_7f(raw_t *raw, unsigned char *buff, int len)
{
const static double gpst0[]={1980,1,6,0,0,0};
char *msg;
unsigned char *p=buff;
unsigned int srec,min,msec;
srec=U1(p); p+=1; /* subrecord id */
min =U4(p); p+=4;
msec=U2(p); p+=2;
raw->time=timeadd(epoch2time(gpst0),min*60.0+msec*0.001);
if (raw->outtype) {
msg=raw->msgtype+strlen(raw->msgtype);
sprintf(msg," subrec=%02X time%s",srec,time_str(raw->time,3));
}
switch (srec) {
case 0x00: return decode_bnx_7f_00(raw,buff+7,len-7);
case 0x01: return decode_bnx_7f_01(raw,buff+7,len-7);
case 0x02: return decode_bnx_7f_02(raw,buff+7,len-7);
case 0x03: return decode_bnx_7f_03(raw,buff+7,len-7);
case 0x04: return decode_bnx_7f_04(raw,buff+7,len-7);
case 0x05: return decode_bnx_7f_05(raw,buff+7,len-7);
}
return 0;
}
Code::Code(AttributeInfo *code) {
unsigned char *data = code->data;
int index = 0;
U2(maxStack,data,index);
U2(maxLocals,data,index);
U4(codeLength,data,index);
this->code = new unsigned char[codeLength];
for (int i = 0; i < codeLength; i++) {
this->code[i] = data[index+i];
}
index += codeLength;
U2(exceptionCount,data,index);
exceptions = new ExceptionEntry*[exceptionCount];
for (int i = 0; i < exceptionCount; i++) {
exceptions[i] = new ExceptionEntry;
U2(exceptions[i]->startPC,data,index);
U2(exceptions[i]->endPC,data,index);
U2(exceptions[i]->handlerPC,data,index);
U2(exceptions[i]->catchType,data,index);
}
U2(attributeCount,data,index);
attributes = new AttributeInfo*[attributeCount];
for (int i = 0; i < attributeCount; i++)
attributes[i] = new AttributeInfo(data, index);
}
AttributeInfo::AttributeInfo(unsigned char *data, int &index) {
U2(nameIndex,data,index);
U4(length,data,index);
this->data = new unsigned char[length];
for (int i = 0; i < length; i++)
this->data[i] = data[i+index];
index += length;
}
/* decode binex mesaage 0x00: site/monument/marker/ref point/setup metadata --*/
static int decode_bnx_00(raw_t *raw, unsigned char *buff, int len)
{
const static double gpst0[]={1980,1,6,0,0,0};
char *msg;
unsigned char *p=buff;
unsigned int min,qsec,src,fid;
int n=6;
min =U4(p); p+=4;
qsec=U1(p); p+=1;
src =U1(p); p+=1;
n+=getbnxi(p,&fid);
raw->time=timeadd(epoch2time(gpst0),min*60.0+qsec*0.25);
if (raw->outtype) {
msg=raw->msgtype+strlen(raw->msgtype);
sprintf(msg," fid=%02X time=%s src=%d",fid,time_str(raw->time,0),src);
}
switch (fid) {
case 0x00: return decode_bnx_00_00(raw,buff+n,len-n);
case 0x01: return decode_bnx_00_01(raw,buff+n,len-n);
case 0x02: return decode_bnx_00_02(raw,buff+n,len-n);
case 0x03: return decode_bnx_00_03(raw,buff+n,len-n);
case 0x04: return decode_bnx_00_04(raw,buff+n,len-n);
case 0x05: return decode_bnx_00_05(raw,buff+n,len-n);
case 0x06: return decode_bnx_00_06(raw,buff+n,len-n);
case 0x07: return decode_bnx_00_07(raw,buff+n,len-n);
case 0x08: return decode_bnx_00_08(raw,buff+n,len-n);
case 0x09: return decode_bnx_00_09(raw,buff+n,len-n);
case 0x0A: return decode_bnx_00_0a(raw,buff+n,len-n);
case 0x0B: return decode_bnx_00_0b(raw,buff+n,len-n);
case 0x0C: return decode_bnx_00_0c(raw,buff+n,len-n);
case 0x0D: return decode_bnx_00_0d(raw,buff+n,len-n);
case 0x0E: return decode_bnx_00_0e(raw,buff+n,len-n);
case 0x0F: return decode_bnx_00_0f(raw,buff+n,len-n);
case 0x10: return decode_bnx_00_10(raw,buff+n,len-n);
case 0x11: return decode_bnx_00_11(raw,buff+n,len-n);
case 0x12: return decode_bnx_00_12(raw,buff+n,len-n);
case 0x13: return decode_bnx_00_13(raw,buff+n,len-n);
case 0x14: return decode_bnx_00_14(raw,buff+n,len-n);
case 0x15: return decode_bnx_00_15(raw,buff+n,len-n);
case 0x16: return decode_bnx_00_16(raw,buff+n,len-n);
case 0x17: return decode_bnx_00_17(raw,buff+n,len-n);
case 0x18: return decode_bnx_00_18(raw,buff+n,len-n);
case 0x19: return decode_bnx_00_19(raw,buff+n,len-n);
case 0x1A: return decode_bnx_00_1a(raw,buff+n,len-n);
case 0x1B: return decode_bnx_00_1b(raw,buff+n,len-n);
case 0x1C: return decode_bnx_00_1c(raw,buff+n,len-n);
case 0x1D: return decode_bnx_00_1d(raw,buff+n,len-n);
case 0x1E: return decode_bnx_00_1e(raw,buff+n,len-n);
case 0x1F: return decode_bnx_00_1f(raw,buff+n,len-n);
case 0x20: return decode_bnx_00_20(raw,buff+n,len-n);
case 0x21: return decode_bnx_00_21(raw,buff+n,len-n);
case 0x22: return decode_bnx_00_22(raw,buff+n,len-n);
case 0x7F: return decode_bnx_00_7f(raw,buff+n,len-n);
}
return 0;
}
static IDirectDrawSurface7 *create_overlay(DWORD width, DWORD height, DWORD format) {
DDSURFACEDESC2 ddsd;
HRESULT hr;
IDirectDrawSurface7 *ret;
memset(&ddsd, 0, sizeof(ddsd));
ddsd.dwSize = sizeof(ddsd);
ddsd.dwFlags = DDSD_CAPS | DDSD_HEIGHT | DDSD_WIDTH | DDSD_PIXELFORMAT;
ddsd.dwWidth = width;
ddsd.dwHeight = height;
ddsd.ddsCaps.dwCaps = DDSCAPS_OVERLAY;
U4(ddsd).ddpfPixelFormat.dwSize = sizeof(U4(ddsd).ddpfPixelFormat);
U4(ddsd).ddpfPixelFormat.dwFlags = DDPF_FOURCC;
U4(ddsd).ddpfPixelFormat.dwFourCC = format;
hr = IDirectDraw7_CreateSurface(ddraw, &ddsd, &ret, NULL);
if(FAILED(hr)) return NULL;
else return ret;
}
/* decode ubx-nav-clk: navigation clock -----------------------------------*/
int decode_navclock(tUbxRawData *raw)
{
UbxMsg01_22_CLK clk;
unsigned char *p = raw->buff + UBX_MSGSTART_SHIFT;
memset(&clk, 0, sizeof(clk));
clk.iTOW = U4(p);
clk.clkBias = I4(p + 4);
clk.clkDrift = I4(p + 8);
clk.unc_clkBias = U4(p + 12);
clk.unc_clkDrift = U4(p + 16);
/* fill up interface for real time processing */
pb_Set_ExternalUbxGNSS_CLK(&clk);
return 0;
}
void SubInv(double *v)
{
double temp1 = U1(v[0],v[1],v[2],v[3]);
double temp2 = U2(v[0],v[1],v[2],v[3]);
double temp3 = U3(v[0],v[1],v[2],v[3]);
double temp4 = U4(v[0],v[1],v[2],v[3]);
v[0] = temp1;
v[1] = temp2;
v[2] = temp3;
v[3] = temp4;
}
/* decode id#67 sbas data ----------------------------------------------------*/
static int decode_ss2sbas(raw_t *raw)
{
gtime_t time;
int i,prn;
unsigned char *p=raw->buff+4;
trace(4,"decode_ss2sbas: len=%d\n",raw->len);
if (raw->len!=54) {
trace(2,"ss2 id#67 length error: len=%d\n",raw->len);
return -1;
}
prn=U4(p+12);
if (prn<MINPRNSBS||MAXPRNSBS<prn) return 0;
raw->sbsmsg.week=U4(p);
raw->sbsmsg.tow=(int)R8(p+4);
time=gpst2time(raw->sbsmsg.week,raw->sbsmsg.tow);
raw->sbsmsg.prn=prn;
for (i=0;i<29;i++) raw->sbsmsg.msg[i]=p[16+i];
return 3;
}
void SubInv(int *v, int m)
{
int temp1 = U1(v[0],v[1],v[2],v[3], m);
int temp2 = U2(v[0],v[1],v[2],v[3], m);
int temp3 = U3(v[0],v[1],v[2],v[3], m);
int temp4 = U4(v[0],v[1],v[2],v[3], m);
v[0] = temp1;
v[1] = temp2;
v[2] = temp3;
v[3] = temp4;
}
int decode_navgpstime(tUbxRawData *raw)
{
unsigned char *p = raw->buff + UBX_MSGSTART_SHIFT;
tExternalGNSSGPSTimePtr gpstime = Msg_Get_ExternalGnssGpsTime();
/* do not reset this struct due to week and leapsecond */
gpstime->iTow = U4(p);
gpstime->fTow = I4(p+4);
gpstime->gpsWeek = (short)(I2(p + 8));
gpstime->leapSec = (short)(I1(p + 10));
return 0;
}
/* decode NVS rxm-sfrb: subframe buffer --------------------------------------*/
static int decode_xe5bit(raw_t *raw)
{
int prn;
int iBlkStartIdx, iExpLen, iIdx;
unsigned int words[10];
unsigned char uiDataBlocks, uiDataType;
unsigned char *p=raw->buff;
trace(4,"decode_xe5bit: len=%d\n",raw->len);
p += 2; /* Discard preamble and message identifier */
uiDataBlocks = U1(p);
if (uiDataBlocks>=16) {
trace(2,"nvs xf5bit message error: data blocks %u\n", uiDataBlocks);
return -1;
}
iBlkStartIdx = 1;
for (iIdx = 0; iIdx < uiDataBlocks; iIdx++) {
iExpLen = (iBlkStartIdx+10);
if ((raw->len) < iExpLen) {
trace(2,"nvs xf5bit message too short (expected at least %d)\n", iExpLen);
return -1;
}
uiDataType = U1(p+iBlkStartIdx+1);
switch (uiDataType) {
case 1: /* Glonass */
iBlkStartIdx += 19;
break;
case 2: /* GPS */
iBlkStartIdx += 47;
break;
case 4: /* SBAS */
prn = U1(p+(iBlkStartIdx+2)) + 120;
/* sat = satno(SYS_SBS, prn); */
/* sys = satsys(sat,&prn); */
memset(words, 0, 10*sizeof(unsigned int));
for (iIdx=0, iBlkStartIdx+=7; iIdx<10; iIdx++, iBlkStartIdx+=4) {
words[iIdx]=U4(p+iBlkStartIdx);
}
words[7] >>= 6;
return sbsdecodemsg(raw->time,prn,words,&raw->sbsmsg) ? 3 : 0;
default:
trace(2,"nvs xf5bit SNS type unknown (got %d)\n", uiDataType);
return -1;
}
}
return 0;
}
/* decode skytraq measurement time info --------------------------------------*/
static int decode_stqtime(raw_t *raw)
{
unsigned char *p=raw->buff+4;
double tow;
int week;
trace(4,"decode_stqtime: len=%d\n",raw->len);
raw->iod=U1(p+1);
week =U2(p+2);
tow =U4(p+4)*0.001;
raw->time=gpst2time(week,tow);
return 0;
}
/* decode raw obs data -------------------------------------------------------*/
static int decode_gw10raw(raw_t *raw)
{
double tow,tows,toff,pr,cp;
int i,j,n,prn,flg,sat,snr;
unsigned char *p=raw->buff+2;
trace(4,"decode_gw10raw: len=%d\n",raw->len);
tow=R8(p);
tows=floor(tow*1000.0+0.5)/1000.0; /* round by 10ms */
toff=CLIGHT*(tows-tow); /* time tag offset (m) */
if (!adjweek(raw,tows)) {
trace(2,"decode_gw10raw: no gps week infomation\n");
return 0;
}
for (i=n=0,p+=8;i<16&&n<MAXOBS;i++,p+=23) {
if (U1(p+1)!=1) continue;
prn=U1(p);
if (!(sat=satno(prn<=MAXPRNGPS?SYS_GPS:SYS_SBS,prn))) {
trace(2,"gw10raw satellite number error: prn=%d\n",prn);
continue;
}
pr =R8(p+ 2)-toff;
snr=U2(p+16);
cp =-(int)(U4(p+18))/256.0-toff/lam_carr[0];
flg=U1(p+22);
if (flg&0x3) {
trace(2,"gw10raw raw data invalid: prn=%d\n",prn);
continue;
}
raw->obs.data[n].time=raw->time;
raw->obs.data[n].sat =sat;
raw->obs.data[n].P[0]=pr;
raw->obs.data[n].L[0]=(flg&0x80)?0.0:((flg&0x40)?cp-0.5:cp);
raw->obs.data[n].D[0]=0.0;
raw->obs.data[n].SNR[0]=(unsigned char)(snr*4.0+0.5);
raw->obs.data[n].LLI[0]=(flg&0x80)?1:0;
raw->obs.data[n].code[0]=CODE_L1C;
for (j=1;j<NFREQ;j++) {
raw->obs.data[n].L[j]=raw->obs.data[n].P[j]=0.0;
raw->obs.data[n].D[j]=0.0;
raw->obs.data[n].SNR[j]=raw->obs.data[n].LLI[j]=0;
raw->obs.data[n].code[j]=CODE_NONE;
}
n++;
}
raw->obs.n=n;
return 1;
}
/* decode ubx-nav-pvt: navigation pvt -----------------------------------*/
int decode_navpvt(tUbxRawData *raw)
{
UbxMsg01_07_PVT pvt;
unsigned char *p = raw->buff + UBX_MSGSTART_SHIFT;
memset(&pvt, 0, sizeof(pvt));
pvt.iTOW = U4(p);
pvt.year = U2(p+4);
pvt.month = U1(p+6);
pvt.day = U1(p+7);
pvt.hour = U1(p+8);
pvt.min = U1(p+9);
pvt.sec = U1(p+10);
pvt.bitfield = U1(p+11);
pvt.timeUnc = U4(p+12);
pvt.nano = I4(p+16);
pvt.fixType = U1(p+20);
pvt.flags = U1(p+21);
pvt.flags2 = U1(p+22);
pvt.numSV = U1(p+23);
pvt.Lon = I4(p+24);
pvt.Lat = I4(p+28);
pvt.Alt = I4(p+32);
pvt.AltMsl = I4(p+36);
pvt.hErr = U4(p+40);
pvt.vErr = U4(p+44);
pvt.Vn = I4(p+48);
pvt.Ve = I4(p+52);
pvt.Vd = I4(p+56);
pvt.groundSpeed = I4(p+60);
pvt.heading = I4(p+64);
pvt.spdErr = U4(p+68);
pvt.headingErr = U4(p+72);
pvt.PDOP = U2(p+76);
memcpy(pvt.reserved1, p+78, 6*sizeof(unsigned char));
pvt.headVel = I4(p+84);
memcpy(pvt.reserved2, p+88, 4*sizeof(unsigned char));
/* fill up interface for real time processing */
pb_Set_ExternalUbxGNSS_PVT(&pvt);
/*----- validate PPS -----*/
pb_Validate_Ubx_PPS(&pvt);
return 0;
}
static void offscreen_test(void) {
IDirectDrawSurface7 *overlay = create_overlay(64, 64, MAKEFOURCC('U','Y','V','Y')), *offscreen = NULL;
HRESULT hr;
DDSURFACEDESC2 ddsd;
/* Try to overlay a NULL surface */
hr = IDirectDrawSurface7_UpdateOverlay(overlay, NULL, NULL, NULL, DDOVER_SHOW, NULL);
ok(hr == DDERR_INVALIDPARAMS, "IDirectDrawSurface7_UpdateOverlay failed with hr=0x%08x\n", hr);
hr = IDirectDrawSurface7_UpdateOverlay(overlay, NULL, NULL, NULL, DDOVER_HIDE, NULL);
ok(hr == DDERR_INVALIDPARAMS, "IDirectDrawSurface7_UpdateOverlay failed with hr=0x%08x\n", hr);
/* Try to overlay an offscreen surface */
memset(&ddsd, 0, sizeof(ddsd));
ddsd.dwSize = sizeof(ddsd);
ddsd.dwFlags = DDSD_CAPS | DDSD_HEIGHT | DDSD_WIDTH | DDSD_PIXELFORMAT;
ddsd.dwWidth = 64;
ddsd.dwHeight = 64;
U4(ddsd).ddpfPixelFormat.dwSize = sizeof(U4(ddsd).ddpfPixelFormat);
U4(ddsd).ddpfPixelFormat.dwFlags = DDPF_RGB;
U4(ddsd).ddpfPixelFormat.dwFourCC = 0;
U1(U4(ddsd).ddpfPixelFormat).dwRGBBitCount = 16;
U2(U4(ddsd).ddpfPixelFormat).dwRBitMask = 0xF800;
U3(U4(ddsd).ddpfPixelFormat).dwGBitMask = 0x07e0;
U4(U4(ddsd).ddpfPixelFormat).dwBBitMask = 0x001F;
ddsd.ddsCaps.dwCaps = DDSCAPS_OFFSCREENPLAIN;
hr = IDirectDraw7_CreateSurface(ddraw, &ddsd, &offscreen, NULL);
ok(hr == DD_OK, "IDirectDraw7_CreateSurface failed with hr=0x%08x\n", hr);
hr = IDirectDrawSurface7_UpdateOverlay(overlay, NULL, offscreen, NULL, DDOVER_SHOW, NULL);
ok(hr == DD_OK || broken(hr == E_NOTIMPL),
"IDirectDrawSurface7_UpdateOverlay failed with hr=0x%08x\n", hr);
/* Try to overlay the primary with a non-overlay surface */
hr = IDirectDrawSurface7_UpdateOverlay(offscreen, NULL, primary, NULL, DDOVER_SHOW, NULL);
ok(hr == DDERR_NOTAOVERLAYSURFACE, "IDirectDrawSurface7_UpdateOverlay failed with hr=0x%08x\n", hr);
IDirectDrawSurface7_Release(offscreen);
IDirectDrawSurface7_Release(overlay);
}
/* decode id#22 ephemeris data ------------------------------------------------*/
static int decode_ss2eph(raw_t *raw)
{
eph_t eph={0};
unsigned int tow;
int i,j,prn,sat;
unsigned char *p=raw->buff+4,buff[90]={0};
trace(4,"decode_ss2eph: len=%d\n",raw->len);
if (raw->len!=79) {
trace(2,"ss2 id#22 length error: len=%d\n",raw->len);
return -1;
}
prn=(U4(p)&0x1F)+1;
if (!(sat=satno(SYS_GPS,prn))) {
trace(2,"ss2 id#22 satellite number error: prn=%d\n",prn);
return -1;
}
if (raw->time.time==0) {
trace(2,"ss2 id#22 week number unknown error\n");
return -1;
}
tow=(unsigned int)(time2gpst(raw->time,NULL)/6.0);
for (i=0;i<3;i++) {
buff[30*i+3]=(unsigned char)(tow>>9); /* add tow + subframe id */
buff[30*i+4]=(unsigned char)(tow>>1);
buff[30*i+5]=(unsigned char)(((tow&1)<<7)+((i+1)<<2));
for (j=0;j<24;j++) buff[30*i+6+j]=p[1+24*i+j];
}
if (decode_frame(buff ,&eph,NULL,NULL,NULL,NULL)!=1||
decode_frame(buff+30,&eph,NULL,NULL,NULL,NULL)!=2||
decode_frame(buff+60,&eph,NULL,NULL,NULL,NULL)!=3) {
trace(2,"ss2 id#22 subframe error: prn=%d\n",prn);
return -1;
}
if (eph.iode==raw->nav.eph[sat-1].iode) return 0; /* unchanged */
eph.sat=sat;
eph.ttr=raw->time;
raw->nav.eph[sat-1]=eph;
raw->ephsat=sat;
return 2;
}
/* decode ubx-nav-sat: navigation channel satellite information ---------------------*/
int decode_navsat(tUbxRawData *raw)
{
unsigned int tow, bitmask;
short nsat, i;
short j;
unsigned char gnssID, flag1;
tExternalGNSS_AuxMeas navauxmeas[UBX_MAX_NUMCHAN]; /* ubx raw channels up to 32, only pick up 20 with stronger signals */
tExternalGNSS_AuxMeas tempauxmeas;
tExternalGNSS_AuxMeasPtr readyAuxMeas = Msg_Get_ExternalGnssAuxMeas();
unsigned char *p = raw->buff + UBX_MSGSTART_SHIFT;
memset(navauxmeas, 0, UBX_MAX_NUMCHAN*sizeof(tExternalGNSS_AuxMeas));
memset(&tempauxmeas, 0, sizeof(tExternalGNSS_AuxMeas));
memset(readyAuxMeas, 0, MAX_GNSS_NUM_CHANS * sizeof(tExternalGNSS_AuxMeas));
tow = U4(p);
nsat = (short)U1(p + 5);
nsat = MINUBX(nsat, UBX_MAX_NUMCHAN);
if(nsat == 0)
return 0;
for (i = 0, p += 8; i < nsat; i++, p += 12)
{
navauxmeas[i].measTOW = (unsigned int)(tow); /* same software time for all channels */
gnssID = (unsigned char)(U1(p));
navauxmeas[i].svid = (unsigned short)(U1(p + 1));
/* gnssID: 0 -- GPS, keep the same ID, 1 -- SBAS, keep the same ID */
if (gnssID == GALILEO_SYS_ID) /* Galileo */
navauxmeas[i].svid += GALILEO_LOW_SVID;
else if (gnssID == BDS_SYS_ID) /* BDS */
navauxmeas[i].svid += BDS_LOW_SVID;
else if (gnssID == GLONASS_SYS_ID) /* GLONASS */
navauxmeas[i].svid += GLONASS_LOW_SVID; /* 65 ~ 96 */
else if (gnssID == IMES_SYS_ID)
navauxmeas[i].svid += IMES_LOW_SVID; /* IMES */
else if (gnssID == QZSS_SYS_ID) /* QZSS */
navauxmeas[i].svid += QZSS_LOW_SVID;
navauxmeas[i].Cn0 = (unsigned short)(U1(p + 2) * 10);
navauxmeas[i].elevation = I1(p + 3);
navauxmeas[i].azimuth = (short)(I2(p + 4));
navauxmeas[i].prResidual = (int)(I2(p + 6) * 10); /* in cm */
bitmask = U4(p + 8);
flag1 = (unsigned char)(bitmask & 0x00000007);
if (flag1 <= 2)
navauxmeas[i].syncFlag = UBXGNSS_MEASUREMENT_STATE_UNKNOWN;
if (flag1 >= 4)
{
navauxmeas[i].syncFlag = (UBXGNSS_MEASUREMENT_STATE_CODE_LOCK | UBXGNSS_MEASUREMENT_STATE_BIT_SYNC |
UBXGNSS_MEASUREMENT_STATE_SUBFRAME_SYNC | UBXGNSS_MEASUREMENT_STATE_TOW_DECODED);
if (flag1 >= 6)
navauxmeas[i].syncFlag |= UBXGNSS_MEASUREMENT_STATE_CARRIER_PULLIN;
}
flag1 = (unsigned char)(bitmask & 0x00000008);
if (flag1 == 0x08)
navauxmeas[i].svUsed = 1;
else
navauxmeas[i].svUsed = 0;
}
/*------ sort channels in ascending order according to signal level -----*/
for (j = nsat - 1; j > 0; j--)
{
for (i = 0; i < j; i++)
{
if ( navauxmeas[i+1].Cn0 > navauxmeas[i].Cn0 ) /* sorting by signal strength */
{
memcpy(&tempauxmeas, &navauxmeas[i], sizeof(tExternalGNSS_AuxMeas));
memcpy(&navauxmeas[i], &navauxmeas[i+1], sizeof(tExternalGNSS_AuxMeas));
memcpy(&navauxmeas[i+1], &tempauxmeas, sizeof(tExternalGNSS_AuxMeas));
}
}
}
/* get the first 20 (or less) channels, and pass to gluelayer */
nsat = MINUBX(nsat, MAX_GNSS_NUM_CHANS);
for (i = 0; i < nsat; i++)
{
memcpy(&readyAuxMeas[i], &navauxmeas[i], sizeof(tExternalGNSS_AuxMeas));
}
return 0;
}
static void enumdisplaymodes(void)
{
DDSURFACEDESC ddsd;
HRESULT rc;
int count, refresh_count;
ZeroMemory(&ddsd, sizeof(DDSURFACEDESC));
ddsd.dwSize = sizeof(DDSURFACEDESC);
ddsd.dwFlags = DDSD_CAPS;
ddsd.ddsCaps.dwCaps = DDSCAPS_PRIMARYSURFACE;
/* Flags parameter is reserved in very old ddraw versions (3 and older?) and must be 0 */
rc = IDirectDraw_EnumDisplayModes(lpDD, 0, &ddsd, 0, enummodescallback);
ok(rc==DD_OK, "EnumDisplayModes returned: %x\n",rc);
count = modes16bpp_cnt;
modes16bpp_cnt = 0;
ddsd.dwFlags = DDSD_PIXELFORMAT;
ddsd.ddpfPixelFormat.dwFlags = DDPF_RGB;
U1(ddsd.ddpfPixelFormat).dwRGBBitCount = 16;
U2(ddsd.ddpfPixelFormat).dwRBitMask = 0xf800;
U3(ddsd.ddpfPixelFormat).dwGBitMask = 0x07e0;
U4(ddsd.ddpfPixelFormat).dwBBitMask = 0x001F;
rc = IDirectDraw_EnumDisplayModes(lpDD, 0, &ddsd, 0, enummodescallback_16bit);
ok(rc==DD_OK, "EnumDisplayModes returned: %x\n",rc);
ok(modes16bpp_cnt == count, "Expected %d modes got %d\n", count, modes16bpp_cnt);
modes16bpp_cnt = 0;
U2(ddsd.ddpfPixelFormat).dwRBitMask = 0x0000;
U3(ddsd.ddpfPixelFormat).dwGBitMask = 0x0000;
U4(ddsd.ddpfPixelFormat).dwBBitMask = 0x0000;
rc = IDirectDraw_EnumDisplayModes(lpDD, 0, &ddsd, 0, enummodescallback_16bit);
ok(rc==DD_OK, "EnumDisplayModes returned: %x\n",rc);
ok(modes16bpp_cnt == count, "Expected %d modes got %d\n", count, modes16bpp_cnt);
modes16bpp_cnt = 0;
U2(ddsd.ddpfPixelFormat).dwRBitMask = 0xF0F0;
U3(ddsd.ddpfPixelFormat).dwGBitMask = 0x0F00;
U4(ddsd.ddpfPixelFormat).dwBBitMask = 0x000F;
rc = IDirectDraw_EnumDisplayModes(lpDD, 0, &ddsd, 0, enummodescallback_16bit);
ok(rc==DD_OK, "EnumDisplayModes returned: %x\n",rc);
ok(modes16bpp_cnt == count, "Expected %d modes got %d\n", count, modes16bpp_cnt);
modes16bpp_cnt = 0;
ddsd.ddpfPixelFormat.dwFlags = DDPF_YUV;
rc = IDirectDraw_EnumDisplayModes(lpDD, 0, &ddsd, 0, enummodescallback_16bit);
ok(rc==DD_OK,"EnumDisplayModes returned: %x\n",rc);
ok(modes16bpp_cnt == count, "Expected %d modes got %d\n", count, modes16bpp_cnt);
modes16bpp_cnt = 0;
ddsd.ddpfPixelFormat.dwFlags = DDPF_PALETTEINDEXED8;
rc = IDirectDraw_EnumDisplayModes(lpDD, 0, &ddsd, 0, enummodescallback_16bit);
ok(rc==DD_OK,"EnumDisplayModes returned: %x\n",rc);
ok(modes16bpp_cnt == count, "Expected %d modes got %d\n", count, modes16bpp_cnt);
modes16bpp_cnt = 0;
ddsd.dwFlags = DDSD_PIXELFORMAT;
ddsd.ddpfPixelFormat.dwFlags = 0;
rc = IDirectDraw_EnumDisplayModes(lpDD, 0, &ddsd, 0, enummodescallback_16bit);
ok(rc==DD_OK,"EnumDisplayModes returned: %x\n",rc);
ok(modes16bpp_cnt == count, "Expected %d modes got %d\n", count, modes16bpp_cnt);
modes16bpp_cnt = 0;
ddsd.dwFlags = 0;
rc = IDirectDraw_EnumDisplayModes(lpDD, 0, &ddsd, 0, enummodescallback_count);
ok(rc==DD_OK,"EnumDisplayModes returned: %x\n",rc);
ok(modes16bpp_cnt == modes_cnt, "Expected %d modes got %d\n", modes_cnt, modes16bpp_cnt);
modes16bpp_cnt = 0;
ddsd.dwFlags = DDSD_PIXELFORMAT | DDSD_PITCH;
U1(ddsd).lPitch = 123;
rc = IDirectDraw_EnumDisplayModes(lpDD, 0, &ddsd, 0, enummodescallback_16bit);
ok(rc==DD_OK,"EnumDisplayModes returned: %x\n",rc);
ok(modes16bpp_cnt == count, "Expected %d modes got %d\n", count, modes16bpp_cnt);
modes16bpp_cnt = 0;
ddsd.dwFlags = DDSD_PIXELFORMAT | DDSD_REFRESHRATE;
U2(ddsd).dwRefreshRate = 1;
rc = IDirectDraw_EnumDisplayModes(lpDD, 0, &ddsd, 0, enummodescallback_16bit);
ok(rc==DD_OK,"EnumDisplayModes returned: %x\n",rc);
ok(modes16bpp_cnt == 0, "Expected 0 modes got %d\n", modes16bpp_cnt);
modes16bpp_cnt = 0;
ddsd.dwFlags = DDSD_PIXELFORMAT;
rc = IDirectDraw_EnumDisplayModes(lpDD, DDEDM_REFRESHRATES, &ddsd, 0, enummodescallback_16bit);
if(rc == DDERR_INVALIDPARAMS)
{
skip("Ddraw version too old. Skipping.\n");
return;
}
ok(rc==DD_OK,"EnumDisplayModes returned: %x\n",rc);
refresh_count = refresh_rate_cnt;
if(refresh_rate)
{
modes16bpp_cnt = 0;
ddsd.dwFlags = DDSD_PIXELFORMAT | DDSD_REFRESHRATE;
U2(ddsd).dwRefreshRate = refresh_rate;
rc = IDirectDraw_EnumDisplayModes(lpDD, 0, &ddsd, 0, enummodescallback_16bit);
ok(rc==DD_OK,"EnumDisplayModes returned: %x\n",rc);
ok(modes16bpp_cnt == refresh_count, "Expected %d modes got %d\n", refresh_count, modes16bpp_cnt);
}
rc = IDirectDraw_EnumDisplayModes(lpDD, 0, NULL, 0, enummodescallback);
ok(rc==DD_OK, "EnumDisplayModes returned: %x\n",rc);
}
static void test_ddraw_objects(void)
{
HRESULT hr;
unsigned long ref;
IDirectDraw7 *DDraw7;
IDirectDraw4 *DDraw4;
IDirectDraw2 *DDraw2;
IDirectDraw *DDraw1;
IDirectDrawPalette *palette;
IDirectDrawSurface7 *surface;
IDirectDrawSurface *surface1;
IDirectDrawSurface4 *surface4;
PALETTEENTRY Table[256];
DDSURFACEDESC2 ddsd;
hr = pDirectDrawCreateEx(NULL, (void **) &DDraw7, &IID_IDirectDraw7, NULL);
ok(hr == DD_OK || hr==DDERR_NODIRECTDRAWSUPPORT, "DirectDrawCreateEx returned: %x\n", hr);
if(!DDraw7)
{
trace("Couldn't create DDraw interface, skipping tests\n");
return;
}
hr = IDirectDraw7_QueryInterface(DDraw7, &IID_IDirectDraw4, (void **) &DDraw4);
ok(hr == DD_OK, "IDirectDraw7_QueryInterface returned %08x\n", hr);
hr = IDirectDraw7_QueryInterface(DDraw7, &IID_IDirectDraw2, (void **) &DDraw2);
ok(hr == DD_OK, "IDirectDraw7_QueryInterface returned %08x\n", hr);
hr = IDirectDraw7_QueryInterface(DDraw7, &IID_IDirectDraw, (void **) &DDraw1);
ok(hr == DD_OK, "IDirectDraw7_QueryInterface returned %08x\n", hr);
ref = getRefcount( (IUnknown *) DDraw7);
ok(ref == 1, "Got refcount %ld, expected 1\n", ref);
/* Fails without a cooplevel */
hr = IDirectDraw7_CreatePalette(DDraw7, DDPCAPS_ALLOW256 | DDPCAPS_8BIT, Table, &palette, NULL);
ok(hr == DDERR_NOCOOPERATIVELEVELSET, "CreatePalette returned %08x\n", hr);
/* This check is before the cooplevel check */
hr = IDirectDraw7_CreatePalette(DDraw7, DDPCAPS_ALLOW256 | DDPCAPS_8BIT, Table, &palette, (void *) 0xdeadbeef);
ok(hr == CLASS_E_NOAGGREGATION, "CreatePalette returned %08x\n", hr);
hr = IDirectDraw7_SetCooperativeLevel(DDraw7, 0, DDSCL_NORMAL);
ok(hr == DD_OK, "SetCooperativeLevel failed with %08x\n", hr);
memset(&ddsd, 0, sizeof(ddsd));
ddsd.dwSize = sizeof(ddsd);
ddsd.dwFlags = DDSD_CAPS | DDSD_WIDTH | DDSD_HEIGHT | DDSD_PIXELFORMAT;
ddsd.dwWidth = 64;
ddsd.dwHeight = 64;
ddsd.ddsCaps.dwCaps = DDSCAPS_OFFSCREENPLAIN;
U4(ddsd).ddpfPixelFormat.dwSize = sizeof(U4(ddsd).ddpfPixelFormat);
U4(ddsd).ddpfPixelFormat.dwFlags = DDPF_PALETTEINDEXED8 | DDPF_RGB;
U1(U4(ddsd).ddpfPixelFormat).dwRGBBitCount = 8;
hr = IDirectDraw7_CreateSurface(DDraw7, &ddsd, &surface, NULL);
ok(hr == DD_OK, "CreateSurface failed with %08x\n", hr);
/* DDraw refcount increased by 1 */
ref = getRefcount( (IUnknown *) DDraw7);
ok(ref == 2, "Got refcount %ld, expected 2\n", ref);
/* Surface refcount starts with 1 */
ref = getRefcount( (IUnknown *) surface);
ok(ref == 1, "Got refcount %ld, expected 1\n", ref);
hr = IDirectDraw7_CreatePalette(DDraw7, DDPCAPS_ALLOW256 | DDPCAPS_8BIT, Table, &palette, NULL);
ok(hr == DD_OK, "CreatePalette returned %08x\n", hr);
/* DDraw refcount increased by 1 */
ref = getRefcount( (IUnknown *) DDraw7);
ok(ref == 3, "Got refcount %ld, expected 3\n", ref);
/* Palette starts with 1 */
ref = getRefcount( (IUnknown *) palette);
ok(ref == 1, "Got refcount %ld, expected 1\n", ref);
/* Test attaching a palette to a surface */
hr = IDirectDrawSurface7_SetPalette(surface, palette);
ok(hr == DD_OK, "IDirectDrawSurface_SetPalette failed with %08x\n", hr);
/* Palette refcount increased, surface stays the same */
ref = getRefcount( (IUnknown *) palette);
ok(ref == 2, "Got refcount %ld, expected 2\n", ref);
ref = getRefcount( (IUnknown *) surface);
ok(ref == 1, "Got refcount %ld, expected 1\n", ref);
IDirectDrawSurface7_Release(surface);
/* Increased before - decrease now */
ref = getRefcount( (IUnknown *) DDraw7);
ok(ref == 2, "Got refcount %ld, expected 2\n", ref);
/* Releasing the surface detaches the palette */
ref = getRefcount( (IUnknown *) palette);
ok(ref == 1, "Got refcount %ld, expected 1\n", ref);
IDirectDrawPalette_Release(palette);
/* Increased before - decrease now */
ref = getRefcount( (IUnknown *) DDraw7);
ok(ref == 1, "Got refcount %ld, expected 1\n", ref);
/* Not all interfaces are AddRefed when a palette is created */
hr = IDirectDraw4_CreatePalette(DDraw4, DDPCAPS_ALLOW256 | DDPCAPS_8BIT, Table, &palette, NULL);
ok(hr == DD_OK, "CreatePalette returned %08x\n", hr);
ref = getRefcount( (IUnknown *) DDraw4);
ok(ref == 2, "Got refcount %ld, expected 2\n", ref);
IDirectDrawPalette_Release(palette);
/* No addref here */
hr = IDirectDraw2_CreatePalette(DDraw2, DDPCAPS_ALLOW256 | DDPCAPS_8BIT, Table, &palette, NULL);
ok(hr == DD_OK, "CreatePalette returned %08x\n", hr);
ref = getRefcount( (IUnknown *) DDraw2);
ok(ref == 1, "Got refcount %ld, expected 1\n", ref);
IDirectDrawPalette_Release(palette);
/* No addref here */
hr = IDirectDraw_CreatePalette(DDraw1, DDPCAPS_ALLOW256 | DDPCAPS_8BIT, Table, &palette, NULL);
ok(hr == DD_OK, "CreatePalette returned %08x\n", hr);
ref = getRefcount( (IUnknown *) DDraw1);
ok(ref == 1, "Got refcount %ld, expected 1\n", ref);
IDirectDrawPalette_Release(palette);
/* Similar for surfaces */
hr = IDirectDraw4_CreateSurface(DDraw4, &ddsd, &surface4, NULL);
ok(hr == DD_OK, "CreateSurface returned %08x\n", hr);
ref = getRefcount( (IUnknown *) DDraw4);
ok(ref == 2, "Got refcount %ld, expected 2\n", ref);
IDirectDrawSurface4_Release(surface4);
ddsd.dwSize = sizeof(DDSURFACEDESC);
hr = IDirectDraw2_CreateSurface(DDraw2, (DDSURFACEDESC *) &ddsd, &surface1, NULL);
ok(hr == DD_OK, "CreateSurface returned %08x\n", hr);
ref = getRefcount( (IUnknown *) DDraw2);
ok(ref == 1, "Got refcount %ld, expected 1\n", ref);
IDirectDrawSurface_Release(surface1);
hr = IDirectDraw_CreateSurface(DDraw1, (DDSURFACEDESC *) &ddsd, &surface1, NULL);
ok(hr == DD_OK, "CreateSurface returned %08x\n", hr);
ref = getRefcount( (IUnknown *) DDraw1);
ok(ref == 1, "Got refcount %ld, expected 1\n", ref);
IDirectDrawSurface_Release(surface1);
IDirectDraw7_Release(DDraw7);
IDirectDraw4_Release(DDraw4);
IDirectDraw2_Release(DDraw2);
IDirectDraw_Release(DDraw1);
}
/* decode gps message --------------------------------------------------------*/
static int decode_gw10gps(raw_t *raw)
{
eph_t eph={0};
double tow,ion[8]={0},utc[4]={0};
unsigned int buff=0;
int i,prn,sat,id,leaps;
unsigned char *p=raw->buff+2,subfrm[30];
trace(4,"decode_gw10gps: len=%d\n",raw->len);
tow=U4(p)/1000.0; p+=4;
prn=U1(p); p+=1;
if (!(sat=satno(SYS_GPS,prn))) {
trace(2,"gw10 gps satellite number error: tow=%.1f prn=%d\n",tow,prn);
return -1;
}
for (i=0;i<10;i++) {
buff=(buff<<30)|U4(p); p+=4;
/* check parity of word */
if (!check_parity(buff,subfrm+i*3)) {
trace(2,"gw10 gps frame parity error: tow=%.1f prn=%2d word=%2d\n",
tow,prn,i+1);
return -1;
}
}
id=getbitu(subfrm,43,3); /* subframe id */
if (id<1||5<id) {
trace(2,"gw10 gps frame id error: tow=%.1f prn=%2d id=%d\n",tow,prn,id);
return -1;
}
for (i=0;i<30;i++) raw->subfrm[sat-1][i+(id-1)*30]=subfrm[i];
if (id==3) { /* decode ephemeris */
if (decode_frame(raw->subfrm[sat-1] ,&eph,NULL,NULL,NULL,NULL)!=1||
decode_frame(raw->subfrm[sat-1]+30,&eph,NULL,NULL,NULL,NULL)!=2||
decode_frame(raw->subfrm[sat-1]+60,&eph,NULL,NULL,NULL,NULL)!=3) {
return 0;
}
if (!strstr(raw->opt,"-EPHALL")) {
if (eph.iode==raw->nav.eph[sat-1].iode) return 0; /* unchanged */
}
eph.sat=sat;
raw->nav.eph[sat-1]=eph;
raw->ephsat=sat;
return 2;
}
else if (id==4) { /* decode ion-utc parameters */
if (decode_frame(subfrm,NULL,NULL,ion,utc,&leaps)!=4) {
return 0;
}
if (norm(ion,8)>0.0&&norm(utc,4)>0.0&&leaps!=0) {
for (i=0;i<8;i++) raw->nav.ion_gps[i]=ion[i];
for (i=0;i<4;i++) raw->nav.utc_gps[i]=utc[i];
raw->nav.leaps=leaps;
return 9;
}
}
return 0;
}
/* decode id#23 measurement block --------------------------------------------*/
static int decode_ss2meas(raw_t *raw)
{
const double freqif=1.405396825E6,tslew=1.75E-7;
double tow,slew,code,icp,d;
int i,j,n,prn,sat,nobs;
unsigned char *p=raw->buff+4;
unsigned int sc;
trace(4,"decode_ss2meas: len=%d\n",raw->len);
nobs=U1(p+2);
if (17+nobs*11!=raw->len) {
trace(2,"ss2 id#23 message length error: len=%d\n",raw->len);
return -1;
}
tow=floor(R8(p+3)*1000.0+0.5)/1000.0; /* rounded by 1ms */
if (!adjweek(raw,tow)) {
trace(2,"ss2 id#23 message time adjustment error\n");
return -1;
}
/* time slew defined as uchar (ref [1]) but minus value appears in some f/w */
slew=*(char *)(p)*tslew;
raw->icpc+=4.5803-freqif*slew-FREQ1*(slew-1E-6); /* phase correction */
for (i=n=0,p+=11;i<nobs&&n<MAXOBS;i++,p+=11) {
prn=(p[0]&0x1F)+1;
if (!(sat=satno(p[0]&0x20?SYS_SBS:SYS_GPS,prn))) {
trace(2,"ss2 id#23 satellite number error: prn=%d\n",prn);
continue;
}
raw->obs.data[n].time=raw->time;
raw->obs.data[n].sat=sat;
code=(tow-floor(tow))-(double)(U4(p+2))/2095104000.0;
raw->obs.data[n].P[0]=CLIGHT*(code+(code<0.0?1.0:0.0));
icp=(double)(U4(p+6)>>2)/1024.0+raw->off[sat-1]; /* unwrap */
if (fabs(icp-raw->icpp[sat-1])>524288.0) {
d=icp>raw->icpp[sat-1]?-1048576.0:1048576.0;
raw->off[sat-1]+=d; icp+=d;
}
raw->icpp[sat-1]=icp;
raw->obs.data[n].L[0]=icp+raw->icpc;
raw->obs.data[n].D[0]=0.0;
raw->obs.data[n].SNR[0]=(unsigned char)(floor(U1(p+1)+0.5));
sc=U1(p+10);
raw->obs.data[n].LLI[0]=(int)((unsigned char)sc-(unsigned char)raw->lockt[sat-1][0])>0;
raw->obs.data[n].LLI[0]|=U1(p+6)&1?2:0;
raw->obs.data[n].code[0]=CODE_L1C;
raw->lockt[sat-1][0]=sc;
for (j=1;j<NFREQ;j++) {
raw->obs.data[n].L[j]=raw->obs.data[n].P[j]=0.0;
raw->obs.data[n].D[j]=0.0;
raw->obs.data[n].SNR[j]=raw->obs.data[n].LLI[j]=0;
raw->obs.data[n].code[j]=CODE_NONE;
}
n++;
}
raw->obs.n=n;
return 1;
}
static int I4(unsigned char *p)
{
return (int)U4(p);
}
/* decode binex mesaage 0x01-05: decoded beidou-2/compass ephmemeris ---------*/
static int decode_bnx_01_05(raw_t *raw, unsigned char *buff, int len)
{
eph_t eph={0};
unsigned char *p=buff;
double tow,toc,sqrtA;
int prn,flag1,flag2;
trace(4,"binex 0x01-05: len=%d\n",len);
if (len>=117) {
prn =U1(p); p+=1;
eph.week =U2(p); p+=2;
tow =I4(p); p+=4;
toc =I4(p); p+=4;
eph.toes =I4(p); p+=4;
eph.f2 =R4(p); p+=4;
eph.f1 =R4(p); p+=4;
eph.f0 =R4(p); p+=4;
eph.deln =R4(p)*SC2RAD; p+=4;
eph.M0 =R8(p); p+=8;
eph.e =R8(p); p+=8;
sqrtA =R8(p); p+=8;
eph.cic =R4(p); p+=4;
eph.crc =R4(p); p+=4;
eph.cis =R4(p); p+=4;
eph.crs =R4(p); p+=4;
eph.cuc =R4(p); p+=4;
eph.cus =R4(p); p+=4;
eph.OMG0 =R8(p); p+=8;
eph.omg =R8(p); p+=8;
eph.i0 =R8(p); p+=8;
eph.OMGd =R4(p)*SC2RAD; p+=4;
eph.idot =R4(p)*SC2RAD; p+=4;
flag1 =U2(p); p+=2;
flag2 =U4(p);
}
else {
trace(2,"binex 0x01-05: length error len=%d\n",len);
return -1;
}
if (!(eph.sat=satno(SYS_CMP,prn))) {
trace(2,"binex 0x01-05: satellite error prn=%d\n",prn);
return 0;
}
eph.A=sqrtA*sqrtA;
eph.toe=gpst2time(eph.week+1356,eph.toes+14.0); /* bdt -> gpst */
eph.toc=gpst2time(eph.week+1356,eph.toes+14.0); /* bdt -> gpst */
eph.ttr=adjweek(eph.toe,tow+14.0); /* bdt -> gpst */
eph.iodc=(flag1>>1)&0x1F;
eph.iode=(flag1>>6)&0x1F;
eph.svh=flag1&0x01;
eph.sva=flag2&0x0F; /* ura index */
eph.tgd[0]=bds_tgd(flag2>> 4); /* TGD1 (s) */
eph.tgd[1]=bds_tgd(flag2>>14); /* TGD2 (s) */
eph.flag=(flag1>>11)&0x07; /* nav type (0:unknown,1:IGSO/MEO,2:GEO) */
eph.code=(flag2>>25)&0x7F;
/* message source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q)*/
if (!strstr(raw->opt,"-EPHALL")) {
if (raw->nav.eph[eph.sat-1].iode==eph.iode&&
raw->nav.eph[eph.sat-1].iodc==eph.iodc) return 0; /* unchanged */
}
raw->nav.eph[eph.sat-1]=eph;
raw->ephsat=eph.sat;
return 2;
}