read_rinex

rs92/nav_gps.c

@@ -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;
+}
+
 
 /* ---------------------------------------------------------------------------------------------------- */
 

rs92/rs92gps.c

@@ -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;
-
+        if (range[prns[j]].ca == 0x555555) {
-        range[prns[j]].time = gpstime;
+            range[prns[j]].ca = 0;
-
+            continue;
-        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 ( 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] == prns[j]) break; }
-            for (i = 0; i < k; i++) { if (prn[i] == prn[k]) break; }
+            if (i == k) {
-            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);
+        /* i = read_RNXephemeris(fp_eph, eph);
-        if (i == 0) {
+           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;
 }