📄 fixarithmetic.java
字号:
isecs = isecs - 86400 * dayofw;
h = (int) isecs / 3600;
isecs = (int) (isecs - 3600 * h);
m = (int) isecs / 60;
s = (int) (isecs - 60 * m);
fix_hour = h;// (h+8)%24;
// if((h+8)>24)fix_hour=(h+8)%24; else fix_hour=h+8 ;
fix_minute = m;
fix_second = s;
ttlday = dayofw + 7 * weekno;
ttlday -= 360;
yr = 1981;
while (ttlday > 366)
{
ttlday -= 365;
if (yr % 4 == 0 && yr % 100 != 0 || yr % 400 == 0)
ttlday -= 1;
yr += 1;
}
if (ttlday == 366)
{
if (yr % 4 == 0 && yr % 100 != 0 || yr % 400 == 0)
{
fix_year = yr;
dayofy = 366;
}
else
{
fix_year = yr + 1;
dayofy = 1;
}
}
else if (ttlday < 366)
{
fix_year = yr;
dayofy = ttlday;
}
for (mn = 1; mn <= 12; mn++)
{
if ((ttlday <= dinmth[mn - 1]) && (ttlday > 0))
{
fix_day = ttlday;
fix_month = mn;
ttlday = 0;
}
else if (mn == 2)
{
if (fix_year % 4 == 0 && fix_year % 100 != 0
|| fix_year % 400 == 0)
{
if (ttlday > 29)
ttlday -= 29;
else
{
fix_day = 29;
fix_month = 2;
ttlday = 0;
}
}
else
ttlday -= 28;
}
else
ttlday -= dinmth[mn - 1];
if (ttlday == 0)
break;
}
if (weekno == 0)
fix_year = 0;
}
// 功能：计算空间卫星到用户的伪距
// 入口：卫星的测量值：wholechips和fracchips；测量时间：gpstow；星历钟差的参数：ephemToc；
// 电离层参数：tgd ； 钟差修正参数：ephemaf0，ephemaf1，ephemaf2
// 出口：无
// 作者：lrs
double calPseuRange(int codephase,int intcodephase ,int gpsbitnum,double gpstow,long ephemToc,
double tgd, double ephemaf0, double ephemaf1,double ephemaf2)
{
double total_ms, total_s;
double delt_tsv; // 用来计算卫星的钟差
double pseurange;
total_ms = codephase*1.0 / 1023;
total_s = (total_ms +intcodephase+gpsbitnum*20)* 0.001 - tgd;
delt_tsv = ephemaf0 + ephemaf1 * (gpstow - ephemToc) + ephemaf2
* (gpstow - ephemToc) * (gpstow - ephemToc);
pseurange = (total_s - delt_tsv)*1000;
return pseurange;
}
/****/
/*************************************************************************/
/****/
/*
* author :xuhongwei
*
*/
void calSvXYZ(double delay)
{
double n0, n;
double tk, mk;
double ek, ek1, ek2, vk, phik;
double deltau, deltar, deltai, uk, rk, ik, xk, yk, omegak;
long timeintvalue;
double measuretime;
timeintvalue = (long)(para_gpstow)/14400;
measuretime = timeintvalue*14400 + para_terminal_gpstow*0.001;
//System.out.println("xhw---->measuretime="+measuretime);
measuretime -= (delay/1000.0);
// System.out.println("xhw---->para_gpstow="+para_gpstow);
//System.out.println("xhw---->measuretime="+measuretime);
n0 = Math.sqrt(para_gm)/(para_apowerhalf * para_apowerhalf * para_apowerhalf);
//System.out.println("xhw---->para_apowerhalf="+para_apowerhalf);
//System.out.println("xhw---->n0="+n0);
n = n0 + para_deltan;//semi-circles/sec
//System.out.println("xhw---->n="+n);
tk = measuretime - para_toe;
if (tk > 302400)
tk -= 604800;
else if (tk < -302400)
tk += 604800;
//System.out.println("xhw---->tk="+tk);
mk = (para_m0 + n * tk);//semi-circles
mk *= para_pi;//radians
ek1 = mk;
do
{
ek2 = mk + para_e * Math.sin(ek1);
if (Math.abs(ek2 - ek1) <= 1.0e-10) //-12
break;
ek1 = ek2;
} while (true);
ek = ek2;
//System.out.println("xhw---->ek="+ek);
vk = Get_atan((Math.cos(ek) - para_e), Math.sqrt(1.0 - para_e * para_e)
* Math.sin(ek));
phik = vk + para_w * para_pi;//radians
deltau = para_cuc * Math.cos(2.0 * phik) +
para_cus * Math.sin(2.0 * phik);//radians
deltar = para_crc * Math.cos(2.0 * phik) +
para_crs * Math.sin(2.0 * phik);//meters
deltai = para_cic * Math.cos(2.0 * phik) +
para_cis * Math.sin(2.0 * phik);//radians
uk = phik + deltau;//radians
rk = Math.pow(para_apowerhalf,2) * (1.0 - para_e * Math.cos(ek))
+ deltar;//meters
ik = para_i0 * para_pi + deltai + para_idot * tk * para_pi;//radians
xk = rk * Math.cos(uk);
yk = rk * Math.sin(uk);
omegak = para_omega0 * para_pi + (para_omegadot*para_pi - para_omegae)*
tk - para_omegae * para_toe;//radians
para_X = xk * Math.cos(omegak) - yk * Math.cos(ik) * Math.sin(omegak);
para_Y = xk * Math.sin(omegak) + yk * Math.cos(ik) * Math.cos(omegak);
para_Z = yk * Math.sin(ik);
/***xhw*/
// System.out.println("xhw---->para_X = "+para_X);
// System.out.println("xhw---->para_Y = "+para_Y);
// System.out.println("xhw---->para_Z = "+para_Z);
}
/*
* calculate pseudorange
*/
double calPseuRange(double cx,double cy,double cz,double delay)
{
double pseurange=0;
double temprange;
double distance;
long timeintvalue;
double measuretime,tk;
double deltatsv;
timeintvalue = (long)(para_gpstow)/14400;
measuretime = timeintvalue*14400 + para_terminal_gpstow*0.001;
measuretime -= delay/1000.0;
tk = measuretime - para_ephemToc;
if (tk > 302400)
tk -= 604800;
else if (tk < -302400)
tk += 604800;
deltatsv = para_ephemaf0 + para_ephemaf1 * tk + para_ephemaf2 * tk * tk;//S
//System.out.println("xhw---->deltatsv="+deltatsv);
distance = Math.sqrt(Math.pow((para_X-cx),2)+Math.pow((para_Y-cy),2)+Math.pow((para_Z-cz),2));
temprange = (distance/para_light_speed)*1000.0;//ms
//System.out.println("xhw---->temprange="+temprange);
pseurange = (int)temprange + (para_wholechips+para_fracchips)/1023.0;
pseurange = (pseurange/1000.0 - deltatsv + para_tgd) * 1000.0;
//pseurange = (pseurange/1000.0 - para_tgd) * 1000.0;
//System.out.println("xhw---->wholefracChips="+(wholechips+fracchips)/1023.0);
if((pseurange-temprange)>0.5)
pseurange -= 1.0;
else if((pseurange-temprange)<-0.5)
pseurange += 1.0;
System.out.println("xhw---->pseurange="+pseurange);
return pseurange;
}
/****/
/**************************************************************************/
/****/
double calRealRange(double pseuRange0,double pseuRange)
{
double diffPseuRange;
double mValue=pseuRange;
diffPseuRange=pseuRange-pseuRange0;
if( diffPseuRange>40.00 )
mValue=pseuRange-80.0;
else if( diffPseuRange<-40.00)
mValue=pseuRange+80.0;
return mValue;
}
double calSvSelfModify(double userX, double userY ,double satX,double satY)
{
double modifyVable;
modifyVable=1000*para_omegae*((satX-userX)*satY-(satY-userY)*satX)/para_light_speed ;
return modifyVable;
}
//功能：
// 入口：
//
// 出口：无
// 作者：hjs
private double getDelayTime(int codephase, int intcodephase, int gpsbitnum)
{
double tow;
if(gpsbitnum == 3)
{
System.out.println("intcodephase>>" + intcodephase + "*****************************************************************");
System.out.println("codephase>>" + codephase + "*****************************************************************");
System.out.println("gpsbitnum>>" + gpsbitnum + "*****************************************************************");
tow = intcodephase + codephase*0.0009775;
tow = 100 - tow;
System.out.println("tow>>" + tow + "*****************************************************************");
return tow;
}
else
{
System.out.println("intcodephase>>" + intcodephase + "*****************************************************************");
System.out.println("codephase>>" + codephase + "*****************************************************************");
System.out.println("gpsbitnum>>" + gpsbitnum + "*****************************************************************");
tow = gpsbitnum * 20 + intcodephase + codephase*0.0009775;
tow = 80 - tow;
return tow;
}
}
// private double getDelayTime(int codephase, int intcodephase, int gpsbitnum)
// {
// double tow;
// if(gpsbitnum == 0 )
// {
// System.out.println("intcodephase>>" + intcodephase + "*****************************************************************");
// System.out.println("codephase>>" + codephase + "*****************************************************************");
// System.out.println("gpsbitnum>>" + gpsbitnum + "*****************************************************************");
//
// tow = 4 * 20 + intcodephase + codephase*0.0009775;
// //time = gpsbitnum*20 + intcodephase + codephase*0.0009775;
//
// System.out.println("tow>>" + tow + "*****************************************************************");
// return tow;
// }
// else
// {
// System.out.println("intcodephase>>" + intcodephase + "*****************************************************************");
// System.out.println("codephase>>" + codephase + "*****************************************************************");
// System.out.println("gpsbitnum>>" + gpsbitnum + "*****************************************************************");
//
// tow = gpsbitnum * 20 + intcodephase + codephase*0.0009775;
// System.out.println("tow>>" + tow + "*****************************************************************");
// //time = gpsbitnum*20 + intcodephase + codephase*0.0009775;
// return tow;
// }
// }
private double getRangeTime(double delayTime, int para_wholechips, double para_fracchips)
{
long []delayTime1 = {(long)(delayTime - 1), (long)(delayTime), (long)(delayTime + 1)};
double []delayTime_1 = new double[3];
delayTime0[0] = delayTime_1[0] = (double)delayTime1[0] + ((double)para_wholechips + para_fracchips)/1023;
delayTime0[1] = delayTime_1[1] = (double)delayTime1[1] + ((double)para_wholechips + para_fracchips)/1023;
delayTime0[2] = delayTime_1[2] = (double)delayTime1[2] + ((double)para_wholechips + para_fracchips)/1023;
// System.out.println("delayTime_1>>" + delayTime_1[0] + "////////////////////////////////////");
// System.out.println("delayTime_2>>" + delayTime_1[1] + "////////////////////////////////////");
// System.out.println("delayTime_3>>" + delayTime_1[2] + "////////////////////////////////////");
//
double []subTime = new double[3];
subTime[0] = Math.abs(delayTime_1[0] - delayTime);
subTime[1] = Math.abs(delayTime_1[1] - delayTime);
subTime[2] = Math.abs(delayTime_1[2] - delayTime);
System.out.println("subTime1>>" + subTime[0] + "////////////////////////////////////");
System.out.println("subTime2>>" + subTime[1] + "////////////////////////////////////");
System.out.println("subTime3>>" + subTime[2] + "////////////////////////////////////");
double subTimeRlt = subTime[0];
double RangeTime = delayTime_1[0];
if(subTimeRlt > subTime[1])
{
subTimeRlt = subTime[1];
RangeTime = delayTime_1[1];
}
if(subTimeRlt > subTime[2])
{
subTimeRlt = subTime[2];
RangeTime = delayTime_1[2];
}
return RangeTime;
}
// **********下面取测量的一些相关的参数************//
public int getFixYear()
{
return fix_year;
}
public int getFixMonth()
{
return fix_month;
}
public int getFixDay()
{
return fix_day;
}
public int getFixHour()
{
return fix_hour;
}
public int getFixMinute()
{
return fix_minute;
}
public int getFixSecond()
{
return fix_second;
}
public int getFixSvNumber()
{
return fix_svnumber;
}
public double getFixLongitude()
{
return fix_longitude;
}
public double getFixLatitude()
{
return fix_latitude;
}
public double getFixHigh()
{
return fix_high;
}
public double getFixXCoordinate()
{
return fix_xcoordinate;
}
public double getFixYCoordinate()
{
return fix_ycoordinate;
}
public double getFixZCoordinate()
{
return fix_zcoordinate;
}
public double getFixError()
{
return fix_error;
}
// **********end取卫星和测量的一些相关的参数************//
}⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -