/* 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;
}
static void yv12_test(void)
{
HRESULT hr;
DDSURFACEDESC2 desc;
IDirectDrawSurface7 *surface;
surface = create_overlay(256, 256, MAKEFOURCC('Y','V','1','2'));
if(!surface) {
skip("YV12 surfaces not available\n");
return;
}
memset(&desc, 0, sizeof(desc));
desc.dwSize = sizeof(desc);
hr = IDirectDrawSurface7_Lock(surface, NULL, &desc, 0, NULL);
ok(hr == DD_OK, "IDirectDrawSurface7_Lock returned 0x%08x, expected DD_OK\n", hr);
ok(desc.dwFlags == (DDSD_WIDTH | DDSD_HEIGHT | DDSD_PIXELFORMAT | DDSD_CAPS | DDSD_PITCH),
"Unexpected desc.dwFlags 0x%08x\n", desc.dwFlags);
ok(desc.ddsCaps.dwCaps == (DDSCAPS_OVERLAY | DDSCAPS_VIDEOMEMORY | DDSCAPS_LOCALVIDMEM) ||
desc.ddsCaps.dwCaps == (DDSCAPS_OVERLAY | DDSCAPS_VIDEOMEMORY | DDSCAPS_LOCALVIDMEM | DDSCAPS_HWCODEC),
"Unexpected desc.ddsCaps.dwCaps 0x%08x\n", desc.ddsCaps.dwCaps);
ok(desc.dwWidth == 256 && desc.dwHeight == 256, "Expected size 256x256, got %ux%u\n",
desc.dwWidth, desc.dwHeight);
/* The overlay pitch seems to have 256 byte alignment */
ok((U1(desc).lPitch & 0xff) == 0, "Expected 256 byte aligned pitch, got %u\n", U1(desc).lPitch);
hr = IDirectDrawSurface7_Unlock(surface, NULL);
ok(hr == DD_OK, "IDirectDrawSurface7_Unlock returned 0x%08x, expected DD_OK\n", hr);
IDirectDrawSurface7_Release(surface);
}
/* 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;
}
/* input superstar 2 raw message from file -------------------------------------
* input next superstar 2 raw 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_ss2f(raw_t *raw, FILE *fp)
{
int i,data;
trace(4,"input_ss2f:\n");
/* synchronize frame */
if (raw->nbyte==0) {
for (i=0;;i++) {
if ((data=fgetc(fp))==EOF) return -2;
if (sync_ss2(raw->buff,(unsigned char)data)) break;
if (i>=4096) return 0;
}
}
if (fread(raw->buff+3,1,1,fp)<1) return -2;
raw->nbyte=4;
if ((raw->len=U1(raw->buff+3)+6)>MAXRAWLEN) {
trace(2,"ss2 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 superstar 2 raw message */
return decode_ss2(raw);
}
/* input superstar 2 raw message from stream -----------------------------------
* input next superstar 2 raw message from stream
* args : raw_t *raw IO receiver raw data control struct
* unsigned char 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 : needs #20 or #21 message to get proper week number of #23 raw
* observation data
*-----------------------------------------------------------------------------*/
extern int input_ss2(raw_t *raw, unsigned char data)
{
trace(5,"input_ss2: data=%02x\n",data);
/* synchronize frame */
if (raw->nbyte==0) {
if (!sync_ss2(raw->buff,data)) return 0;
raw->nbyte=3;
return 0;
}
raw->buff[raw->nbyte++]=data;
if (raw->nbyte==4) {
if ((raw->len=U1(raw->buff+3)+6)>MAXRAWLEN) {
trace(2,"ss2 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 superstar 2 raw message */
return decode_ss2(raw);
}
void rubikStep(char *step)
{
u8 m=0;
for(m=0;step[m]!=0;m++)
{
switch(step[m])
{
case 7:allright90();break;
case 11:F1();break;
case 12:F2();break;
case 13:F3();break;
case 21:B1();break;
case 22:B2();break;
case 23:B3();break;
case 31:R1();break;
case 32:R2();break;
case 33:R3();break;
case 41:L1();break;
case 42:L2();break;
case 43:L3();break;
case 51:U1();break;
case 52:U2();break;
case 53:U3();break;
case 61:D1();break;
case 62:D2();break;
case 63:D3();break;
default:break;
}
}
}
/* decode ublox rxm-sfrb: subframe buffer ------------------------------------*/
static int decode_rxmsfrb(raw_t *raw)
{
unsigned int words[10];
int i,prn,sat,sys,id;
unsigned char *p=raw->buff+6;
trace(4,"decode_rxmsfrb: len=%d\n",raw->len);
if (raw->len<42) {
trace(2,"ubx rxmsfrb length error: len=%d\n",raw->len);
return -1;
}
prn=U1(p+1);
if (!(sat=satno(MINPRNSBS<=prn?SYS_SBS:SYS_GPS,prn))) {
trace(2,"ubx rxmsfrb satellite number error: prn=%d\n",prn);
return -1;
}
sys=satsys(sat,&prn);
if (sys==SYS_GPS) {
id=save_subfrm(sat,raw);
if (id==3) return decode_ephem(sat,raw);
if (id==4) return decode_alm1 (sat,raw);
if (id==5) return decode_alm2 (sat,raw);
return 0;
}
else if (sys==SYS_SBS) {
for (i=0,p+=2;i<10;i++,p+=4) words[i]=U4(p);
return sbsdecodemsg(raw->time,prn,words,&raw->sbsmsg)?3:0;
}
return 0;
}
bool TTetrahedron::IsInner(const TPoint& P) const {
TVector N, U1(X1, X2), U2(X1, X3), U3(X1, X4), U4(X2, X3), U5(X2, X4), U6(X2, X1);
TVector Up1(X1, P), Up2(X2, P);
N = Cross(U1, U2); //X4 is on opposite side of plain X1,X2,X3 than P
if (Dot(N, U3) * Dot(N, Up1) < 0) {
return false;
}
N = Cross(U1, U3); //X3 x P
if (Dot(N, U2) * Dot(N, Up1) < 0) {
return false;
}
N = Cross(U2, U3); //X2 x P
if (Dot(N, U1) * Dot(N, Up1) < 0) {
return false;
}
N = Cross(U4, U5); //X1 x P
if (Dot(N, U6) * Dot(N, Up2) < 0) {
return false;
}
return true;
}
std::map<GaugeGroupName, int> Statistics::Count_Gauge_Groups
(const std::set<LEEFT>& Unique_LEEFTs)
{
GaugeGroupName U1('U',1,1);
std::map<GaugeGroupName, int> Gauge_Groups;
std::set<LEEFT>::const_iterator itUnique_LEEFTs = Unique_LEEFTs.begin();
for(; itUnique_LEEFTs != Unique_LEEFTs.end(); ++itUnique_LEEFTs)
{
std::set<GaugeGroupName> Unique_GaugeGroupNames;
//Unique gauge group factors for a given model.
for(int a=0; a<static_cast<int>((itUnique_LEEFTs->Gauge_Groups()).size()); a++)
Unique_GaugeGroupNames.insert((itUnique_LEEFTs->Gauge_Groups()).at(a));
if(itUnique_LEEFTs->U1_Factors()>0)
Unique_GaugeGroupNames.insert(U1);
std::set<GaugeGroupName>::iterator itUnique_GaugeGroupNames =
Unique_GaugeGroupNames.begin();
for(; itUnique_GaugeGroupNames != Unique_GaugeGroupNames.end();
++itUnique_GaugeGroupNames)
{
if(Gauge_Groups.find(*itUnique_GaugeGroupNames) !=
Gauge_Groups.end())
Gauge_Groups[*itUnique_GaugeGroupNames]++;
else
Gauge_Groups[*itUnique_GaugeGroupNames] = 1;
}//Close for loop on Unique_GaugeGroupNames.
}//Close for loop on Unique_LEEFTs.
return Gauge_Groups;
}//Close Count_Gauge_Group_Factors.
/* 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 waas messages ------------------------------------------------------*/
static int decode_gw10sbs(raw_t *raw)
{
double tow;
int i,prn;
unsigned char *p=raw->buff+2;
trace(4,"decode_gw10sbs : len=%d\n",raw->len);
tow=U4(p)/1000.0;
prn=U1(p+4);
if (prn<MINPRNSBS||MAXPRNSBS<prn) {
trace(2,"gw10 sbs satellite number error: prn=%d\n",prn);
return -1;
}
raw->sbsmsg.prn=prn;
raw->sbsmsg.tow=(int)tow;
tow=time2gpst(raw->time,&raw->sbsmsg.week);
if (raw->sbsmsg.tow<tow-302400.0) raw->sbsmsg.week++;
else if (raw->sbsmsg.tow>tow+302400.0) raw->sbsmsg.week--;
for (i=0;i<29;i++) {
raw->sbsmsg.msg[i]=*(p+5+i);
}
raw->sbsmsg.msg[28]&=0xC0;
return 3;
}
/* decode skytraq subframe buffer --------------------------------------------*/
static int decode_stqsfrb(raw_t *raw)
{
int prn,sat,sys,id;
unsigned char *p=raw->buff+4;
trace(4,"decode_stqsfrb: len=%d\n",raw->len);
if (raw->len<40) {
trace(2,"stq subframe length error: len=%d\n",raw->len);
return -1;
}
prn=U1(p+1);
if (prn>MAXPRNGPS) prn+=MINPRNSBS-38;
if (!(sat=satno(MINPRNSBS<=prn?SYS_SBS:SYS_GPS,prn))) {
trace(2,"stq subframe satellite number error: prn=%d\n",prn);
return -1;
}
sys=satsys(sat,&prn);
if (sys==SYS_GPS) {
id=save_subfrm(sat,raw);
if (id==3) return decode_ephem(sat,raw);
if (id==4) return decode_alm1 (sat,raw);
if (id==5) return decode_alm2 (sat,raw);
return 0;
}
return 0;
}
void AdvanceState(std::valarray<double> &U, double dt) const
{
std::valarray<double> U1(U.size());
Mara->godunov->SetTimeStepDt(dt);
U1 = U + dt*Mara->godunov->dUdt(U);
U = 0.5*(U + U1 + dt*Mara->godunov->dUdt(U1));
}
bool ElasticBeam::initElement (const std::vector<int>& MNPC,
const FiniteElement& fe, const Vec3&, size_t,
LocalIntegral& elmInt)
{
if (!this->initElement(MNPC,elmInt))
return false;
Vec3 e1 = fe.XC[1] - fe.XC[0]; // Initial local X-axis
const Vector& eV = elmInt.vec.front();
if (!eV.empty())
{
// Fetch nodal displacements
Vec3 U0(eV.ptr());
Vec3 U1(eV.ptr()+eV.size()-npv);
e1 += U1 - U0; // Deformed local X-axis
}
// Calculate the co-rotated element coordinate system
if (e1.normalize() <= 1.0e-8)
{
std::cerr <<" *** ElasticBeam::initElement: Zero beam length"<< std::endl;
return false;
}
if (fe.Tn.size() < 2)
{
std::cerr <<" *** ElasticBeam::initElement: No end rotations"<< std::endl;
return false;
}
Vec3 e2(fe.Tn[0][1]+fe.Tn[1][1]); // Sum of the nodal Y-axes
Vec3 e3(fe.Tn[0][2]+fe.Tn[1][2]); // Sum of the nodal Z-axes
if (e3*e1 < e2*e1)
{
e2.cross(e3,e1); // Local Y-axis = e3xe1 / |e3xe1|
e2.normalize();
e3.cross(e1,e2); // Local Z-axis = e1xe2
}
else
{
e3.cross(e1,e2); // Local Z-axis = e1xe2 / |e1xe2|
e3.normalize();
e2.cross(e3,e1); // Local Y-axis = e3xe1
}
Matrix& Tlg = this->getLocalAxes(elmInt);
Tlg.fillColumn(1,e1.ptr());
Tlg.fillColumn(2,e2.ptr());
Tlg.fillColumn(3,e3.ptr());
#if INT_DEBUG > 1
std::cout <<"ElasticBeam: local-to-global transformation matrix:"<< Tlg
<<"ElasticBeam: T1n\n"<< fe.Tn[0] <<"ElasticBeam: T2n\n"<< fe.Tn[1];
#endif
return true;
}
void AdvanceState(std::valarray<double> &U, double dt) const
{
std::valarray<double> U1(U.size());
Mara->godunov->SetTimeStepDt(dt);
U1 = U + dt * Mara->godunov->dUdt(U );
U1 = 3./4*U + 1./4*U1 + 1./4 * dt * Mara->godunov->dUdt(U1);
U = 1./3*U + 2./3*U1 + 2./3 * dt * Mara->godunov->dUdt(U1);
}
/* decode ublox raw message --------------------------------------------------*/
static int decode_ubx(raw_t *raw)
{
int type=(U1(raw->buff+2)<<8)+U1(raw->buff+3);
trace(3,"decode_ubx: type=%04x len=%d\n",type,raw->len);
/* checksum */
if (!checksum(raw->buff,raw->len)) {
trace(2,"ubx checksum error: type=%04x len=%d\n",type,raw->len);
return -1;
}
sprintf(raw->msgtype,"UBLOX: type=%2d len=%3d",type,raw->len);
switch (type) {
case ID_RXMRAW : return decode_rxmraw(raw);
case ID_RXMSFRB: return decode_rxmsfrb(raw);
}
return 0;
}
/* decode binex mesaage 0x01: gnss navigaion informtion ----------------------*/
static int decode_bnx_01(raw_t *raw, unsigned char *buff, int len)
{
char *msg;
int srec=U1(buff),prn=U1(buff+1);
if (raw->outtype) {
msg=raw->msgtype+strlen(raw->msgtype);
prn=srec==0x01||srec==0x02||srec==0x04?prn+1:(srec==0x00?0:prn);
sprintf(msg," subrec=%02X prn=%d",srec,prn);
}
switch (srec) {
case 0x00: return decode_bnx_01_00(raw,buff+1,len-1);
case 0x01: return decode_bnx_01_01(raw,buff+1,len-1);
case 0x02: return decode_bnx_01_02(raw,buff+1,len-1);
case 0x03: return decode_bnx_01_03(raw,buff+1,len-1);
case 0x04: return decode_bnx_01_04(raw,buff+1,len-1);
case 0x05: return decode_bnx_01_05(raw,buff+1,len-1);
case 0x06: return decode_bnx_01_06(raw,buff+1,len-1);
}
return 0;
}
