/* decode ephemeris ----------------------------------------------------------*/
static int decode_gpsephem(int sat, raw_t *raw)
{
eph_t eph={0};
unsigned char *puiTmp = (raw->buff)+2;
unsigned short week;
double toc;
trace(4,"decode_ephem: sat=%2d\n",sat);
eph.crs = R4(&puiTmp[ 2]);
eph.deln = R4(&puiTmp[ 6]) * 1e+3;
eph.M0 = R8(&puiTmp[ 10]);
eph.cuc = R4(&puiTmp[ 18]);
eph.e = R8(&puiTmp[ 22]);
eph.cus = R4(&puiTmp[ 30]);
eph.A = pow(R8(&puiTmp[ 34]), 2);
eph.toes = R8(&puiTmp[ 42]) * 1e-3;
eph.cic = R4(&puiTmp[ 50]);
eph.OMG0 = R8(&puiTmp[ 54]);
eph.cis = R4(&puiTmp[ 62]);
eph.i0 = R8(&puiTmp[ 66]);
eph.crc = R4(&puiTmp[ 74]);
eph.omg = R8(&puiTmp[ 78]);
eph.OMGd = R8(&puiTmp[ 86]) * 1e+3;
eph.idot = R8(&puiTmp[ 94]) * 1e+3;
eph.tgd[0] = R4(&puiTmp[102]) * 1e-3;
toc = R8(&puiTmp[106]) * 1e-3;
eph.f2 = R4(&puiTmp[114]) * 1e+3;
eph.f1 = R4(&puiTmp[118]);
eph.f0 = R4(&puiTmp[122]) * 1e-3;
eph.sva = uraindex(I2(&puiTmp[126]));
eph.iode = I2(&puiTmp[128]);
eph.iodc = I2(&puiTmp[130]);
eph.code = I2(&puiTmp[132]);
eph.flag = I2(&puiTmp[134]);
week = I2(&puiTmp[136]);
eph.fit = 0;
if (week>=4096) {
trace(2,"nvs gps ephemeris week error: sat=%2d week=%d\n",sat,week);
return -1;
}
eph.week=adjgpsweek(week);
eph.toe=gpst2time(eph.week,eph.toes);
eph.toc=gpst2time(eph.week,toc);
eph.ttr=raw->time;
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;
}
/* decode NVS x4btime --------------------------------------------------------*/
static int decode_x4btime(raw_t *raw)
{
unsigned char *p=raw->buff+2;
trace(4,"decode_x4btime: len=%d\n", raw->len);
raw->nav.utc_gps[1] = R8(p );
raw->nav.utc_gps[0] = R8(p+ 8);
raw->nav.utc_gps[2] = I4(p+16);
raw->nav.utc_gps[3] = I2(p+20);
raw->nav.leaps = I1(p+22);
return 9;
}
/* decode skytraq raw channel mesurement -------------------------------------*/
static int decode_stqraw(raw_t *raw)
{
int i,j,iod,prn,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)?R8(p+ 2):0.0;
raw->obs.data[n].L[0] =(ind&0x4)?R8(p+10):0.0;
raw->obs.data[n].D[0] =(ind&0x2)?R4(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 (prn>MAXPRNGPS) prn+=MINPRNSBS-38;
if (!(sat=satno(MINPRNSBS<=prn?SYS_SBS:SYS_GPS,prn))) {
trace(2,"stq raw satellite number error: prn=%d\n",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 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;
}
static int usbtmc_bulk_in_header_read(void *header, uint8_t MsgID,
unsigned char bTag,
int32_t *TransferSize,
uint8_t *bmTransferAttributes)
{
if (R8(header+0) != MsgID ||
R8(header+1) != bTag ||
R8(header+2) != (unsigned char)~bTag)
return SR_ERR;
if (TransferSize)
*TransferSize = RL32(header+4);
if (bmTransferAttributes)
*bmTransferAttributes = R8(header+8);
return SR_OK;
}
/* 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;
}
/**
* Send a Modbus read coils command and receive the corresponding coils values.
*
* @param modbus Previously initialized Modbus device structure.
* @param address The Modbus address of the first coil to read, or -1 to read
* the reply of a previouly sent read coils command.
* @param nb_coils The number of coils to read.
* @param coils Buffer to store all the received coils values (1 bit per coil),
* or NULL to send the read coil command without reading the reply.
*
* @return SR_OK upon success, SR_ERR_ARG upon invalid arguments,
* SR_ERR_DATA upon invalid data, or SR_ERR on failure.
*/
SR_PRIV int sr_modbus_read_coils(struct sr_modbus_dev_inst *modbus,
int address, int nb_coils, uint8_t *coils)
{
uint8_t request[5], reply[2 + (nb_coils + 7) / 8];
int ret;
if (address < -1 || address > 0xFFFF || nb_coils < 1 || nb_coils > 2000)
return SR_ERR_ARG;
W8(request + 0, MODBUS_READ_COILS);
WB16(request + 1, address);
WB16(request + 3, nb_coils);
if (address >= 0) {
ret = sr_modbus_request(modbus, request, sizeof(request));
if (ret != SR_OK)
return ret;
}
if (coils) {
ret = sr_modbus_reply(modbus, reply, sizeof(reply));
if (ret != SR_OK)
return ret;
if (sr_modbus_error_check(reply))
return SR_ERR_DATA;
if (reply[0] != request[0] || R8(reply + 1) != (uint8_t)((nb_coils + 7) / 8))
return SR_ERR_DATA;
memcpy(coils, reply + 2, (nb_coils + 7) / 8);
}
return SR_OK;
}
/**
* Send a Modbus read holding registers command and receive the corresponding
* registers values.
*
* @param modbus Previously initialized Modbus device structure.
* @param address The Modbus address of the first register to read, or -1 to
* read the reply of a previouly sent read registers command.
* @param nb_registers The number of registers to read.
* @param registers Buffer to store all the received registers values,
* or NULL to send the read holding registers command
* without reading the reply.
*
* @return SR_OK upon success, SR_ERR_ARG upon invalid arguments,
* SR_ERR_DATA upon invalid data, or SR_ERR on failure.
*/
SR_PRIV int sr_modbus_read_holding_registers(struct sr_modbus_dev_inst *modbus,
int address, int nb_registers, uint16_t *registers)
{
uint8_t request[5], reply[2 + (2 * nb_registers)];
int ret;
if (address < -1 || address > 0xFFFF
|| nb_registers < 1 || nb_registers > 125)
return SR_ERR_ARG;
W8(request + 0, MODBUS_READ_HOLDING_REGISTERS);
WB16(request + 1, address);
WB16(request + 3, nb_registers);
if (address >= 0) {
ret = sr_modbus_request(modbus, request, sizeof(request));
if (ret != SR_OK)
return ret;
}
if (registers) {
ret = sr_modbus_reply(modbus, reply, sizeof(reply));
if (ret != SR_OK)
return ret;
if (sr_modbus_error_check(reply))
return SR_ERR_DATA;
if (reply[0] != request[0] || R8(reply + 1) != (uint8_t)(2 * nb_registers))
return SR_ERR_DATA;
memcpy(registers, reply + 2, 2 * nb_registers);
}
return SR_OK;
}
void code2(int mode, LPSYMBOL s1, LPSYMBOL s2)
{
switch (mode) {
case AM_R8:
R8(s1->instr, s2->val8);
break;
case AM_R16:
R16(s1->instr, s2->val8);
break;
case AM_M8:
M8(s1->instr, s2->val8);
break;
case AM_M16:
M16(s1->instr, s2->val8);
break;
case AM_A8:
A8(s1->instr, s2->val16);
break;
case AM_A16:
A16(s1->instr, s2->val16);
break;
case AM_I16:
I16(s1->instr, s2->val16);
break;
case AM_I8:
I8(s1->instr, s2->val8);
break;
default:
break;
}
}
/* decode id#21 navigation data (ecef) ---------------------------------------*/
static int decode_ss2ecef(raw_t *raw)
{
unsigned char *p=raw->buff+4;
trace(4,"decode_ss2ecef: len=%d\n",raw->len);
if (raw->len!=85) {
trace(2,"ss2 id#21 length error: len=%d\n",raw->len);
return -1;
}
raw->time=gpst2time(U2(p+8),R8(p));
return 0;
}
/* decode id#20 navigation data (user) ---------------------------------------*/
static int decode_ss2llh(raw_t *raw)
{
double ep[6];
unsigned char *p=raw->buff+4;
trace(4,"decode_ss2llh: len=%d\n",raw->len);
if (raw->len!=77) {
trace(2,"ss2 id#20 length error: len=%d\n",raw->len);
return -1;
}
ep[3]=U1(p ); ep[4]=U1(p+ 1); ep[5]=R8(p+ 2);
ep[2]=U1(p+10); ep[1]=U1(p+11); ep[0]=U2(p+12);
raw->time=utc2gpst(epoch2time(ep));
return 0;
}
/* 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;
}
static int read_probe_eeprom(unsigned int addr, struct probe_eeprom *eeprom)
{
GString *path = g_string_sized_new(64);
char eeprom_buf[EEPROM_SIZE];
ssize_t rd;
int fd;
probe_eeprom_path(addr, path);
fd = g_open(path->str, O_RDONLY);
g_string_free(path, TRUE);
if (fd < 0)
return -1;
rd = read(fd, eeprom_buf, EEPROM_SIZE);
g_close(fd, NULL);
if (rd != EEPROM_SIZE)
return -1;
eeprom->type = RB32(eeprom_buf + EEPROM_OFF_TYPE);
eeprom->rev = RB32(eeprom_buf + EEPROM_OFF_REV);
eeprom->shunt = RB32(eeprom_buf + EEPROM_OFF_SHUNT);
eeprom->pwr_sw = R8(eeprom_buf + EEPROM_OFF_PWR_SW);
/* Don't care about the serial number and tag for now. */
/* Check if we have some sensible values. */
if (eeprom->rev != 'B')
/* 'B' is the only supported revision with EEPROM for now. */
return -1;
if (eeprom->type != EEPROM_PROBE_TYPE_USB &&
eeprom->type != EEPROM_PROBE_TYPE_JACK &&
eeprom->type != EEPROM_PROBE_TYPE_HE10)
return -1;
return 0;
}
FenPrincipale::FenPrincipale()
{
showMaximized();
//showNormal();
QMenu *menuFichier = menuBar()->addMenu("&Fichier");
QAction *actionLoadImage = menuFichier->addAction("&Load Image");
actionLoadImage->setIcon(QIcon("Icons/fileopen.png"));
menuFichier->addAction(actionLoadImage);
QAction *actionExit = menuFichier->addAction("&Quitter");
actionExit->setIcon(QIcon("Icons/fileclose.png"));
menuFichier->addAction(actionExit);
//QMenu *menuTools = menuBar()->addMenu("&Tools");
QMenu *menuComm = menuBar()->addMenu("&Communication");
QAction *actionSend = menuComm->addAction("&Send to server");
actionSend->setIcon(QIcon("Icons/ok.png"));
// Création de la barre d'outils
QToolBar *toolBarFichier = addToolBar("Fichier");
toolBarFichier->addAction(actionLoadImage);
toolBarFichier->addAction(actionExit);
toolBarFichier->addSeparator();
toolBarFichier->addAction(actionSend);
QObject::connect(actionLoadImage, SIGNAL(triggered()), this, SLOT(LoadImageW()));
QObject::connect(actionExit, SIGNAL(triggered()), this, SLOT(close()));
QObject::connect(actionSend, SIGNAL(triggered()), this, SLOT(SendServer()));
QObject::connect(actionSend, SIGNAL(triggered()), this, SLOT(ShowResults()));
//
// Création des docks
//
// dock IMAGE
dockImage = new QDockWidget("Image", this);
setCentralWidget(dockImage);
ImageWidget = new QWidget;
dockImage->setWidget(ImageWidget);
// dock SERVEUR
dockServeur = new QDockWidget("Serveur", this);
addDockWidget(Qt::LeftDockWidgetArea, dockServeur);
QWidget *paramDock = new QWidget;
dockServeur->setWidget(paramDock);
QSize s(220,20);
adressServer1 = new QLineEdit(QString("138.195.102.25"));
adressServer1->setMaximumSize(s);
QHBoxLayout *adressLayout = new QHBoxLayout;
adressLayout->addWidget(adressServer1);
QWidget *adressWidget = new QWidget;
adressWidget->setLayout(adressLayout);
portAdressServer = new QLineEdit(QString("6006"));
portAdressServer->setMaximumSize(s);
QHBoxLayout *portLayout = new QHBoxLayout;
portLayout->addWidget(portAdressServer);
QWidget *portAdressWidget = new QWidget;
portAdressWidget->setLayout(portLayout);
QLabel *adressLabel = new QLabel("Adresse IP",paramDock);
QLabel *portLabel = new QLabel("Port",paramDock);
QPushButton *okServer = new QPushButton("Send");
QObject::connect(okServer, SIGNAL(clicked()),this,SLOT(SendServer()));
QObject::connect(okServer, SIGNAL(clicked()),this,SLOT(ShowResults()));
QVBoxLayout *paramLayout = new QVBoxLayout(paramDock);
paramLayout->addWidget(adressLabel);
paramLayout->addWidget(adressWidget);
paramLayout->addWidget(portLabel);
paramLayout->addWidget(portAdressWidget);
paramLayout->addWidget(okServer);
paramLayout->setAlignment(Qt::AlignLeft);
paramDock->setLayout(paramLayout);
//dockServeur->setGeometry(QRect(100,200,200,250));
dockServeur->setMaximumSize(250,200);
// dock RESULTATS
dockResults = new QDockWidget("Resultats",this);
addDockWidget(Qt::RightDockWidgetArea, dockResults);
dockResults->setMaximumWidth(250);
WidgetResults = new QWidget;
scrollArea = new QScrollArea;
dockResults->setWidget(scrollArea);
QPixmap *mini = new QPixmap(QString("test.jpg"));
Result R(mini,98,"t");
QPixmap *mini2 = new QPixmap(QString("9143.gif.jpg"));
Result R2(mini2,95,"v");
QPixmap *mini3 = new QPixmap(QString("test.jpg"));
Result R3(mini3,92,"t");
QPixmap *mini4 = new QPixmap(QString("test.jpg"));
Result R4(mini4,90,"t");
QPixmap *mini5 = new QPixmap(QString("test.jpg"));
Result R5(mini5,88,"t");
QPixmap *mini6 = new QPixmap(QString("test.jpg"));
Result R6(mini6,86,"t");
QPixmap *mini7 = new QPixmap(QString("test.jpg"));
Result R7(mini7,84,"t");
QPixmap *mini8 = new QPixmap(QString("test.jpg"));
Result R8(mini8,82,"t");
QPixmap *mini9 = new QPixmap(QString("test.jpg"));
Result R9(mini9,81,"t");
QPixmap *mini10 = new QPixmap(QString("test.jpg"));
Result R10(mini10,79,"t");
QPixmap *mini11 = new QPixmap(QString("test.jpg"));
Result R11(mini11,78,"t");
QPixmap *mini12 = new QPixmap(QString("test.jpg"));
Result R12(mini12,77,"t");
QPixmap *mini13 = new QPixmap(QString("test.jpg"));
Result R13(mini13,76,"t");
QPixmap *mini14 = new QPixmap(QString("test.jpg"));
Result R14(mini14,75,"t");
T.push_back(R);
T.push_back(R2);
T.push_back(R3);
T.push_back(R4);
T.push_back(R5);
T.push_back(R6);
T.push_back(R7);
T.push_back(R8);
T.push_back(R9);
T.push_back(R10);
T.push_back(R11);
T.push_back(R12);
T.push_back(R13);
T.push_back(R14);
}
/* decode ublox rxm-raw: raw measurement data --------------------------------*/
static int decode_rxmraw(raw_t *raw)
{
gtime_t time;
double tow,tt,tadj=0.0,toff=0.0,tn;
int i,j,prn,sat,n=0,nsat,week;
unsigned char *p=raw->buff+6;
char *q,tstr[32];
trace(4,"decode_rxmraw: len=%d\n",raw->len);
/* time tag adjustment option (-tadj) */
if ((q=strstr(raw->opt,"-tadj"))) {
sscanf(q,"-tadj %lf",&tadj);
}
tow =U4(p );
week=U2(p+4);
nsat=U1(p+6);
if (raw->len<12+24*nsat) {
trace(2,"ubx rxmraw length error: len=%d nsat=%d\n",raw->len,nsat);
return -1;
}
time=gpst2time(week,tow*0.001);
/* time tag adjustment */
if (tadj>0.0) {
tn=time2gpst(time,&week)/tadj;
toff=(tn-floor(tn+0.5))*tadj;
time=timeadd(time,-toff);
}
if (fabs(tt=timediff(time,raw->time))<=1e-3) {
time2str(time,tstr,3);
trace(2,"ubx rxmraw time tag duplicated: time=%s\n",tstr);
return 0;
}
for (i=0,p+=8;i<nsat&&i<MAXOBS;i++,p+=24) {
raw->obs.data[n].time=time;
raw->obs.data[n].L[0] =R8(p )-toff*FREQ1;
raw->obs.data[n].P[0] =R8(p+ 8)-toff*CLIGHT;
raw->obs.data[n].D[0] =R4(p+16);
prn =U1(p+20);
raw->obs.data[n].SNR[0]=(unsigned char)(I1(p+22)*4.0+0.5);
raw->obs.data[n].LLI[0]=U1(p+23);
raw->obs.data[n].code[0]=CODE_L1C;
/* phase polarity flip option (-invcp) */
if (strstr(raw->opt,"-invcp")) {
raw->obs.data[n].L[0]=-raw->obs.data[n].L[0];
}
if (!(sat=satno(MINPRNSBS<=prn?SYS_SBS:SYS_GPS,prn))) {
trace(2,"ubx rxmraw sat number error: prn=%d\n",prn);
continue;
}
raw->obs.data[n].sat=sat;
if (raw->obs.data[n].LLI[0]&1) raw->lockt[sat-1][0]=0.0;
else if (tt<0.0||10.0<tt) raw->lockt[sat-1][0]=0.0;
else raw->lockt[sat-1][0]+=tt;
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->time=time;
raw->obs.n=n;
return 1;
}
/* 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);
#if 0
raw->obs.data[n].SNR[0]=(unsigned char)(U1(p+1)*4.0+0.5);
#else
raw->obs.data[n].SNR[0]=(unsigned char)(U1(p+1)*8.0+0.5);
#endif
raw->obs.data[n].P[0] =(ind&0x1)?R8(p+ 2):0.0;
raw->obs.data[n].L[0] =(ind&0x4)?R8(p+10):0.0;
raw->obs.data[n].D[0] =(ind&0x2)?R4(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 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;
}
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;
}
TextureFormat const *
TextureFormat::fromCode(Code code)
{
switch (code)
{
case Code::L8:
return L8();
case Code::L16:
return L16();
case Code::L16F:
return L16F();
case Code::L32F:
return L32F();
case Code::A8:
return A8();
case Code::A16:
return A16();
case Code::A16F:
return A16F();
case Code::A32F:
return A32F();
case Code::LA4:
return LA4();
case Code::LA8:
return LA8();
case Code::LA16:
return LA16();
case Code::LA16F:
return LA16F();
break;
case Code::LA32F:
return LA32F();
case Code::RGB5:
return RGB5();
case Code::RGB5A1:
return RGB5A1();
case Code::RGB8:
return RGB8();
case Code::RGB10:
return RGB10();
case Code::RGB10A2:
return RGB10A2();
case Code::RGB16:
return RGB16();
case Code::RGB32F:
return RGB32F();
case Code::R11G11B10F:
return R11G11B10F();
case Code::RGB9E5F:
return RGB9E5F();
case Code::RGB8I:
return RGB8I();
case Code::RGB8UI:
return RGB8UI();
case Code::ARGB8:
return NULL;
case Code::BGR8:
return BGR8();
case Code::BGRA8:
return BGRA8();
case Code::BGR16:
return BGR16();
case Code::BGRA16:
return BGRA16();
case Code::BGR32F:
return BGR32F();
case Code::BGRA32F:
return BGRA32F();
case Code::R8:
return R8();
case Code::RG8:
return RG8();
case Code::RG8I:
return RG8I();
case Code::RG8UI:
return RG8UI();
case Code::RG16F:
return RG16F();
case Code::RGBA8:
return RGBA8();
case Code::RGBA16:
return RGBA16();
case Code::RGBA16F:
return RGBA16F();
case Code::RGBA32F:
return RGBA32F();
case Code::RGBA32UI:
return RGBA32UI();
case Code::BAYER_RGGB8:
// TODO
case Code::BAYER_GRBG8:
// TODO
case Code::BAYER_GBRG8:
// TODO
case Code::BAYER_BGGR8:
// TODO
case Code::BAYER_RGGB32F:
// TODO
case Code::BAYER_GRBG32F:
// TODO
case Code::BAYER_GBRG32F:
// TODO
case Code::BAYER_BGGR32F:
// TODO
case Code::HSV8:
// TODO
case Code::HSV32F:
// TODO
return NULL;
break;
case Code::RGB_DXT1:
return RGB_DXT1();
break;
case Code::RGBA_DXT1:
return RGBA_DXT1();
break;
case Code::RGBA_DXT3:
return RGBA_DXT3();
break;
case Code::RGBA_DXT5:
return RGBA_DXT5();
break;
case Code::SRGB8:
return SRGB8();
break;
case Code::SRGBA8:
return SRGBA8();
break;
case Code::SL8:
return SL8();
break;
case Code::SLA8:
return SLA8();
break;
case Code::SRGB_DXT1:
return SRGB_DXT1();
break;
case Code::SRGBA_DXT1:
return SRGBA_DXT1();
break;
case Code::SRGBA_DXT3:
return SRGBA_DXT3();
break;
case Code::SRGBA_DXT5:
return SRGBA_DXT5();
break;
case Code::DEPTH16:
return DEPTH16();
break;
case Code::DEPTH24:
return DEPTH24();
break;
case Code::DEPTH32:
return DEPTH32();
break;
case Code::DEPTH32F:
return DEPTH32F();
break;
case Code::STENCIL1:
return STENCIL1();
break;
case Code::STENCIL4:
return STENCIL4();
break;
case Code::STENCIL8:
return STENCIL8();
break;
case Code::STENCIL16:
return STENCIL16();
break;
case Code::DEPTH24_STENCIL8:
return DEPTH24_STENCIL8();
break;
case Code::YUV420_PLANAR:
return YUV420_PLANAR();
break;
case Code::YUV422:
return YUV422();
break;
case Code::YUV444:
return YUV444();
break;
default:
return NULL;
}
}
void process_dvi_data(UINT8* dvi_data, int baseoffset, int regionsize)
{
DVI_Header DVI;
UINT8* currptr = dvi_data + baseoffset;
DVI.dv1 = R32(&currptr);
DVI.dv2 = R16(&currptr);
DVI.dv3 = R16(&currptr);
DVI.dv4 = R32(&currptr);
dviprintf("DVI Header\n");
dviprintf("Header ID %08x %c%c%c%c\n", DVI.dv1, (DVI.dv1>>24)&0xff, (DVI.dv1>>16)&0xff, (DVI.dv1>>8)&0xff, (DVI.dv1>>0)&0xff);
dviprintf("Header Size (or 1) %04x\n", DVI.dv2);
dviprintf("Header Version %04x\n", DVI.dv3);
dviprintf("Annotation Offset %08x\n", DVI.dv4);
if (DVI.dv1 != 0x56445649) { dviprintf("Header Error\n"); return; }
dviprintf("\n");
AVSS_Header AVSS;
AVSS.av1 = R32(&currptr);
AVSS.av2 = R16(&currptr);
AVSS.av3 = R16(&currptr);
AVSS.av4 = R16(&currptr);
AVSS.av5 = R16(&currptr);
AVSS.av6 = R32(&currptr);
AVSS.av7 = R16(&currptr);
AVSS.av8 = R16(&currptr);
AVSS.av9 = R16(&currptr);
AVSS.av10 = R16(&currptr);
AVSS.av11 = R32(&currptr);
AVSS.av12 = R32(&currptr);
AVSS.av13 = R32(&currptr);
AVSS.av14 = R32(&currptr);
AVSS.av15 = R16(&currptr);
AVSS.av16 = R16(&currptr);
AVSS.av17 = R32(&currptr);
AVSS.av18 = R16(&currptr);
AVSS.av19 = R16(&currptr);
AVSS.av20 = R32(&currptr);
AVSS.av21 = R32(&currptr);
AVSS.av22 = R32(&currptr);
AVSS.av23 = R32(&currptr);
AVSS.av24 = R16(&currptr);
AVSS.av25 = R16(&currptr);
AVSS.av26 = R32(&currptr);
AVSS.av27 = R16(&currptr);
AVSS.av28 = R16(&currptr);
AVSS.av29 = R32(&currptr);
AVSS.av30 = R32(&currptr);
for (int i=0;i<32;i++) { AVSS.av31[i] = R8(&currptr); }
dviprintf(" AVSS Header\n");
dviprintf(" Header ID %08x %c%c%c%c\n", AVSS.av1, (AVSS.av1>>24)&0xff, (AVSS.av1>>16)&0xff, (AVSS.av1>>8)&0xff, (AVSS.av1>>0)&0xff);
dviprintf(" Header Size %04x\n", AVSS.av2);
dviprintf(" Header Version %04x\n", AVSS.av3);
dviprintf(" Stream Group Count %04x\n", AVSS.av4);
dviprintf(" Stream Group Size %04x\n", AVSS.av5);
dviprintf(" Stream Group Offs %08x\n", AVSS.av6);
dviprintf(" Stream Group Vers %04x\n", AVSS.av7);
dviprintf(" Stream Size %04x\n", AVSS.av8);
dviprintf(" Stream Version %04x\n", AVSS.av9);
dviprintf(" Stream Count %04x\n", AVSS.av10);
dviprintf(" Stream Offset %08x\n", AVSS.av11);
dviprintf(" Header Pool Offset %08x\n", AVSS.av12);
dviprintf(" Label Count %08x\n", AVSS.av13);
dviprintf(" Label Offset %08x\n", AVSS.av14);
dviprintf(" Label Size %04x\n", AVSS.av15);
dviprintf(" Label Version %04x\n", AVSS.av16);
dviprintf(" Vid Seq Hdr Offset %08x\n", AVSS.av17);
dviprintf(" Vid Seq Hdr Size %04x\n", AVSS.av18);
dviprintf(" Frame Version %04x\n", AVSS.av19);
dviprintf(" Frame Count %08x\n", AVSS.av20);
dviprintf(" Frame Size %08x\n", AVSS.av21);
dviprintf(" First Frame Offset %08x\n", AVSS.av22);
dviprintf(" EO Frame Offset %08x\n", AVSS.av23);
dviprintf(" Frame Header Size %04x\n", AVSS.av24);
dviprintf(" Frame Dir Size %04x\n", AVSS.av25);
dviprintf(" Frame Dir Offset %08x\n", AVSS.av26);
dviprintf(" Frame Dir Vers %04x\n", AVSS.av27);
dviprintf(" Frame PerSec %04x\n", AVSS.av28);
dviprintf(" UpdateFlag %08x\n", AVSS.av29);
dviprintf(" FreeBlock %08x\n", AVSS.av30);
dviprintf(" Patch "); for (int i=0;i<32;i++) { dviprintf("%02x", AVSS.av31[i]); }; dviprintf("\n");
dviprintf("\n");
for (int s=0;s<AVSS.av10;s++)
{
STRM_Header STRM;
STRM.st1 = R32(&currptr);
STRM.st2 = R16(&currptr);
STRM.st3 = R16(&currptr);
STRM.st4 = R16(&currptr);
STRM.st5 = R16(&currptr);
STRM.st6 = R16(&currptr);
STRM.st7 = R16(&currptr);
STRM.st8 = R16(&currptr);
STRM.st9 = R16(&currptr);
STRM.st10 = R32(&currptr);
STRM.st11 = R32(&currptr);
for (int i=0;i<16;i++) { STRM.st12[i] = R8(&currptr); }
dviprintf(" STRM Header\n");
dviprintf(" Header ID %08x %c%c%c%c\n", STRM.st1, (STRM.st1>>24)&0xff, (STRM.st1>>16)&0xff, (STRM.st1>>8)&0xff, (STRM.st1>>0)&0xff);
dviprintf(" Header Size %04x\n", STRM.st2);
dviprintf(" Type %04x\n", STRM.st3);
dviprintf(" SubType %04x\n", STRM.st4);
dviprintf(" Header Count %04x\n", STRM.st5);
dviprintf(" Next Stream Number %04x\n", STRM.st6);
dviprintf(" Stream Group Numbr %04x\n", STRM.st7);
dviprintf(" Padding %04x\n", STRM.st8);
dviprintf(" Flag %04x\n", STRM.st9);
dviprintf(" FrameSize %08x\n", STRM.st10);
dviprintf(" FirstHeaderOffset %08x\n", STRM.st11);
dviprintf(" Name "); for (int i=0;i<16;i++) { dviprintf("%02x", STRM.st12[i]); }; dviprintf("\n");
dviprintf("\n");
UINT8* subptr = dvi_data+STRM.st11+baseoffset;
for (int h=0;h<STRM.st5;h++)
{
// I believe in these can be either CIMG or AUDI blocks, but we don't have any of the latter
CIMG_Header CIMG;
CIMG.ci1 = R32(&subptr);
CIMG.ci2 = R16(&subptr);
CIMG.ci3 = R16(&subptr);
for (int i=0;i<80;i++) { CIMG.ci4[i] = R8(&subptr); }
CIMG.ci5 = R32(&subptr);
CIMG.ci6 = R16(&subptr);
CIMG.ci7 = R16(&subptr);
CIMG.ci8 = R32(&subptr);
CIMG.ci9 = R32(&subptr);
CIMG.ci10 = R16(&subptr);
CIMG.ci11 = R16(&subptr);
CIMG.ci12 = R16(&subptr);
CIMG.ci13 = R16(&subptr);
CIMG.ci14 = R16(&subptr);
CIMG.ci15 = R16(&subptr);
CIMG.ci16 = R16(&subptr);
CIMG.ci17 = R16(&subptr);
CIMG.ci18 = R32(&subptr);
CIMG.ci19 = R16(&subptr);
CIMG.ci20 = R16(&subptr);
CIMG.ci21 = R16(&subptr);
CIMG.ci22 = R16(&subptr);
CIMG.ci23 = R32(&subptr);
dviprintf(" CIMG Header\n");
dviprintf(" Header ID %08x %c%c%c%c\n", CIMG.ci1, (CIMG.ci1>>24)&0xff, (CIMG.ci1>>16)&0xff, (CIMG.ci1>>8)&0xff, (CIMG.ci1>>0)&0xff);
dviprintf(" Header Size %04x\n", CIMG.ci2);
dviprintf(" Version %04x\n", CIMG.ci3);
dviprintf(" Original Name "); for (int i=0;i<80;i++) { dviprintf("%02x", CIMG.ci4[i]); }; dviprintf("\n");
dviprintf(" Original Frame %08x\n", CIMG.ci5);
dviprintf(" Original Stream %04x\n", CIMG.ci6);
dviprintf(" Padding %04x\n", CIMG.ci7);
dviprintf(" Frame Count %08x\n", CIMG.ci8);
dviprintf(" Next Header Offset %08x\n", CIMG.ci9);
dviprintf(" X Position %04x\n", CIMG.ci10);
dviprintf(" Y Position %04x\n", CIMG.ci11);
dviprintf(" X Length %04x\n", CIMG.ci12);
dviprintf(" Y Length %04x\n", CIMG.ci13);
dviprintf(" X Crop %04x\n", CIMG.ci14);
dviprintf(" Y Crop %04x\n", CIMG.ci15);
dviprintf(" Drop Frame %04x\n", CIMG.ci16);
dviprintf(" Drop Phrase %04x\n", CIMG.ci17);
dviprintf(" Still Period %08x\n", CIMG.ci18);
dviprintf(" Buffer Minimum %04x\n", CIMG.ci19);
dviprintf(" Buffer Maximum %04x\n", CIMG.ci20);
dviprintf(" Decode Algorithm %04x\n", CIMG.ci21);
dviprintf(" Padding(2) %04x\n", CIMG.ci22);
dviprintf(" DCFID %08x\n", CIMG.ci23);
dviprintf("\n");
subptr = dvi_data+CIMG.ci9+baseoffset;
}
}
/* Frame Dictionaries etc. */
/* is this just a seek table of sorts? the first doesn't have an entry (pointed to by First Frame Offset - AVSS.av22) the first two here are actually bad values, and the last one points to nothing */
/* some entries also seem to point to the wrong places?? I'm guessing this isn't used when playing back the data ... it seems like the upper and lower words are out of sync... */
dviprintf("Frame Dictionaries\n\n");
UINT8* frameptr = dvi_data + AVSS.av26 + 2 + baseoffset; // +2 ??
// UINT8* frameptr = dvi_data + AVSS.av26 + baseoffset;
for (int f=0;f<AVSS.av20;f++)
{
FDICT_Header FDICT;
FDICT.fd1 = R32(&frameptr);
FDICT.fd1 = ((FDICT.fd1 & 0xffff0000)>>16) | ((FDICT.fd1 & 0x0000ffff)<<16);
dviprintf(" %04d Frame Offset %08x\n", f, FDICT.fd1);
UINT8* frameptr2 = dvi_data + ((FDICT.fd1 + baseoffset)&(regionsize-1)) ;
FHEAD_Header FHEAD;
FHEAD.fh1 = R32(&frameptr2);
FHEAD.fh2 = R32(&frameptr2);
FHEAD.fh3 = R32(&frameptr2);
dviprintf(" Frame Num %08x\n", FHEAD.fh1);
dviprintf(" Previous Offset %08x\n", FHEAD.fh2);
dviprintf(" Frame Checksum %08x\n", FHEAD.fh3);
}
dviprintf("\n");
}
/* decode ubx-rxm-rawx: multi-gnss raw measurement data -----------*/
int decode_rxmrawx(tUbxRawData *raw)
{
double tow;
short i, j, nsat;
unsigned char gnssID, trackingStatus, halfv, halfc[UBX_MAX_NUMCHAN], tempHalfc;
double pr, dr, cp;
unsigned short lockt[UBX_MAX_NUMCHAN], tempLockt, week;
unsigned char *p = raw->buff + 6;
tExternalGNSS_Meas rawchan[UBX_MAX_NUMCHAN]; /* 32 channels */
tExternalGNSS_Meas temprawchan;
tGnssChanCycleSlipPtr lastCycleSlip = Msg_Get_CycleSlip();
tExternalGNSS_MeasPtr readyRawChan = Msg_Get_ExternalGNSS_RawChan(); /* 20 channels */
memset(rawchan, 0, UBX_MAX_NUMCHAN*sizeof(tExternalGNSS_Meas));
memset(&temprawchan, 0, sizeof(tExternalGNSS_Meas));
memset(readyRawChan, 0, MAX_GNSS_NUM_CHANS*sizeof(tExternalGNSS_Meas));
memset(halfc, 0, sizeof(halfc));
memset(lockt, 0, sizeof(lockt));
tow = R8(p);
week = U2(p + 8);
if (week == 0)
return 0;
nsat = (short)U1(p + 11);
nsat = MINUBX(nsat, UBX_MAX_NUMCHAN); /* up to 32 channels */
if(nsat == 0)
return -1;
for (i = 0, p += 16; i < nsat; i++, p += 32)
{
rawchan[i].measTOW = (unsigned int)(tow * 1000 + 0.5); /* same software time for all channels */
gnssID = (unsigned char)(U1(p + 20));
rawchan[i].svid = (unsigned short)(U1(p + 21));
/* gnssID: 0 -- GPS, keep the same ID, 1 -- SBAS, keep the same ID */
if (gnssID == GALILEO_SYS_ID) /* Galileo */
rawchan[i].svid += GALILEO_LOW_SVID;
else if (gnssID == BDS_SYS_ID) /* BDS */
rawchan[i].svid += BDS_LOW_SVID;
else if (gnssID == GLONASS_SYS_ID) /* GLONASS */
rawchan[i].svid += GLONASS_LOW_SVID; /* 65 ~ 96 */
else if (gnssID == IMES_SYS_ID)
rawchan[i].svid += IMES_LOW_SVID; /* IMES */
else if (gnssID == QZSS_SYS_ID) /* QZSS */
rawchan[i].svid += QZSS_LOW_SVID;
trackingStatus = U1(p + 30); /* tracking status */
pr = trackingStatus & 1 ? R8(p) : 0.0;
cp = trackingStatus & 2 ? R8(p + 8) : 0.0; /* in L1 cycles */
dr = R4(p + 16); /* in Hz */
if (cp == -0.5) cp = 0.0; /* invalid phase */
if (trackingStatus & 0x01)
{
rawchan[i].MeasurmentState = (unsigned int)(UBXGNSS_MEASUREMENT_STATE_CODE_LOCK | UBXGNSS_MEASUREMENT_STATE_BIT_SYNC |
UBXGNSS_MEASUREMENT_STATE_SUBFRAME_SYNC | UBXGNSS_MEASUREMENT_STATE_TOW_DECODED);
if (trackingStatus & 0x02)
rawchan[i].MeasurmentState |= UBXGNSS_MEASUREMENT_STATE_CARRIER_PULLIN;
}
else
rawchan[i].MeasurmentState = 0;
rawchan[i].Pseudorange = (double)pr; /* in m */
rawchan[i].CarrierPhase = (double)(cp * L1_LENGTH_UBX); /* in m */
rawchan[i].DeltaRange = (double)(dr * L1_LENGTH_UBX); /* in m/s */
rawchan[i].Cn0 = (unsigned short)(U1(p + 26) * 10);
halfv = trackingStatus & 4 ? 1 : 0; /* half cycle valid */
halfc[i] = trackingStatus & 8 ? 1 : 0; /* half cycle subtracted from phase */
rawchan[i].HalfCycleStatus |= ((halfv ? 0 : 1) << 0); /* bit 0, if set, half cycle invalid */
lockt[i] = U2(p + 24); /* lock time count (ms) */
} // end of i
/*------ sort channels in ascending order according to signal level -----*/
for (j = nsat - 1; j > 0; j--)
{
for (i = 0; i < j; i++)
{
if ( rawchan[i+1].Cn0 > rawchan[i].Cn0 ) /* sorting by signal strength */
{
memcpy(&temprawchan, &rawchan[i], sizeof(tExternalGNSS_Meas));
memcpy(&rawchan[i], &rawchan[i+1], sizeof(tExternalGNSS_Meas));
memcpy(&rawchan[i+1], &temprawchan, sizeof(tExternalGNSS_Meas));
memcpy(&tempHalfc, &halfc[i], sizeof(unsigned char));
memcpy(&halfc[i], &halfc[i+1], sizeof(unsigned char));
memcpy(&halfc[i+1], &tempHalfc, sizeof(unsigned char));
memcpy(&tempLockt, &lockt[i], sizeof(unsigned short));
memcpy(&lockt[i], &lockt[i+1], sizeof(unsigned short));
memcpy(&lockt[i+1], &tempLockt, sizeof(unsigned short));
}
}
}
/* 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(&readyRawChan[i], &rawchan[i], sizeof(tExternalGNSS_Meas));
/*----------- flag if cycle slip happens ----------- */
readyRawChan[i].CycleSlip = 1;
for (j = 0; j < MAX_GNSS_NUM_CHANS; j++)
{
if (readyRawChan[i].svid == lastCycleSlip[j].svid)
{
if (lastCycleSlip[j].locktime > 0 && lastCycleSlip[j].locktime <= lockt[i])
readyRawChan[i].CycleSlip = 0; /* no cycle slip happened */
else
readyRawChan[i].CycleSlip = 1;
if (readyRawChan[i].CarrierPhase != 0.0)
{
readyRawChan[i].HalfCycleStatus |= (((halfc[i] != lastCycleSlip[j].halfcyclesubtracted) ? 1 : 0) << 1);
}
break;
}
} // end of j
} // end of i
for (i = 0; i<nsat; i++) /* update cycleslip variable */
{
lastCycleSlip[i].svid = readyRawChan[i].svid;
lastCycleSlip[i].locktime = (unsigned short)(lockt[i]);
lastCycleSlip[i].halfcyclesubtracted = halfc[i];
}
return 1;
}
/* decode binex mesaage 0x01-06: decoded qzss ephmemeris ---------------------*/
static int decode_bnx_01_06(raw_t *raw, unsigned char *buff, int len)
{
eph_t eph={0};
unsigned char *p=buff;
double tow,ura,sqrtA;
int prn,flag;
trace(4,"binex 0x01-06: len=%d\n",len);
if (len>=127) {
prn =U1(p); p+=1;
eph.week =U2(p); p+=2;
tow =I4(p); p+=4;
eph.toes =I4(p); p+=4;
eph.tgd[0]=R4(p); p+=4;
eph.iodc =I4(p); p+=4;
eph.f2 =R4(p); p+=4;
eph.f1 =R4(p); p+=4;
eph.f0 =R4(p); p+=4;
eph.iode =I4(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;
ura =R4(p)*0.1; p+=4;
eph.svh =U2(p); p+=2;
flag =U2(p);
}
else {
trace(2,"binex 0x01-06: length error len=%d\n",len);
return -1;
}
if (!(eph.sat=satno(SYS_QZS,prn))) {
trace(2,"binex 0x01-06: satellite error prn=%d\n",prn);
return 0;
}
eph.A=sqrtA*sqrtA;
eph.toe=gpst2time(eph.week,eph.toes);
eph.toc=gpst2time(eph.week,eph.toes);
eph.ttr=adjweek(eph.toe,tow);
eph.fit=(flag&0x01)?0.0:2.0; /* 0:2hr,1:>2hr */
eph.sva=uraindex(ura);
eph.code=2; /* codes on L2 channel */
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;
}
/* 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;
}
/* decode binex mesaage 0x01-04: decoded galileo ephmemeris ------------------*/
static int decode_bnx_01_04(raw_t *raw, unsigned char *buff, int len)
{
eph_t eph={0};
unsigned char *p=buff;
double tow,ura,sqrtA;
int prn;
trace(4,"binex 0x01-04: len=%d\n",len);
if (len>=127) {
prn =U1(p)+1; p+=1;
eph.week =U2(p); p+=2;
tow =I4(p); p+=4;
eph.toes =I4(p); p+=4;
eph.tgd[0]=R4(p); p+=4; /* BGD E5a/E1 */
eph.tgd[1]=R4(p); p+=4; /* BGD E5b/E1 */
eph.iode =I4(p); p+=4; /* IODnav */
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;
ura =R4(p)*0.1; p+=4;
eph.svh =U2(p); p+=2;
eph.code =U2(p); /* data source */
}
else {
trace(2,"binex 0x01-04: length error len=%d\n",len);
return -1;
}
if (!(eph.sat=satno(SYS_GAL,prn))) {
trace(2,"binex 0x01-04: satellite error prn=%d\n",prn);
return -1;
}
eph.A=sqrtA*sqrtA;
eph.iode=eph.iodc;
eph.toe=gpst2time(eph.week,eph.toes);
eph.toc=gpst2time(eph.week,eph.toes);
eph.ttr=adjweek(eph.toe,tow);
eph.sva=uraindex(ura);
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;
}
/* decode gloephem -----------------------------------------------------------*/
static int decode_gloephem(int sat, raw_t *raw)
{
geph_t geph={0};
unsigned char *p=(raw->buff)+2;
int prn,tk,tb;
if (raw->len>=93) {
prn =I1(p+ 1);
geph.frq =I1(p+ 2);
geph.pos[0]=R8(p+ 3);
geph.pos[1]=R8(p+11);
geph.pos[2]=R8(p+19);
geph.vel[0]=R8(p+27) * 1e+3;
geph.vel[1]=R8(p+35) * 1e+3;
geph.vel[2]=R8(p+43) * 1e+3;
geph.acc[0]=R8(p+51) * 1e+6;
geph.acc[1]=R8(p+59) * 1e+6;
geph.acc[2]=R8(p+67) * 1e+6;
tb = R8(p+75) * 1e-3;
tk = tb;
geph.gamn =R4(p+83);
geph.taun =R4(p+87) * 1e-3;
geph.age =I2(p+91);
}
else {
trace(2,"nvs NE length error: len=%d\n",raw->len);
return -1;
}
if (!(geph.sat=satno(SYS_GLO,prn))) {
trace(2,"nvs NE satellite error: prn=%d\n",prn);
return -1;
}
if (raw->time.time==0) return 0;
geph.iode=(tb/900)&0x3F;
geph.toe=utc2gpst(adjday(raw->time,tb-10800.0));
geph.tof=utc2gpst(adjday(raw->time,tk-10800.0));
#if 0
/* check illegal ephemeris by toe */
tt=timediff(raw->time,geph.toe);
if (fabs(tt)>3600.0) {
trace(3,"nvs NE illegal toe: prn=%2d tt=%6.0f\n",prn,tt);
return 0;
}
#endif
#if 0
/* check illegal ephemeris by frequency number consistency */
if (raw->nav.geph[prn-MINPRNGLO].toe.time&&
geph.frq!=raw->nav.geph[prn-MINPRNGLO].frq) {
trace(2,"nvs NE illegal freq change: prn=%2d frq=%2d->%2d\n",prn,
raw->nav.geph[prn-MINPRNGLO].frq,geph.frq);
return -1;
}
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;
}
#endif
raw->nav.geph[prn-1]=geph;
raw->ephsat=geph.sat;
return 2;
}
/* 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 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;
}
/* decode NVS xf5-raw: raw measurement data ----------------------------------*/
static int decode_xf5raw(raw_t *raw)
{
gtime_t time;
double tadj=0.0,toff=0.0,tn;
int dTowInt;
double dTowUTC, dTowGPS, dTowFrac, L1, P1, D1;
double gpsutcTimescale;
unsigned char rcvTimeScaleCorr, sys, carrNo;
int i,j,prn,sat,n=0,nsat,week;
unsigned char *p=raw->buff+2;
char *q,tstr[32],flag;
trace(4,"decode_xf5raw: len=%d\n",raw->len);
/* time tag adjustment option (-TADJ) */
if ((q=strstr(raw->opt,"-tadj"))) {
sscanf(q,"-TADJ=%lf",&tadj);
}
dTowUTC =R8(p);
week = U2(p+8);
gpsutcTimescale = R8(p+10);
/* glonassutcTimescale = R8(p+18); */
rcvTimeScaleCorr = I1(p+26);
/* check gps week range */
if (week>=4096) {
trace(2,"nvs xf5raw obs week error: week=%d\n",week);
return -1;
}
week=adjgpsweek(week);
if ((raw->len - 31)%30) {
/* Message length is not correct: there could be an error in the stream */
trace(2,"nvs xf5raw len=%d seems not be correct\n",raw->len);
return -1;
}
nsat = (raw->len - 31)/30;
dTowGPS = dTowUTC + gpsutcTimescale;
/* Tweak pseudoranges to allow Rinex to represent the NVS time of measure */
dTowInt = 10.0*floor((dTowGPS/10.0)+0.5);
dTowFrac = dTowGPS - (double) dTowInt;
time=gpst2time(week, dTowInt*0.001);
/* time tag adjustment */
if (tadj>0.0) {
tn=time2gpst(time,&week)/tadj;
toff=(tn-floor(tn+0.5))*tadj;
time=timeadd(time,-toff);
}
/* check time tag jump */
if (raw->time.time&&fabs(timediff(time,raw->time))>86400.0) {
time2str(time,tstr,3);
trace(2,"nvs xf5raw time tag jump error: time=%s\n",tstr);
return 0;
}
if (fabs(timediff(time,raw->time))<=1e-3) {
time2str(time,tstr,3);
trace(2,"nvs xf5raw time tag duplicated: time=%s\n",tstr);
return 0;
}
for (i=0,p+=27;(i<nsat) && (n<MAXOBS); i++,p+=30) {
raw->obs.data[n].time = time;
sys = (U1(p)==1)?SYS_GLO:((U1(p)==2)?SYS_GPS:((U1(p)==4)?SYS_SBS:SYS_NONE));
prn = U1(p+1);
if (sys == SYS_SBS) prn += 120; /* Correct this */
if (!(sat=satno(sys,prn))) {
trace(2,"nvs xf5raw satellite number error: sys=%d prn=%d\n",sys,prn);
continue;
}
carrNo = I1(p+2);
L1 = R8(p+ 4);
P1 = R8(p+12);
D1 = R8(p+20);
/* check range error */
if (L1<-1E10||L1>1E10||P1<-1E10||P1>1E10||D1<-1E5||D1>1E5) {
trace(2,"nvs xf5raw obs range error: sat=%2d L1=%12.5e P1=%12.5e D1=%12.5e\n",
sat,L1,P1,D1);
continue;
}
raw->obs.data[n].SNR[0]=(unsigned char)(I1(p+3)*4.0+0.5);
if (sys==SYS_GLO) {
raw->obs.data[n].L[0] = L1 - toff*(FREQ1_GLO+DFRQ1_GLO*carrNo);
} else {
raw->obs.data[n].L[0] = L1 - toff*FREQ1;
}
raw->obs.data[n].P[0] = (P1-dTowFrac)*CLIGHT*0.001 - toff*CLIGHT; /* in ms, needs to be converted */
raw->obs.data[n].D[0] = (float)D1;
/* set LLI if meas flag 4 (carrier phase present) off -> on */
flag=U1(p+28);
raw->obs.data[n].LLI[0]=(flag&0x08)&&!(raw->halfc[sat-1][0]&0x08)?1:0;
raw->halfc[sat-1][0]=flag;
#if 0
if (raw->obs.data[n].SNR[0] > 160) {
time2str(time,tstr,3);
trace(2,"%s, obs.data[%d]: SNR=%.3f LLI=0x%02x\n", tstr,
n, (raw->obs.data[n].SNR[0])/4.0, U1(p+28) );
}
#endif
raw->obs.data[n].code[0] = CODE_L1C;
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->time=time;
raw->obs.n=n;
return 1;
}