/*------------------------------------------------------------------------------
* skytraq.c : skytraq receiver dependent functions
*
*          Copyright (C) 2009-2020 by T.TAKASU, All rights reserved.
*
* reference :
*     [1] Skytraq, Application Note AN0023 Binary Message of SkyTraq Venus 6 
*         GPS Receiver, ver 1.4.8, August 21, 2008
*     [2] Skytraq, Application Note AN0024 Raw Measurement Binary Message
*         Extension of SkyTraq Venus 6 GPS Receiver, ver 0.5, October 9, 2009
*     [3] Skytraq, Application Note AN0024G2 Binary Message of SkyTraq Venus 7
*         GLONASS/GPS Receiver (Raw Measurement F/W), ver 1.4.26, April 26, 2012
*     [4] Skytraq, Application Note AN0030 Binary Message of Raw Measurement
*         Data Extension of SkyTraq Venus 8 GNSS Receiver, ver.1.4.29,
*         April 3, 2014
*     [5] Skytraq, Application Note AN0030 Binary Message of Raw Measurement
*         Data Extension of SkyTraq Venus 8 GNSS Receiver, ver.1.4.31,
*         August 12, 2014
*     [6] Skytraq, Application Note AN0030 Binary Message of Raw Measurement
*         Data Extension of SkyTraq Venus 8 GNSS Receiver, ver.1.4.32,
*         Sep 26, 2016
*     [7] Skytraq, Application Note AN0039 Binary Messages of Raw Measurement
*         Data Extension of SkyTraq Phoneix GNSS Receiver, ver.1.4.39,
*         Dec 30, 2020
*
* notes   :
*     The byte order of S1315F raw message is big-endian inconsistent to [1].
*
* version : $Revision:$
* history : 2009/10/10 1.0 new
*           2009/11/08 1.1 flip carrier-phase polarity for F/W 1.8.23-20091106
*           2011/05/27 1.2 add almanac decoding
*                          fix problem with ARM compiler
*           2011/07/01 1.3 suppress warning
*           2013/03/10 1.5 change option -invcp to -INVCP
*           2014/11/09 1.6 support glonass, qzss and beidou
*           2016/10/09 1.7 support F/W version specified as ref [6]
*           2017/04/11 1.8 (char *) -> (signed char *)
*           2017/05/08 1.9 fix bug on decoding extended raw meas v.1 (0xE5)
*                          fix bug on encoding CFG-BIN message (0x1E)
*                          add decode of ack/nack to request msg (0x83/0x84)
*           2020/10/30 1.10 add adjustment of gps week by cpu time
*                           CODE_L1I -> CODE_L2I for BDS
*                           use integer type in stdint.h
*                           suppress warnings
*-----------------------------------------------------------------------------*/
#include "rtklib.h"

#define STQSYNC1    0xA0        /* skytraq binary sync code 1 */
#define STQSYNC2    0xA1        /* skytraq binary sync code 2 */

#define ID_STQTIME  0xDC        /* skytraq message id: measurement epoch */
#define ID_STQRAW   0xDD        /* skytraq message id: raw measurement */
#define ID_STQSVCH  0xDE        /* skytraq message id: SV and channel status */
#define ID_STQSTAT  0xDF        /* skytraq message id: navigation status */
#define ID_STQGPS   0xE0        /* skytraq message id: gps/qzs subframe */
#define ID_STQGLO   0xE1        /* skytraq message id: glonass string */
#define ID_STQBDSD1 0xE2        /* skytraq message id: beidou d1 subframe */
#define ID_STQBDSD2 0xE3        /* skytraq message id: beidou d2 subframe */
#define ID_STQRAWX  0xE5        /* skytraq message id: extended raw meas v.1 */
#define ID_STQGENE  0xE6        /* skytraq message id: general subframe data */
#define ID_STQGLOE  0x5C        /* skytraq message id: glonass ephemeris */
#define ID_STQACK   0x83        /* skytraq message id: ack to request msg */
#define ID_STQNACK  0x84        /* skytraq message id: nack to request msg */

#define ID_RESTART  0x01        /* skytraq message id: system restart */
#define ID_CFGSERI  0x05        /* skytraq message id: configure serial port */
#define ID_CFGFMT   0x09        /* skytraq message id: configure message format */
#define ID_CFGRATE  0x12        /* skytraq message id: configure message rate */
#define ID_CFGBIN   0x1E        /* skytraq message id: configure binary message */
#define ID_GETGLOEPH 0x5B       /* skytraq message id: get glonass ephemeris */

