C++ U1函数代码示例

/* decode skytraq glonass string buffer --------------------------------------*/
static int decode_stqglostr(raw_t *raw)
{
    int i,prn,sat,strno;
    unsigned char *p=raw->buff+4;

    trace(4,"decode_stqglostr: len=%d\n",raw->len);

    if (raw->len<19) {
        trace(2,"stq glo string length error: len=%d\n",raw->len);
        return -1;
    }
    prn=U1(p+1)-64;
    if (!(sat=satno(SYS_GLO,prn))) {
        trace(2,"stq glo string satellite number error: prn=%d\n",prn);
        return -1;
    }
    strno=U1(p+2);
    if (strno<1||4<strno) {
        trace(2,"stq glo string number error: prn=%d strno=%d\n",prn,strno);
        return -1;
    }
    for (i=0; i<9; i++) {
        raw->subfrm[sat-1][strno*9-1-i]=p[3+i];
    }
    if (strno<5) return 0;

    return 0;
}

/* decode NVS epehemerides in clear ------------------------------------------*/
static int decode_xf7eph(raw_t *raw)
{
    int prn,sat,sys;
    unsigned char *p=raw->buff;
    
    trace(4,"decode_xf7eph: len=%d\n",raw->len);
    
    if ((raw->len)<93) {
        trace(2,"nvs xf7eph length error: len=%d\n",raw->len);
        return -1;
    }
    sys = (U1(p+2)==1)?SYS_GPS:((U1(p+2)==2)?SYS_GLO:SYS_NONE);
    prn = U1(p+3);
    if (!(sat=satno(sys==1?SYS_GPS:SYS_GLO,prn))) {
        trace(2,"nvs xf7eph satellite number error: prn=%d\n",prn);
        return -1;
    }
    if (sys==SYS_GPS) {
        return decode_gpsephem(sat,raw);
    }
    else if (sys==SYS_GLO) {
        return decode_gloephem(sat,raw);
    }
    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 ublox raw message --------------------------------------------------*/
int decode_ubx(tUbxRawData *raw)
{
    int type = (U1(raw->buff+2)<<8) + U1(raw->buff+3);
    
    /* checksum */
    if (!ubx_ComputeCheckSum(raw->buff, raw->length))
        return -1;
		
    switch (type) 
    {
       case ID_NAVPVT:
          return decode_navpvt(raw);
       case ID_NAVCLK:
          return decode_navclock(raw);
       case ID_NAVCOV:
          return decode_navcov(raw);
       case ID_NAVSAT:
          return decode_navsat(raw);
       case ID_NAVGPSTIME :
          return decode_navgpstime(raw);

//#ifndef CHIP_VERSION
//       case ID_RXMRAWX : 
//          return decode_rxmrawx(raw);
//#else
//        case ID_TRKD5   : 
//           return decode_trkd5(raw);        /* for tightly couple or DGNSS/RTK */
//#endif
        //case ID_RXMSFRBX: 
        //   return decode_rxmsfrbx(raw);    /* for tightly couple or DGNSS/RTK */
    }

    return -1;
}

static HRESULT WINAPI enummodescallback(LPDDSURFACEDESC lpddsd, LPVOID lpContext)
{
    trace("Width = %i, Height = %i, Refresh Rate = %i, Pitch = %i, flags =%02X\n",
        lpddsd->dwWidth, lpddsd->dwHeight,
          U2(*lpddsd).dwRefreshRate, U1(*lpddsd).lPitch, lpddsd->dwFlags);

    /* Check that the pitch is valid if applicable */
    if(lpddsd->dwFlags & DDSD_PITCH)
    {
        ok(U1(*lpddsd).lPitch != 0, "EnumDisplayModes callback with bad pitch\n");
    }

    /* Check that frequency is valid if applicable
     *
     * This fails on some Windows drivers or Windows versions, so it isn't important
     * apparently
    if(lpddsd->dwFlags & DDSD_REFRESHRATE)
    {
        ok(U2(*lpddsd).dwRefreshRate != 0, "EnumDisplayModes callback with bad refresh rate\n");
    }
     */

    adddisplaymode(lpddsd);

    return DDENUMRET_OK;
}

/* decode skytraq raw channel mesurement -------------------------------------*/
static int decode_stqraw(raw_t *raw)
{
    int i,j,iod,prn,sys,sat,n=0,nsat;
    unsigned char *p=raw->buff+4,ind;

    trace(4,"decode_stqraw: len=%d\n",raw->len);

    iod=U1(p+1);
    if (iod!=raw->iod) {
        trace(2,"stq raw iod error: iod=%d %d\n",iod,raw->iod);
        return -1;
    }
    nsat=U1(p+2);
    if (raw->len<8+23*nsat) {
        trace(2,"stq raw length error: len=%d nsat=%d\n",raw->len,nsat);
        return -1;
    }
    for (i=0,p+=3; i<nsat&&i<MAXOBS; i++,p+=23) {
        ind                    =U1(p+22);
        prn                    =U1(p);
        raw->obs.data[n].SNR[0]=(unsigned char)(U1(p+1)*4.0+0.5);
        raw->obs.data[n].P[0]  =(ind&0x1)?R8r(p+ 2):0.0;
        raw->obs.data[n].L[0]  =(ind&0x4)?R8r(p+10):0.0;
        raw->obs.data[n].D[0]  =(ind&0x2)?R4r(p+18):0.0f;
        raw->obs.data[n].LLI[0]=(ind&0x8)?1:0; /* slip */
        raw->obs.data[n].code[0]=CODE_L1C;

        /* receiver dependent options */
        if (strstr(raw->opt,"-invcp")) {
            raw->obs.data[n].L[0]=-raw->obs.data[n].L[0];
        }
        if (MINPRNGPS<=prn&&prn<=MAXPRNGPS) {
            sys=SYS_GPS;
        }
        else if (MINPRNGLO<=prn-64&&prn-64<=MAXPRNGLO) {
            sys=SYS_GLO;
            prn-=64;
        }
        else {
            trace(2,"stq raw satellite number error: prn=%d\n",prn);
            continue;
        }
        if (!(sat=satno(sys,prn))) {
            trace(2,"stq raw satellite number error: sys=%d prn=%d\n",sys,prn);
            continue;
        }
        raw->obs.data[n].time=raw->time;
        raw->obs.data[n].sat =sat;

        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 n>0?1:0;
}

/* 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;
}

/* 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;
}

/* 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 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;
}

static void mdlOutputs(SimStruct *S,int_T tid) {
	InputRealPtrsType uPtrs0 = ssGetInputPortRealSignalPtrs(S,0);
	InputRealPtrsType uPtrs1 = ssGetInputPortRealSignalPtrs(S,1);
	real_T prev = ssGetRWorkValue(S,0);
	bool dataPort = PARAM(2)[0];
	int_T i;
#ifndef MATLAB_MEX_FILE
	rosShmData_t *shm = (rosShmData_t *)ssGetPWorkValue(S,0);
	SEM *sem = (SEM *)ssGetPWorkValue(S,1);
#endif
	char_T *msg;
	unsigned int strlen = sizeof(char_T)*(PARAM_SIZE(1)+1);

	UNUSED_ARG(tid);	 /* not used in single tasking mode */

	if (U0(0) > 0.5 && U0(0) > prev) {
		msg = (char_T *)malloc(strlen);
		mxGetString(ssGetSFcnParam(S,1), msg, strlen);
#ifndef MATLAB_MEX_FILE
		if (dataPort) {
			for (i = 0; i < ssGetInputPortWidth(S,1); ++i) {
				asprintf(&msg, "%s %f", msg, U1(i));
			}
		}
		if (rt_sem_wait_if(sem) != 0) {
			memcpy(shm->msg.text, msg, MAX_LOG_MSG_SIZE);
			shm->msg.state = NEW_VALUE;
			rt_sem_signal(sem);
		}
#else
		switch ((int)PARAM(0)[0]) {
			case 1: printf("DEBUG"); break;
			case 2: printf("INFO"); break;
			case 3: printf("WARN"); break;
			case 4: printf("ERROR"); break;
			case 5: printf("FATAL"); break;
			default: printf("NONE"); break;
		}
		printf(": %s", msg);
		if (dataPort) {
			for (i = 0; i < ssGetInputPortWidth(S,1); ++i) {
				printf(" %f", U1(i));
			}
		}
		printf("\n");
#endif
		free(msg);
	}
	ssSetRWorkValue(S,0,U0(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 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 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;
}

/* decode skytraq message ----------------------------------------------------*/
static int decode_stq(raw_t *raw)
{
    int type=U1(raw->buff+4);
    unsigned char cs,*p=raw->buff+raw->len-3;

    trace(3,"decode_stq: type=%02x len=%d\n",type,raw->len);

    /* checksum */
    cs=checksum(raw->buff,raw->len);
    if (cs!=*p||*(p+1)!=0x0D||*(p+2)!=0x0A) {
        trace(2,"stq checksum error: type=%02X cs=%02X tail=%02X%02X%02X\n",
              type,cs,*p,*(p+1),*(p+2));
        return -1;
    }
    if (raw->outtype) {
        sprintf(raw->msgtype,"SKYTRAQ 0x%02x (%4d):",type,raw->len);
    }
    switch (type) {
    case ID_STQTIME  :
        return decode_stqtime  (raw);
    case ID_STQRAW   :
        return decode_stqraw   (raw);
    case ID_STQSFRB  :
        return decode_stqsfrb  (raw);
    case ID_STQGLOSTR:
        return decode_stqglostr(raw);
    }
    return 0;
}