mirror
/
RS-tracker
kopia lustrzana https://github.com/rs1729/RS
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

read_rinex

dump
Zilog80 2015-11-05 00:22:05 +01:00
rodzic 95a7c023cf
commit 79fea4b339
2 zmienionych plików z 230 dodań i 16 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

140
rs92/nav_gps.c
Wyświetl plik

@ -110,6 +110,7 @@ typedef struct {
double Z;
int ephhr;
double PR;
double ephtime;
} SAT_t;
@ -283,6 +284,145 @@ int read_RNXephemeris(FILE *fp, EPHEM_t eph[][24]) {
return 0;
}
EPHEM_t *read_RNXpephs(FILE *fp) {
int l, i, n;
char buffer[82];
char buf[64], str[20];
char *pbuf;
unsigned ui;
double dbl;
int c;
EPHEM_t ephem = {}, *te = NULL;
int count = 0;
long fpos;
do { // header-Zeilen: 80 Zeichen
pbuf = fgets(buffer, 82, fp); // max 82-1 Zeichen + '\0'
buffer[82] = '\0'; // doppelt haelt besser
} while ( pbuf && !strstr(buffer, "END OF HEADER") );
if (pbuf == NULL) return NULL;
fpos = ftell(fp);
count = 0;
while (count >= 0) { // data-Zeilen: 79 Zeichen
pbuf = fgets(buffer, 82, fp); if (pbuf == 0) break;
strncpy(str, buffer, 3);
str[3] = '\0';
sscanf(str, "%d", &ui);
if (ui < 33) count++;
for (i = 0; i < 7; i++) {
pbuf = fgets(buffer, 82, fp); if (pbuf == 0) break;
}
}
//printf("Ephemerides: %d\n", count);
fseek(fp, fpos, SEEK_SET);
te = calloc( count+1, sizeof(ephem) ); // calloc( 1, sizeof(ephem) );
if (te == NULL) return NULL;
n = 0;
while (count > 0) { // brdc/hour-rinex sollte nur Daten von einem Tag enthalten
//memset(&ephem, 0, sizeof(ephem));
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; sscanf(buf, "%d", &ui);
ephem.prn = ui;
for (i = 0; i < 16; i++) ephem.epoch[i] = '0';
ephem.epoch[16] = '\0';
l = fread(buf, 19, 1, fp); if (l != 1) break; buf[19] = 0;
for (i = 0; i < 6; i++) {
c = buf[3*i ]; if (c == ' ') c = '0'; str[2*i ] = c;
c = buf[3*i+1]; if (c == ' ') c = '0'; str[2*i+1] = c;
}
str[12] = buf[17];
str[13] = buf[18];
str[14] = '\0';
strncpy(ephem.epoch , "20", 2); // vorausgesetzt 21.Jhd; Datum steht auch im Header
strncpy(ephem.epoch+2, str, 15);
ephem.epoch[16] = '\0';
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af0 = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af1 = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af2 = dbl;
if ((c=fgetc(fp)) == EOF) break;
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iode = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.crs = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.delta_n = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.M0 = dbl;
if ((c=fgetc(fp)) == EOF) break;
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cuc = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.e = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cus = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.sqrta = dbl;
if ((c=fgetc(fp)) == EOF) break;
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.toe = dbl;
ephem.toc = ephem.toe;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cic = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.Omega0 = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cis = dbl;
if ((c=fgetc(fp)) == EOF) break;
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.i0 = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.crc = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.w = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.OmegaDot = dbl;
if ((c=fgetc(fp)) == EOF) break;
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.idot = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.codeL2 = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.gpsweek = (int)dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iodc = dbl;
if ((c=fgetc(fp)) == EOF) break;
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.sva = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.svh = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.tgd = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iodc = dbl;
if ((c=fgetc(fp)) == EOF) break;
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.ttom = dbl;
pbuf = fgets(buffer, 82, fp);
/* // die letzten beiden Felder (spare) sind manchmal leer (statt 0.00); manchmal fehlt sogar das drittletzte Feld
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.fit = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.spare1 = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.spare2 = dbl;
if ((c=fgetc(fp)) == EOF) break; */
ephem.week = 1; // ephem.gpsweek
te[n] = ephem;
n += 1;
//tmp = realloc( te, (count+1) * sizeof(ephem) );
//if (tmp == NULL) break;
//te = tmp;
if (pbuf == NULL) break;
}
te[n].prn = 0;
return te;
}
/* ---------------------------------------------------------------------------------------------------- */

106
rs92/rs92gps.c
Wyświetl plik

@ -477,7 +477,9 @@ int get_Cal() {
#include "nav_gps.c"
EPHEM_t alm[33];
EPHEM_t eph[33][24];
//EPHEM_t eph[33][24];
EPHEM_t *ephs = NULL;
SAT_t sat[33];
@ -579,6 +581,54 @@ int calc_satpos_rnx(EPHEM_t eph[][24], double t, SAT_t *satp) {
return 0;
}
int calc_satpos_rnx2(EPHEM_t *eph, double t, SAT_t *satp) {
double X, Y, Z;
int j;
int week;
double cl_corr;
double tdiff, td;
int count, count0;
for (j = 1; j < 33; j++) {
count = count0 = 0;
// Woche hat 604800 sec
tdiff = 604800;
while (eph[count].prn > 0) {
if (eph[count].prn == j) {
if (t - eph[count].toe > 604800/2) rollover = +1;
else if (t - eph[count].toe < -604800/2) rollover = -1;
else rollover = 0;
td = fabs( t - eph[count].toe - rollover*604800);
if ( td < tdiff ) {
tdiff = td;
week = eph[count].week - rollover;
count0 = count;
}
}
count += 1;
}
GPS_SatellitePosition_Ephem(
week, t, eph[count0],
&cl_corr, &X, &Y, &Z
);
satp[j].X = X;
satp[j].Y = Y;
satp[j].Z = Z;
satp[j].clock_corr = cl_corr;
satp[j].ephtime = eph[count0].toe;
}
return 0;
}
typedef struct {
@ -620,30 +670,45 @@ int get_pseudorange() {
if (almanac) calc_satpos_alm(alm, gpstime/1000.0, sat);
if (ephem) calc_satpos_rnx(eph, gpstime/1000.0, sat);
//if (ephem) calc_satpos_rnx(eph, gpstime/1000.0, sat);
if (ephem) calc_satpos_rnx2(ephs, gpstime/1000.0, sat);
k = 0;
for (j = 0; j < 12; j++) {
for (i = 0; i < 4; i++) { pseudobytes[i] = frame[posGPS_DATA+8*j+i]; }
memcpy(&byteval, pseudobytes, 4);
if ( prns[j] == 0 && byteval != 0x7FFFFFFF ) prns[j] = 32;
range[prns[j]].chips = byteval;
if ( byteval == 0x7FFFFFFF || byteval == 0x55555555 ) {
range[prns[j]].chips = 0;
continue;
}
/*
if ( byteval > 0x10000000 && byteval < 0xF0000000 ) {
range[prns[j]].chips = 0;
continue;
}
*/
if ( prns[j] == 0 ) prns[j] = 32;
range[prns[j]].chips = byteval;
range[prns[j]].time = gpstime;
/*
for (i = 0; i < 4; i++) { pseudobytes[i] = frame[posGPS_DATA+8*j+4+i]; }
memcpy(&byteval, pseudobytes, 4);
range[prns[j]].ca = byteval & 0xFFFFFF;
//range[prns[j]].dop = (byteval >> 24) & 0xFF;
range[prns[j]].time = gpstime;
if ( (range[prns[j]].chips != 0x7FFFFFFF && dist(sat[prns[j]].X, sat[prns[j]].Y, sat[prns[j]].Z, 0, 0, 0) > 6700000)
&& (range[prns[j]].chips != 0x55555555 /* && range[prns[j]].ca != 0x555555 */ )
)
if (range[prns[j]].ca == 0x555555) {
range[prns[j]].ca = 0;
continue;
}
*/
if ( dist(sat[prns[j]].X, sat[prns[j]].Y, sat[prns[j]].Z, 0, 0, 0) > 6700000 )
{
prn[k] = prns[j];
for (i = 0; i < k; i++) { if (prn[i] == prn[k]) break; }
k++;
for (i = 0; i < k; i++) { if (prn[i] == prns[j]) break; }
if (i == k) {
prn[k] = prns[j];
k++;
}
}
}
@ -660,7 +725,8 @@ int get_pseudorange() {
if (prj < pr0) pr0 = prj;
}
for (j = 0; j < k; j++) sat[prn[j]].PR = sat[prn[j]].pseudorange + sat[prn[j]].clock_corr - pr0 + 20e6;
// es kann PRNs geben, die zeitweise stark abweichende PR liefern;
// eventuell Standardabweichung ermitteln und fehlerhafte Sats weglassen
return k;
}
@ -912,8 +978,14 @@ int main(int argc, char *argv[]) {
fclose(fp_alm);
}
if (fp_eph) {
i = read_RNXephemeris(fp_eph, eph);
if (i == 0) {
/* i = read_RNXephemeris(fp_eph, eph);
if (i == 0) {
ephem = 1;
almanac = 0;
}
fclose(fp_eph); */
ephs = read_RNXpephs(fp_eph);
if (ephs) {
ephem = 1;
almanac = 0;
}
@ -974,7 +1046,9 @@ int main(int argc, char *argv[]) {
}
}
fclose(fp);
free(ephs);
return 0;
}