/* 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;
}
Code::Code(AttributeInfo *code) {
unsigned char *data = code->data;
int index = 0;
U2(maxStack,data,index);
U2(maxLocals,data,index);
U4(codeLength,data,index);
this->code = new unsigned char[codeLength];
for (int i = 0; i < codeLength; i++) {
this->code[i] = data[index+i];
}
index += codeLength;
U2(exceptionCount,data,index);
exceptions = new ExceptionEntry*[exceptionCount];
for (int i = 0; i < exceptionCount; i++) {
exceptions[i] = new ExceptionEntry;
U2(exceptions[i]->startPC,data,index);
U2(exceptions[i]->endPC,data,index);
U2(exceptions[i]->handlerPC,data,index);
U2(exceptions[i]->catchType,data,index);
}
U2(attributeCount,data,index);
attributes = new AttributeInfo*[attributeCount];
for (int i = 0; i < attributeCount; i++)
attributes[i] = new AttributeInfo(data, index);
}
MethodInfo::MethodInfo(unsigned char *data, int &index) {
U2(accessFlags,data,index);
U2(nameIndex,data,index);
U2(descriptorIndex,data,index);
U2(attributeCount,data,index);
attributes = new AttributeInfo*[attributeCount];
for (int i = 0; i < attributeCount; i++)
attributes[i] = new AttributeInfo(data, index);
nargs = -1;
argsc = -1;
result = -1;
native = 0;
clazz = 0;
resobject = 0;
}
/* input ublox raw message from file -------------------------------------------
* fetch next ublox 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_ubxf(raw_t *raw, FILE *fp)
{
int i,data;
trace(4,"input_ubxf:\n");
/* synchronize frame */
if (raw->nbyte==0) {
for (i=0;;i++) {
if ((data=fgetc(fp))==EOF) return -2;
if (sync_ubx(raw->buff,(unsigned char)data)) break;
if (i>=4096) return 0;
}
}
if (fread(raw->buff+2,1,4,fp)<4) return -2;
raw->nbyte=6;
if ((raw->len=U2(raw->buff+4)+8)>MAXRAWLEN) {
trace(2,"ubx length error: len=%d\n",raw->len);
raw->nbyte=0;
return -1;
}
if (fread(raw->buff+6,1,raw->len-6,fp)<(size_t)(raw->len-6)) return -2;
raw->nbyte=0;
/* decode ubx raw message */
return decode_ubx(raw);
}
/* input ublox raw message from stream -----------------------------------------
* fetch next ublox raw data and input a mesasge from stream
* args : raw_t *raw IO receiver raw data control struct
* raw->opt : u-blox raw options
* "-invcp" : inversed polarity of carrier-phase
* "-EPHALL" : output all ephemerides
* 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)
*-----------------------------------------------------------------------------*/
extern int input_ubx(raw_t *raw, unsigned char data)
{
trace(5,"input_ubx: data=%02x\n",data);
/* synchronize frame */
if (raw->nbyte==0) {
if (!sync_ubx(raw->buff,data)) return 0;
raw->nbyte=2;
return 0;
}
raw->buff[raw->nbyte++]=data;
if (raw->nbyte==6) {
if ((raw->len=U2(raw->buff+4)+8)>MAXRAWLEN) {
trace(2,"ubx length error: len=%d\n",raw->len);
raw->nbyte=0;
return -1;
}
}
if (raw->nbyte<6||raw->nbyte<raw->len) return 0;
raw->nbyte=0;
/* decode ublox raw message */
return decode_ubx(raw);
}
int testmaxmin() {
vector<double> x;
x.push_back(1);
x.push_back(2);
x.push_back(3);
x.push_back(1.1);
x.push_back(1);
x.push_back(9);
x.push_back(1.01);
x.push_back(11);
x.push_back(0.001);
vector<double> U(x.size()),L(x.size());
vector<double> U2(x.size()),L2(x.size());
for(uint constraint = 0; constraint<x.size(); ++constraint) {
computeEnvelope(x,constraint,U,L);
Envelope env;
env.compute(x,constraint,U2,L2);
if((U!=U2) or (L!=L2)) { cout<< "bug! "<<constraint<<endl;
cout <<" x U L U2 L2"<<endl;
for(uint k = 0; k<x.size();++k)
cout<<"k="<<k<<" "<<x[k]<<" "<< U[k]<<" "<< L[k]<<" " <<U2[k]<<" "<< L2[k]<<endl;
}
return -1;
}
return 0;
}
/** ptr: &uinfo[] */
void U1(struct user_info * p)
{
xlog(LOG_U, "U1(). %s arrives at floor %d, destination is %d.\n",
p->name, p->IN, p->OUT);
U2(p);
}
static HRESULT WINAPI enummodescallback(LPDDSURFACEDESC lpddsd, LPVOID lpContext)
{
trace("Width = %i, Height = %i, Refresh Rate = %i, Pitch = %i, flags =%02X\r\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;
}
/* 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
* raw->opt : skytraq raw options
* "-invcp" : inversed polarity of carrier-phase
* 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)
*-----------------------------------------------------------------------------*/
extern int input_stq(raw_t *raw, unsigned char 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,(unsigned char)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);
}
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 solution -----------------------------------------------------------*/
static int decode_gw10sol(raw_t *raw)
{
gtime_t time;
double ep[6]={0},sec;
unsigned char *p=raw->buff+6;
trace(4,"decode_gw10sol : len=%d\n",raw->len);
if (U2(p+42)&0xC00) { /* time valid? */
trace(2,"gw10 sol time/day invalid\n");
return 0;
}
sec=U4(p+27)/16384.0;
sec=floor(sec*1000.0+0.5)/1000.0;
ep[2]=bcd2num(p[31]);
ep[1]=bcd2num(p[32]);
ep[0]=bcd2num(p[33])*100+bcd2num(p[34]);
time=utc2gpst(timeadd(epoch2time(ep),sec));
/* set time if no time available */
if (raw->time.time==0) {
raw->time=time;
}
return 0;
}
void
MultivariateModel
::ComputeOrthonormalBasis()
{
/// It computes a basis of vector orthogonal to the geodesic gamma_derivative at t0, with respect to
/// the scalar product defined on the riemannian manifold
/// Further information about the mathematical operation in the documentation
ScalarType V0 = - m_G / (m_G + 1) * exp(m_RandomVariables.GetRandomVariable("Ksi")->GetParameter("Mean"));
VectorType U(m_ManifoldDimension);
ScalarType * u = U.memptr();
ScalarType * d = m_Deltas.memptr();
for(size_t i = 0; i < m_ManifoldDimension; ++i)
u[i] = V0 * (1.0/m_G + exp(d[i]));
/// Compute the initial pivot vector U
double Norm = U.magnitude();
U(0) += copysign(1, -U(0)) * Norm;
// Check the vectorise_row_wise function of the ArmadilloMatrixWrapper
MatrixType U2(U);
double NormU2 = U.squared_magnitude();
MatrixType FinalMatrix2 = (-2.0/NormU2) * U2*U2.transpose();
for(size_t i = 0; i < m_ManifoldDimension; ++i)
FinalMatrix2(i, i ) += 1;
m_OrthogonalBasis = FinalMatrix2;
}
/*--- input_ubx is supposed to be called from tread ---*/
int input_ubx(tUbxRawData *raw, unsigned char data)
{
/* synchronize frame */
if (raw->nbyte == 0)
{
if (!sync_ubx(raw->buff, data))
return 0;
raw->nbyte=2;
return 0; /* wait for new bytes join in */
}
raw->buff[raw->nbyte++]=data;
if (raw->nbyte == UBX_MSGSTART_SHIFT)
{
raw->length = U2(raw->buff+4) + 8;
if (raw->length > MAXRAWLEN)
{
raw->nbyte = 0;
return -1;
}
}
if((raw->nbyte < UBX_MSGSTART_SHIFT) || (raw->nbyte < raw->length))
return 0; /* return until length is satisfied */
raw->nbyte = 0;
raw->frameReady = TRUE;
memcpy(&readyRawUbx, raw, sizeof(tUbxRawData));
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;
}
static void test_title(void)
{
HPROPSHEETPAGE hpsp[1];
PROPSHEETPAGEA psp;
PROPSHEETHEADERA psh;
HWND hdlg;
memset(&psp, 0, sizeof(psp));
psp.dwSize = sizeof(psp);
psp.dwFlags = 0;
psp.hInstance = GetModuleHandleW(NULL);
U(psp).pszTemplate = "prop_page1";
U2(psp).pszIcon = NULL;
psp.pfnDlgProc = page_dlg_proc;
psp.lParam = 0;
hpsp[0] = CreatePropertySheetPageA(&psp);
memset(&psh, 0, sizeof(psh));
psh.dwSize = sizeof(psh);
psh.dwFlags = PSH_MODELESS | PSH_USECALLBACK;
psh.pszCaption = "test caption";
psh.nPages = 1;
psh.hwndParent = GetDesktopWindow();
U3(psh).phpage = hpsp;
psh.pfnCallback = sheet_callback;
hdlg = (HWND)PropertySheetA(&psh);
DestroyWindow(hdlg);
}
static void test_disableowner(void)
{
HPROPSHEETPAGE hpsp[1];
PROPSHEETPAGEA psp;
PROPSHEETHEADERA psh;
register_parent_wnd_class();
parent = CreateWindowA("parent class", "", WS_CAPTION | WS_SYSMENU | WS_VISIBLE, 100, 100, 100, 100, GetDesktopWindow(), NULL, GetModuleHandleA(NULL), 0);
memset(&psp, 0, sizeof(psp));
psp.dwSize = sizeof(psp);
psp.dwFlags = 0;
psp.hInstance = GetModuleHandleW(NULL);
U(psp).pszTemplate = "prop_page1";
U2(psp).pszIcon = NULL;
psp.pfnDlgProc = NULL;
psp.lParam = 0;
hpsp[0] = CreatePropertySheetPageA(&psp);
memset(&psh, 0, sizeof(psh));
psh.dwSize = sizeof(psh);
psh.dwFlags = PSH_USECALLBACK;
psh.pszCaption = "test caption";
psh.nPages = 1;
psh.hwndParent = parent;
U3(psh).phpage = hpsp;
psh.pfnCallback = disableowner_callback;
PropertySheetA(&psh);
ok(IsWindowEnabled(parent) != 0, "parent window should be enabled\n");
DestroyWindow(parent);
}
/* 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;
}
static void test_buttons(void)
{
HPROPSHEETPAGE hpsp[1];
PROPSHEETPAGEA psp;
PROPSHEETHEADERA psh;
HWND hdlg;
HWND button;
RECT rc;
int prevRight, top;
memset(&psp, 0, sizeof(psp));
psp.dwSize = sizeof(psp);
psp.dwFlags = 0;
psp.hInstance = GetModuleHandleA(NULL);
U(psp).pszTemplate = "prop_page1";
U2(psp).pszIcon = NULL;
psp.pfnDlgProc = page_dlg_proc;
psp.lParam = 0;
hpsp[0] = CreatePropertySheetPageA(&psp);
memset(&psh, 0, sizeof(psh));
psh.dwSize = PROPSHEETHEADERA_V1_SIZE;
psh.dwFlags = PSH_MODELESS | PSH_USECALLBACK;
psh.pszCaption = "test caption";
psh.nPages = 1;
psh.hwndParent = GetDesktopWindow();
U3(psh).phpage = hpsp;
psh.pfnCallback = sheet_callback;
hdlg = (HWND)PropertySheetA(&psh);
ok(hdlg != INVALID_HANDLE_VALUE, "got null handle\n");
/* OK button */
button = GetDlgItem(hdlg, IDOK);
GetWindowRect(button, &rc);
prevRight = rc.right;
top = rc.top;
/* Cancel button */
button = GetDlgItem(hdlg, IDCANCEL);
GetWindowRect(button, &rc);
ok(rc.top == top, "Cancel button should have same top as OK button\n");
ok(rc.left > prevRight, "Cancel button should be to the right of OK button\n");
prevRight = rc.right;
button = GetDlgItem(hdlg, IDC_APPLY_BUTTON);
GetWindowRect(button, &rc);
ok(rc.top == top, "Apply button should have same top as OK button\n");
ok(rc.left > prevRight, "Apply button should be to the right of Cancel button\n");
prevRight = rc.right;
button = GetDlgItem(hdlg, IDHELP);
GetWindowRect(button, &rc);
ok(rc.top == top, "Help button should have same top as OK button\n");
ok(rc.left > prevRight, "Help button should be to the right of Apply button\n");
DestroyWindow(hdlg);
}
AttributeInfo::AttributeInfo(unsigned char *data, int &index) {
U2(nameIndex,data,index);
U4(length,data,index);
this->data = new unsigned char[length];
for (int i = 0; i < length; i++)
this->data[i] = data[i+index];
index += length;
}
void SubInv(double *v)
{
double temp1 = U1(v[0],v[1],v[2],v[3]);
double temp2 = U2(v[0],v[1],v[2],v[3]);
double temp3 = U3(v[0],v[1],v[2],v[3]);
double temp4 = U4(v[0],v[1],v[2],v[3]);
v[0] = temp1;
v[1] = temp2;
v[2] = temp3;
v[3] = temp4;
}
void SubInv(int *v, int m)
{
int temp1 = U1(v[0],v[1],v[2],v[3], m);
int temp2 = U2(v[0],v[1],v[2],v[3], m);
int temp3 = U3(v[0],v[1],v[2],v[3], m);
int temp4 = U4(v[0],v[1],v[2],v[3], m);
v[0] = temp1;
v[1] = temp2;
v[2] = temp3;
v[3] = temp4;
}
/* 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;
}
static void create_assembly(LPCSTR file)
{
HANDLE hfile;
DWORD written;
/* nameless unions initialized here */
assembly.tableshdr.MaskValid.u.HighPart = 0x101;
assembly.tableshdr.MaskValid.u.LowPart = 0x00000005;
assembly.tableshdr.MaskSorted.u.HighPart = 0x1600;
assembly.tableshdr.MaskSorted.u.LowPart = 0x3301FA00;
U1(assembly.labelres).Name = 0x10;
U2(assembly.labelres).OffsetToData = 0x80000018;
U1(assembly.label11res).Name = 0x1;
U2(assembly.label11res).OffsetToData = 0x80000030;
U1(assembly.label10res).Name = 0x0;
U2(assembly.label10res).OffsetToData = 0x48;
hfile = CreateFileA(file, GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, 0, 0);
WriteFile(hfile, &assembly, sizeof(ASSEMBLY), &written, NULL);
CloseHandle(hfile);
}
/* 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 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 skytraq measurement time info --------------------------------------*/
static int decode_stqtime(raw_t *raw)
{
unsigned char *p=raw->buff+4;
double tow;
int week;
trace(4,"decode_stqtime: len=%d\n",raw->len);
raw->iod=U1(p+1);
week =U2(p+2);
tow =U4(p+4)*0.001;
raw->time=gpst2time(week,tow);
return 0;
}
static HRESULT WINAPI enummodescallback_16bit(DDSURFACEDESC *lpddsd, void *lpContext)
{
trace("Width = %i, Height = %i, bpp = %i, Refresh Rate = %i, Pitch = %i, flags =%02X\n",
lpddsd->dwWidth, lpddsd->dwHeight, U1(lpddsd->ddpfPixelFormat).dwRGBBitCount,
U2(*lpddsd).dwRefreshRate, U1(*lpddsd).lPitch, lpddsd->dwFlags);
ok(lpddsd->dwFlags == (DDSD_HEIGHT|DDSD_WIDTH|DDSD_PIXELFORMAT|DDSD_PITCH|DDSD_REFRESHRATE),
"Wrong surface description flags %02X\n", lpddsd->dwFlags);
ok(lpddsd->ddpfPixelFormat.dwFlags == DDPF_RGB, "Wrong pixel format flag %02X\n",
lpddsd->ddpfPixelFormat.dwFlags);
ok(U1(lpddsd->ddpfPixelFormat).dwRGBBitCount == 16, "Expected 16 bpp got %i\n",
U1(lpddsd->ddpfPixelFormat).dwRGBBitCount);
/* Check that the pitch is valid if applicable */
if(lpddsd->dwFlags & DDSD_PITCH)
{
ok(U1(*lpddsd).lPitch != 0, "EnumDisplayModes callback with bad pitch\n");
}
if(!refresh_rate)
{
if(U2(*lpddsd).dwRefreshRate )
{
refresh_rate = U2(*lpddsd).dwRefreshRate;
refresh_rate_cnt++;
}
}
else
{
if(refresh_rate == U2(*lpddsd).dwRefreshRate)
refresh_rate_cnt++;
}
modes16bpp_cnt++;
return DDENUMRET_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;
}
/* 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;
}