/* validate solution ---------------------------------------------------------*/
static int valsol(const double *azel, const int *vsat, int n,
const prcopt_t *opt, const double *v, int nv, int nx,
char *msg)
{
double azels[MAXOBS * 2], dop[4], vv;
int i, ns;
//trace(3, "valsol : n=%d nv=%d\n", n, nv);
/* chi-square validation of residuals */
vv = dot(v, v, nv);
if (nv > nx&&vv > chisqr[nv - nx - 1]) {
//sprintf(msg, "chi-square error nv=%d vv=%.1f cs=%.1f", nv, vv, chisqr[nv - nx - 1]);
return 0;
}
/* large gdop check */
for (i = ns = 0; i<n; i++) {
if (!vsat[i]) continue;
azels[ns * 2] = azel[i * 2];
azels[1 + ns * 2] = azel[1 + i * 2];
ns++;
}
dops(ns, azels, opt->elmin, dop);
if (dop[0] <= 0.0 || dop[0]>opt->maxgdop) {
//sprintf(msg, "gdop error nv=%d gdop=%.1f", nv, dop[0]);
return 0;
}
return 1;
}
static void RtkCommon_dops(JNIEnv* env, jclass clazz, jdoubleArray j_azel, jint j_ns, jdouble j_elmin, jobject j_dst)
{
static jmethodID set_dops_mid = NULL;
double dst[4];
double *azel;
azel = (*env)->GetDoubleArrayElements(env, j_azel, NULL);
if (azel == NULL)
return;
dops(j_ns, azel, j_elmin, dst);
(*env)->ReleaseDoubleArrayElements(env, j_azel, azel, 0);
if (set_dops_mid == NULL) {
set_dops_mid = (*env)->GetMethodID(env,
(*env)->GetObjectClass(env, j_dst),
"setDops",
"(DDDD)V");
if (set_dops_mid == NULL) {
LOGV("setDops() not found");
return;
}
}
(*env)->CallVoidMethod(env, j_dst, set_dops_mid,
dst[0], dst[1], dst[2], dst[3]);
}
// update time-information for observation-data plot ------------------------
void __fastcall TPlot::UpdateTimeObs(void)
{
AnsiString msgs1[]={" OBS=L1/2 "," L1 "," L2 "," L1/2/5 "," L1/5 ",""," L5 "};
AnsiString msgs2[]={" SNR=...45.","..40.","..35.","..30.","..25 ",""," <25 "};
AnsiString msgs3[]={" SYS=GPS ","GLO ","GAL ","QZS ","BDS ","SBS ",""};
AnsiString msgs4[]={" MP=..0.6","..0.3","..0.0..","-0.3..","-0.6..","",""};
AnsiString msg,msgs[8],s;
double azel[MAXOBS*2],dop[4]={0};
int i,ns=0,no=0,ind=ObsIndex;
char tstr[64];
trace(3,"UpdateTimeObs\n");
if (BtnSol1->Down&&0<=ind&&ind<NObs) {
for (i=IndexObs[ind];i<Obs.n&&i<IndexObs[ind+1];i++,no++) {
if (SatMask[Obs.data[i].sat-1]||!SatSel[Obs.data[i].sat-1]) continue;
if (El[i]<ElMask*D2R) continue;
if (ElMaskP&&El[i]<ElMaskData[(int)(Az[i]*R2D+0.5)]) continue;
azel[ ns*2]=Az[i];
azel[1+ns*2]=El[i];
ns++;
}
}
if (ns>=0) {
dops(ns,azel,ElMask*D2R,dop);
TimeStr(Obs.data[IndexObs[ind]].time,3,1,tstr);
msg.sprintf("[1]%s : N=%d ",tstr,no);
if (PlotType==PLOT_DOP) {
msgs[0].sprintf("NSAT=%d",ns);
msgs[1].sprintf(" GDOP=%.1f",dop[0]);
msgs[2].sprintf(" PDOP=%.1f",dop[1]);
msgs[3].sprintf(" HDOP=%.1f",dop[2]);
msgs[4].sprintf(" VDOP=%.1f",dop[3]);
}
else if (PlotType<=PLOT_SKY&&ObsType->ItemIndex==0) {
msg+=s.sprintf("NSAT=%d ",ns);
for (i=0;i<7;i++) msgs[i]=SimObs?msgs3[i]:msgs1[i];
}
else if (PlotType==PLOT_MPS) {
msg+=s.sprintf("NSAT=%d ",ns);
for (i=0;i<7;i++) msgs[i]=msgs4[i];
}
else {
msg+=s.sprintf("NSAT=%d ",ns);
for (i=0;i<7;i++) msgs[i]=SimObs?msgs3[i]:msgs2[i];
}
}
ShowMsg(msg);
ShowLegend(msgs);
}
// save dop -----------------------------------------------------------------
void __fastcall TPlot::SaveDop(AnsiString file)
{
FILE *fp;
gtime_t time;
double azel[MAXOBS*2],dop[4],tow;
int i,j,ns,week;
char tstr[64],*tlabel;
trace(3,"SaveDop: file=%s\n",file.c_str());
if (!(fp=fopen(file.c_str(),"w"))) return;
tlabel=TimeLabel<=1?"TIME (GPST)":(TimeLabel<=2?"TIME (UTC)":"TIME (JST)");
fprintf(fp,"%% %-*s %6s %8s %8s %8s %8s (EL>=%.0fdeg)\n",TimeLabel==0?13:19,
tlabel,"NSAT","GDOP","PDOP","HDOP","VDOP",ElMask);
for (i=0;i<NObs;i++) {
ns=0;
for (j=IndexObs[i];j<Obs.n&&j<IndexObs[i+1];j++) {
if (SatMask[Obs.data[j].sat-1]) continue;
if (El[j]<ElMask*D2R) continue;
if (ElMaskP&&El[j]<ElMaskData[(int)(Az[j]*R2D+0.5)]) continue;
azel[ ns*2]=Az[j];
azel[1+ns*2]=El[j];
ns++;
}
if (ns<=0) continue;
dops(ns,azel,ElMask*D2R,dop);
time=Obs.data[IndexObs[i]].time;
if (TimeLabel==0) {
tow=time2gpst(time,&week);
sprintf(tstr,"%4d %8.1f ",week,tow);
}
else if (TimeLabel==1) {
time2str(time,tstr,1);
}
else if (TimeLabel==2) {
time2str(gpst2utc(time),tstr,1);
}
else {
time2str(timeadd(gpst2utc(time),9*3600.0),tstr,1);
}
fprintf(fp,"%s %6d %8.1f %8.1f %8.1f %8.1f\n",tstr,ns,dop[0],dop[1],
dop[2], dop[3]);
}
fclose(fp);
}
/*this is the main routine*/
void doshell()
{
char line[80];
/*run forever - the shell shouldn't end*/
while(1==1)
{
/*read in a line*/
printstring("SHELL>\0");
readstring(line);
/*match it against each possible command*/
/*if the user presses return, ignore it*/
if (line[0]==0xd)
continue;
else if (iscommand(line,"CLS\0")==1)
doclear();
else if (iscommand(line,"cls\0")==1)
doclear();
else if (iscommand(line,"COPY\0")==1)
docopy();
else if (iscommand(line,"copy\0")==1)
docopy();
else if (iscommand(line,"CREATE \0")==1)
docreate(line);
else if (iscommand(line,"create \0")==1)
docreate(line);
else if (iscommand(line,"DELETE \0")==1)
dodelete(line);
else if (iscommand(line,"delete \0")==1)
dodelete(line);
else if (iscommand(line,"DIR\0")==1)
dodir();
else if (iscommand(line,"dir\0")==1)
dodir();
else if (iscommand(line,"EXEC \0")==1)
doexecute(line,1);
else if (iscommand(line,"exec \0")==1)
doexecute(line,1);
else if (iscommand(line,"EXECBACK \0")==1)
doexecute(line,0);
else if (iscommand(line,"execback \0")==1)
doexecute(line,0);
else if (iscommand(line,"HELP\0")==1)
dohelp();
else if (iscommand(line,"help\0")==1)
dohelp();
else if (line[0]=='?')
dohelp();
else if (iscommand(line,"TYPE \0")==1)
dotype(line);
else if (iscommand(line,"type \0")==1)
dotype(line);
else if (iscommand(line,"KILL \0")==1)
dokill(line);
else if (iscommand(line,"kill \0")==1)
dokill(line);
else if (iscommand(line,"PS\0")==1)
dops();
else if (iscommand(line,"ps\0")==1)
dops();
else if (iscommand(line,"mkdir\0")==1)
domkdir(line);
else if (iscommand(line,"MKDIR\0")==1)
domkdir(line);
else if (iscommand(line,"ls\0")==1)
dols(line);
else if (iscommand(line,"LS\0")==1)
dols(line);
else
printstring("Command not found\r\n\0");
printstring("\r\n\0");
}
}
void
nav_fix (void)
{
int n;
double tr_time[N_channels + 1], ipart, clock_error;
static double t_cor[N_channels + 1];
unsigned int i;
struct measurement *meas = measurements.measurement;
int tr_prn[N_channels + 1];
ecef rp_ecef;
eceft dm_gps_sat[N_channels + 1], dp_gps_sat[N_channels + 1];
int year, month, day, hour, minute, flag;
double second;
n = 1;
i = 0;
/* For every measurement we receive (they're null terminated) */
while ((meas->transmit_time_offset != 0)
&& (meas->transmit_time_offset != 0)
&& i < N_channels) {
/* Extract the PRN */
int prn = meas->doppler_prn & 0x1f;
/* Make sure we have a good healthy ephemeris for this PRN */
if ((gps_eph[prn].valid == 1) && (gps_eph[prn].health == 0)) {
meas_dop[n] =
(double) (meas->doppler_prn) / 1048576.0 / TIC_dt * (double) (nav_tic);
/**
* (1024 phases / half chip) * (2046 half chips / ms)
* * (1000 ms / sec) = 2096104000 phases / second
**/
tr_time[n] = (double) measurements.bit_of_week / 50.0
+ (double) meas->transmit_time_offset / 2095104000.0;
tr_prn[n] = prn;
n++;
}
/* Next measurement */
meas = &measurements.measurement[++i];
}
n_track = n - 1;
TIC_dt = measurements.i_TIC_dt * 175.0e-9; /*each clock count is 175 ns */
/* use basic TIC interval for ICP */
/* if (ICP_CTL==1)TIC_dt=TIC_dt/float(nav_tic); */
if (out_debug)
fprintf (debug, "n_track= %d\n", n_track);
for (i = 1; i <= (unsigned) n_track; i++) {
track_sat[i] = satpos_ephemeris (tr_time[i], gps_eph + tr_prn[i]);
/* process Carrier Tracking Loop or Integrated Carrier Phase */
if (ICP_CTL == 0) { /* satellite velocity */
dm_gps_sat[i] = satpos_ephemeris (tr_time[i] - TIC_dt / 2.0,
gps_eph + tr_prn[i]); /* for CTL */
dp_gps_sat[i] = satpos_ephemeris (tr_time[i] + TIC_dt / 2.0,
gps_eph + tr_prn[i]);
d_sat[i].x =
(dp_gps_sat[i].x - dm_gps_sat[i].x) / TIC_dt -
track_sat[i].y * omegae;
d_sat[i].y =
(dp_gps_sat[i].y - dm_gps_sat[i].y) / TIC_dt +
track_sat[i].x * omegae;
d_sat[i].z = (dp_gps_sat[i].z - dm_gps_sat[i].z) / TIC_dt;
}
else {
dm_gps_sat[i] = satpos_ephemeris (tr_time[i] - TIC_dt / (float) (nav_tic), gps_eph + tr_prn[i]); /* for ICP */
dp_gps_sat[i] = track_sat[i];
d_sat[i].x =
(dp_gps_sat[i].x - dm_gps_sat[i].x) / TIC_dt * (float) (nav_tic) -
track_sat[i].y * omegae;
d_sat[i].y =
(dp_gps_sat[i].y - dm_gps_sat[i].y) / TIC_dt * (float) (nav_tic) +
track_sat[i].x * omegae;
d_sat[i].z =
(dp_gps_sat[i].z - dm_gps_sat[i].z) / TIC_dt * (float) (nav_tic);
}
t_cor[i] = track_sat[i].tb -
tropo_iono (tr_prn[i], track_sat[i].az, track_sat[i].el, tr_time[i]);
dt[i] = m_time[1] - (tr_time[i] - t_cor[i]);
}
if (n_track >= 4) {
rpvt = pos_vel_time (n_track);
cbias = rpvt.dt;
clock_error = rpvt.df;
m_time[1] = m_time[1] - cbias;
rp_ecef.x = rpvt.x;
rp_ecef.y = rpvt.y;
rp_ecef.z = rpvt.z;
rp_llh = ecef_to_llh (rp_ecef);
/* a position reasonableness check */
if (rp_llh.hae > -200000.0 && rp_llh.hae < 1800000) {
/**
* Translate velocity into North, East, Up coordinates
**/
get_velocity ();
/* a velocity reasonableness check */
if (sqrt (SQ (receiver.vel.north) +
SQ (receiver.vel.east) +
SQ (receiver.vel.up)) < 514.0) {
if (fabs (clock_error) < 500.0)
clock_offset = clock_error;
/* if we have a good fix we're navigating */
status = navigating;
if (align_t == 1) {
long TIC_counter;
delta_m_time = modf (m_time[1], &ipart);
if (nav_up < 1.0) {
delta_m_time = modf (delta_m_time / nav_up, &ipart);
if (delta_m_time > 0.5)
m_error = (delta_m_time - 1.0) * nav_up;
else
m_error = delta_m_time * nav_up;
}
else {
if (delta_m_time > 0.5)
m_error = (delta_m_time - 1.0) / nav_up;
else
m_error = delta_m_time / nav_up;
}
TIC_counter =
(TIC_ref - m_error * TIC_ref / 10) * (1.0 -
clock_offset * 1.0e-6);
set_TIC (TIC_counter);
}
rec_pos_llh.lon = rp_llh.lon;
rec_pos_llh.lat = rp_llh.lat;
rec_pos_llh.hae = rp_llh.hae;
current_loc.lon = rp_llh.lon;
current_loc.lat = rp_llh.lat;
current_loc.hae = rp_llh.hae;
rec_pos_xyz.x = rp_ecef.x;
rec_pos_xyz.y = rp_ecef.y;
rec_pos_xyz.z = rp_ecef.z;
/**
* Calculate DOPS
**/
dops (n_track);
if (out_pos == 1)
fprintf (output,
"%20.10f, %f, %f, %f,",
m_time[1], rec_pos_llh.lat * r_to_d,
rec_pos_llh.lon * r_to_d, rec_pos_llh.hae);
if (out_vel == 1)
fprintf (output, " %f, %f, %f,",
receiver.vel.north, receiver.vel.east, receiver.vel.up);
if (out_time == 1)
fprintf (output, " %f,", clock_offset);
if (out_pos || out_vel || out_time)
fprintf (output, " %f, %f, %f\n", hdop, vdop, tdop);
/**
* Since we have a valid position/velocity narrow the
* doppler search window to +-5 doppler bins
**/
search_max_f = 5;
m_time[0] = m_time[1];
}
}
}
else {
/* less than 4 sats */
m_time[1] = m_time[1] + TIC_dt * (1.0 + clock_offset / 1.e6);
rp_ecef.x = 0.0;
rp_ecef.y = 0.0;
rp_ecef.z = 0.0;
rpvt.xv = 0.0;
rpvt.yv = 0.0;
rpvt.zv = 0.0;
}
if (out_kalman == 1) { /* Kalman filter output */
fprintf (kalm,
"time %20.10f, rpx %15.10f, rpy %15.10f, rpz %15.10f, ",
m_time[1], rp_ecef.x, rp_ecef.y, rp_ecef.z);
fprintf (kalm, "rvx %15.10f, rvy %15.10f, rvz %15.10f, Nsats %d\n",
rpvt.xv, rpvt.yv, rpvt.zv, n_track);
}
if (out_rinex == 1) { /* RINEX OBSERVATION EPOCH/SAT */
/* Calculate the calendar GPS time corresponding to the current GPS time
adding the leapseconds as these are automatically taken out */
gps2utc(m_time[1]+dtls, 1024+gps_eph[tr_prn[1]].week, &year, &month, &day, &hour, &minute, &second);
/* Check if this is the first observation, if so, write the
RINEX header */
if(write_rinex_obs_head)
{
write_rinex_obs_head = 0;
rinex_head_obs(year+2000, month, day, hour, minute, second);
}
/* Follows the RINEX-2 Format */
flag = 0;
fprintf(rinex_obs," %02d %02d %02d %02d %02d%11.7f %1d%3d", year, month, day, hour, minute, second, flag, n_track);
for (i = 1; i <= (unsigned) n_track; i++)
{
/* Print each satellite in view */
fprintf(rinex_obs,"G%2d", tr_prn[i]);
}
fprintf(rinex_obs,"\n");
}
for (i = 1; i <= (unsigned) n_track; i++) {
satellite[tr_prn[i]].Pr = (m_time[1] - tr_time[i]) * c; /*(m_time[1] - (tr_time[i] - t_cor[i])) * c;*/
satellite[tr_prn[i]].dPr = meas_dop[i] * lambda;
if (out_kalman == 1) { /* Kalman filter output */
fprintf (kalm,
" PRN %2d, px %20.10f, py %20.10f, pz %20.10f, ",
tr_prn[i], track_sat[i].x, track_sat[i].y, track_sat[i].z);
fprintf (kalm, " vx %16.10f, vy %16.10f, vz %16.10f, ",
d_sat[i].x, d_sat[i].y, d_sat[i].z);
fprintf (kalm, " Pr %20.10f, dPr %16.10f\n",
satellite[tr_prn[i]].Pr + t_cor[i] * c, satellite[tr_prn[i]].dPr);
}
if(out_rinex == 1) { /* RINEX data output */
/* Currently we only are only saving the pseudorange and Doppler. The LLI and signal
strength indicators need to be implemented as well and are currently left blank */
fprintf(rinex_obs, "%14.3f %14.3f\n", satellite[tr_prn[i]].Pr, meas_dop[i]);
}
}
}
