public XYZCoord GetRinexSVPos(string filename, int svPRN, double TOW, out double ionoError, out double svClkBias, out XYZCoord svVel)
{
XYZCoord coord = new XYZCoord();
if (this.m_RinexFile.GetNumEphemerides() == 0)
{
this.m_RinexFile.Read(@"..\scripts\TunnelsMeasAndNav\brdc2140.07n");
}
RinexEph eph = this.m_RinexFile.SearchRinexArrayList((byte)svPRN, (int)TOW);
if (eph == null)
{
ionoError = 0.0;
svClkBias = 0.0;
svVel.X = svVel.Y = svVel.Z = 0.0;
return(coord);
}
tSVD_SVState st = new tSVD_SVState();
tGPSTime gTime = new tGPSTime();
gTime.isTOWValid = true;
gTime.isWeekValid = true;
gTime.time = TOW;
gTime.week = (int)eph.weekNo;
new ComputeSVPos().computeSVState(st, (short)svPRN, eph, gTime);
coord.X = st.pos[0];
coord.Y = st.pos[1];
coord.Z = st.pos[2];
svVel.X = st.vel[0];
svVel.Y = st.vel[1];
svVel.Z = st.vel[2];
ionoError = st.ionoDelayStd;
svClkBias = st.clockBias;
return(coord);
}
public void computeSVState(tSVD_SVState st, short svid, RinexEph eph, tGPSTime gTime)
{
double num50 = 0.0;
double ntcld = 0.0;
double num52 = 0.0;
double num53 = 0.0;
double num54 = 0.0;
double num55 = 0.0;
double num56 = 0.0;
tSVD_EphemerisConsts constptr = new tSVD_EphemerisConsts();
st.svid = (byte)svid;
double num59 = ((SECONDS_IN_WEEK * (gTime.week - eph.weekNo)) + gTime.time) - eph.toe;
if ((eph.status & EPHSTATUS_EPH_AVAIL) != 0)
{
if (eph.age == 0)
{
st.dataAvail = 1;
}
else if ((eph.age & EPH_AGE_CGEE_INDICATOR) != 0)
{
st.dataAvail = 6;
}
else
{
st.dataAvail = 5;
}
}
else if ((eph.status & EPHSTATUS_ALM_AVAIL) != 0)
{
if ((num59 < 302400.0) && (num59 > -302400.0))
{
st.dataAvail = 4;
}
else
{
st.dataAvail = 2;
}
}
else
{
st.dataAvail = 0;
return;
}
this.setEphemerisConstants(ref constptr, svid, ref eph, ref gTime);
num59 = ((SECONDS_IN_WEEK * (gTime.week - eph.weekNo)) + gTime.time) - eph.toe;
double a = eph.m0 + (constptr.ntcn * num59);
double num14 = Math.Sin(a);
double num25 = Math.Cos(a);
double num15 = num14 * num14;
double num16 = num15 * num14;
double num17 = num16 * num14;
double num18 = num17 * num14;
double num19 = num18 * num14;
double num20 = num19 * num14;
double num23 = constptr.ntck * (((((((constptr.ntcs1 * num14) + ((constptr.ntcs2 * num14) * num25)) + (constptr.ntcs3 * num16)) + ((constptr.ntcs4 * num16) * num25)) + (constptr.ntcs5 * num18)) + ((constptr.ntcs6 * num18) * num25)) + (constptr.ntcs7 * num20));
double num28 = (((((num25 + (constptr.ntcc2 * num15)) + ((constptr.ntcc3 * num15) * num25)) + (constptr.ntcc4 * num17)) + ((constptr.ntcc5 * num17) * num25)) + (constptr.ntcc6 * num19)) + ((constptr.ntcc7 * num19) * num25);
double num21 = (num23 * constptr.ntccw) + (num28 * constptr.ntcsw);
double num26 = (num28 * constptr.ntccw) - (num23 * constptr.ntcsw);
double num12 = (2.0 * num21) * num26;
double num24 = (num26 * num26) - (num21 * num21);
double num45 = (eph.Cus * num12) + (eph.Cuc * num24);
double num13 = num45;
double num48 = 1.0 - ((num13 * num13) / 2.0);
double num22 = (num21 * num48) + (num26 * num13);
double num27 = (num26 * num48) - (num21 * num13);
double num = 1.0 + (eph.ecc * num28);
double num8 = constptr.ntcrx / num;
double num10 = (eph.Crs * num12) + (eph.Crc * num24);
double num37 = num8 + num10;
double num39 = num37 * num27;
num50 = num37 * num22;
num52 = (eph.Cis * num12) + (eph.Cic * num24);
double num29 = (eph.i0 + num52) + (eph.idot * num59);
ntcld = constptr.ntcld;
double d = (eph.omega0 + (ntcld * num59)) - constptr.ntcl2;
num53 = Math.Cos(d);
num55 = Math.Sin(d);
num54 = Math.Sin(num29);
num56 = Math.Cos(num29);
st.pos[0] = (num39 * num53) - ((num50 * num56) * num55);
st.pos[1] = (num39 * num55) + ((num50 * num56) * num53);
st.pos[2] = num50 * num54;
double num5 = ((constptr.ntcn * num) * num) / constptr.ntck2;
double num44 = (eph.Cus * num24) - (eph.Cuc * num12);
double num3 = 2.0 * num5;
double num42 = num5 + (num3 * num44);
double num11 = (eph.Crs * num24) - (eph.Crc * num12);
double num57 = ((constptr.sqrta_2 * eph.ecc) * constptr.ntcn) / constptr.ntck;
double num38 = (num57 * num23) + (num3 * num11);
double num40 = (num38 * num27) - (num50 * num42);
double num41 = (num38 * num22) + (num39 * num42);
double num32 = (eph.Cis * num24) - (eph.Cic * num12);
double num30 = (num3 * num32) + eph.idot;
double num33 = num40 - ((num50 * ntcld) * num56);
double num35 = ((num41 * num56) - (st.pos[2] * num30)) + (num39 * ntcld);
st.vel[0] = (num33 * num53) - (num35 * num55);
st.vel[1] = (num33 * num55) + (num35 * num53);
st.vel[2] = (num41 * num54) + ((num50 * num30) * num56);
double num2 = num3 * num3;
double num6 = (-(((num3 * eph.ecc) * constptr.ntcn) * num23) / constptr.ntck2) * num;
double num9 = (((num57 * num5) * num28) + ((2.0 * num6) * num11)) - (num2 * num10);
double num43 = (num6 + ((2.0 * num6) * num44)) - (num2 * num45);
double num46 = (((num9 * num27) - ((num38 * num42) * num22)) - (num41 * num42)) - (num43 * num50);
double num47 = (((num9 * num22) + ((num38 * num42) * num27)) + (num40 * num42)) + (num43 * num39);
double num31 = ((2.0 * num6) * num32) - (num2 * num52);
double num34 = (num46 - ((num41 * ntcld) * num56)) + (((num50 * num30) * ntcld) * num54);
double num36 = ((((num47 * num56) - ((num41 * num30) * num54)) - (st.vel[2] * num30)) - (st.pos[2] * num31)) + (num40 * ntcld);
st.acc[0] = (float)(((-ntcld * st.vel[1]) + (num34 * num53)) - (num36 * num55));
st.acc[1] = (float)(((ntcld * st.vel[0]) + (num34 * num55)) + (num36 * num53));
st.acc[2] = (float)((((num47 * num54) + (((2.0 * num41) * num30) * num56)) + ((num50 * num31) * num56)) - ((num30 * num30) * st.pos[2]));
st.jrk[0] = (float)((((-3.0 * NLERTE) * NLERTE) * ((float)st.vel[0])) + ((2.0 * NLERTE) * st.acc[1]));
st.jrk[1] = (float)((((-3.0 * NLERTE) * NLERTE) * st.vel[1]) - ((2.0 * NLERTE) * st.acc[0]));
st.jrk[2] = (float)(((-4.0 * NLERTE) * NLERTE) * st.vel[2]);
double num49 = constptr.ntecos / (1.0 + (eph.ecc * num28));
st.tcr = (float)((num49 * constptr.ntck) * num23);
st.rcd = (float)((num49 * constptr.ntcn) * (eph.ecc + num28));
st.slw = 0;
st.tow = (int)eph.toe;
st.wno = (short)eph.weekNo;
st.cct = (int)eph.toc;
st.tgd = eph.tgd;
st.iode = (byte)(((byte)eph.iode) & 0xff);
st.af0 = eph.af0;
st.af1 = eph.af1;
st.af2 = eph.af2;
st.gct = gTime.time;
st.gcw = (short)gTime.week;
if (st.dataAvail == 5)
{
st.posVar = g_posVar[eph.age];
st.clkVar = g_svClkVar[eph.age];
st.ephAge = eph.age;
}
else if (st.dataAvail == 6)
{
if (eph.age == 1)
{
st.posVar = g_posVar[3];
}
else if (eph.age == 2)
{
st.posVar = g_posVar[4];
}
else if (eph.age == 3)
{
st.posVar = g_posVar[6];
}
else
{
st.posVar = g_posVar[eph.age];
}
st.clkVar = g_svClkVar[eph.age];
st.ephAge = eph.age;
}
else
{
st.posVar = 0f;
st.clkVar = 0f;
st.ephAge = 0;
}
double num58 = ((SECONDS_PER_WEEK * (st.gcw - st.wno)) + st.gct) - st.cct;
st.clockBias = ((st.af0 + (num58 * (st.af1 + (num58 * st.af2)))) + st.tcr) - st.tgd;
st.clockDrift = (st.af1 + ((2.0 * st.af2) * num58)) + st.rcd;
}
private void setEphemerisConstants(ref tSVD_EphemerisConsts constptr, short svid, ref RinexEph eph, ref tGPSTime gTime)
{
constptr.sqrta_2 = eph.sqrta * eph.sqrta;
if (constptr.sqrta_2 != 0.0)
{
constptr.ntcn = (NLSQMU / (constptr.sqrta_2 * eph.sqrta)) + eph.deltan;
}
else
{
constptr.ntcn = eph.deltan;
}
double num = eph.ecc * eph.ecc;
double num2 = num * eph.ecc;
double num3 = num2 * eph.ecc;
double num4 = num3 * eph.ecc;
double num5 = num4 * eph.ecc;
constptr.ntcs1 = ((1.0 + (3.0 * num)) + (5.0 * num3)) + (7.0 * num5);
constptr.ntcs2 = ((2.0 * eph.ecc) + (4.0 * num2)) + (6.0 * num4);
constptr.ntcs3 = -(((4.5 * num) + (28.3333333333333 * num3)) + (96.8333333333333 * num5));
constptr.ntcs4 = -((10.6666666666667 * num2) + (52.0 * num4));
constptr.ntcs5 = (26.0416666666667 * num3) + (247.975 * num5);
constptr.ntcs6 = 64.8 * num4;
constptr.ntcs7 = -163.401388888889 * num5;
constptr.ntcc2 = -(((2.0 * eph.ecc) + (8.0 * num2)) + (18.0 * num4));
constptr.ntcc3 = -(((4.5 * num) + (12.5 * num3)) + (24.5 * num5));
constptr.ntcc4 = (10.6666666666667 * num2) + (78.0 * num4);
constptr.ntcc5 = (26.0416666666667 * num3) + (151.083333333333 * num5);
constptr.ntcc6 = -constptr.ntcs6;
constptr.ntcc7 = constptr.ntcs7;
constptr.ntc1e2 = 1.0 - num;
constptr.ntcrx = constptr.sqrta_2 * constptr.ntc1e2;
constptr.ntck = Math.Sqrt(constptr.ntc1e2);
constptr.ntck2 = constptr.ntck * constptr.ntc1e2;
constptr.ntcsw = Math.Sin(eph.omega);
constptr.ntccw = Math.Cos(eph.omega);
constptr.ntcl = eph.omega0 - (eph.omegaDot * eph.toe);
constptr.ntcld = eph.omegaDot - NLERTE;
constptr.ntcl2 = NLERTE * eph.toe;
constptr.ntecos = (-NLRF * eph.ecc) * eph.sqrta;
}