/* extract field (big-endian) ------------------------------------------------*/
#define U1(p)       (*((uint8_t *)(p)))
#define I1(p)       (*((int8_t  *)(p)))

static uint16_t U2(uint8_t *p)
{
    uint16_t value;
    uint8_t *q=(uint8_t *)&value+1;
    int i;
    for (i=0;i<2;i++) *q--=*p++;
    return value;
}
static uint32_t U4(uint8_t *p)
{
    uint32_t value;
    uint8_t *q=(uint8_t *)&value+3;
    int i;
    for (i=0;i<4;i++) *q--=*p++;
    return value;
}
static float R4(uint8_t *p)
{
    float value;
    uint8_t *q=(uint8_t *)&value+3;
    int i;
    for (i=0;i<4;i++) *q--=*p++;
    return value;
}
static double R8(uint8_t *p)
{
    double value;
    uint8_t *q=(uint8_t *)&value+7;
    int i;
    for (i=0;i<8;i++) *q--=*p++;
    return value;
}
/* checksum ------------------------------------------------------------------*/
static uint8_t checksum(uint8_t *buff, int len)
{
    uint8_t cs=0;
    int i;
    
    for (i=4;i<len-3;i++) {
        cs^=buff[i];
    }
    return cs;
}
/* 8-bit week -> full week ---------------------------------------------------*/
static void adj_utcweek(gtime_t time, double *utc)
{
    int week;
    
    if (utc[3]>=256.0) return;
    time2gpst(time,&week);
    utc[3]+=week/256*256;
    if      (utc[3]<week-128) utc[3]+=256.0;
    else if (utc[3]>week+128) utc[3]-=256.0;
}
/* GNSSId to system  ---------------------------------------------------------*/
static int sky_sys(int gnssid)
{
    switch (gnssid) {
        case 0: return SYS_GPS;
        case 1: return SYS_SBS;
        case 2: return SYS_GLO;
        case 3: return SYS_GAL;
        case 4: return SYS_QZS;
        case 5: return SYS_CMP;
        case 6: return SYS_IRN;
    }
    return 0;
}
/* UBX SigId to signal (ref [5] 1.5.4) ---------------------------------------*/
static int sky_sig(int sys, int signal_type) {
    if (sys==SYS_GPS) {
        switch (signal_type) {
            case  1: return CODE_L1X;
            case  2: return CODE_L2X;
            case  4: return CODE_L5X;
            default: return CODE_L1C;
        }
    }
    else if (sys==SYS_SBS) {
        return CODE_L1C;
    }
    else if (sys==SYS_GLO) {
        switch (signal_type) {
            case  2: return CODE_L2C;
            case  4: return CODE_L3X;
            default: return CODE_L1C;
        }
    }
    else if (sys==SYS_GAL) {
        switch (signal_type) {
            case  4: return CODE_L5X;
            case  5: return CODE_L7X;
            case  6: return CODE_L6X;
            default: return CODE_L1C;
        }
    }
    else if (sys==SYS_QZS) {
        switch (signal_type) {
            case  1: return CODE_L1X;
            case  2: return CODE_L2X;
            case  4: return CODE_L5X;
            case  6: return CODE_L6X;
            default: return CODE_L1C;
        }
    }
    else if (sys==SYS_CMP) { /* BeiDou */
        switch (signal_type) {
            case  1: return CODE_L1X;
            case  4: return CODE_L5X;
            case  5: return CODE_L7I;
            case  7: return CODE_L6I;
            default: return CODE_L2I;
        }
    }
    else {
        trace(2,"stq rawx gnss type error: type=%d\n",sys);
        return(CODE_NONE);
    }
}
/* decode skytraq measurement epoch (0xDC) -----------------------------------*/
static int decode_stqtime(raw_t *raw)
{
    uint8_t *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);
    week    =adjgpsweek(week);
    tow     =U4(p+4)*0.001;
    raw->time=gpst2time(week,tow);
    
    if (raw->outtype) {
        sprintf(raw->msgtype,"SKYTRAQ EPOCH (%4d): iod=%d week=%d tow=%.3f",
                raw->len,raw->iod,week,tow);
    }
    return 0;
}
/* decode skytraq raw measurement (0xDD) -------------------------------------*/
static int decode_stqraw(raw_t *raw)
{
    uint8_t *p=raw->buff+4,ind;
    double pr1,cp1;
    int i,j,iod,prn,sys,sat,n=0,nsat;
    
    trace(4,"decode_stqraw: len=%d\n",raw->len);
    
    if (raw->outtype) {
        sprintf(raw->msgtype,"SKYTRAQ RAW   (%4d): nsat=%d",raw->len,U1(p+2));
    }
    iod=U1(p+1);
    if (iod!=raw->iod) { /* need preceding measurement epoch (0xDC) */
        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) {
        prn=U1(p);
        
        if (MINPRNGPS<=prn&&prn<=MAXPRNGPS) {
            sys=SYS_GPS;
        }
        else if (MINPRNGLO<=prn-64&&prn-64<=MAXPRNGLO) {
            sys=SYS_GLO;
            prn-=64;
        }
        else if (MINPRNQZS<=prn&&prn<=MAXPRNQZS) {
            sys=SYS_QZS;
        }
        else if (MINPRNCMP<=prn-200&&prn-200<=MAXPRNCMP) {
            sys=SYS_CMP;
            prn-=200;
        }
        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;
        }
        ind=U1(p+22);
        pr1=!(ind&1)?0.0:R8(p+ 2);
        cp1=!(ind&4)?0.0:R8(p+10);
        cp1-=floor((cp1+1E9)/2E9)*2E9; /* -10^9 < cp1 < 10^9 */
        
        raw->obs.data[n].P[0]=pr1;
        raw->obs.data[n].L[0]=cp1;
        raw->obs.data[n].D[0]=!(ind&2)?0.0:R4(p+18);
        raw->obs.data[n].SNR[0]=(uint16_t)(U1(p+1)/SNR_UNIT+0.5);
        raw->obs.data[n].LLI[0]=0;
        raw->obs.data[n].code[0]=sys==SYS_CMP?CODE_L2I:CODE_L1C;
        
        raw->lockt[sat-1][0]=ind&8?1:0; /* cycle slip */
        
        if (raw->obs.data[n].L[0]!=0.0) {
            raw->obs.data[n].LLI[0]=(uint8_t)raw->lockt[sat-1][0];
            raw->lockt[sat-1][0]=0;
        }
        /* receiver dependent options */
        if (strstr(raw->opt,"-INVCP")) {
            raw->obs.data[n].L[0]*=-1.0;
        }
        raw->obs.data[n].time=raw->time;
        raw->obs.data[n].sat =sat;
        
        for (j=1;j<NFREQ+NEXOBS;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 skytraq extended raw measurement data v.1 (0xE5) -------------------*/
static int decode_stqrawx(raw_t *raw)
{
    uint8_t *p=raw->buff+4,ind;
    double tow,peri,pr1,cp1;
    int i,j,k,ver,week,nsat,sys,sig,prn,sat,n=0,idx;

    trace(4,"decode_stqraw: len=%d\n",raw->len);
    
    if (raw->outtype) {
        sprintf(raw->msgtype,"SKYTRAQ RAWX  (%4d): nsat=%2d",raw->len,U1(p+13));
    }
    ver=U1(p+1);
    raw->iod=U1(p+2);
    week=U2(p+3);
    week=adjgpsweek(week);
    tow =U4(p+5)*0.001;
    raw->time=gpst2time(week,tow);
    peri=U2(p+9)*0.001;
    nsat=U1(p+13);
    if (raw->len<19+31*nsat) {
        trace(2,"stq raw length error: len=%d nsat=%d\n",raw->len,nsat);
        return -1;
    }
    for (i=0,p+=14;i<nsat&&i<MAXOBS;i++,p+=31) {
        sys = sky_sys(U1(p)&0xF);
        sig = sky_sig(sys,(U1(p)>>4)&0xF);
        idx=code2idx(sys,sig);
        prn=U1(p+1);
        if (!(sat=satno(sys,prn))) {
            trace(2,"stq raw satellite number error: sys=%d prn=%d\n",sys,prn);
            continue;
        }
        /* set glonass freq channel number */
        if (sys==SYS_GLO) {
            raw->nav.geph[prn-1].frq=(int)(U1(p+2)&0xF)-7;
        }
        ind=U2(p+27);
        pr1=!(ind&1)?0.0:R8(p+ 4);
        cp1=!(ind&4)?0.0:R8(p+12);
        cp1-=floor((cp1+1E9)/2E9)*2E9; /* -10^9 < cp1 < 10^9 */

        for (j=0;j<n;j++) {
            if (raw->obs.data[j].sat==sat) break;
        }
        if (j>=n) {
            raw->obs.data[n].time=raw->time;
            raw->obs.data[n].sat=sat;
            raw->obs.data[n].rcv=0;
            for (k=0;k<NFREQ+NEXOBS;k++) {
                raw->obs.data[n].L[k]=raw->obs.data[n].P[k]=0.0;
                raw->obs.data[n].D[k]=0.0;
                raw->obs.data[n].SNR[k]=raw->obs.data[n].LLI[k]=0;
                raw->obs.data[n].code[k]=CODE_NONE;
            }
            n++;
        }
        raw->obs.data[j].P[idx]=pr1;
        raw->obs.data[j].L[idx]=cp1;
        raw->obs.data[j].D[idx]=!(ind&2)?0.0:R4(p+20);
        raw->obs.data[j].SNR[idx]=(uint16_t)(U1(p+3)/SNR_UNIT+0.5);
        raw->obs.data[j].LLI[idx]=0;
        raw->obs.data[j].code[idx]=sig;

        raw->lockt[sat-1][idx]=ind&8?1:0; /* cycle slip */

        if (raw->obs.data[j].L[idx]!=0.0) {
            raw->obs.data[j].LLI[idx]=(uint8_t)raw->lockt[sat-1][idx];
            raw->lockt[sat-1][idx]=0;
        }
        /* receiver dependent options */
        if (strstr(raw->opt,"-INVCP")) {
            raw->obs.data[n].L[idx]*=-1.0;
        }
        raw->obs.data[n].time=raw->time;
        raw->obs.data[n].sat =sat;
    }
    raw->obs.n=n;
    return n>0?1:0;
}
/* decode galileo ephemeris (0xE6) ----------------------------------------------*/
static int decode_stqgene(raw_t *raw)
{
    eph_t eph={0};
    int i,j,prn,sat,m,sys;
    int part1,page1,part2,page2,type;
    double ion[4]={0},utc[8]={0};
    uint8_t *p=raw->buff+4,buff[32],crc_buff[26]={0};

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

    if (raw->len<44) {
        trace(2,"stq gene string length error: len=%d\n",raw->len);
        return -1;
    }
    prn=U1(p+3);
    sys=sky_sys(U1(p+2)&0xF);
    if (!(sat=satno(sys,prn))) {
        trace(2,"stq raw satellite number error: sys=%d prn=%d\n",sys,prn);
        return 0;
    }
    if (sys!=SYS_GAL) {
        trace(2,"stq sys not supported: sys=%d\n",sys);
        return 0;
    }
    if (raw->outtype) {
        sprintf(raw->msgtype,"SKYTRAQ GENE (%4d): prn=%2d",raw->len,prn);
    }
    for (i=0;i<8;i++,p+=4) {
        setbitu(buff,32*i,32,U4(p+5));
    }
    part1=getbitu(buff   ,0,1);
    page1=getbitu(buff   ,1,1);
    part2=getbitu(buff,128,1);
    page2=getbitu(buff,129,1);

    if (part1!=0||part2!=1) {
        trace(3,"ubx rxmsfrbx enav page even/odd error: sat=%d\n",sat);
        return -1;
    }
    if (page1==1||page2==1) return 0; /* alert page */

    /* test crc (4(pad) + 114 + 82 bits) */
    for (i=0,j=  4;i<15;i++,j+=8) setbitu(crc_buff,j,8,getbitu(buff   ,i*8,8));
    for (i=0,j=118;i<11;i++,j+=8) setbitu(crc_buff,j,8,getbitu(buff,i*8+128,8));
    if (rtk_crc24q(crc_buff,25)!=getbitu(buff,128+82,24)) {
        trace(2,"ubx rxmsfrbx enav crc error: sat=%d\n",sat);
        return -1;
    }
    type=getbitu(buff,2,6); /* word type */

    if (type>6) return 0;

    /* save 128 (112:even+16:odd) bits word */
    for (i=0,j=2;i<14;i++,j+=8) {
        raw->subfrm[sat-1][type*16+i]=getbitu(buff,j,8);
    }
    for (i=14,j=130;i<16;i++,j+=8) {
        raw->subfrm[sat-1][type*16+i]=getbitu(buff,j,8);
    }
    if (type!=5) return 0;
    if (!decode_gal_inav(raw->subfrm[sat-1],&eph,ion,utc)) return 0;

    if (eph.sat!=sat) {
        trace(2,"skytraq enav satellite error: sat=%d %d\n",sat,eph.sat);
        return -1;
    }
    eph.code|=(1<<0); /* data source: E1 */

    adj_utcweek(raw->time,utc);
    matcpy(raw->nav.ion_gal,ion,4,1);
    matcpy(raw->nav.utc_gal,utc,8,1);

    if (!strstr(raw->opt,"-EPHALL")) {
        if (eph.iode==raw->nav.eph[sat-1].iode&&
            timediff(eph.toe,raw->nav.eph[sat-1].toe)==0.0&&
            timediff(eph.toc,raw->nav.eph[sat-1].toc)==0.0) return 0;
    }
    raw->nav.eph[sat-1]=eph;
    raw->ephsat=sat;
    raw->ephset=0; /* 0:I/NAV */
    return 2;
}
/* save gps/qzss subframe -------------------------------------------------------------*/
static int save_subfrm_gps(int sat, raw_t *raw)
{
    uint8_t *p=raw->buff+7,*q;
    int i,id;

    trace(4,"save_subfrm_gps: sat=%2d\n",sat);

    /* check navigation subframe preamble */
    if (p[0]!=0x8B) {
        trace(2,"stq subframe preamble error: 0x%02X\n",p[0]);
        return 0;
    }
    id=(p[5]>>2)&0x7;

    /* check subframe id */
    if (id<1||5<id) {
        trace(2,"stq subframe id error: id=%d\n",id);
        return 0;
    }
    q=raw->subfrm[sat-1]+(id-1)*30;

    for (i=0;i<30;i++) q[i]=p[i];

    return id;
}
/* decode ephemeris ----------------------------------------------------------*/
static int decode_ephem(int sat, raw_t *raw)
{
    eph_t eph={0};
    
    trace(4,"decode_ephem: sat=%2d\n",sat);
    
    if (!decode_frame(raw->subfrm[sat-1],&eph,NULL,NULL,NULL)) return 0;
    
    if (!strstr(raw->opt,"-EPHALL")) {
        if (eph.iode==raw->nav.eph[sat-1].iode&&
            eph.iodc==raw->nav.eph[sat-1].iodc) return 0; /* unchanged */
    }
    eph.sat=sat;
    raw->nav.eph[sat-1]=eph;
    raw->ephsat=sat;
    raw->ephset=0;
    return 2;
}
/* decode almanac and ion/utc ------------------------------------------------*/
static int decode_alm1(int sat, raw_t *raw)
{
    int sys=satsys(sat,NULL);
    
    trace(4,"decode_alm1 : sat=%2d\n",sat);
    
    if (sys==SYS_GPS) {
        decode_frame(raw->subfrm[sat-1],NULL,raw->nav.alm,raw->nav.ion_gps,
                     raw->nav.utc_gps);
        adj_utcweek(raw->time,raw->nav.utc_gps);
    }
    else if (sys==SYS_QZS) {
        decode_frame(raw->subfrm[sat-1],NULL,raw->nav.alm,raw->nav.ion_qzs,
                     raw->nav.utc_qzs);
        adj_utcweek(raw->time,raw->nav.utc_qzs);
    }
    return 9;
}
/* decode almanac ------------------------------------------------------------*/
static int decode_alm2(int sat, raw_t *raw)
{
    int sys=satsys(sat,NULL);
    
    trace(4,"decode_alm2 : sat=%2d\n",sat);
    
    if (sys==SYS_GPS) {
        decode_frame(raw->subfrm[sat-1],NULL,raw->nav.alm,NULL,NULL);
    }
    else if (sys==SYS_QZS) {
        decode_frame(raw->subfrm[sat-1],NULL,raw->nav.alm,raw->nav.ion_qzs,
                     raw->nav.utc_qzs);
        adj_utcweek(raw->time,raw->nav.utc_qzs);
    }
    return  0;
}
/* decode gps/qzss subframe (0xE0) -------------------------------------------*/
static int decode_stqgps(raw_t *raw)
{
    int prn,sat,id;
    uint8_t *p=raw->buff+4;

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

    if (raw->len<40) {
        trace(2,"stq gps/qzss subframe length error: len=%d\n",raw->len);
        return -1;
    }
    if (raw->outtype) {
        sprintf(raw->msgtype,"SKYTRAQ GPSSF (%4d): prn=%2d id=%d",raw->len,
                U1(p+1),(p[8]>>2)&0x7);
    }
    prn=U1(p+1);
    if (!(sat=satno(MINPRNQZS<=prn&&prn<=MAXPRNQZS?SYS_QZS:SYS_GPS,prn))) {
        trace(2,"stq gps/qzss subframe satellite number error: prn=%d\n",prn);
        return -1;
    }
    id=save_subfrm_gps(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;
}
/* decode glonass string (0xE1) ----------------------------------------------*/
static int decode_stqglo(raw_t *raw)
{
    geph_t geph={0};
    int i,prn,sat,m;
    uint8_t *p=raw->buff+4;

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

    if (raw->len<19) {
        trace(2,"stq glo string length error: len=%d\n",raw->len);
        return -1;
    }
    if (raw->outtype) {
        sprintf(raw->msgtype,"SKYTRAQ GLSTR (%4d): prn=%2d no=%d",raw->len,
                U1(p+1)-64,U1(p+2));
    }
    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;
    }
    m=U1(p+2); /* string number */
    if (m<1||4<m) {
        return 0; /* non-immediate info and almanac */
    }
    setbitu(raw->subfrm[sat-1]+(m-1)*10,1,4,m);
    for (i=0;i<9;i++) {
        setbitu(raw->subfrm[sat-1]+(m-1)*10,5+i*8,8,p[3+i]);
    }
    if (m!=4) return 0;

    /* decode glonass ephemeris strings */
    geph.tof=raw->time;
    if (!decode_glostr(raw->subfrm[sat-1],&geph,NULL)||geph.sat!=sat) return 0;

    if (!strstr(raw->opt,"-EPHALL")) {
        if (geph.iode==raw->nav.geph[prn-1].iode) return 0; /* unchanged */
    }
    /* keep freq channel number */
    geph.frq=raw->nav.geph[prn-1].frq;
    raw->nav.geph[prn-1]=geph;
    raw->ephsat=sat;
    raw->ephset=0;
    return 2;
}
/* decode glonass string (requested) (0x5C) ----------------------------------*/
static int decode_stqgloe(raw_t *raw)
{
    int prn,sat;
    uint8_t *p=raw->buff+4;
    
    trace(4,"decode_stqgloe: len=%d\n",raw->len);
    
    if (raw->len<50) {
        trace(2,"stq glo string length error: len=%d\n",raw->len);
        return -1;
    }
    prn=U1(p+1);
    if (!(sat=satno(SYS_GLO,prn))) {
        trace(2,"stq gloe string satellite number error: prn=%d\n",prn);
        return -1;
    }
    /* set frequency channel number */
    raw->nav.geph[prn-1].frq=I1(p+2);
    
    return 0;
}
/* decode beidou subframe (0xE2,0xE3) ----------------------------------------*/
static int decode_stqbds(raw_t *raw)
{
    eph_t eph={0};
    uint32_t word;
    int i,j=0,id,pgn,prn,sat;
    uint8_t *p=raw->buff+4;
    
    trace(4,"decode_stqbds: len=%d\n",raw->len);
    
    if (raw->len<38) {
        trace(2,"stq bds subframe length error: len=%d\n",raw->len);
        return -1;
    }
    if (raw->outtype) {
        sprintf(raw->msgtype,"SKYTRAQ BDSSF (%4d): prn=%2d id=%d",raw->len,
                U1(p+1)-200,U1(p+2));
    }
    prn=U1(p+1)-200;
    if (!(sat=satno(SYS_CMP,prn))) {
        trace(2,"stq bds subframe satellite number error: prn=%d\n",prn);
        return -1;
    }
    id=U1(p+2); /* subframe id */
    if (id<1||5<id) {
        trace(2,"stq bds subframe id error: prn=%2d\n",prn);
        return -1;
    }
    if (prn>5) { /* IGSO/MEO */
        word=getbitu(p+3,j,26)<<4; j+=26;
        setbitu(raw->subfrm[sat-1]+(id-1)*38,0,30,word);
        
        for (i=1;i<10;i++) {
            word=getbitu(p+3,j,22)<<8; j+=22;
            setbitu(raw->subfrm[sat-1]+(id-1)*38,i*30,30,word);
        }
        if (id!=3) return 0;
        if (!decode_bds_d1(raw->subfrm[sat-1],&eph,NULL,NULL)) return 0;
    }
    else { /* GEO */
        if (id!=1) return 0;
        
        pgn=getbitu(p+3,26+12,4); /* page number */
        if (pgn<1||10<pgn) {
            trace(2,"stq bds subframe page number error: prn=%2d pgn=%d\n",prn,pgn);
            return -1;
        }
        word=getbitu(p+3,j,26)<<4; j+=26;
        setbitu(raw->subfrm[sat-1]+(pgn-1)*38,0,30,word);
        
        for (i=1;i<10;i++) {
            word=getbitu(p+3,j,22)<<8; j+=22;
            setbitu(raw->subfrm[sat-1]+(pgn-1)*38,i*30,30,word);
        }
        if (pgn!=10) return 0;
        if (!decode_bds_d2(raw->subfrm[sat-1],&eph,NULL)) return 0;
    }
    if (!strstr(raw->opt,"-EPHALL")) {
        if (timediff(eph.toe,raw->nav.eph[sat-1].toe)==0.0) return 0; /* unchanged */
    }
    eph.sat=sat;
    raw->nav.eph[sat-1]=eph;
    raw->ephsat=sat;
    raw->ephset=0;
    return 2;
}
/* decode ack to request msg (0x83) ------------------------------------------*/
static int decode_stqack(raw_t *raw)
{
    uint8_t *p=raw->buff+4;
    
    trace(4,"decode_stqack: len=%d\n",raw->len);

    if (raw->len<9) {
        trace(2,"stq ack length error: len=%d\n",raw->len);
        return -1;
    }
    if (raw->outtype) {
        sprintf(raw->msgtype,"SKYTRAQ ACK   (%4d): msg=0x%02X",raw->len,
                U1(p+1));
    }
    return 0;
}
/* decode nack to request msg (0x84) -----------------------------------------*/
static int decode_stqnack(raw_t *raw)
{
    uint8_t *p=raw->buff+4;
    
    trace(4,"decode_stqnack: len=%d\n",raw->len);
    
    if (raw->len<9) {
        trace(2,"stq nack length error: len=%d\n",raw->len);
        return -1;
    }
    if (raw->outtype) {
        sprintf(raw->msgtype,"SKYTRAQ NACK  (%4d): msg=0x%02X",raw->len,
                U1(p+1));
    }
    return 0;
}
/* decode skytraq message ----------------------------------------------------*/
static int decode_stq(raw_t *raw)
{
    int type=U1(raw->buff+4);
    uint8_t 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_STQRAWX : return decode_stqrawx(raw);
        case ID_STQGPS  : return decode_stqgps (raw);
        case ID_STQGLO  : return decode_stqglo (raw);
        case ID_STQGLOE : return decode_stqgloe(raw);
        case ID_STQGENE : return decode_stqgene(raw);
        case ID_STQBDSD1: return decode_stqbds (raw);
        case ID_STQBDSD2: return decode_stqbds (raw);
        /* case ID_STQGENE : return decode_stqgene(raw); */
        case ID_STQACK  : return decode_stqack (raw);
        case ID_STQNACK : return decode_stqnack(raw);
    }
    return 0;
}
/* sync code -----------------------------------------------------------------*/
static int sync_stq(uint8_t *buff, uint8_t data)
{
    buff[0]=buff[1]; buff[1]=data;
    return buff[0]==STQSYNC1&&buff[1]==STQSYNC2;
}
/* input skytraq raw message from stream ---------------------------------------
* fetch next skytraq raw data and input a mesasge from stream
* args   : raw_t *raw       IO  receiver raw data control struct
*          uint8_t data     I   stream data (1 byte)
* return : status (-1: error message, 0: no message, 1: input observation data,
*                  2: input ephemeris, 3: input sbas message,
*                  9: input ion/utc parameter)
*
* notes  : to specify input options, set raw->opt to the following option
*          strings separated by spaces.
*
*          -INVCP     : inverse polarity of carrier-phase
*
*-----------------------------------------------------------------------------*/
extern int input_stq(raw_t *raw, uint8_t data)
{
    trace(5,"input_stq: data=%02x\n",data);
    
    /* synchronize frame */
    if (raw->nbyte==0) {
        if (!sync_stq(raw->buff,data)) return 0;
        raw->nbyte=2;
        return 0;
    }
    raw->buff[raw->nbyte++]=data;
    
    if (raw->nbyte==4) {
        if ((raw->len=U2(raw->buff+2)+7)>MAXRAWLEN) {
            trace(2,"stq message length error: len=%d\n",raw->len);
            raw->nbyte=0;
            return -1;
        }
    }
    if (raw->nbyte<4||raw->nbyte<raw->len) return 0;
    raw->nbyte=0;
    
    /* decode skytraq raw message */
    return decode_stq(raw);
}
/* input skytraq raw message from file -----------------------------------------
* fetch next skytraq raw data and input a message from file
* args   : raw_t  *raw      IO  receiver raw data control struct
*          FILE   *fp       I   file pointer
* return : status(-2: end of file, -1...9: same as above)
*-----------------------------------------------------------------------------*/
extern int input_stqf(raw_t *raw, FILE *fp)
{
    int i,data;
    
    trace(4,"input_stqf:\n");
    
    /* synchronize frame */
    if (raw->nbyte==0) {
        for (i=0;;i++) {
            if ((data=fgetc(fp))==EOF) return -2;
            if (sync_stq(raw->buff,(uint8_t)data)) break;
            if (i>=4096) return 0;
        }
    }
    if (fread(raw->buff+2,1,2,fp)<2) return -2;
    raw->nbyte=4;
    
    if ((raw->len=U2(raw->buff+2)+7)>MAXRAWLEN) {
        trace(2,"stq message length error: len=%d\n",raw->len);
        raw->nbyte=0;
        return -1;
    }
    if (fread(raw->buff+4,1,raw->len-4,fp)<(size_t)(raw->len-4)) return -2;
    raw->nbyte=0;
    
    /* decode skytraq raw message */
    return decode_stq(raw);
}
/* generate skytraq binary message ---------------------------------------------
* generate skytraq binary message from message string
* args   : char  *msg       I   message string 
*            "RESTART  [arg...]" system restart
*            "CFG-SERI [arg...]" configure serial port propperty
*            "CFG-FMT  [arg...]" configure output message format
*            "CFG-RATE [arg...]" configure binary measurement output rates
*            "CFG-BIN  [arg...]" configure general binary
*            "GET-GLOEPH [slot]" get glonass ephemeris for freq channel number
*          uint8_t *buff O binary message
* return : length of binary message (0: error)
* note   : see reference [1][2][3][4] for details.
*-----------------------------------------------------------------------------*/
extern int gen_stq(const char *msg, uint8_t *buff)
{
    const char *hz[]={"1Hz","2Hz","4Hz","5Hz","10Hz","20Hz",""};
    uint8_t *q=buff;
    char mbuff[1024],*args[32],*p;
    int i,n,narg=0;
    
    trace(4,"gen_stq: msg=%s\n",msg);
    
    strcpy(mbuff,msg);
    for (p=strtok(mbuff," ");p&&narg<32;p=strtok(NULL," ")) {
        args[narg++]=p;
    }
    if (narg<1) {
        return 0;
    }
    *q++=STQSYNC1;
    *q++=STQSYNC2;
    if (!strcmp(args[0],"RESTART")) {
        *q++=0;
        *q++=15;
        *q++=ID_RESTART;
        *q++=narg>2?(uint8_t)atoi(args[1]):0;
        for (i=1;i<15;i++) *q++=0; /* set all 0 */
    }
    else if (!strcmp(args[0],"CFG-SERI")) {
        *q++=0;
        *q++=4;
        *q++=ID_CFGSERI;
        for (i=1;i<4;i++) *q++=narg>i+1?(uint8_t)atoi(args[i]):0;
    }
    else if (!strcmp(args[0],"CFG-FMT")) {
        *q++=0;
        *q++=3;
        *q++=ID_CFGFMT;
        for (i=1;i<3;i++) *q++=narg>i+1?(uint8_t)atoi(args[i]):0;
    }
    else if (!strcmp(args[0],"CFG-RATE")) {
        *q++=0;
        *q++=8;
        *q++=ID_CFGRATE;
        if (narg>2) {
            for (i=0;*hz[i];i++) if (!strcmp(args[1],hz[i])) break;
            if (*hz[i]) *q++=i; else *q++=(uint8_t)atoi(args[1]);
        }
        else *q++=0;
        for (i=2;i<8;i++) *q++=narg>i+1?(uint8_t)atoi(args[i]):0;
    }
    else if (!strcmp(args[0],"CFG-BIN")) {
        *q++=0;
        *q++=9; /* F/W 1.4.32 */
        *q++=ID_CFGBIN;
        if (narg>2) {
            for (i=0;*hz[i];i++) if (!strcmp(args[1],hz[i])) break;
            if (*hz[i]) *q++=i; else *q++=(uint8_t)atoi(args[1]);
        }
        else *q++=0;
        for (i=2;i<9;i++) *q++=narg>i+1?(uint8_t)atoi(args[i]):0;
    }
    else if (!strcmp(args[0],"GET-GLOEPH")) {
        *q++=0;
        *q++=2;
        *q++=ID_GETGLOEPH;
        *q++=narg>=2?(uint8_t)atoi(args[1]):0;
    }
    else return 0;
    
    n=(int)(q-buff);
    *q++=checksum(buff,n+3);
    *q++=0x0D;
    *q=0x0A;
    
    trace(4,"gen_stq: buff=\n"); traceb(4,buff,n+3);
    return n+3;
}