├── .gitignore ├── LICENSE ├── Makefile ├── README.md ├── circle.csv ├── ocxo_mod ├── L1000089.JPG ├── L1000090.JPG ├── L1000092.JPG ├── README.md ├── Y3_removed.jpg ├── Y3_soldered.jpg └── pluto_ocxo_mod_schematic.pdf ├── plutogpssim.c └── plutogpssim.h /.gitignore: -------------------------------------------------------------------------------- 1 | # Object files 2 | *.o 3 | *.ko 4 | *.obj 5 | *.elf 6 | 7 | # Libraries 8 | *.lib 9 | *.a 10 | *.la 11 | *.lo 12 | 13 | # Shared objects (inc. Windows DLLs) 14 | *.so 15 | *.so.* 16 | 17 | # Executables 18 | *.out 19 | *.hex 20 | pluto-gps-sim 21 | 22 | # Debug files 23 | *.dSYM/ 24 | 25 | # Output 26 | *.bin 27 | 28 | # Netbeans project folder 29 | nbproject/* 30 | 31 | # Rinex files 32 | *.gz 33 | 34 | # Almanac files 35 | *.sem 36 | *.sem.txt 37 | *.yuma 38 | *.alm -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2018 Mictronics 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | DIALECT = -std=c11 2 | CFLAGS += $(DIALECT) -O0 -g -W -Wall -D_GNU_SOURCE 3 | LIBS = -lm -lpthread -lcurl -lz 4 | 5 | CFLAGS += $(shell pkg-config --cflags libiio libad9361) 6 | 7 | all: pluto-gps-sim 8 | 9 | %.o: %.c *.h 10 | $(CC) $(CPPFLAGS) $(CFLAGS) -c $< -o $@ 11 | 12 | pluto-gps-sim: plutogpssim.o $(COMPAT) 13 | ${CC} $< ${LDFLAGS} $(LIBS) -o $@ $(shell pkg-config --cflags libiio libad9361) $(shell pkg-config --libs libiio libad9361) 14 | 15 | clean: 16 | rm -f *.o pluto-gps-sim 17 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PLUTO-GPS-SIM 2 | 3 | __Project closed!__ Development continues on [multi-sdr-gps-sim](https://github.com/Mictronics/multi-sdr-gps-sim). 4 | 5 | PLUTO-GPS-SIM generates a GPS baseband signal IQ data stream, which is then transmitted by the 6 | software-defined radio (SDR) platform [ADALM-Pluto](https://wiki.analog.com/university/tools/pluto). 7 | 8 | Project based on [gps-sdr-sim](https://github.com/osqzss/gps-sdr-sim). Credits to Takuji Ebinuma. 9 | 10 | ### Generating the GPS signal 11 | 12 | The user is able to assign a static location directly through the command line. 13 | 14 | The user also specifies the GPS satellite constellation through a GPS broadcast 15 | ephemeris file. The daily GPS broadcast ephemeris file (brdc) is a merge of the 16 | individual site navigation files into one. 17 | 18 | These files are then used to generate the simulated pseudorange and 19 | Doppler for the GPS satellites in view. This simulated range data is 20 | then used to generate the digitized I/Q samples for the GPS signal 21 | which are then feed into ADALM-Pluto SDR to transmit the GPS L1 frequency. 22 | 23 | The simulation start time can be specified if the corresponding set of ephemerides 24 | is available. Otherwise the first time of ephemeris in the RINEX navigation file 25 | is selected. 26 | 27 | __Note__ 28 | 29 | The ADLAM-Pluto SDR internal oscillator is not precise enough (frequency offset) and stable (temperature drift) 30 | for GPS signal generation. 31 | 32 | Do not run this code without Pluto oscillator modification, either by TCXO or more expensive (but highest precision) OCXO. 33 | Search for [adlam pluto tcxo](https://duckduckgo.com/?q=adlam+pluto+tcxo) 34 | 35 | ### Ephemeris files 36 | 37 | NASA CDDIS anonymous ftp service has been discontinued on October 31, 2020. 38 | 39 | The new way to get ephemeris data is as follows: 40 | 41 | Register an account with https://urs.earthdata.nasa.gov/ (There is no way around this step). 42 | 43 | Create a file called .netrc with the following format. 44 | machine urs.earthdata.nasa.gov login password where and are the values you set when you created your Earthdata login account. Don't forget to sudo chmod 004 .netrc so that no one can read your file and get your username and password. 45 | 46 | You can automate with the process with following script. 47 | 48 | ``` 49 | #!/bin/sh 50 | day=$(date +%j) 51 | year=$(date +%Y) 52 | yr=$(date +%y) 53 | 54 | curl -c /tmp/cookie -n -L -o "brdc""$day""0.$yr""n.Z" "https://cddis.gsfc.nasa.gov/archive/gnss/data/daily/$year""/brdc/brdc""$day""0.$yr""n.Z" 55 | 56 | uncompress "brdc""$day""0.$yr""n.Z" 57 | echo "brdc""$day""0.$yr""n.Z" 58 | ``` 59 | 60 | The .netrc file along with the -n flag allows curl to automatically authenticate when attempting to retrieve the file. 61 | 62 | ### Build instructions 63 | 64 | #### Dependencies 65 | 66 | Build depends on libm, libpthread and libcurl4-openssl-dev. 67 | 68 | You will also need the latest build and install of libad9361-dev and libiio-dev. The Debian packages 69 | libad9361-dev that is available up to Debian 9 (stretch) is outdated and missing a required function. 70 | So you have to build packages from source: 71 | 72 | The first step is to fetch the dependencies, which as of now is only libxml2. On a Debian-flavoured GNU/Linux distribution, like Ubuntu for instance: 73 | 74 | ``` 75 | $ sudo apt-get install libxml2 libxml2-dev bison flex libcdk5-dev cmake 76 | ``` 77 | 78 | Depending on the backend (how you want to attach the IIO device), you may need at least one of: 79 | 80 | ``` 81 | $ sudo apt-get install libaio-dev libusb-1.0-0-dev libserialport-dev libxml2-dev libavahi-client-dev doxygen graphviz 82 | ``` 83 | 84 | Then build in this order: 85 | 86 | ``` 87 | $ git clone https://github.com/analogdevicesinc/libiio.git 88 | $ cd libiio 89 | $ cmake ./ 90 | $ make 91 | $ sudo make install 92 | ``` 93 | 94 | ``` 95 | $ git clone https://github.com/analogdevicesinc/libad9361-iio.git 96 | $ cd libad9361-iio 97 | $ cmake ./ 98 | $ make 99 | $ sudo make install 100 | ``` 101 | 102 | #### Building under Linux with GCC 103 | 104 | ``` 105 | $ git clone https://github.com/mictronics/pluto-gps-sim 106 | $ cd pluto-gps-sim 107 | $ make all 108 | ``` 109 | 110 | ### Usage 111 | 112 | ```` 113 | pluto-gps-sim [options] 114 | Options: 115 | -e RINEX navigation file for GPS ephemerides (required) 116 | -u User motion file (dynamic mode) 10Hz, Max 3000 points 117 | -3 Use RINEX version 3 format 118 | -f Pull actual RINEX navigation file from NASA FTP server 119 | -c ECEF X,Y,Z in meters (static mode) e.g. 3967283.15,1022538.18,4872414.48 120 | -l Lat,Lon,Hgt (static mode) e.g. 30.286502,120.032669,100 121 | -t Scenario start time YYYY/MM/DD,hh:mm:ss 122 | -T Overwrite TOC and TOE to scenario start time (use ```now``` for actual time) 123 | -s Sampling frequency [Hz] (default: 2600000) 124 | -i Disable ionospheric delay for spacecraft scenario 125 | -v Show details about simulated channels 126 | -A Set TX attenuation [dB] (default -20.0) 127 | -B Set RF bandwidth [MHz] (default 5.0) 128 | -U ADALM-Pluto URI (eg. usb:1.2.5) 129 | -N ADALM-Pluto network IP or hostname (default pluto.local) 130 | ```` 131 | 132 | Set static mode location: 133 | 134 | ``` 135 | > pluto-gps-sim -e brdc3540.14n -l 30.286502,120.032669,100 136 | ``` 137 | 138 | Set user motion mode: 139 | 140 | ``` 141 | > pluto-gps-sim -e brdc3540.14n -u circle.csv 142 | ``` 143 | 144 | Set TX attenuation: 145 | 146 | ``` 147 | > pluto-gps-sim -e brdc3540.14n -A -30.0 148 | ``` 149 | 150 | Default -20.0dB. Applicable range 0.0dB to -80.0dB in 0.25dB steps. 151 | 152 | Set RF bandwidth: 153 | 154 | ``` 155 | > pluto-gps-sim -e brdc3540.14n -B 3.0 156 | ``` 157 | 158 | Default 3.0MHz. Applicable range 1.0MHz to 5.0MHz 159 | 160 | ### Transmitting the samples 161 | 162 | The TX port of a particular SDR platform is connected to the GPS receiver 163 | under test through a DC block and a fixed 50-60dB attenuator. 164 | 165 | ### License 166 | 167 | Copyright © 2018 Mictronics 168 | Distributed under the [MIT License](http://www.opensource.org/licenses/mit-license.php). 169 | -------------------------------------------------------------------------------- /ocxo_mod/L1000089.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mictronics/pluto-gps-sim/dc71c18af630c43c3bebf33b3c1f1e737bba15d4/ocxo_mod/L1000089.JPG -------------------------------------------------------------------------------- /ocxo_mod/L1000090.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mictronics/pluto-gps-sim/dc71c18af630c43c3bebf33b3c1f1e737bba15d4/ocxo_mod/L1000090.JPG -------------------------------------------------------------------------------- /ocxo_mod/L1000092.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mictronics/pluto-gps-sim/dc71c18af630c43c3bebf33b3c1f1e737bba15d4/ocxo_mod/L1000092.JPG -------------------------------------------------------------------------------- /ocxo_mod/README.md: -------------------------------------------------------------------------------- 1 | # ADLAM-Pluto OCXO Modification 2 | 3 | ## About OCXO 4 | 5 | A crystal oven is a temperature-controlled chamber used to maintain the quartz crystal in electronic crystal oscillators at a constant temperature, 6 | in order to prevent changes in the frequency due to variations in ambient temperature. 7 | An oscillator of this type is known as an oven-controlled crystal oscillator (OCXO, where "XO" is an old abbreviation for "crystal oscillator".) 8 | The oven-controlled oscillator achieves the best frequency stability possible from a crystal. The short term frequency stability of OCXOs is typically 9 | 1×10^−12 over a few seconds, while the long term stability is limited to around 1×10^−8 (10 ppb) per year by aging of the crystal. 10 | 11 | See [Wikipedia](https://en.wikipedia.org/wiki/Crystal_oven) for details. 12 | 13 | ## Details 14 | 15 | This modification exchanges the original ADLAM-Pluto crystal oscillator by an temperature-controlled crystal oscillator. 16 | 17 | The original oscillator (RXO3225M) is rated with a temperature stability of +-25 ppm. 18 | 19 | The replacement temperature-controlled oscillator is rated with +-20 ppb or 0.02 ppm (1 ppb = 1x10^-9). 20 | A Connor-Winfield DOCSC series OCXO is used, type DOCSC022F-040.0M, available from 21 | [Digikey](https://www.digikey.de/product-detail/de/connor-winfield/DOCSC022F-040-0M/CW884CT-ND/5399028) order number CW884CT-ND. 22 | 23 | In addition a 3.3V linear regulator and two capacitors are required. In fact the ADLAM-Pluto doesn't provide 3.3V for the OCXO anywhere on its PCB. 24 | So we have to convert down from 5V USB supply. The linear voltage regulator is a LT1117-3.3 in this modification. But any other will do as long as 25 | it can handle the power/current requirements. 26 | 27 | __The OCXO requires up to 3W power supply, hence up to 1A @ 3.3V__ 28 | 29 | My measurements on two units showing 680mA drawn from 5V on power up for few seconds when the OCXO initally heats up. Current then drops down to 30 | ~170mA depending on enviroment and Pluto internal temperature. The current drawn decreases with raising temperature and vice versa. 31 | 32 | ## Preparation 33 | 34 | Original crystal oscillator Y3. 35 | 36 | ![Y3 soldered](Y3_soldered.jpg) 37 | 38 | The original crystal oscillator Y3 on Pluto PCB must be removed. Warranty will be lost! 39 | 40 | ![Y3 removed](Y3_removed.jpg) 41 | 42 | ## Modification 43 | 44 | Example modification. The LT1117-3.3 requires a small heat sink. 45 | 46 | ![Image 1](L1000089.JPG) 47 | 48 | ![Image 2](L1000090.JPG) 49 | 50 | ![Image 3](L1000092.JPG) 51 | 52 | ## Schematic 53 | 54 | See [pluto_ocxo_mod_schematic.pdf](pluto_ocxo_mod_schematic.pdf) for details. 55 | 56 | ## Calibration 57 | 58 | Each ADLAM-Pluto unit is factory calibrated for the installed crystal oscillator. However, this calibration is usually way off for the new OCXO and 59 | has to reverted. 60 | 61 | Calibration is done by setting the `xo_correction` environment variable in Pluto bootloader. See last command on this page 62 | [https://wiki.analog.com/university/tools/pluto/devs/booting#examples](https://wiki.analog.com/university/tools/pluto/devs/booting#examples) 63 | 64 | Step by step: 65 | 66 | 1. Connect to Pluto by ssh: `ssh -l root 192.168.2.1` use password `analog` 67 | 2. Set enviroment variable: `fw_setenv xo_correction 40000000` 68 | 3. `reboot` or cycle Pluto power. 69 | 70 | All environment variables can be printed with `fw_printenv`. 71 | 72 | Alternative way: 73 | 74 | Open config.txt in Pluto's USB mass storage device. Edit or add `xo_correction` in system section if not exists. Reboot or cycle power. 75 | 76 | ``` 77 | [SYSTEM] 78 | xo_correction = 40000000 79 | ``` 80 | 81 | In case you have a precise measurement tool, for example a spectrum analyzer, then you can calibrate your Pluto at the desired operating 82 | frequency and fine tune the value for xo_correction. In that case, wait at least 30min, keep the Pluto powered on during that time, to heat up 83 | and temperature stabilize the entire unit. 84 | 85 | __Note__ 86 | 87 | Keep in mind that any OCXO minimal offset increases with Pluto TX frequency due to internal multiplication. 88 | For example, changing the calibration value `xo_correction` by 1Hz will change the offset by 25Hz @ 1GHz TX frequency. (1GHz / 40MHz = factor 25) 89 | -------------------------------------------------------------------------------- /ocxo_mod/Y3_removed.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mictronics/pluto-gps-sim/dc71c18af630c43c3bebf33b3c1f1e737bba15d4/ocxo_mod/Y3_removed.jpg -------------------------------------------------------------------------------- /ocxo_mod/Y3_soldered.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mictronics/pluto-gps-sim/dc71c18af630c43c3bebf33b3c1f1e737bba15d4/ocxo_mod/Y3_soldered.jpg -------------------------------------------------------------------------------- /ocxo_mod/pluto_ocxo_mod_schematic.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mictronics/pluto-gps-sim/dc71c18af630c43c3bebf33b3c1f1e737bba15d4/ocxo_mod/pluto_ocxo_mod_schematic.pdf -------------------------------------------------------------------------------- /plutogpssim.c: -------------------------------------------------------------------------------- 1 | /** 2 | * pluto-gps-sim generates a IQ data stream on-the-fly to simulate a 3 | * GPS L1 baseband signal using the ADALM-Pluto SDR platform. 4 | * 5 | * This file is part of the pluto-gps-sim project at 6 | * https://github.com/mictronics/pluto-gps-sim.git 7 | * 8 | * Copyright © 2019 Mictronics 9 | * Distributed under the MIT License. 10 | * 11 | */ 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #if defined(__MACH__) || defined(__APPLE__) 27 | #include 28 | #include 29 | #include 30 | #endif 31 | #include "plutogpssim.h" 32 | 33 | #define RINEX2_FILE_NAME "rinex2.gz" 34 | #define RINEX3_FILE_NAME "rinex3.gz" 35 | #define RINEX_FTP_URL "ftp://igs.bkg.bund.de/IGS/" 36 | #define RINEX2_SUBFOLDER "nrt" 37 | #define RINEX3_SUBFOLDER "nrt_v3" 38 | #define RINEX_FTP_FILE "%s/%03i/%02i/%4s%03i%c.%02in.gz" 39 | 40 | #define NOTUSED(V) ((void) V) 41 | #define MHZ(x) ((long long)(x*1000000.0 + .5)) 42 | #define GHZ(x) ((long long)(x*1000000000.0 + .5)) 43 | #define TX_SAMPLE_FREQ 3000000 44 | #define NUM_SAMPLES (TX_SAMPLE_FREQ / 10) 45 | #define BUFFER_SIZE (NUM_SAMPLES * 2 * sizeof(int16_t)) 46 | 47 | #if defined(__MACH__) || defined(__APPLE__) 48 | 49 | typedef struct cpu_set { 50 | uint32_t count; 51 | } cpu_set_t; 52 | 53 | static inline void 54 | CPU_ZERO(cpu_set_t *cs) { 55 | cs->count = 0; 56 | } 57 | 58 | static inline void 59 | CPU_SET(int num, cpu_set_t *cs) { 60 | cs->count |= (1 << num); 61 | } 62 | 63 | static inline int 64 | CPU_ISSET(int num, cpu_set_t *cs) { 65 | return (cs->count & (1 << num)); 66 | } 67 | #endif 68 | 69 | struct stream_cfg { 70 | long long bw_hz; // Analog banwidth in Hz 71 | long long fs_hz; // Baseband sample rate in Hz 72 | long long lo_hz; // Local oscillator frequency in Hz 73 | const char* rfport; // Port name 74 | double gain_db; // Hardware gain 75 | pthread_cond_t data_cond; 76 | pthread_t tx_thread; 77 | pthread_mutex_t data_mutex; // Mutex to synchronize buffer access 78 | const char *uri; 79 | const char *hostname; 80 | bool exit; // Exit from the main loop when true 81 | }; 82 | 83 | static struct stream_cfg plutotx; 84 | static short *iq_buff = NULL; 85 | static pthread_t pluto_thread; 86 | static char rinex_date[21]; 87 | 88 | struct ftp_file { 89 | const char *filename; 90 | FILE *stream; 91 | }; 92 | 93 | const int sinTable512[] = { 94 | 1, 7, 13, 19, 26, 32, 38, 44, 51, 57, 63, 69, 75, 82, 88, 94, 95 | 100, 106, 112, 119, 125, 131, 137, 143, 149, 155, 161, 167, 173, 179, 184, 190, 96 | 196, 202, 208, 213, 219, 225, 230, 236, 241, 247, 252, 258, 263, 269, 274, 279, 97 | 284, 290, 295, 300, 305, 310, 315, 320, 325, 329, 334, 339, 344, 348, 353, 357, 98 | 362, 366, 371, 375, 379, 383, 387, 392, 396, 399, 403, 407, 411, 415, 418, 422, 99 | 425, 429, 432, 436, 439, 442, 445, 448, 451, 454, 457, 460, 462, 465, 468, 470, 100 | 473, 475, 477, 479, 482, 484, 486, 488, 489, 491, 493, 495, 496, 498, 499, 500, 101 | 502, 503, 504, 505, 506, 507, 508, 508, 509, 510, 510, 511, 511, 511, 511, 511, 102 | 512, 511, 511, 511, 511, 511, 510, 510, 509, 508, 508, 507, 506, 505, 504, 503, 103 | 502, 500, 499, 498, 496, 495, 493, 491, 489, 488, 486, 484, 482, 479, 477, 475, 104 | 473, 470, 468, 465, 462, 460, 457, 454, 451, 448, 445, 442, 439, 436, 432, 429, 105 | 425, 422, 418, 415, 411, 407, 403, 399, 396, 392, 387, 383, 379, 375, 371, 366, 106 | 362, 357, 353, 348, 344, 339, 334, 329, 325, 320, 315, 310, 305, 300, 295, 290, 107 | 284, 279, 274, 269, 263, 258, 252, 247, 241, 236, 230, 225, 219, 213, 208, 202, 108 | 196, 190, 184, 179, 173, 167, 161, 155, 149, 143, 137, 131, 125, 119, 112, 106, 109 | 100, 94, 88, 82, 75, 69, 63, 57, 51, 44, 38, 32, 26, 19, 13, 7, 110 | 1, -5, -11, -17, -24, -30, -36, -42, -49, -55, -61, -67, -73, -80, -86, -92, 111 | -98, -104, -110, -117, -123, -129, -135, -141, -147, -153, -159, -165, -171, -177, -182, -188, 112 | -194, -200, -206, -211, -217, -223, -228, -234, -239, -245, -250, -256, -261, -267, -272, -277, 113 | -282, -288, -293, -298, -303, -308, -313, -318, -323, -327, -332, -337, -342, -346, -351, -355, 114 | -360, -364, -369, -373, -377, -381, -385, -390, -394, -397, -401, -405, -409, -413, -416, -420, 115 | -423, -427, -430, -434, -437, -440, -443, -446, -449, -452, -455, -458, -460, -463, -466, -468, 116 | -471, -473, -475, -477, -480, -482, -484, -486, -487, -489, -491, -493, -494, -496, -497, -498, 117 | -500, -501, -502, -503, -504, -505, -506, -506, -507, -508, -508, -509, -509, -509, -509, -509, 118 | -510, -509, -509, -509, -509, -509, -508, -508, -507, -506, -506, -505, -504, -503, -502, -501, 119 | -500, -498, -497, -496, -494, -493, -491, -489, -487, -486, -484, -482, -480, -477, -475, -473, 120 | -471, -468, -466, -463, -460, -458, -455, -452, -449, -446, -443, -440, -437, -434, -430, -427, 121 | -423, -420, -416, -413, -409, -405, -401, -397, -394, -390, -385, -381, -377, -373, -369, -364, 122 | -360, -355, -351, -346, -342, -337, -332, -327, -323, -318, -313, -308, -303, -298, -293, -288, 123 | -282, -277, -272, -267, -261, -256, -250, -245, -239, -234, -228, -223, -217, -211, -206, -200, 124 | -194, -188, -182, -177, -171, -165, -159, -153, -147, -141, -135, -129, -123, -117, -110, -104, 125 | -98, -92, -86, -80, -73, -67, -61, -55, -49, -42, -36, -30, -24, -17, -11, -5, 126 | }; 127 | 128 | const int cosTable512[] = { 129 | 512, 511, 511, 511, 511, 511, 510, 510, 509, 508, 508, 507, 506, 505, 504, 503, 130 | 502, 500, 499, 498, 496, 495, 493, 491, 489, 488, 486, 484, 482, 479, 477, 475, 131 | 473, 470, 468, 465, 462, 460, 457, 454, 451, 448, 445, 442, 439, 436, 432, 429, 132 | 425, 422, 418, 415, 411, 407, 403, 399, 396, 392, 387, 383, 379, 375, 371, 366, 133 | 362, 357, 353, 348, 344, 339, 334, 329, 325, 320, 315, 310, 305, 300, 295, 290, 134 | 284, 279, 274, 269, 263, 258, 252, 247, 241, 236, 230, 225, 219, 213, 208, 202, 135 | 196, 190, 184, 179, 173, 167, 161, 155, 149, 143, 137, 131, 125, 119, 112, 106, 136 | 100, 94, 88, 82, 75, 69, 63, 57, 51, 44, 38, 32, 26, 19, 13, 7, 137 | 1, -5, -11, -17, -24, -30, -36, -42, -49, -55, -61, -67, -73, -80, -86, -92, 138 | -98, -104, -110, -117, -123, -129, -135, -141, -147, -153, -159, -165, -171, -177, -182, -188, 139 | -194, -200, -206, -211, -217, -223, -228, -234, -239, -245, -250, -256, -261, -267, -272, -277, 140 | -282, -288, -293, -298, -303, -308, -313, -318, -323, -327, -332, -337, -342, -346, -351, -355, 141 | -360, -364, -369, -373, -377, -381, -385, -390, -394, -397, -401, -405, -409, -413, -416, -420, 142 | -423, -427, -430, -434, -437, -440, -443, -446, -449, -452, -455, -458, -460, -463, -466, -468, 143 | -471, -473, -475, -477, -480, -482, -484, -486, -487, -489, -491, -493, -494, -496, -497, -498, 144 | -500, -501, -502, -503, -504, -505, -506, -506, -507, -508, -508, -509, -509, -509, -509, -509, 145 | -510, -509, -509, -509, -509, -509, -508, -508, -507, -506, -506, -505, -504, -503, -502, -501, 146 | -500, -498, -497, -496, -494, -493, -491, -489, -487, -486, -484, -482, -480, -477, -475, -473, 147 | -471, -468, -466, -463, -460, -458, -455, -452, -449, -446, -443, -440, -437, -434, -430, -427, 148 | -423, -420, -416, -413, -409, -405, -401, -397, -394, -390, -385, -381, -377, -373, -369, -364, 149 | -360, -355, -351, -346, -342, -337, -332, -327, -323, -318, -313, -308, -303, -298, -293, -288, 150 | -282, -277, -272, -267, -261, -256, -250, -245, -239, -234, -228, -223, -217, -211, -206, -200, 151 | -194, -188, -182, -177, -171, -165, -159, -153, -147, -141, -135, -129, -123, -117, -110, -104, 152 | -98, -92, -86, -80, -73, -67, -61, -55, -49, -42, -36, -30, -24, -17, -11, -5, 153 | 0, 7, 13, 19, 26, 32, 38, 44, 51, 57, 63, 69, 75, 82, 88, 94, 154 | 100, 106, 112, 119, 125, 131, 137, 143, 149, 155, 161, 167, 173, 179, 184, 190, 155 | 196, 202, 208, 213, 219, 225, 230, 236, 241, 247, 252, 258, 263, 269, 274, 279, 156 | 284, 290, 295, 300, 305, 310, 315, 320, 325, 329, 334, 339, 344, 348, 353, 357, 157 | 362, 366, 371, 375, 379, 383, 387, 392, 396, 399, 403, 407, 411, 415, 418, 422, 158 | 425, 429, 432, 436, 439, 442, 445, 448, 451, 454, 457, 460, 462, 465, 468, 470, 159 | 473, 475, 477, 479, 482, 484, 486, 488, 489, 491, 493, 495, 496, 498, 499, 500, 160 | 502, 503, 504, 505, 506, 507, 508, 508, 509, 510, 510, 511, 511, 511, 511, 511, 161 | }; 162 | 163 | // Receiver antenna attenuation in dB for boresight angle = 0:5:180 [deg] 164 | const double ant_pat_db[37] = { 165 | 0.00, 0.00, 0.22, 0.44, 0.67, 1.11, 1.56, 2.00, 2.44, 2.89, 3.56, 4.22, 166 | 4.89, 5.56, 6.22, 6.89, 7.56, 8.22, 8.89, 9.78, 10.67, 11.56, 12.44, 13.33, 167 | 14.44, 15.56, 16.67, 17.78, 18.89, 20.00, 21.33, 22.67, 24.00, 25.56, 27.33, 29.33, 168 | 31.56 169 | }; 170 | 171 | static int allocatedSat[MAX_SAT]; 172 | 173 | /*! \brief Subtract two vectors of double 174 | * \param[out] y Result of subtraction 175 | * \param[in] x1 Minuend of subtracion 176 | * \param[in] x2 Subtrahend of subtracion 177 | */ 178 | static void subVect(double *y, const double *x1, const double *x2) { 179 | y[0] = x1[0] - x2[0]; 180 | y[1] = x1[1] - x2[1]; 181 | y[2] = x1[2] - x2[2]; 182 | 183 | return; 184 | } 185 | 186 | /*! \brief Compute Norm of Vector 187 | * \param[in] x Input vector 188 | * \returns Length (Norm) of the input vector 189 | */ 190 | static double normVect(const double *x) { 191 | return (sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2])); 192 | } 193 | 194 | /*! \brief Compute dot-product of two vectors 195 | * \param[in] x1 First multiplicand 196 | * \param[in] x2 Second multiplicand 197 | * \returns Dot-product of both multiplicands 198 | */ 199 | static double dotProd(const double *x1, const double *x2) { 200 | return (x1[0] * x2[0] + x1[1] * x2[1] + x1[2] * x2[2]); 201 | } 202 | 203 | /* !\brief generate the C/A code sequence for a given Satellite Vehicle PRN 204 | * \param[in] prn PRN nuber of the Satellite Vehicle 205 | * \param[out] ca Caller-allocated integer array of 1023 bytes 206 | */ 207 | static void codegen(int *ca, int prn) { 208 | int delay[] = { 209 | 5, 6, 7, 8, 17, 18, 139, 140, 141, 251, 210 | 252, 254, 255, 256, 257, 258, 469, 470, 471, 472, 211 | 473, 474, 509, 512, 513, 514, 515, 516, 859, 860, 212 | 861, 862 213 | }; 214 | 215 | int g1[CA_SEQ_LEN], g2[CA_SEQ_LEN]; 216 | int r1[N_DWRD_SBF], r2[N_DWRD_SBF]; 217 | int c1, c2; 218 | int i, j; 219 | 220 | if (prn < 1 || prn > 32) 221 | return; 222 | 223 | for (i = 0; i < N_DWRD_SBF; i++) 224 | r1[i] = r2[i] = -1; 225 | 226 | for (i = 0; i < CA_SEQ_LEN; i++) { 227 | g1[i] = r1[9]; 228 | g2[i] = r2[9]; 229 | c1 = r1[2] * r1[9]; 230 | c2 = r2[1] * r2[2] * r2[5] * r2[7] * r2[8] * r2[9]; 231 | 232 | for (j = 9; j > 0; j--) { 233 | r1[j] = r1[j - 1]; 234 | r2[j] = r2[j - 1]; 235 | } 236 | r1[0] = c1; 237 | r2[0] = c2; 238 | } 239 | 240 | for (i = 0, j = CA_SEQ_LEN - delay[prn - 1]; i < CA_SEQ_LEN; i++, j++) 241 | ca[i] = (1 - g1[i] * g2[j % CA_SEQ_LEN]) / 2; 242 | 243 | return; 244 | } 245 | 246 | /*! \brief Convert a UTC date into a GPS date 247 | * \param[in] t input date in UTC form 248 | * \param[out] g output date in GPS form 249 | */ 250 | static void date2gps(const datetime_t *t, gpstime_t *g) { 251 | int doy[12] = {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334}; 252 | int ye; 253 | int de; 254 | int lpdays; 255 | 256 | ye = t->y - 1980; 257 | 258 | // Compute the number of leap days since Jan 5/Jan 6, 1980. 259 | lpdays = ye / 4 + 1; 260 | if ((ye % 4) == 0 && t->m <= 2) 261 | lpdays--; 262 | 263 | // Compute the number of days elapsed since Jan 5/Jan 6, 1980. 264 | de = ye * 365 + doy[t->m - 1] + t->d + lpdays - 6; 265 | 266 | // Convert time to GPS weeks and seconds. 267 | g->week = de / 7; 268 | g->sec = (double) (de % 7) * SECONDS_IN_DAY + t->hh * SECONDS_IN_HOUR 269 | + t->mm * SECONDS_IN_MINUTE + t->sec; 270 | 271 | return; 272 | } 273 | 274 | static void gps2date(const gpstime_t *g, datetime_t *t) { 275 | // Convert Julian day number to calendar date 276 | int c = (int) (7 * g->week + floor(g->sec / 86400.0) + 2444245.0) + 1537; 277 | int d = (int) ((c - 122.1) / 365.25); 278 | int e = 365 * d + d / 4; 279 | int f = (int) ((c - e) / 30.6001); 280 | 281 | t->d = c - e - (int) (30.6001 * f); 282 | t->m = f - 1 - 12 * (f / 14); 283 | t->y = d - 4715 - ((7 + t->m) / 10); 284 | 285 | t->hh = ((int) (g->sec / 3600.0)) % 24; 286 | t->mm = ((int) (g->sec / 60.0)) % 60; 287 | t->sec = g->sec - 60.0 * floor(g->sec / 60.0); 288 | 289 | return; 290 | } 291 | 292 | /*! \brief Convert Earth-centered Earth-fixed (ECEF) into Lat/Long/Heighth 293 | * \param[in] xyz Input Array of X, Y and Z ECEF coordinates 294 | * \param[out] llh Output Array of Latitude, Longitude and Height 295 | */ 296 | static void xyz2llh(const double *xyz, double *llh) { 297 | double a, eps, e, e2; 298 | double x, y, z; 299 | double rho2, dz, zdz, nh, slat, n, dz_new; 300 | 301 | a = WGS84_RADIUS; 302 | e = WGS84_ECCENTRICITY; 303 | 304 | eps = 1.0e-3; 305 | e2 = e*e; 306 | 307 | if (normVect(xyz) < eps) { 308 | // Invalid ECEF vector 309 | llh[0] = 0.0; 310 | llh[1] = 0.0; 311 | llh[2] = -a; 312 | 313 | return; 314 | } 315 | 316 | x = xyz[0]; 317 | y = xyz[1]; 318 | z = xyz[2]; 319 | 320 | rho2 = x * x + y*y; 321 | dz = e2*z; 322 | 323 | while (1) { 324 | zdz = z + dz; 325 | nh = sqrt(rho2 + zdz * zdz); 326 | slat = zdz / nh; 327 | n = a / sqrt(1.0 - e2 * slat * slat); 328 | dz_new = n * e2*slat; 329 | 330 | if (fabs(dz - dz_new) < eps) 331 | break; 332 | 333 | dz = dz_new; 334 | } 335 | 336 | llh[0] = atan2(zdz, sqrt(rho2)); 337 | llh[1] = atan2(y, x); 338 | llh[2] = nh - n; 339 | 340 | return; 341 | } 342 | 343 | /*! \brief Convert Lat/Long/Height into Earth-centered Earth-fixed (ECEF) 344 | * \param[in] llh Input Array of Latitude, Longitude and Height 345 | * \param[out] xyz Output Array of X, Y and Z ECEF coordinates 346 | */ 347 | static void llh2xyz(const double *llh, double *xyz) { 348 | double n; 349 | double a; 350 | double e; 351 | double e2; 352 | double clat; 353 | double slat; 354 | double clon; 355 | double slon; 356 | double d, nph; 357 | double tmp; 358 | 359 | a = WGS84_RADIUS; 360 | e = WGS84_ECCENTRICITY; 361 | e2 = e*e; 362 | 363 | clat = cos(llh[0]); 364 | slat = sin(llh[0]); 365 | clon = cos(llh[1]); 366 | slon = sin(llh[1]); 367 | d = e*slat; 368 | 369 | n = a / sqrt(1.0 - d * d); 370 | nph = n + llh[2]; 371 | 372 | tmp = nph*clat; 373 | xyz[0] = tmp*clon; 374 | xyz[1] = tmp*slon; 375 | xyz[2] = ((1.0 - e2) * n + llh[2]) * slat; 376 | 377 | return; 378 | } 379 | 380 | /*! \brief Compute the intermediate matrix for LLH to ECEF 381 | * \param[in] llh Input position in Latitude-Longitude-Height format 382 | * \param[out] t Three-by-Three output matrix 383 | */ 384 | static void ltcmat(const double *llh, double t[3][3]) { 385 | double slat, clat; 386 | double slon, clon; 387 | 388 | slat = sin(llh[0]); 389 | clat = cos(llh[0]); 390 | slon = sin(llh[1]); 391 | clon = cos(llh[1]); 392 | 393 | t[0][0] = -slat*clon; 394 | t[0][1] = -slat*slon; 395 | t[0][2] = clat; 396 | t[1][0] = -slon; 397 | t[1][1] = clon; 398 | t[1][2] = 0.0; 399 | t[2][0] = clat*clon; 400 | t[2][1] = clat*slon; 401 | t[2][2] = slat; 402 | 403 | return; 404 | } 405 | 406 | /*! \brief Convert Earth-centered Earth-Fixed to ? 407 | * \param[in] xyz Input position as vector in ECEF format 408 | * \param[in] t Intermediate matrix computed by \ref ltcmat 409 | * \param[out] neu Output position as North-East-Up format 410 | */ 411 | static void ecef2neu(const double *xyz, double t[3][3], double *neu) { 412 | neu[0] = t[0][0] * xyz[0] + t[0][1] * xyz[1] + t[0][2] * xyz[2]; 413 | neu[1] = t[1][0] * xyz[0] + t[1][1] * xyz[1] + t[1][2] * xyz[2]; 414 | neu[2] = t[2][0] * xyz[0] + t[2][1] * xyz[1] + t[2][2] * xyz[2]; 415 | 416 | return; 417 | } 418 | 419 | /*! \brief Convert North-Eeast-Up to Azimuth + Elevation 420 | * \param[in] neu Input position in North-East-Up format 421 | * \param[out] azel Output array of azimuth + elevation as double 422 | */ 423 | static void neu2azel(double *azel, const double *neu) { 424 | double ne; 425 | 426 | azel[0] = atan2(neu[1], neu[0]); 427 | if (azel[0] < 0.0) 428 | azel[0] += (2.0 * PI); 429 | 430 | ne = sqrt(neu[0] * neu[0] + neu[1] * neu[1]); 431 | azel[1] = atan2(neu[2], ne); 432 | 433 | return; 434 | } 435 | 436 | /*! \brief Compute Satellite position, velocity and clock at given time 437 | * \param[in] eph Ephemeris data of the satellite 438 | * \param[in] g GPS time at which position is to be computed 439 | * \param[out] pos Computed position (vector) 440 | * \param[out] vel Computed velociy (vector) 441 | * \param[clk] clk Computed clock 442 | */ 443 | static void satpos(ephem_t eph, gpstime_t g, double *pos, double *vel, double *clk) { 444 | // Computing Satellite Velocity using the Broadcast Ephemeris 445 | // http://www.ngs.noaa.gov/gps-toolbox/bc_velo.htm 446 | 447 | double tk; 448 | double mk; 449 | double ek; 450 | double ekold; 451 | double ekdot; 452 | double cek, sek; 453 | double pk; 454 | double pkdot; 455 | double c2pk, s2pk; 456 | double uk; 457 | double ukdot; 458 | double cuk, suk; 459 | double ok; 460 | double sok, cok; 461 | double ik; 462 | double ikdot; 463 | double sik, cik; 464 | double rk; 465 | double rkdot; 466 | double xpk, ypk; 467 | double xpkdot, ypkdot; 468 | 469 | double relativistic, OneMinusecosE, tmp; 470 | 471 | tk = g.sec - eph.toe.sec; 472 | 473 | if (tk > SECONDS_IN_HALF_WEEK) 474 | tk -= SECONDS_IN_WEEK; 475 | else if (tk<-SECONDS_IN_HALF_WEEK) 476 | tk += SECONDS_IN_WEEK; 477 | 478 | mk = eph.m0 + eph.n*tk; 479 | ek = mk; 480 | ekold = ek + 1.0; 481 | 482 | OneMinusecosE = 0; // Suppress the uninitialized warning. 483 | while (fabs(ek - ekold) > 1.0E-14) { 484 | ekold = ek; 485 | OneMinusecosE = 1.0 - eph.ecc * cos(ekold); 486 | ek = ek + (mk - ekold + eph.ecc * sin(ekold)) / OneMinusecosE; 487 | } 488 | 489 | sek = sin(ek); 490 | cek = cos(ek); 491 | 492 | ekdot = eph.n / OneMinusecosE; 493 | 494 | relativistic = -4.442807633E-10 * eph.ecc * eph.sqrta*sek; 495 | 496 | pk = atan2(eph.sq1e2*sek, cek - eph.ecc) + eph.aop; 497 | pkdot = eph.sq1e2 * ekdot / OneMinusecosE; 498 | 499 | s2pk = sin(2.0 * pk); 500 | c2pk = cos(2.0 * pk); 501 | 502 | uk = pk + eph.cus * s2pk + eph.cuc*c2pk; 503 | suk = sin(uk); 504 | cuk = cos(uk); 505 | ukdot = pkdot * (1.0 + 2.0 * (eph.cus * c2pk - eph.cuc * s2pk)); 506 | 507 | rk = eph.A * OneMinusecosE + eph.crc * c2pk + eph.crs*s2pk; 508 | rkdot = eph.A * eph.ecc * sek * ekdot + 2.0 * pkdot * (eph.crs * c2pk - eph.crc * s2pk); 509 | 510 | ik = eph.inc0 + eph.idot * tk + eph.cic * c2pk + eph.cis*s2pk; 511 | sik = sin(ik); 512 | cik = cos(ik); 513 | ikdot = eph.idot + 2.0 * pkdot * (eph.cis * c2pk - eph.cic * s2pk); 514 | 515 | xpk = rk*cuk; 516 | ypk = rk*suk; 517 | xpkdot = rkdot * cuk - ypk*ukdot; 518 | ypkdot = rkdot * suk + xpk*ukdot; 519 | 520 | ok = eph.omg0 + tk * eph.omgkdot - OMEGA_EARTH * eph.toe.sec; 521 | sok = sin(ok); 522 | cok = cos(ok); 523 | 524 | pos[0] = xpk * cok - ypk * cik*sok; 525 | pos[1] = xpk * sok + ypk * cik*cok; 526 | pos[2] = ypk*sik; 527 | 528 | tmp = ypkdot * cik - ypk * sik*ikdot; 529 | 530 | vel[0] = -eph.omgkdot * pos[1] + xpkdot * cok - tmp*sok; 531 | vel[1] = eph.omgkdot * pos[0] + xpkdot * sok + tmp*cok; 532 | vel[2] = ypk * cik * ikdot + ypkdot*sik; 533 | 534 | // Satellite clock correction 535 | tk = g.sec - eph.toc.sec; 536 | 537 | if (tk > SECONDS_IN_HALF_WEEK) 538 | tk -= SECONDS_IN_WEEK; 539 | else if (tk<-SECONDS_IN_HALF_WEEK) 540 | tk += SECONDS_IN_WEEK; 541 | 542 | clk[0] = eph.af0 + tk * (eph.af1 + tk * eph.af2) + relativistic - eph.tgd; 543 | clk[1] = eph.af1 + 2.0 * tk * eph.af2; 544 | 545 | return; 546 | } 547 | 548 | /*! \brief Compute Subframe from Ephemeris 549 | * \param[in] eph Ephemeris of given SV 550 | * \param[out] sbf Array of five sub-frames, 10 long words each 551 | */ 552 | static void eph2sbf(const ephem_t eph, const ionoutc_t ionoutc, unsigned long sbf[5][N_DWRD_SBF]) { 553 | unsigned long wn; 554 | unsigned long toe; 555 | unsigned long toc; 556 | unsigned long iode; 557 | unsigned long iodc; 558 | long deltan; 559 | long cuc; 560 | long cus; 561 | long cic; 562 | long cis; 563 | long crc; 564 | long crs; 565 | unsigned long ecc; 566 | unsigned long sqrta; 567 | long m0; 568 | long omg0; 569 | long inc0; 570 | long aop; 571 | long omgdot; 572 | long idot; 573 | long af0; 574 | long af1; 575 | long af2; 576 | long tgd; 577 | int svhlth; 578 | int codeL2; 579 | 580 | unsigned long ura = 0UL; 581 | unsigned long dataId = 1UL; 582 | unsigned long sbf4_page25_svId = 63UL; 583 | unsigned long sbf5_page25_svId = 51UL; 584 | 585 | unsigned long wna; 586 | unsigned long toa; 587 | 588 | signed long alpha0, alpha1, alpha2, alpha3; 589 | signed long beta0, beta1, beta2, beta3; 590 | signed long A0, A1; 591 | signed long dtls, dtlsf; 592 | unsigned long tot, wnt, wnlsf, dn; 593 | unsigned long sbf4_page18_svId = 56UL; 594 | 595 | // FIXED: This has to be the "transmission" week number, not for the ephemeris reference time 596 | //wn = (unsigned long)(eph.toe.week%1024); 597 | wn = 0UL; 598 | toe = (unsigned long) (eph.toe.sec / 16.0); 599 | toc = (unsigned long) (eph.toc.sec / 16.0); 600 | iode = (unsigned long) (eph.iode); 601 | iodc = (unsigned long) (eph.iodc); 602 | deltan = (long) (eph.deltan / POW2_M43 / PI); 603 | cuc = (long) (eph.cuc / POW2_M29); 604 | cus = (long) (eph.cus / POW2_M29); 605 | cic = (long) (eph.cic / POW2_M29); 606 | cis = (long) (eph.cis / POW2_M29); 607 | crc = (long) (eph.crc / POW2_M5); 608 | crs = (long) (eph.crs / POW2_M5); 609 | ecc = (unsigned long) (eph.ecc / POW2_M33); 610 | sqrta = (unsigned long) (eph.sqrta / POW2_M19); 611 | m0 = (long) (eph.m0 / POW2_M31 / PI); 612 | omg0 = (long) (eph.omg0 / POW2_M31 / PI); 613 | inc0 = (long) (eph.inc0 / POW2_M31 / PI); 614 | aop = (long) (eph.aop / POW2_M31 / PI); 615 | omgdot = (long) (eph.omgdot / POW2_M43 / PI); 616 | idot = (long) (eph.idot / POW2_M43 / PI); 617 | af0 = (long) (eph.af0 / POW2_M31); 618 | af1 = (long) (eph.af1 / POW2_M43); 619 | af2 = (long) (eph.af2 / POW2_M55); 620 | tgd = (long) (eph.tgd / POW2_M31); 621 | svhlth = (unsigned long) (eph.svhlth); 622 | codeL2 = (unsigned long) (eph.codeL2); 623 | 624 | wna = (unsigned long) (eph.toe.week % 256); 625 | toa = (unsigned long) (eph.toe.sec / 4096.0); 626 | 627 | alpha0 = (signed long) round(ionoutc.alpha0 / POW2_M30); 628 | alpha1 = (signed long) round(ionoutc.alpha1 / POW2_M27); 629 | alpha2 = (signed long) round(ionoutc.alpha2 / POW2_M24); 630 | alpha3 = (signed long) round(ionoutc.alpha3 / POW2_M24); 631 | beta0 = (signed long) round(ionoutc.beta0 / 2048.0); 632 | beta1 = (signed long) round(ionoutc.beta1 / 16384.0); 633 | beta2 = (signed long) round(ionoutc.beta2 / 65536.0); 634 | beta3 = (signed long) round(ionoutc.beta3 / 65536.0); 635 | A0 = (signed long) round(ionoutc.A0 / POW2_M30); 636 | A1 = (signed long) round(ionoutc.A1 / POW2_M50); 637 | dtls = (signed long) (ionoutc.dtls); 638 | tot = (unsigned long) (ionoutc.tot / 4096); 639 | wnt = (unsigned long) (ionoutc.wnt % 256); 640 | // TO DO: Specify scheduled leap seconds in command options 641 | // 2016/12/31 (Sat) -> WNlsf = 1929, DN = 7 (http://navigationservices.agi.com/GNSSWeb/) 642 | // Days are counted from 1 to 7 (Sunday is 1). 643 | wnlsf = 1929 % 256; 644 | dn = 7; 645 | dtlsf = 18; 646 | 647 | // Subframe 1 648 | sbf[0][0] = 0x8B0000UL << 6; 649 | sbf[0][1] = 0x1UL << 8; 650 | sbf[0][2] = ((wn & 0x3FFUL) << 20) | ((codeL2 & 0x3UL) << 18) | ((ura & 0xFUL) << 14) | ((svhlth & 0x3FUL) << 8) | (((iodc >> 8)&0x3UL) << 6); 651 | sbf[0][3] = 0UL; 652 | sbf[0][4] = 0UL; 653 | sbf[0][5] = 0UL; 654 | sbf[0][6] = (tgd & 0xFFUL) << 6; 655 | sbf[0][7] = ((iodc & 0xFFUL) << 22) | ((toc & 0xFFFFUL) << 6); 656 | sbf[0][8] = ((af2 & 0xFFUL) << 22) | ((af1 & 0xFFFFUL) << 6); 657 | sbf[0][9] = (af0 & 0x3FFFFFUL) << 8; 658 | 659 | // Subframe 2 660 | sbf[1][0] = 0x8B0000UL << 6; 661 | sbf[1][1] = 0x2UL << 8; 662 | sbf[1][2] = ((iode & 0xFFUL) << 22) | ((crs & 0xFFFFUL) << 6); 663 | sbf[1][3] = ((deltan & 0xFFFFUL) << 14) | (((m0 >> 24)&0xFFUL) << 6); 664 | sbf[1][4] = (m0 & 0xFFFFFFUL) << 6; 665 | sbf[1][5] = ((cuc & 0xFFFFUL) << 14) | (((ecc >> 24)&0xFFUL) << 6); 666 | sbf[1][6] = (ecc & 0xFFFFFFUL) << 6; 667 | sbf[1][7] = ((cus & 0xFFFFUL) << 14) | (((sqrta >> 24)&0xFFUL) << 6); 668 | sbf[1][8] = (sqrta & 0xFFFFFFUL) << 6; 669 | sbf[1][9] = (toe & 0xFFFFUL) << 14; 670 | 671 | // Subframe 3 672 | sbf[2][0] = 0x8B0000UL << 6; 673 | sbf[2][1] = 0x3UL << 8; 674 | sbf[2][2] = ((cic & 0xFFFFUL) << 14) | (((omg0 >> 24)&0xFFUL) << 6); 675 | sbf[2][3] = (omg0 & 0xFFFFFFUL) << 6; 676 | sbf[2][4] = ((cis & 0xFFFFUL) << 14) | (((inc0 >> 24)&0xFFUL) << 6); 677 | sbf[2][5] = (inc0 & 0xFFFFFFUL) << 6; 678 | sbf[2][6] = ((crc & 0xFFFFUL) << 14) | (((aop >> 24)&0xFFUL) << 6); 679 | sbf[2][7] = (aop & 0xFFFFFFUL) << 6; 680 | sbf[2][8] = (omgdot & 0xFFFFFFUL) << 6; 681 | sbf[2][9] = ((iode & 0xFFUL) << 22) | ((idot & 0x3FFFUL) << 8); 682 | 683 | if (ionoutc.vflg == true) { 684 | // Subframe 4, page 18 685 | sbf[3][0] = 0x8B0000UL << 6; 686 | sbf[3][1] = 0x4UL << 8; 687 | sbf[3][2] = (dataId << 28) | (sbf4_page18_svId << 22) | ((alpha0 & 0xFFUL) << 14) | ((alpha1 & 0xFFUL) << 6); 688 | sbf[3][3] = ((alpha2 & 0xFFUL) << 22) | ((alpha3 & 0xFFUL) << 14) | ((beta0 & 0xFFUL) << 6); 689 | sbf[3][4] = ((beta1 & 0xFFUL) << 22) | ((beta2 & 0xFFUL) << 14) | ((beta3 & 0xFFUL) << 6); 690 | sbf[3][5] = (A1 & 0xFFFFFFUL) << 6; 691 | sbf[3][6] = ((A0 >> 8)&0xFFFFFFUL) << 6; 692 | sbf[3][7] = ((A0 & 0xFFUL) << 22) | ((tot & 0xFFUL) << 14) | ((wnt & 0xFFUL) << 6); 693 | sbf[3][8] = ((dtls & 0xFFUL) << 22) | ((wnlsf & 0xFFUL) << 14) | ((dn & 0xFFUL) << 6); 694 | sbf[3][9] = (dtlsf & 0xFFUL) << 22; 695 | 696 | } else { 697 | // Subframe 4, page 25 698 | sbf[3][0] = 0x8B0000UL << 6; 699 | sbf[3][1] = 0x4UL << 8; 700 | sbf[3][2] = (dataId << 28) | (sbf4_page25_svId << 22); 701 | sbf[3][3] = 0UL; 702 | sbf[3][4] = 0UL; 703 | sbf[3][5] = 0UL; 704 | sbf[3][6] = 0UL; 705 | sbf[3][7] = 0UL; 706 | sbf[3][8] = 0UL; 707 | sbf[3][9] = 0UL; 708 | } 709 | 710 | // Subframe 5, page 25 711 | sbf[4][0] = 0x8B0000UL << 6; 712 | sbf[4][1] = 0x5UL << 8; 713 | sbf[4][2] = (dataId << 28) | (sbf5_page25_svId << 22) | ((toa & 0xFFUL) << 14) | ((wna & 0xFFUL) << 6); 714 | sbf[4][3] = 0UL; 715 | sbf[4][4] = 0UL; 716 | sbf[4][5] = 0UL; 717 | sbf[4][6] = 0UL; 718 | sbf[4][7] = 0UL; 719 | sbf[4][8] = 0UL; 720 | sbf[4][9] = 0UL; 721 | 722 | return; 723 | } 724 | 725 | /*! \brief Count number of bits set to 1 726 | * \param[in] v long word in which bits are counted 727 | * \returns Count of bits set to 1 728 | */ 729 | static unsigned long countBits(unsigned long v) { 730 | unsigned long c; 731 | const int S[] = {1, 2, 4, 8, 16}; 732 | const unsigned long B[] = { 733 | 0x55555555, 0x33333333, 0x0F0F0F0F, 0x00FF00FF, 0x0000FFFF 734 | }; 735 | 736 | c = v; 737 | c = ((c >> S[0]) & B[0]) + (c & B[0]); 738 | c = ((c >> S[1]) & B[1]) + (c & B[1]); 739 | c = ((c >> S[2]) & B[2]) + (c & B[2]); 740 | c = ((c >> S[3]) & B[3]) + (c & B[3]); 741 | c = ((c >> S[4]) & B[4]) + (c & B[4]); 742 | 743 | return (c); 744 | } 745 | 746 | /*! \brief Compute the Checksum for one given word of a subframe 747 | * \param[in] source The input data 748 | * \param[in] nib Does this word contain non-information-bearing bits? 749 | * \returns Computed Checksum 750 | */ 751 | static unsigned long computeChecksum(unsigned long source, int nib) { 752 | /* 753 | Bits 31 to 30 = 2 LSBs of the previous transmitted word, D29* and D30* 754 | Bits 29 to 6 = Source data bits, d1, d2, ..., d24 755 | Bits 5 to 0 = Empty parity bits 756 | */ 757 | 758 | /* 759 | Bits 31 to 30 = 2 LSBs of the previous transmitted word, D29* and D30* 760 | Bits 29 to 6 = Data bits transmitted by the SV, D1, D2, ..., D24 761 | Bits 5 to 0 = Computed parity bits, D25, D26, ..., D30 762 | */ 763 | 764 | /* 765 | 1 2 3 766 | bit 12 3456 7890 1234 5678 9012 3456 7890 767 | --- ------------------------------------- 768 | D25 11 1011 0001 1111 0011 0100 1000 0000 769 | D26 01 1101 1000 1111 1001 1010 0100 0000 770 | D27 10 1110 1100 0111 1100 1101 0000 0000 771 | D28 01 0111 0110 0011 1110 0110 1000 0000 772 | D29 10 1011 1011 0001 1111 0011 0100 0000 773 | D30 00 1011 0111 1010 1000 1001 1100 0000 774 | */ 775 | 776 | unsigned long bmask[6] = { 777 | 0x3B1F3480UL, 0x1D8F9A40UL, 0x2EC7CD00UL, 778 | 0x1763E680UL, 0x2BB1F340UL, 0x0B7A89C0UL 779 | }; 780 | 781 | unsigned long D; 782 | unsigned long d = source & 0x3FFFFFC0UL; 783 | unsigned long D29 = (source >> 31)&0x1UL; 784 | unsigned long D30 = (source >> 30)&0x1UL; 785 | 786 | if (nib) // Non-information bearing bits for word 2 and 10 787 | { 788 | /* 789 | Solve bits 23 and 24 to presearve parity check 790 | with zeros in bits 29 and 30. 791 | */ 792 | 793 | if ((D30 + countBits(bmask[4] & d)) % 2) 794 | d ^= (0x1UL << 6); 795 | if ((D29 + countBits(bmask[5] & d)) % 2) 796 | d ^= (0x1UL << 7); 797 | } 798 | 799 | D = d; 800 | if (D30) 801 | D ^= 0x3FFFFFC0UL; 802 | 803 | D |= ((D29 + countBits(bmask[0] & d)) % 2) << 5; 804 | D |= ((D30 + countBits(bmask[1] & d)) % 2) << 4; 805 | D |= ((D29 + countBits(bmask[2] & d)) % 2) << 3; 806 | D |= ((D30 + countBits(bmask[3] & d)) % 2) << 2; 807 | D |= ((D30 + countBits(bmask[4] & d)) % 2) << 1; 808 | D |= ((D29 + countBits(bmask[5] & d)) % 2); 809 | 810 | D &= 0x3FFFFFFFUL; 811 | //D |= (source & 0xC0000000UL); // Add D29* and D30* from source data bits 812 | 813 | return (D); 814 | } 815 | 816 | /*! \brief Replace all 'E' exponential designators to 'D' 817 | * \param str String in which all occurrences of 'E' are replaced with * 'D' 818 | * \param len Length of input string in bytes 819 | * \returns Number of characters replaced 820 | */ 821 | static int replaceExpDesignator(char *str, int len) { 822 | int i, n = 0; 823 | 824 | for (i = 0; i < len; i++) { 825 | if (str[i] == 0) { 826 | break; 827 | } 828 | 829 | if (str[i] == 'D' || str[i] == 'd') { 830 | n++; 831 | str[i] = 'E'; 832 | } 833 | } 834 | 835 | return (n); 836 | } 837 | 838 | static double subGpsTime(gpstime_t g1, gpstime_t g0) { 839 | double dt; 840 | 841 | dt = g1.sec - g0.sec; 842 | dt += (double) (g1.week - g0.week) * SECONDS_IN_WEEK; 843 | 844 | return (dt); 845 | } 846 | 847 | static gpstime_t incGpsTime(gpstime_t g0, double dt) { 848 | gpstime_t g1; 849 | 850 | g1.week = g0.week; 851 | g1.sec = g0.sec + dt; 852 | 853 | g1.sec = round(g1.sec * 1000.0) / 1000.0; // Avoid rounding error 854 | 855 | while (g1.sec >= SECONDS_IN_WEEK) { 856 | g1.sec -= SECONDS_IN_WEEK; 857 | g1.week++; 858 | } 859 | 860 | while (g1.sec < 0.0) { 861 | g1.sec += SECONDS_IN_WEEK; 862 | g1.week--; 863 | } 864 | 865 | return (g1); 866 | } 867 | 868 | /*! \brief Read Ephemeris data from the RINEX v2 Navigation file */ 869 | 870 | /* \param[out] eph Array of Output SV ephemeris data 871 | * \param[in] fname File name of the RINEX file 872 | * \returns Number of sets of ephemerides in the file 873 | */ 874 | static int readRinex2(ephem_t eph[][MAX_SAT], ionoutc_t *ionoutc, const char *fname) { 875 | struct gzFile_s *fp; 876 | int ieph; 877 | 878 | int sv; 879 | char str[MAX_CHAR]; 880 | char tmp[20]; 881 | double ver = 0.0; 882 | 883 | datetime_t t; 884 | gpstime_t g; 885 | gpstime_t g0; 886 | double dt; 887 | 888 | int flags = 0x0; 889 | 890 | if (NULL == (fp = gzopen(fname, "rt"))) 891 | return (-1); 892 | 893 | // Clear valid flag 894 | for (ieph = 0; ieph < EPHEM_ARRAY_SIZE; ieph++) 895 | for (sv = 0; sv < MAX_SAT; sv++) 896 | eph[ieph][sv].vflg = false; 897 | 898 | // Read header lines 899 | while (1) { 900 | if (NULL == gzgets(fp, str, MAX_CHAR)) 901 | break; 902 | 903 | if (strncmp(str + 60, "COMMENT", 7) == 0) { 904 | continue; 905 | } else if (strncmp(str + 60, "END OF HEADER", 13) == 0) { 906 | break; 907 | } else if (strncmp(str + 60, "RINEX VERSION / TYPE", 20) == 0) { 908 | strncpy(tmp, str, 9); 909 | tmp[9] = 0; 910 | replaceExpDesignator(tmp, 9); 911 | ver = atof(tmp); 912 | if (ver > 3.0) { 913 | gzclose(fp); 914 | return -2; 915 | } 916 | 917 | if (str[20] != 'N') { 918 | gzclose(fp); 919 | return -3; 920 | } 921 | } else if (strncmp(str + 60, "PGM / RUN BY / DATE", 19) == 0) { 922 | strncpy(rinex_date, str + 40, 20); 923 | rinex_date[20] = 0; 924 | } else if (strncmp(str + 60, "ION ALPHA", 9) == 0) { 925 | strncpy(tmp, str + 2, 12); 926 | tmp[12] = 0; 927 | replaceExpDesignator(tmp, 12); 928 | ionoutc->alpha0 = atof(tmp); 929 | 930 | strncpy(tmp, str + 14, 12); 931 | tmp[12] = 0; 932 | replaceExpDesignator(tmp, 12); 933 | ionoutc->alpha1 = atof(tmp); 934 | 935 | strncpy(tmp, str + 26, 12); 936 | tmp[12] = 0; 937 | replaceExpDesignator(tmp, 12); 938 | ionoutc->alpha2 = atof(tmp); 939 | 940 | strncpy(tmp, str + 38, 12); 941 | tmp[12] = 0; 942 | replaceExpDesignator(tmp, 12); 943 | ionoutc->alpha3 = atof(tmp); 944 | 945 | flags |= 0x1; 946 | } else if (strncmp(str + 60, "ION BETA", 8) == 0) { 947 | strncpy(tmp, str + 2, 12); 948 | tmp[12] = 0; 949 | replaceExpDesignator(tmp, 12); 950 | ionoutc->beta0 = atof(tmp); 951 | 952 | strncpy(tmp, str + 14, 12); 953 | tmp[12] = 0; 954 | replaceExpDesignator(tmp, 12); 955 | ionoutc->beta1 = atof(tmp); 956 | 957 | strncpy(tmp, str + 26, 12); 958 | tmp[12] = 0; 959 | replaceExpDesignator(tmp, 12); 960 | ionoutc->beta2 = atof(tmp); 961 | 962 | strncpy(tmp, str + 38, 12); 963 | tmp[12] = 0; 964 | replaceExpDesignator(tmp, 12); 965 | ionoutc->beta3 = atof(tmp); 966 | 967 | flags |= 0x1 << 1; 968 | } else if (strncmp(str + 60, "DELTA-UTC", 9) == 0) { 969 | strncpy(tmp, str + 3, 19); 970 | tmp[19] = 0; 971 | replaceExpDesignator(tmp, 19); 972 | ionoutc->A0 = atof(tmp); 973 | 974 | strncpy(tmp, str + 22, 19); 975 | tmp[19] = 0; 976 | replaceExpDesignator(tmp, 19); 977 | ionoutc->A1 = atof(tmp); 978 | 979 | strncpy(tmp, str + 41, 9); 980 | tmp[9] = 0; 981 | ionoutc->tot = atoi(tmp); 982 | 983 | strncpy(tmp, str + 50, 9); 984 | tmp[9] = 0; 985 | ionoutc->wnt = atoi(tmp); 986 | 987 | if (ionoutc->tot % 4096 == 0) 988 | flags |= 0x1 << 2; 989 | } else if (strncmp(str + 60, "LEAP SECONDS", 12) == 0) { 990 | strncpy(tmp, str, 6); 991 | tmp[6] = 0; 992 | ionoutc->dtls = atoi(tmp); 993 | 994 | flags |= 0x1 << 3; 995 | } 996 | } 997 | 998 | ionoutc->vflg = false; 999 | if (flags == 0xF) // Read all Iono/UTC lines 1000 | ionoutc->vflg = true; 1001 | 1002 | // Read ephemeris blocks 1003 | g0.week = -1; 1004 | ieph = 0; 1005 | 1006 | while (1) { 1007 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1008 | break; 1009 | 1010 | // PRN 1011 | strncpy(tmp, str, 2); 1012 | tmp[2] = 0; 1013 | sv = atoi(tmp) - 1; 1014 | 1015 | // EPOCH 1016 | strncpy(tmp, str + 3, 2); 1017 | tmp[2] = 0; 1018 | t.y = atoi(tmp) + 2000; 1019 | 1020 | strncpy(tmp, str + 6, 2); 1021 | tmp[2] = 0; 1022 | t.m = atoi(tmp); 1023 | 1024 | strncpy(tmp, str + 9, 2); 1025 | tmp[2] = 0; 1026 | t.d = atoi(tmp); 1027 | 1028 | strncpy(tmp, str + 12, 2); 1029 | tmp[2] = 0; 1030 | t.hh = atoi(tmp); 1031 | 1032 | strncpy(tmp, str + 15, 2); 1033 | tmp[2] = 0; 1034 | t.mm = atoi(tmp); 1035 | 1036 | strncpy(tmp, str + 18, 4); 1037 | tmp[2] = 0; 1038 | t.sec = atof(tmp); 1039 | 1040 | date2gps(&t, &g); 1041 | 1042 | if (g0.week == -1) 1043 | g0 = g; 1044 | 1045 | // Check current time of clock 1046 | dt = subGpsTime(g, g0); 1047 | 1048 | if (dt > SECONDS_IN_HOUR) { 1049 | g0 = g; 1050 | ieph++; // a new set of ephemerides 1051 | 1052 | if (ieph >= EPHEM_ARRAY_SIZE) 1053 | break; 1054 | } 1055 | 1056 | // Date and time 1057 | eph[ieph][sv].t = t; 1058 | 1059 | // SV CLK 1060 | eph[ieph][sv].toc = g; 1061 | 1062 | strncpy(tmp, str + 22, 19); 1063 | tmp[19] = 0; 1064 | replaceExpDesignator(tmp, 19); // tmp[15]='E'; 1065 | eph[ieph][sv].af0 = atof(tmp); 1066 | 1067 | strncpy(tmp, str + 41, 19); 1068 | tmp[19] = 0; 1069 | replaceExpDesignator(tmp, 19); 1070 | eph[ieph][sv].af1 = atof(tmp); 1071 | 1072 | strncpy(tmp, str + 60, 19); 1073 | tmp[19] = 0; 1074 | replaceExpDesignator(tmp, 19); 1075 | eph[ieph][sv].af2 = atof(tmp); 1076 | 1077 | // BROADCAST ORBIT - 1 1078 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1079 | break; 1080 | 1081 | strncpy(tmp, str + 3, 19); 1082 | tmp[19] = 0; 1083 | replaceExpDesignator(tmp, 19); 1084 | eph[ieph][sv].iode = (int) atof(tmp); 1085 | 1086 | strncpy(tmp, str + 22, 19); 1087 | tmp[19] = 0; 1088 | replaceExpDesignator(tmp, 19); 1089 | eph[ieph][sv].crs = atof(tmp); 1090 | 1091 | strncpy(tmp, str + 41, 19); 1092 | tmp[19] = 0; 1093 | replaceExpDesignator(tmp, 19); 1094 | eph[ieph][sv].deltan = atof(tmp); 1095 | 1096 | strncpy(tmp, str + 60, 19); 1097 | tmp[19] = 0; 1098 | replaceExpDesignator(tmp, 19); 1099 | eph[ieph][sv].m0 = atof(tmp); 1100 | 1101 | // BROADCAST ORBIT - 2 1102 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1103 | break; 1104 | 1105 | strncpy(tmp, str + 3, 19); 1106 | tmp[19] = 0; 1107 | replaceExpDesignator(tmp, 19); 1108 | eph[ieph][sv].cuc = atof(tmp); 1109 | 1110 | strncpy(tmp, str + 22, 19); 1111 | tmp[19] = 0; 1112 | replaceExpDesignator(tmp, 19); 1113 | eph[ieph][sv].ecc = atof(tmp); 1114 | 1115 | strncpy(tmp, str + 41, 19); 1116 | tmp[19] = 0; 1117 | replaceExpDesignator(tmp, 19); 1118 | eph[ieph][sv].cus = atof(tmp); 1119 | 1120 | strncpy(tmp, str + 60, 19); 1121 | tmp[19] = 0; 1122 | replaceExpDesignator(tmp, 19); 1123 | eph[ieph][sv].sqrta = atof(tmp); 1124 | 1125 | // BROADCAST ORBIT - 3 1126 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1127 | break; 1128 | 1129 | strncpy(tmp, str + 3, 19); 1130 | tmp[19] = 0; 1131 | replaceExpDesignator(tmp, 19); 1132 | eph[ieph][sv].toe.sec = atof(tmp); 1133 | 1134 | strncpy(tmp, str + 22, 19); 1135 | tmp[19] = 0; 1136 | replaceExpDesignator(tmp, 19); 1137 | eph[ieph][sv].cic = atof(tmp); 1138 | 1139 | strncpy(tmp, str + 41, 19); 1140 | tmp[19] = 0; 1141 | replaceExpDesignator(tmp, 19); 1142 | eph[ieph][sv].omg0 = atof(tmp); 1143 | 1144 | strncpy(tmp, str + 60, 19); 1145 | tmp[19] = 0; 1146 | replaceExpDesignator(tmp, 19); 1147 | eph[ieph][sv].cis = atof(tmp); 1148 | 1149 | // BROADCAST ORBIT - 4 1150 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1151 | break; 1152 | 1153 | strncpy(tmp, str + 3, 19); 1154 | tmp[19] = 0; 1155 | replaceExpDesignator(tmp, 19); 1156 | eph[ieph][sv].inc0 = atof(tmp); 1157 | 1158 | strncpy(tmp, str + 22, 19); 1159 | tmp[19] = 0; 1160 | replaceExpDesignator(tmp, 19); 1161 | eph[ieph][sv].crc = atof(tmp); 1162 | 1163 | strncpy(tmp, str + 41, 19); 1164 | tmp[19] = 0; 1165 | replaceExpDesignator(tmp, 19); 1166 | eph[ieph][sv].aop = atof(tmp); 1167 | 1168 | strncpy(tmp, str + 60, 19); 1169 | tmp[19] = 0; 1170 | replaceExpDesignator(tmp, 19); 1171 | eph[ieph][sv].omgdot = atof(tmp); 1172 | 1173 | // BROADCAST ORBIT - 5 1174 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1175 | break; 1176 | 1177 | strncpy(tmp, str + 3, 19); 1178 | tmp[19] = 0; 1179 | replaceExpDesignator(tmp, 19); 1180 | eph[ieph][sv].idot = atof(tmp); 1181 | 1182 | strncpy(tmp, str + 22, 19); 1183 | tmp[19] = 0; 1184 | replaceExpDesignator(tmp, 19); 1185 | eph[ieph][sv].codeL2 = (int) atof(tmp); 1186 | 1187 | strncpy(tmp, str + 41, 19); 1188 | tmp[19] = 0; 1189 | replaceExpDesignator(tmp, 19); 1190 | eph[ieph][sv].toe.week = (int) atof(tmp); 1191 | 1192 | // BROADCAST ORBIT - 6 1193 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1194 | break; 1195 | 1196 | strncpy(tmp, str + 22, 19); 1197 | tmp[19] = 0; 1198 | replaceExpDesignator(tmp, 19); 1199 | eph[ieph][sv].svhlth = (int) atof(tmp); 1200 | if ((eph[ieph][sv].svhlth > 0) && (eph[ieph][sv].svhlth < 32)) 1201 | eph[ieph][sv].svhlth += 32; // Set MSB to 1 1202 | 1203 | strncpy(tmp, str + 41, 19); 1204 | tmp[19] = 0; 1205 | replaceExpDesignator(tmp, 19); 1206 | eph[ieph][sv].tgd = atof(tmp); 1207 | 1208 | strncpy(tmp, str + 60, 19); 1209 | tmp[19] = 0; 1210 | replaceExpDesignator(tmp, 19); 1211 | eph[ieph][sv].iodc = (int) atof(tmp); 1212 | 1213 | // BROADCAST ORBIT - 7 1214 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1215 | break; 1216 | 1217 | // Set valid flag 1218 | eph[ieph][sv].vflg = true; 1219 | 1220 | // Update the working variables 1221 | eph[ieph][sv].A = eph[ieph][sv].sqrta * eph[ieph][sv].sqrta; 1222 | eph[ieph][sv].n = sqrt(GM_EARTH / (eph[ieph][sv].A * eph[ieph][sv].A * eph[ieph][sv].A)) + eph[ieph][sv].deltan; 1223 | eph[ieph][sv].sq1e2 = sqrt(1.0 - eph[ieph][sv].ecc * eph[ieph][sv].ecc); 1224 | eph[ieph][sv].omgkdot = eph[ieph][sv].omgdot - OMEGA_EARTH; 1225 | } 1226 | 1227 | gzclose(fp); 1228 | 1229 | if (g0.week >= 0) 1230 | ieph += 1; // Number of sets of ephemerides 1231 | 1232 | return (ieph); 1233 | } 1234 | 1235 | /*! \brief Read Ephemeris data from the RINEX v3 Navigation file */ 1236 | 1237 | /* \param[out] eph Array of Output SV ephemeris data 1238 | * \param[in] fname File name of the RINEX file 1239 | * \returns Number of sets of ephemerides in the file 1240 | */ 1241 | static int readRinex3(ephem_t eph[][MAX_SAT], ionoutc_t *ionoutc, const char *fname) { 1242 | struct gzFile_s *fp; 1243 | int ieph; 1244 | 1245 | int sv; 1246 | char str[MAX_CHAR]; 1247 | char tmp[20]; 1248 | double ver = 0.0; 1249 | 1250 | datetime_t t; 1251 | gpstime_t g; 1252 | gpstime_t g0; 1253 | double dt; 1254 | 1255 | int flags = 0x0; 1256 | 1257 | if (NULL == (fp = gzopen(fname, "rt"))) 1258 | return (-1); 1259 | 1260 | // Clear valid flag 1261 | for (ieph = 0; ieph < EPHEM_ARRAY_SIZE; ieph++) 1262 | for (sv = 0; sv < MAX_SAT; sv++) 1263 | eph[ieph][sv].vflg = false; 1264 | 1265 | // Read header lines 1266 | while (1) { 1267 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1268 | break; 1269 | 1270 | if (strncmp(str + 60, "COMMENT", 7) == 0) { 1271 | continue; 1272 | } else if (strncmp(str + 60, "END OF HEADER", 13) == 0) { 1273 | break; 1274 | } else if (strncmp(str + 60, "RINEX VERSION / TYPE", 20) == 0) { 1275 | strncpy(tmp, str, 9); 1276 | tmp[9] = 0; 1277 | replaceExpDesignator(tmp, 9); 1278 | ver = atof(tmp); 1279 | if (ver < 3.0) { 1280 | gzclose(fp); 1281 | return -2; 1282 | } 1283 | 1284 | if (str[20] != 'N' && str[40] != 'G') { 1285 | gzclose(fp); 1286 | return -3; 1287 | } 1288 | } else if (strncmp(str + 60, "PGM / RUN BY / DATE", 19) == 0) { 1289 | strncpy(rinex_date, str + 40, 20); 1290 | rinex_date[20] = 0; 1291 | } else if (strncmp(str + 60, "IONOSPHERIC CORR", 16) == 0) { 1292 | if (strncmp(str, "GPSA", 4) == 0) { 1293 | strncpy(tmp, str + 5, 12); 1294 | tmp[12] = 0; 1295 | replaceExpDesignator(tmp, 12); 1296 | ionoutc->alpha0 = atof(tmp); 1297 | 1298 | strncpy(tmp, str + 17, 12); 1299 | tmp[12] = 0; 1300 | replaceExpDesignator(tmp, 12); 1301 | ionoutc->alpha1 = atof(tmp); 1302 | 1303 | strncpy(tmp, str + 29, 12); 1304 | tmp[12] = 0; 1305 | replaceExpDesignator(tmp, 12); 1306 | ionoutc->alpha2 = atof(tmp); 1307 | 1308 | strncpy(tmp, str + 41, 12); 1309 | tmp[12] = 0; 1310 | replaceExpDesignator(tmp, 12); 1311 | ionoutc->alpha3 = atof(tmp); 1312 | 1313 | flags |= 0x1; 1314 | } else if (strncmp(str, "GPSB", 4) == 0) { 1315 | strncpy(tmp, str + 5, 12); 1316 | tmp[12] = 0; 1317 | replaceExpDesignator(tmp, 12); 1318 | ionoutc->beta0 = atof(tmp); 1319 | 1320 | strncpy(tmp, str + 17, 12); 1321 | tmp[12] = 0; 1322 | replaceExpDesignator(tmp, 12); 1323 | ionoutc->beta1 = atof(tmp); 1324 | 1325 | strncpy(tmp, str + 29, 12); 1326 | tmp[12] = 0; 1327 | replaceExpDesignator(tmp, 12); 1328 | ionoutc->beta2 = atof(tmp); 1329 | 1330 | strncpy(tmp, str + 41, 12); 1331 | tmp[12] = 0; 1332 | replaceExpDesignator(tmp, 12); 1333 | ionoutc->beta3 = atof(tmp); 1334 | 1335 | flags |= 0x1 << 1; 1336 | } 1337 | } else if (strncmp(str + 60, "TIME SYSTEM CORR", 16) == 0 && strncmp(str, "GPUT", 4) == 0) { 1338 | strncpy(tmp, str + 5, 17); 1339 | tmp[17] = 0; 1340 | replaceExpDesignator(tmp, 17); 1341 | ionoutc->A0 = atof(tmp); 1342 | 1343 | strncpy(tmp, str + 22, 16); 1344 | tmp[16] = 0; 1345 | replaceExpDesignator(tmp, 16); 1346 | ionoutc->A1 = atof(tmp); 1347 | 1348 | strncpy(tmp, str + 38, 7); 1349 | tmp[7] = 0; 1350 | replaceExpDesignator(tmp, 7); 1351 | ionoutc->tot = atoi(tmp); 1352 | 1353 | strncpy(tmp, str + 45, 6); 1354 | tmp[6] = 0; 1355 | ionoutc->wnt = atoi(tmp); 1356 | 1357 | if (ionoutc->tot % 4096 == 0) 1358 | flags |= 0x1 << 2; 1359 | } else if (strncmp(str + 60, "LEAP SECONDS", 12) == 0) { 1360 | strncpy(tmp, str, 6); 1361 | tmp[6] = 0; 1362 | ionoutc->dtls = atoi(tmp); 1363 | 1364 | flags |= 0x1 << 3; 1365 | } 1366 | } 1367 | 1368 | ionoutc->vflg = false; 1369 | if (flags == 0xF) // Read all Iono/UTC lines 1370 | ionoutc->vflg = true; 1371 | 1372 | // Read ephemeris blocks 1373 | g0.week = -1; 1374 | ieph = 0; 1375 | 1376 | while (1) { 1377 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1378 | break; 1379 | 1380 | // Check for GPS data record 1381 | if (str[0] != 'G') { 1382 | continue; 1383 | } 1384 | 1385 | // PRN 1386 | strncpy(tmp, str + 1, 2); 1387 | tmp[2] = 0; 1388 | sv = atoi(tmp) - 1; 1389 | 1390 | // EPOCH 1391 | strncpy(tmp, str + 4, 4); 1392 | tmp[4] = 0; 1393 | t.y = atoi(tmp); 1394 | 1395 | strncpy(tmp, str + 9, 2); 1396 | tmp[2] = 0; 1397 | t.m = atoi(tmp); 1398 | 1399 | strncpy(tmp, str + 12, 2); 1400 | tmp[2] = 0; 1401 | t.d = atoi(tmp); 1402 | 1403 | strncpy(tmp, str + 15, 2); 1404 | tmp[2] = 0; 1405 | t.hh = atoi(tmp); 1406 | 1407 | strncpy(tmp, str + 18, 2); 1408 | tmp[2] = 0; 1409 | t.mm = atoi(tmp); 1410 | 1411 | strncpy(tmp, str + 21, 2); 1412 | tmp[2] = 0; 1413 | t.sec = (double) atoi(tmp); 1414 | 1415 | date2gps(&t, &g); 1416 | 1417 | if (g0.week == -1) 1418 | g0 = g; 1419 | 1420 | // Check current time of clock 1421 | dt = subGpsTime(g, g0); 1422 | 1423 | if (dt > SECONDS_IN_HOUR) { 1424 | g0 = g; 1425 | ieph++; // a new set of ephemerides 1426 | 1427 | if (ieph >= EPHEM_ARRAY_SIZE) 1428 | break; 1429 | } 1430 | 1431 | // Date and time 1432 | eph[ieph][sv].t = t; 1433 | 1434 | // SV CLK 1435 | eph[ieph][sv].toc = g; 1436 | 1437 | strncpy(tmp, str + 23, 19); 1438 | tmp[19] = 0; 1439 | replaceExpDesignator(tmp, 19); 1440 | eph[ieph][sv].af0 = atof(tmp); 1441 | 1442 | strncpy(tmp, str + 42, 19); 1443 | tmp[19] = 0; 1444 | replaceExpDesignator(tmp, 19); 1445 | eph[ieph][sv].af1 = atof(tmp); 1446 | 1447 | strncpy(tmp, str + 61, 19); 1448 | tmp[19] = 0; 1449 | replaceExpDesignator(tmp, 19); 1450 | eph[ieph][sv].af2 = atof(tmp); 1451 | 1452 | // BROADCAST ORBIT - 1 1453 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1454 | break; 1455 | 1456 | strncpy(tmp, str + 4, 19); 1457 | tmp[19] = 0; 1458 | replaceExpDesignator(tmp, 19); 1459 | eph[ieph][sv].iode = (int) atof(tmp); 1460 | 1461 | strncpy(tmp, str + 23, 19); 1462 | tmp[19] = 0; 1463 | replaceExpDesignator(tmp, 19); 1464 | eph[ieph][sv].crs = atof(tmp); 1465 | 1466 | strncpy(tmp, str + 42, 19); 1467 | tmp[19] = 0; 1468 | replaceExpDesignator(tmp, 19); 1469 | eph[ieph][sv].deltan = atof(tmp); 1470 | 1471 | strncpy(tmp, str + 61, 19); 1472 | tmp[19] = 0; 1473 | replaceExpDesignator(tmp, 19); 1474 | eph[ieph][sv].m0 = atof(tmp); 1475 | 1476 | // BROADCAST ORBIT - 2 1477 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1478 | break; 1479 | 1480 | strncpy(tmp, str + 4, 19); 1481 | tmp[19] = 0; 1482 | replaceExpDesignator(tmp, 19); 1483 | eph[ieph][sv].cuc = atof(tmp); 1484 | 1485 | strncpy(tmp, str + 23, 19); 1486 | tmp[19] = 0; 1487 | replaceExpDesignator(tmp, 19); 1488 | eph[ieph][sv].ecc = atof(tmp); 1489 | 1490 | strncpy(tmp, str + 42, 19); 1491 | tmp[19] = 0; 1492 | replaceExpDesignator(tmp, 19); 1493 | eph[ieph][sv].cus = atof(tmp); 1494 | 1495 | strncpy(tmp, str + 61, 19); 1496 | tmp[19] = 0; 1497 | replaceExpDesignator(tmp, 19); 1498 | eph[ieph][sv].sqrta = atof(tmp); 1499 | 1500 | // BROADCAST ORBIT - 3 1501 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1502 | break; 1503 | 1504 | strncpy(tmp, str + 4, 19); 1505 | tmp[19] = 0; 1506 | replaceExpDesignator(tmp, 19); 1507 | eph[ieph][sv].toe.sec = atof(tmp); 1508 | 1509 | strncpy(tmp, str + 23, 19); 1510 | tmp[19] = 0; 1511 | replaceExpDesignator(tmp, 19); 1512 | eph[ieph][sv].cic = atof(tmp); 1513 | 1514 | strncpy(tmp, str + 42, 19); 1515 | tmp[19] = 0; 1516 | replaceExpDesignator(tmp, 19); 1517 | eph[ieph][sv].omg0 = atof(tmp); 1518 | 1519 | strncpy(tmp, str + 61, 19); 1520 | tmp[19] = 0; 1521 | replaceExpDesignator(tmp, 19); 1522 | eph[ieph][sv].cis = atof(tmp); 1523 | 1524 | // BROADCAST ORBIT - 4 1525 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1526 | break; 1527 | 1528 | strncpy(tmp, str + 4, 19); 1529 | tmp[19] = 0; 1530 | replaceExpDesignator(tmp, 19); 1531 | eph[ieph][sv].inc0 = atof(tmp); 1532 | 1533 | strncpy(tmp, str + 23, 19); 1534 | tmp[19] = 0; 1535 | replaceExpDesignator(tmp, 19); 1536 | eph[ieph][sv].crc = atof(tmp); 1537 | 1538 | strncpy(tmp, str + 42, 19); 1539 | tmp[19] = 0; 1540 | replaceExpDesignator(tmp, 19); 1541 | eph[ieph][sv].aop = atof(tmp); 1542 | 1543 | strncpy(tmp, str + 61, 19); 1544 | tmp[19] = 0; 1545 | replaceExpDesignator(tmp, 19); 1546 | eph[ieph][sv].omgdot = atof(tmp); 1547 | 1548 | // BROADCAST ORBIT - 5 1549 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1550 | break; 1551 | 1552 | strncpy(tmp, str + 4, 19); 1553 | tmp[19] = 0; 1554 | replaceExpDesignator(tmp, 19); 1555 | eph[ieph][sv].idot = atof(tmp); 1556 | 1557 | strncpy(tmp, str + 23, 19); 1558 | tmp[19] = 0; 1559 | replaceExpDesignator(tmp, 19); 1560 | eph[ieph][sv].codeL2 = (int) atof(tmp); 1561 | 1562 | strncpy(tmp, str + 42, 19); 1563 | tmp[19] = 0; 1564 | replaceExpDesignator(tmp, 19); 1565 | eph[ieph][sv].toe.week = (int) atof(tmp); 1566 | 1567 | // BROADCAST ORBIT - 6 1568 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1569 | break; 1570 | 1571 | // SV accuracy not read 1572 | 1573 | strncpy(tmp, str + 23, 19); 1574 | tmp[19] = 0; 1575 | replaceExpDesignator(tmp, 19); 1576 | eph[ieph][sv].svhlth = (int) atof(tmp); 1577 | if ((eph[ieph][sv].svhlth > 0) && (eph[ieph][sv].svhlth < 32)) 1578 | eph[ieph][sv].svhlth += 32; // Set MSB to 1 1579 | 1580 | strncpy(tmp, str + 42, 19); 1581 | tmp[19] = 0; 1582 | replaceExpDesignator(tmp, 19); 1583 | eph[ieph][sv].tgd = atof(tmp); 1584 | 1585 | strncpy(tmp, str + 61, 19); 1586 | tmp[19] = 0; 1587 | replaceExpDesignator(tmp, 19); 1588 | eph[ieph][sv].iodc = (int) atof(tmp); 1589 | 1590 | // BROADCAST ORBIT - 7 1591 | if (NULL == gzgets(fp, str, MAX_CHAR)) 1592 | break; 1593 | 1594 | // Set valid flag 1595 | eph[ieph][sv].vflg = true; 1596 | 1597 | // Update the working variables 1598 | eph[ieph][sv].A = eph[ieph][sv].sqrta * eph[ieph][sv].sqrta; 1599 | eph[ieph][sv].n = sqrt(GM_EARTH / (eph[ieph][sv].A * eph[ieph][sv].A * eph[ieph][sv].A)) + eph[ieph][sv].deltan; 1600 | eph[ieph][sv].sq1e2 = sqrt(1.0 - eph[ieph][sv].ecc * eph[ieph][sv].ecc); 1601 | eph[ieph][sv].omgkdot = eph[ieph][sv].omgdot - OMEGA_EARTH; 1602 | } 1603 | 1604 | gzclose(fp); 1605 | 1606 | if (g0.week >= 0) 1607 | ieph += 1; // Number of sets of ephemerides 1608 | 1609 | return (ieph); 1610 | } 1611 | 1612 | static double ionosphericDelay(const ionoutc_t *ionoutc, gpstime_t g, double *llh, double *azel) { 1613 | double iono_delay = 0.0; 1614 | double E, phi_u, lam_u, F; 1615 | 1616 | if (ionoutc->enable == false) 1617 | return (0.0); // No ionospheric delay 1618 | 1619 | E = azel[1] / PI; 1620 | phi_u = llh[0] / PI; 1621 | lam_u = llh[1] / PI; 1622 | 1623 | // Obliquity factor 1624 | F = 1.0 + 16.0 * pow((0.53 - E), 3.0); 1625 | 1626 | if (ionoutc->vflg == false) 1627 | iono_delay = F * 5.0e-9 * SPEED_OF_LIGHT; 1628 | else { 1629 | double t, psi, phi_i, lam_i, phi_m, phi_m2, phi_m3; 1630 | double AMP, PER, X, X2, X4; 1631 | 1632 | // Earth's central angle between the user position and the earth projection of 1633 | // ionospheric intersection point (semi-circles) 1634 | psi = 0.0137 / (E + 0.11) - 0.022; 1635 | 1636 | // Geodetic latitude of the earth projection of the ionospheric intersection point 1637 | // (semi-circles) 1638 | phi_i = phi_u + psi * cos(azel[0]); 1639 | if (phi_i > 0.416) 1640 | phi_i = 0.416; 1641 | else if (phi_i<-0.416) 1642 | phi_i = -0.416; 1643 | 1644 | // Geodetic longitude of the earth projection of the ionospheric intersection point 1645 | // (semi-circles) 1646 | lam_i = lam_u + psi * sin(azel[0]) / cos(phi_i * PI); 1647 | 1648 | // Geomagnetic latitude of the earth projection of the ionospheric intersection 1649 | // point (mean ionospheric height assumed 350 km) (semi-circles) 1650 | phi_m = phi_i + 0.064 * cos((lam_i - 1.617) * PI); 1651 | phi_m2 = phi_m*phi_m; 1652 | phi_m3 = phi_m2*phi_m; 1653 | 1654 | AMP = ionoutc->alpha0 + ionoutc->alpha1 * phi_m 1655 | + ionoutc->alpha2 * phi_m2 + ionoutc->alpha3*phi_m3; 1656 | if (AMP < 0.0) 1657 | AMP = 0.0; 1658 | 1659 | PER = ionoutc->beta0 + ionoutc->beta1 * phi_m 1660 | + ionoutc->beta2 * phi_m2 + ionoutc->beta3*phi_m3; 1661 | if (PER < 72000.0) 1662 | PER = 72000.0; 1663 | 1664 | // Local time (sec) 1665 | t = SECONDS_IN_DAY / 2.0 * lam_i + g.sec; 1666 | while (t >= SECONDS_IN_DAY) 1667 | t -= SECONDS_IN_DAY; 1668 | while (t < 0) 1669 | t += SECONDS_IN_DAY; 1670 | 1671 | // Phase (radians) 1672 | X = 2.0 * PI * (t - 50400.0) / PER; 1673 | 1674 | if (fabs(X) < 1.57) { 1675 | X2 = X*X; 1676 | X4 = X2*X2; 1677 | iono_delay = F * (5.0e-9 + AMP * (1.0 - X2 / 2.0 + X4 / 24.0)) * SPEED_OF_LIGHT; 1678 | } else 1679 | iono_delay = F * 5.0e-9 * SPEED_OF_LIGHT; 1680 | } 1681 | 1682 | return (iono_delay); 1683 | } 1684 | 1685 | /*! \brief Compute range between a satellite and the receiver 1686 | * \param[out] rho The computed range 1687 | * \param[in] eph Ephemeris data of the satellite 1688 | * \param[in] g GPS time at time of receiving the signal 1689 | * \param[in] xyz position of the receiver 1690 | */ 1691 | static void computeRange(range_t *rho, ephem_t eph, ionoutc_t *ionoutc, gpstime_t g, double xyz[]) { 1692 | double pos[3], vel[3], clk[2]; 1693 | double los[3]; 1694 | double tau; 1695 | double range, rate; 1696 | double xrot, yrot; 1697 | 1698 | double llh[3], neu[3]; 1699 | double tmat[3][3]; 1700 | 1701 | // SV position at time of the pseudorange observation. 1702 | satpos(eph, g, pos, vel, clk); 1703 | 1704 | // Receiver to satellite vector and light-time. 1705 | subVect(los, pos, xyz); 1706 | tau = normVect(los) / SPEED_OF_LIGHT; 1707 | 1708 | // Extrapolate the satellite position backwards to the transmission time. 1709 | pos[0] -= vel[0] * tau; 1710 | pos[1] -= vel[1] * tau; 1711 | pos[2] -= vel[2] * tau; 1712 | 1713 | // Earth rotation correction. The change in velocity can be neglected. 1714 | xrot = pos[0] + pos[1] * OMEGA_EARTH*tau; 1715 | yrot = pos[1] - pos[0] * OMEGA_EARTH*tau; 1716 | pos[0] = xrot; 1717 | pos[1] = yrot; 1718 | 1719 | // New observer to satellite vector and satellite range. 1720 | subVect(los, pos, xyz); 1721 | range = normVect(los); 1722 | rho->d = range; 1723 | 1724 | // Pseudorange. 1725 | rho->range = range - SPEED_OF_LIGHT * clk[0]; 1726 | 1727 | // Relative velocity of SV and receiver. 1728 | rate = dotProd(vel, los) / range; 1729 | 1730 | // Pseudorange rate. 1731 | rho->rate = rate; // - SPEED_OF_LIGHT*clk[1]; 1732 | 1733 | // Time of application. 1734 | rho->g = g; 1735 | 1736 | // Azimuth and elevation angles. 1737 | xyz2llh(xyz, llh); 1738 | ltcmat(llh, tmat); 1739 | ecef2neu(los, tmat, neu); 1740 | neu2azel(rho->azel, neu); 1741 | 1742 | // Add ionospheric delay 1743 | rho->iono_delay = ionosphericDelay(ionoutc, g, llh, rho->azel); 1744 | rho->range += rho->iono_delay; 1745 | 1746 | return; 1747 | } 1748 | 1749 | /*! \brief Compute the code phase for a given channel (satellite) 1750 | * \param chan Channel on which we operate (is updated) 1751 | * \param[in] rho1 Current range, after \a dt has expired 1752 | * \param[in dt delta-t (time difference) in seconds 1753 | */ 1754 | static void computeCodePhase(channel_t *chan, range_t rho1, double dt) { 1755 | double ms; 1756 | int ims; 1757 | double rhorate; 1758 | 1759 | // Pseudorange rate. 1760 | rhorate = (rho1.range - chan->rho0.range) / dt; 1761 | 1762 | // Carrier and code frequency. 1763 | chan->f_carr = -rhorate / LAMBDA_L1; 1764 | chan->f_code = CODE_FREQ + chan->f_carr*CARR_TO_CODE; 1765 | 1766 | // Initial code phase and data bit counters. 1767 | ms = ((subGpsTime(chan->rho0.g, chan->g0) + 6.0) - chan->rho0.range / SPEED_OF_LIGHT)*1000.0; 1768 | 1769 | ims = (int) ms; 1770 | chan->code_phase = (ms - (double) ims) * CA_SEQ_LEN; // in chip 1771 | 1772 | chan->iword = ims / 600; // 1 word = 30 bits = 600 ms 1773 | ims -= chan->iword * 600; 1774 | 1775 | chan->ibit = ims / 20; // 1 bit = 20 code = 20 ms 1776 | ims -= chan->ibit * 20; 1777 | 1778 | chan->icode = ims; // 1 code = 1 ms 1779 | 1780 | chan->codeCA = chan->ca[(int) chan->code_phase]*2 - 1; 1781 | chan->dataBit = (int) ((chan->dwrd[chan->iword]>>(29 - chan->ibit)) & 0x1UL)*2 - 1; 1782 | 1783 | // Save current pseudorange 1784 | chan->rho0 = rho1; 1785 | 1786 | return; 1787 | } 1788 | 1789 | /*! \brief Read the list of user motions from the input file 1790 | * \param[out] xyz Output array of ECEF vectors for user motion 1791 | * \param[[in] filename File name of the text input file 1792 | * \returns Number of user data motion records read, -1 on error 1793 | */ 1794 | static int readUserMotion(double xyz[USER_MOTION_SIZE][3], const char *filename) { 1795 | FILE *fp; 1796 | int numd; 1797 | char str[MAX_CHAR]; 1798 | double t, x, y, z; 1799 | 1800 | if (NULL == (fp = fopen(filename, "rt"))) 1801 | return (-1); 1802 | 1803 | for (numd = 0; numd < USER_MOTION_SIZE; numd++) { 1804 | if (fgets(str, MAX_CHAR, fp) == NULL) 1805 | break; 1806 | 1807 | if (EOF == sscanf(str, "%lf,%lf,%lf,%lf", &t, &x, &y, &z)) // Read CSV line 1808 | break; 1809 | 1810 | xyz[numd][0] = x; 1811 | xyz[numd][1] = y; 1812 | xyz[numd][2] = z; 1813 | } 1814 | 1815 | fclose(fp); 1816 | 1817 | return (numd); 1818 | } 1819 | 1820 | static int generateNavMsg(gpstime_t g, channel_t *chan, int init) { 1821 | int iwrd, isbf; 1822 | gpstime_t g0; 1823 | unsigned long wn, tow; 1824 | unsigned sbfwrd; 1825 | unsigned long prevwrd; 1826 | int nib; 1827 | 1828 | g0.week = g.week; 1829 | g0.sec = (double) (((unsigned long) (g.sec + 0.5)) / 30UL) * 30.0; // Align with the full frame length = 30 sec 1830 | chan->g0 = g0; // Data bit reference time 1831 | 1832 | wn = (unsigned long) (g0.week % 1024); 1833 | tow = ((unsigned long) g0.sec) / 6UL; 1834 | 1835 | if (init == 1) // Initialize subframe 5 1836 | { 1837 | prevwrd = 0UL; 1838 | 1839 | for (iwrd = 0; iwrd < N_DWRD_SBF; iwrd++) { 1840 | sbfwrd = chan->sbf[4][iwrd]; 1841 | 1842 | // Add TOW-count message into HOW 1843 | if (iwrd == 1) 1844 | sbfwrd |= ((tow & 0x1FFFFUL) << 13); 1845 | 1846 | // Compute checksum 1847 | sbfwrd |= (prevwrd << 30) & 0xC0000000UL; // 2 LSBs of the previous transmitted word 1848 | nib = ((iwrd == 1) || (iwrd == 9)) ? 1 : 0; // Non-information bearing bits for word 2 and 10 1849 | chan->dwrd[iwrd] = computeChecksum(sbfwrd, nib); 1850 | 1851 | prevwrd = chan->dwrd[iwrd]; 1852 | } 1853 | } else // Save subframe 5 1854 | { 1855 | for (iwrd = 0; iwrd < N_DWRD_SBF; iwrd++) { 1856 | chan->dwrd[iwrd] = chan->dwrd[N_DWRD_SBF * N_SBF + iwrd]; 1857 | 1858 | prevwrd = chan->dwrd[iwrd]; 1859 | } 1860 | /* 1861 | // Sanity check 1862 | if (((chan->dwrd[1])&(0x1FFFFUL<<13)) != ((tow&0x1FFFFUL)<<13)) 1863 | { 1864 | fprintf(stderr, "\nWARNING: Invalid TOW in subframe 5.\n"); 1865 | return(0); 1866 | } 1867 | */ 1868 | } 1869 | 1870 | for (isbf = 0; isbf < N_SBF; isbf++) { 1871 | tow++; 1872 | 1873 | for (iwrd = 0; iwrd < N_DWRD_SBF; iwrd++) { 1874 | sbfwrd = chan->sbf[isbf][iwrd]; 1875 | 1876 | // Add transmission week number to Subframe 1 1877 | if ((isbf == 0)&&(iwrd == 2)) 1878 | sbfwrd |= (wn & 0x3FFUL) << 20; 1879 | 1880 | // Add TOW-count message into HOW 1881 | if (iwrd == 1) 1882 | sbfwrd |= ((tow & 0x1FFFFUL) << 13); 1883 | 1884 | // Compute checksum 1885 | sbfwrd |= (prevwrd << 30) & 0xC0000000UL; // 2 LSBs of the previous transmitted word 1886 | nib = ((iwrd == 1) || (iwrd == 9)) ? 1 : 0; // Non-information bearing bits for word 2 and 10 1887 | chan->dwrd[(isbf + 1) * N_DWRD_SBF + iwrd] = computeChecksum(sbfwrd, nib); 1888 | 1889 | prevwrd = chan->dwrd[(isbf + 1) * N_DWRD_SBF + iwrd]; 1890 | } 1891 | } 1892 | 1893 | return (1); 1894 | } 1895 | 1896 | static int checkSatVisibility(ephem_t eph, gpstime_t g, double *xyz, double elvMask, double *azel) { 1897 | double llh[3], neu[3]; 1898 | double pos[3], vel[3], clk[3], los[3]; 1899 | double tmat[3][3]; 1900 | 1901 | if (eph.vflg == false) 1902 | return (-1); // Invalid 1903 | 1904 | xyz2llh(xyz, llh); 1905 | ltcmat(llh, tmat); 1906 | 1907 | satpos(eph, g, pos, vel, clk); 1908 | subVect(los, pos, xyz); 1909 | ecef2neu(los, tmat, neu); 1910 | neu2azel(azel, neu); 1911 | 1912 | if (azel[1] * R2D > elvMask) 1913 | return (1); // Visible 1914 | // else 1915 | return (0); // Invisible 1916 | } 1917 | 1918 | static int allocateChannel(channel_t *chan, ephem_t *eph, ionoutc_t ionoutc, gpstime_t grx, double *xyz, double elvMask) { 1919 | NOTUSED(elvMask); 1920 | int nsat = 0; 1921 | int i, sv; 1922 | double azel[2]; 1923 | 1924 | range_t rho; 1925 | double ref[3] = {0.0}; 1926 | double r_ref, r_xyz; 1927 | double phase_ini; 1928 | 1929 | for (sv = 0; sv < MAX_SAT; sv++) { 1930 | if (checkSatVisibility(eph[sv], grx, xyz, 0.0, azel) == 1) { 1931 | nsat++; // Number of visible satellites 1932 | 1933 | if (allocatedSat[sv] == -1) // Visible but not allocated 1934 | { 1935 | // Allocated new satellite 1936 | for (i = 0; i < MAX_CHAN; i++) { 1937 | if (chan[i].prn == 0) { 1938 | // Initialize channel 1939 | chan[i].prn = sv + 1; 1940 | chan[i].azel[0] = azel[0]; 1941 | chan[i].azel[1] = azel[1]; 1942 | 1943 | // C/A code generation 1944 | codegen(chan[i].ca, chan[i].prn); 1945 | 1946 | // Generate subframe 1947 | eph2sbf(eph[sv], ionoutc, chan[i].sbf); 1948 | 1949 | // Generate navigation message 1950 | generateNavMsg(grx, &chan[i], 1); 1951 | 1952 | // Initialize pseudorange 1953 | computeRange(&rho, eph[sv], &ionoutc, grx, xyz); 1954 | chan[i].rho0 = rho; 1955 | 1956 | // Initialize carrier phase 1957 | r_xyz = rho.range; 1958 | 1959 | computeRange(&rho, eph[sv], &ionoutc, grx, ref); 1960 | r_ref = rho.range; 1961 | 1962 | phase_ini = (2.0 * r_ref - r_xyz) / LAMBDA_L1; 1963 | #ifdef FLOAT_CARR_PHASE 1964 | chan[i].carr_phase = phase_ini - floor(phase_ini); 1965 | #else 1966 | phase_ini -= floor(phase_ini); 1967 | chan[i].carr_phase = (unsigned int) (512.0 * 65536.0 * phase_ini); 1968 | #endif 1969 | // Done. 1970 | break; 1971 | } 1972 | } 1973 | 1974 | // Set satellite allocation channel 1975 | if (i < MAX_CHAN) 1976 | allocatedSat[sv] = i; 1977 | } 1978 | } else if (allocatedSat[sv] >= 0) // Not visible but allocated 1979 | { 1980 | // Clear channel 1981 | chan[allocatedSat[sv]].prn = 0; 1982 | 1983 | // Clear satellite allocation flag 1984 | allocatedSat[sv] = -1; 1985 | } 1986 | } 1987 | 1988 | return (nsat); 1989 | } 1990 | 1991 | static void usage(void) { 1992 | fprintf(stderr, "Usage: pluto-gps-sim [options]\n" 1993 | "Options:\n" 1994 | " -e RINEX navigation file for GPS ephemerides (required)\n" 1995 | " -u User motion file (dynamic mode) 10Hz, Max %u points\n" 1996 | " -3 Use RINEX version 3 format\n" 1997 | " -f Pull actual RINEX navigation file from NASA FTP server\n" 1998 | " -c ECEF X,Y,Z in meters (static mode) e.g. 3967283.154,1022538.181,4872414.484\n" 1999 | " -l Lat,Lon,Hgt (static mode) e.g. 35.681298,139.766247,10.0\n" 2000 | " -t Scenario start time YYYY/MM/DD,hh:mm:ss\n" 2001 | " -T Overwrite TOC and TOE to scenario start time (use 'now' for actual time)\n" 2002 | " -s Sampling frequency [Hz] (default: 2600000)\n" 2003 | " -i Disable ionospheric delay for spacecraft scenario\n" 2004 | " -v Show details about simulated channels\n" 2005 | " -A Set TX attenuation [dB] (default -20.0)\n" 2006 | " -B Set RF bandwidth [MHz] (default 3.0)\n" 2007 | " -U ADALM-Pluto URI\n" 2008 | " -N ADALM-Pluto network IP or hostname (default pluto.local)\n", 2009 | (unsigned int) USER_MOTION_SIZE); 2010 | 2011 | return; 2012 | } 2013 | 2014 | static void handle_sig(int sig) { 2015 | NOTUSED(sig); 2016 | signal(SIGINT, SIG_DFL); // reset signal handler - bit extra safety 2017 | pthread_mutex_unlock(&plutotx.data_mutex); 2018 | plutotx.exit = true; 2019 | pthread_join(pluto_thread, NULL); /* Wait on Pluto TX thread exit */ 2020 | pthread_mutex_destroy(&plutotx.data_mutex); 2021 | pthread_cond_destroy(&plutotx.data_cond); 2022 | } 2023 | 2024 | #if defined(__MACH__) || defined(__APPLE__) 2025 | 2026 | static int pthread_setaffinity_np(pthread_t thread, size_t cpu_size, 2027 | cpu_set_t *cpu_set) { 2028 | thread_port_t mach_thread; 2029 | size_t core = 0; 2030 | 2031 | for (core = 0; core < 8 * cpu_size; core++) { 2032 | if (CPU_ISSET(core, cpu_set)) break; 2033 | } 2034 | printf("binding to core %ld\n", core); 2035 | thread_affinity_policy_data_t policy = {core}; 2036 | mach_thread = pthread_mach_thread_np(thread); 2037 | thread_policy_set(mach_thread, THREAD_AFFINITY_POLICY, 2038 | (thread_policy_t) & policy, 1); 2039 | return 0; 2040 | } 2041 | #endif 2042 | 2043 | // Set affinity of calling thread to specific core on a multi-core CPU 2044 | 2045 | static int thread_to_core(int core_id) { 2046 | int num_cores = sysconf(_SC_NPROCESSORS_ONLN); 2047 | if (core_id < 0 || core_id >= num_cores) 2048 | return EINVAL; 2049 | 2050 | cpu_set_t cpuset; 2051 | CPU_ZERO(&cpuset); 2052 | CPU_SET(core_id, &cpuset); 2053 | 2054 | pthread_t current_thread = pthread_self(); 2055 | return pthread_setaffinity_np(current_thread, sizeof (cpu_set_t), &cpuset); 2056 | } 2057 | 2058 | void *pluto_tx_thread_ep(void *arg) { 2059 | NOTUSED(arg); 2060 | char buf[1024]; 2061 | struct iio_context *ctx = NULL; 2062 | struct iio_device *tx = NULL; 2063 | struct iio_device *phydev = NULL; 2064 | struct iio_channel *tx0_i = NULL; 2065 | struct iio_channel *tx0_q = NULL; 2066 | struct iio_buffer *tx_buffer = NULL; 2067 | 2068 | // Try sticking this thread to core 2 2069 | thread_to_core(2); 2070 | 2071 | // Create IIO context to access ADALM-Pluto 2072 | ctx = iio_create_default_context(); 2073 | if (ctx == NULL) { 2074 | if (plutotx.hostname != NULL) { 2075 | ctx = iio_create_network_context(plutotx.hostname); 2076 | } else if (plutotx.uri != NULL) { 2077 | ctx = iio_create_context_from_uri(plutotx.uri); 2078 | } else { 2079 | ctx = iio_create_network_context("pluto.local"); 2080 | } 2081 | } 2082 | 2083 | if (ctx == NULL) { 2084 | iio_strerror(errno, buf, sizeof (buf)); 2085 | fprintf(stderr, "Failed creating IIO context: %s\n", buf); 2086 | goto pluto_thread_exit; 2087 | } 2088 | 2089 | int device_count = iio_context_get_devices_count(ctx); 2090 | if (!device_count) { 2091 | fprintf(stderr, "No supported PLUTOSDR devices found.\n"); 2092 | goto pluto_thread_exit; 2093 | } 2094 | 2095 | tx = iio_context_find_device(ctx, "cf-ad9361-dds-core-lpc"); 2096 | if (tx == NULL) { 2097 | iio_strerror(errno, buf, sizeof (buf)); 2098 | fprintf(stderr, "Error opening PLUTOSDR TX device: %s\n", buf); 2099 | goto pluto_thread_exit; 2100 | } 2101 | 2102 | // Additional IQ kernel buffers, default is 4 2103 | iio_device_set_kernel_buffers_count(tx, 12); 2104 | 2105 | phydev = iio_context_find_device(ctx, "ad9361-phy"); 2106 | struct iio_channel* phy_chn = iio_device_find_channel(phydev, "voltage0", true); 2107 | iio_channel_attr_write(phy_chn, "rf_port_select", plutotx.rfport); 2108 | iio_channel_attr_write_longlong(phy_chn, "rf_bandwidth", plutotx.bw_hz); 2109 | iio_channel_attr_write_longlong(phy_chn, "sampling_frequency", plutotx.fs_hz); 2110 | iio_channel_attr_write_double(phy_chn, "hardwaregain", plutotx.gain_db); 2111 | 2112 | iio_channel_attr_write_bool( 2113 | iio_device_find_channel(phydev, "altvoltage0", true) 2114 | , "powerdown", true); // Turn OFF RX LO 2115 | 2116 | iio_channel_attr_write_longlong( 2117 | iio_device_find_channel(phydev, "altvoltage1", true) 2118 | , "frequency", plutotx.lo_hz); // Set TX LO frequency 2119 | 2120 | tx0_i = iio_device_find_channel(tx, "voltage0", true); 2121 | if (!tx0_i) 2122 | tx0_i = iio_device_find_channel(tx, "altvoltage0", true); 2123 | 2124 | tx0_q = iio_device_find_channel(tx, "voltage1", true); 2125 | if (!tx0_q) 2126 | tx0_q = iio_device_find_channel(tx, "altvoltage1", true); 2127 | 2128 | iio_channel_enable(tx0_i); 2129 | iio_channel_enable(tx0_q); 2130 | 2131 | ad9361_set_bb_rate(iio_context_find_device(ctx, "ad9361-phy"), plutotx.fs_hz); 2132 | 2133 | tx_buffer = iio_device_create_buffer(tx, NUM_SAMPLES, false); 2134 | if (!tx_buffer) { 2135 | fprintf(stderr, "Could not create TX buffer.\n"); 2136 | goto pluto_thread_exit; 2137 | } 2138 | 2139 | iio_channel_attr_write_bool( 2140 | iio_device_find_channel(iio_context_find_device(ctx, "ad9361-phy"), "altvoltage1", true) 2141 | , "powerdown", false); // Turn ON TX LO 2142 | 2143 | int32_t ntx = 0; 2144 | char *ptx_buffer = (char *) iio_buffer_start(tx_buffer); 2145 | 2146 | while (!plutotx.exit) { 2147 | pthread_mutex_lock(&plutotx.data_mutex); 2148 | memcpy(ptx_buffer, iq_buff, BUFFER_SIZE); 2149 | pthread_cond_signal(&plutotx.data_cond); 2150 | pthread_mutex_unlock(&plutotx.data_mutex); 2151 | // Schedule TX buffer 2152 | ntx = iio_buffer_push(tx_buffer); 2153 | if (ntx < 0) { 2154 | fprintf(stderr, "Error pushing buf %d\n", (int) ntx); 2155 | break; 2156 | ; 2157 | } 2158 | } 2159 | 2160 | pluto_thread_exit: 2161 | if (ctx) { 2162 | iio_channel_attr_write_bool( 2163 | iio_device_find_channel(iio_context_find_device(ctx, "ad9361-phy"), "altvoltage1", true) 2164 | , "powerdown", true); // Turn OFF TX LO 2165 | } 2166 | 2167 | if (tx_buffer) { 2168 | iio_buffer_destroy(tx_buffer); 2169 | } 2170 | if (tx0_i) { 2171 | iio_channel_disable(tx0_i); 2172 | } 2173 | if (tx0_q) { 2174 | iio_channel_disable(tx0_q); 2175 | } 2176 | if (ctx) { 2177 | iio_context_destroy(ctx); 2178 | } 2179 | 2180 | // Wake the main thread (if it's still waiting) 2181 | pthread_mutex_lock(&plutotx.data_mutex); 2182 | plutotx.exit = true; // just in case 2183 | pthread_cond_signal(&plutotx.data_cond); 2184 | pthread_mutex_unlock(&plutotx.data_mutex); 2185 | #ifndef _WIN32 2186 | pthread_exit(NULL); 2187 | #else 2188 | return NULL; 2189 | #endif 2190 | } 2191 | 2192 | static size_t fwrite_rinex(void *buffer, size_t size, size_t nmemb, void *stream) { 2193 | struct ftp_file *out = (struct ftp_file *) stream; 2194 | if (out && !out->stream) { 2195 | /* open file for writing */ 2196 | out->stream = fopen(out->filename, "wb"); 2197 | if (!out->stream) 2198 | return -1; /* failure, can't open file to write */ 2199 | } 2200 | return fwrite(buffer, size, nmemb, out->stream); 2201 | } 2202 | 2203 | int main(int argc, char *argv[]) { 2204 | int sv; 2205 | int neph, ieph; 2206 | ephem_t eph[EPHEM_ARRAY_SIZE][MAX_SAT]; 2207 | gpstime_t g0; 2208 | 2209 | double llh[3]; 2210 | 2211 | int i; 2212 | channel_t chan[MAX_CHAN]; 2213 | double elvmask = 0.0; // in degree 2214 | 2215 | int ip, qp; 2216 | int iTable; 2217 | 2218 | gpstime_t grx; 2219 | double delt; 2220 | int isamp; 2221 | 2222 | int numd = 0, iumd = 0; 2223 | // Allocate user motion array 2224 | double xyz[USER_MOTION_SIZE][3]; 2225 | 2226 | int staticLocationMode = true; 2227 | 2228 | bool use_rinex3 = false; 2229 | bool use_ftp = false; 2230 | CURL *curl; 2231 | CURLcode res = CURLE_GOT_NOTHING; 2232 | struct ftp_file ftp = { 2233 | RINEX2_FILE_NAME, 2234 | NULL 2235 | }; 2236 | 2237 | const char* navfile = NULL; 2238 | const char* umfile = NULL; 2239 | 2240 | int result; 2241 | double gain[MAX_CHAN]; 2242 | double path_loss; 2243 | double ant_gain; 2244 | double ant_pat[37]; 2245 | int ibs; // boresight angle index 2246 | 2247 | datetime_t t0, tmin, tmax; 2248 | gpstime_t gmin, gmax; 2249 | double dt; 2250 | int igrx; 2251 | 2252 | bool verb; 2253 | bool timeoverwrite = false; // Overwirte the TOC and TOE in the RINEX file 2254 | ionoutc_t ionoutc; 2255 | 2256 | //////////////////////////////////////////////////////////// 2257 | // Read options 2258 | //////////////////////////////////////////////////////////// 2259 | 2260 | // Default options 2261 | g0.week = -1; // Invalid start time 2262 | verb = false; 2263 | ionoutc.enable = true; 2264 | 2265 | // Default static location; Tokyo 2266 | llh[0] = 35.681298 / R2D; 2267 | llh[1] = 139.766247 / R2D; 2268 | llh[2] = 10.0; 2269 | 2270 | plutotx.bw_hz = (TX_SAMPLE_FREQ * 2); 2271 | plutotx.fs_hz = TX_SAMPLE_FREQ; 2272 | plutotx.lo_hz = GHZ(1.575420); // 1.57542 GHz RF frequency 2273 | plutotx.rfport = "A"; 2274 | plutotx.gain_db = -20.0; 2275 | plutotx.hostname = NULL; 2276 | plutotx.uri = NULL; 2277 | 2278 | pthread_mutex_init(&plutotx.data_mutex, NULL); 2279 | pthread_cond_init(&plutotx.data_cond, NULL); 2280 | 2281 | // signal handlers: 2282 | signal(SIGINT, handle_sig); 2283 | signal(SIGTERM, handle_sig); 2284 | signal(SIGQUIT, handle_sig); 2285 | 2286 | /* On a multi-core CPU we run the main thread and reader thread on different cores. 2287 | * Try sticking the main thread to core 1 2288 | */ 2289 | thread_to_core(1); 2290 | 2291 | if (argc < 3) { 2292 | usage(); 2293 | exit(1); 2294 | } 2295 | 2296 | while ((result = getopt(argc, argv, "e:3:u:g:c:l:s:T:t:A:B:U:N:vfi?")) != -1) { 2297 | switch (result) { 2298 | case 'e': 2299 | navfile = optarg; 2300 | break; 2301 | case 'u': 2302 | umfile = optarg; 2303 | staticLocationMode = false; 2304 | break; 2305 | case '3': 2306 | use_rinex3 = true; 2307 | ftp.filename = RINEX3_FILE_NAME; 2308 | break; 2309 | case 'f': 2310 | use_ftp = true; 2311 | break; 2312 | case 'c': 2313 | // Static ECEF coordinates input mode 2314 | sscanf(optarg, "%lf,%lf,%lf", &xyz[0][0], &xyz[0][1], &xyz[0][2]); 2315 | break; 2316 | case 'l': 2317 | // Static geodetic coordinates input mode 2318 | // Added by scateu@gmail.com 2319 | sscanf(optarg, "%lf,%lf,%lf", &llh[0], &llh[1], &llh[2]); 2320 | llh[0] = llh[0] / R2D; // convert to RAD 2321 | llh[1] = llh[1] / R2D; // convert to RAD 2322 | llh2xyz(llh, xyz[0]); // Convert llh to xyz 2323 | break; 2324 | case 's': 2325 | plutotx.fs_hz = (long long) atoi(optarg); 2326 | if (plutotx.fs_hz < MHZ(1.0)) { 2327 | fprintf(stderr, "ERROR: Invalid sampling frequency.\n"); 2328 | exit(1); 2329 | } 2330 | break; 2331 | case 'T': 2332 | timeoverwrite = true; 2333 | if (strncmp(optarg, "now", 3) == 0) { 2334 | time_t timer; 2335 | struct tm *gmt; 2336 | 2337 | time(&timer); 2338 | gmt = gmtime(&timer); 2339 | 2340 | t0.y = gmt->tm_year + 1900; 2341 | t0.m = gmt->tm_mon + 1; 2342 | t0.d = gmt->tm_mday; 2343 | t0.hh = gmt->tm_hour; 2344 | t0.mm = gmt->tm_min; 2345 | t0.sec = (double) gmt->tm_sec; 2346 | 2347 | date2gps(&t0, &g0); 2348 | 2349 | } 2350 | break; 2351 | case 't': 2352 | sscanf(optarg, "%d/%d/%d,%d:%d:%lf", &t0.y, &t0.m, &t0.d, &t0.hh, &t0.mm, &t0.sec); 2353 | if (t0.y <= 1980 || t0.m < 1 || t0.m > 12 || t0.d < 1 || t0.d > 31 || 2354 | t0.hh < 0 || t0.hh > 23 || t0.mm < 0 || t0.mm > 59 || t0.sec < 0.0 || t0.sec >= 60.0) { 2355 | fprintf(stderr, "ERROR: Invalid date and time.\n"); 2356 | exit(1); 2357 | } 2358 | t0.sec = floor(t0.sec); 2359 | date2gps(&t0, &g0); 2360 | break; 2361 | case 'i': 2362 | ionoutc.enable = false; // Disable ionospheric correction 2363 | break; 2364 | case 'v': 2365 | verb = true; 2366 | break; 2367 | case 'A': 2368 | plutotx.gain_db = atof(optarg); 2369 | if (plutotx.gain_db > 0.0) plutotx.gain_db = 0.0; 2370 | if (plutotx.gain_db < -80.0) plutotx.gain_db = -80.0; 2371 | break; 2372 | case 'B': 2373 | plutotx.bw_hz = MHZ(atof(optarg)); 2374 | if (plutotx.bw_hz > MHZ(5.0)) plutotx.bw_hz = MHZ(5.0); 2375 | if (plutotx.bw_hz < MHZ(1.0)) plutotx.bw_hz = MHZ(1.0); 2376 | break; 2377 | case 'U': 2378 | plutotx.uri = optarg; 2379 | break; 2380 | case 'N': 2381 | plutotx.hostname = optarg; 2382 | break; 2383 | case ':': 2384 | case '?': 2385 | usage(); 2386 | exit(1); 2387 | default: 2388 | break; 2389 | } 2390 | } 2391 | 2392 | if ((navfile == NULL) && (use_ftp == false)) { 2393 | fprintf(stderr, "ERROR: GPS ephemeris file is not specified.\n"); 2394 | exit(1); 2395 | } 2396 | 2397 | delt = 1.0 / plutotx.fs_hz; 2398 | 2399 | //////////////////////////////////////////////////////////// 2400 | // Receiver position 2401 | //////////////////////////////////////////////////////////// 2402 | 2403 | if (!staticLocationMode) { 2404 | // Read user motion file 2405 | numd = readUserMotion(xyz, umfile); 2406 | 2407 | if (numd == -1) { 2408 | fprintf(stderr, "ERROR: Failed to open user motion file.\n"); 2409 | exit(1); 2410 | } else if (numd == 0) { 2411 | fprintf(stderr, "ERROR: Failed to read user motion data.\n"); 2412 | exit(1); 2413 | } 2414 | 2415 | fprintf(stderr, "Using user motion mode.\n"); 2416 | } else { 2417 | // Static geodetic coordinates input mode: "-l" 2418 | fprintf(stderr, "Using static location mode.\n"); 2419 | } 2420 | 2421 | /* 2422 | fprintf(stderr, "xyz = %11.1f, %11.1f, %11.1f\n", xyz[0][0], xyz[0][1], xyz[0][2]); 2423 | fprintf(stderr, "llh = %11.6f, %11.6f, %11.1f\n", llh[0]*R2D, llh[1]*R2D, llh[2]); 2424 | */ 2425 | //////////////////////////////////////////////////////////// 2426 | // Read ephemeris 2427 | //////////////////////////////////////////////////////////// 2428 | if (use_ftp) { 2429 | time_t t = time(NULL); 2430 | struct tm *tm = gmtime(&t); 2431 | char* url = malloc(NAME_MAX); 2432 | const char *station = stations_v2[14].id_v2; 2433 | // We fetch data from previous hour because the actual hour is still in progress 2434 | tm->tm_hour -= 1; 2435 | if (tm->tm_hour < 0) { 2436 | tm->tm_hour = 23; 2437 | } 2438 | 2439 | if (use_rinex3) { 2440 | station = stations_v3[0].id_v2; 2441 | } 2442 | 2443 | // Compose FTP URL 2444 | snprintf(url, NAME_MAX, RINEX_FTP_URL RINEX_FTP_FILE, (use_rinex3) ? RINEX3_SUBFOLDER : RINEX2_SUBFOLDER, 2445 | tm->tm_yday + 1, tm->tm_hour, station, tm->tm_yday + 1, 'a' + tm->tm_hour, tm->tm_year - 100); 2446 | 2447 | curl_global_init(CURL_GLOBAL_DEFAULT); 2448 | curl = curl_easy_init(); 2449 | if (curl) { 2450 | curl_easy_setopt(curl, CURLOPT_URL, url); 2451 | curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, fwrite_rinex); 2452 | curl_easy_setopt(curl, CURLOPT_WRITEDATA, &ftp); 2453 | curl_easy_setopt(curl, CURLOPT_USE_SSL, CURLUSESSL_NONE); 2454 | if (verb) { 2455 | curl_easy_setopt(curl, CURLOPT_VERBOSE, 1L); 2456 | } else { 2457 | curl_easy_setopt(curl, CURLOPT_VERBOSE, 0L); 2458 | } 2459 | curl_easy_setopt(curl, CURLOPT_USERPWD, "anonymous:anonymous"); 2460 | res = curl_easy_perform(curl); 2461 | curl_easy_cleanup(curl); 2462 | } 2463 | 2464 | if (ftp.stream) 2465 | fclose(ftp.stream); 2466 | 2467 | free(url); 2468 | curl_global_cleanup(); 2469 | 2470 | if (res != CURLE_OK) { 2471 | fprintf(stderr, "Curl error: %d\n", res); 2472 | exit(1); 2473 | } 2474 | } 2475 | 2476 | if (use_rinex3) { 2477 | neph = readRinex3(eph, &ionoutc, navfile); 2478 | } else { 2479 | neph = readRinex2(eph, &ionoutc, navfile); 2480 | } 2481 | 2482 | if (neph == 0) { 2483 | fprintf(stderr, "ERROR: No ephemeris available.\n"); 2484 | exit(1); 2485 | } 2486 | 2487 | if ((verb == true)&&(ionoutc.vflg == true)) { 2488 | fprintf(stderr, " %12.3e %12.3e %12.3e %12.3e\n", 2489 | ionoutc.alpha0, ionoutc.alpha1, ionoutc.alpha2, ionoutc.alpha3); 2490 | fprintf(stderr, " %12.3e %12.3e %12.3e %12.3e\n", 2491 | ionoutc.beta0, ionoutc.beta1, ionoutc.beta2, ionoutc.beta3); 2492 | fprintf(stderr, " %19.11e %19.11e %9d %9d\n", 2493 | ionoutc.A0, ionoutc.A1, ionoutc.tot, ionoutc.wnt); 2494 | fprintf(stderr, "%6d\n", ionoutc.dtls); 2495 | } 2496 | 2497 | for (sv = 0; sv < MAX_SAT; sv++) { 2498 | if (eph[0][sv].vflg == true) { 2499 | gmin = eph[0][sv].toc; 2500 | tmin = eph[0][sv].t; 2501 | break; 2502 | } 2503 | } 2504 | 2505 | gmax.sec = 0; 2506 | gmax.week = 0; 2507 | tmax.sec = 0; 2508 | tmax.mm = 0; 2509 | tmax.hh = 0; 2510 | tmax.d = 0; 2511 | tmax.m = 0; 2512 | tmax.y = 0; 2513 | for (sv = 0; sv < MAX_SAT; sv++) { 2514 | if (eph[neph - 1][sv].vflg == true) { 2515 | gmax = eph[neph - 1][sv].toc; 2516 | tmax = eph[neph - 1][sv].t; 2517 | break; 2518 | } 2519 | } 2520 | 2521 | if (g0.week >= 0) // Scenario start time has been set. 2522 | { 2523 | if (timeoverwrite == true) { 2524 | gpstime_t gtmp; 2525 | datetime_t ttmp; 2526 | double dsec; 2527 | 2528 | gtmp.week = g0.week; 2529 | gtmp.sec = (double) (((int) (g0.sec)) / 7200)*7200.0; 2530 | 2531 | dsec = subGpsTime(gtmp, gmin); 2532 | 2533 | // Overwrite the UTC reference week number 2534 | ionoutc.wnt = gtmp.week; 2535 | ionoutc.tot = (int) gtmp.sec; 2536 | 2537 | // Iono/UTC parameters may no longer valid 2538 | //ionoutc.vflg = FALSE; 2539 | 2540 | // Overwrite the TOC and TOE to the scenario start time 2541 | for (sv = 0; sv < MAX_SAT; sv++) { 2542 | for (i = 0; i < neph; i++) { 2543 | if (eph[i][sv].vflg == true) { 2544 | gtmp = incGpsTime(eph[i][sv].toc, dsec); 2545 | gps2date(>mp, &ttmp); 2546 | eph[i][sv].toc = gtmp; 2547 | eph[i][sv].t = ttmp; 2548 | 2549 | gtmp = incGpsTime(eph[i][sv].toe, dsec); 2550 | eph[i][sv].toe = gtmp; 2551 | } 2552 | } 2553 | } 2554 | } else { 2555 | if (subGpsTime(g0, gmin) < 0.0 || subGpsTime(gmax, g0) < 0.0) { 2556 | fprintf(stderr, "ERROR: Invalid start time.\n"); 2557 | fprintf(stderr, "tmin = %4d/%02d/%02d,%02d:%02d:%02.0f (%d:%.0f)\n", 2558 | tmin.y, tmin.m, tmin.d, tmin.hh, tmin.mm, tmin.sec, 2559 | gmin.week, gmin.sec); 2560 | fprintf(stderr, "tmax = %4d/%02d/%02d,%02d:%02d:%02.0f (%d:%.0f)\n", 2561 | tmax.y, tmax.m, tmax.d, tmax.hh, tmax.mm, tmax.sec, 2562 | gmax.week, gmax.sec); 2563 | exit(1); 2564 | } 2565 | } 2566 | } else { 2567 | g0 = gmin; 2568 | t0 = tmin; 2569 | } 2570 | 2571 | fprintf(stderr, "Gain: %.1fdB\n", plutotx.gain_db); 2572 | fprintf(stderr, "RINEX date = %s\n", rinex_date); 2573 | fprintf(stderr, "Start time = %4d/%02d/%02d,%02d:%02d:%02.0f (%d:%.0f)\n", 2574 | t0.y, t0.m, t0.d, t0.hh, t0.mm, t0.sec, g0.week, g0.sec); 2575 | 2576 | // Select the current set of ephemerides 2577 | ieph = -1; 2578 | 2579 | for (i = 0; i < neph; i++) { 2580 | for (sv = 0; sv < MAX_SAT; sv++) { 2581 | if (eph[i][sv].vflg == true) { 2582 | dt = subGpsTime(g0, eph[i][sv].toc); 2583 | if (dt >= -SECONDS_IN_HOUR && dt < SECONDS_IN_HOUR) { 2584 | ieph = i; 2585 | break; 2586 | } 2587 | } 2588 | } 2589 | 2590 | if (ieph >= 0) // ieph has been set 2591 | break; 2592 | } 2593 | 2594 | if (ieph == -1) { 2595 | fprintf(stderr, "ERROR: No current set of ephemerides has been found.\n"); 2596 | exit(1); 2597 | } 2598 | 2599 | //////////////////////////////////////////////////////////// 2600 | // Baseband signal buffer and output file 2601 | //////////////////////////////////////////////////////////// 2602 | 2603 | // Allocate I/Q buffer 2604 | iq_buff = calloc(NUM_SAMPLES, 4); 2605 | 2606 | if (iq_buff == NULL) { 2607 | fprintf(stderr, "ERROR: Faild to allocate 16-bit I/Q buffer.\n"); 2608 | goto exit_main_thread; 2609 | } 2610 | 2611 | //////////////////////////////////////////////////////////// 2612 | // Start ADALM-Pluto TX thread 2613 | //////////////////////////////////////////////////////////// 2614 | pthread_create(&pluto_thread, NULL, pluto_tx_thread_ep, NULL); 2615 | 2616 | //////////////////////////////////////////////////////////// 2617 | // Initialize channels 2618 | //////////////////////////////////////////////////////////// 2619 | 2620 | // Clear all channels 2621 | for (i = 0; i < MAX_CHAN; i++) 2622 | chan[i].prn = 0; 2623 | 2624 | // Clear satellite allocation flag 2625 | for (sv = 0; sv < MAX_SAT; sv++) 2626 | allocatedSat[sv] = -1; 2627 | 2628 | // Initial reception time 2629 | grx = incGpsTime(g0, 0.0); 2630 | 2631 | // Allocate visible satellites 2632 | allocateChannel(chan, eph[ieph], ionoutc, grx, xyz[0], elvmask); 2633 | 2634 | fprintf(stderr, "PRN Az El Range Iono\n"); 2635 | for (i = 0; i < MAX_CHAN; i++) { 2636 | if (chan[i].prn > 0) 2637 | fprintf(stderr, "%02d %6.1f %5.1f %11.1f %5.1f\n", chan[i].prn, 2638 | chan[i].azel[0] * R2D, chan[i].azel[1] * R2D, chan[i].rho0.d, chan[i].rho0.iono_delay); 2639 | } 2640 | 2641 | //////////////////////////////////////////////////////////// 2642 | // Receiver antenna gain pattern 2643 | //////////////////////////////////////////////////////////// 2644 | 2645 | for (i = 0; i < 37; i++) 2646 | ant_pat[i] = pow(10.0, -ant_pat_db[i] / 20.0); 2647 | 2648 | //////////////////////////////////////////////////////////// 2649 | // Generate baseband signals 2650 | //////////////////////////////////////////////////////////// 2651 | 2652 | // Update receiver time 2653 | grx = incGpsTime(grx, 0.1); 2654 | 2655 | while (!plutotx.exit) { 2656 | for (i = 0; i < MAX_CHAN; i++) { 2657 | if (chan[i].prn > 0) { 2658 | // Refresh code phase and data bit counters 2659 | range_t rho; 2660 | sv = chan[i].prn - 1; 2661 | 2662 | // Current pseudorange 2663 | if (!staticLocationMode) { 2664 | computeRange(&rho, eph[ieph][sv], &ionoutc, grx, xyz[iumd]); 2665 | } else { 2666 | computeRange(&rho, eph[ieph][sv], &ionoutc, grx, xyz[0]); 2667 | } 2668 | 2669 | chan[i].azel[0] = rho.azel[0]; 2670 | chan[i].azel[1] = rho.azel[1]; 2671 | 2672 | // Update code phase and data bit counters 2673 | computeCodePhase(&chan[i], rho, 0.1); 2674 | #ifndef FLOAT_CARR_PHASE 2675 | chan[i].carr_phasestep = (int) round(512.0 * 65536.0 * chan[i].f_carr * delt); 2676 | #endif 2677 | // Path loss 2678 | path_loss = 20200000.0 / rho.d; 2679 | 2680 | // Receiver antenna gain 2681 | ibs = (int) ((90.0 - rho.azel[1] * R2D) / 5.0); // covert elevation to boresight 2682 | ant_gain = ant_pat[ibs]; 2683 | 2684 | // Signal gain 2685 | gain[i] = (double) (path_loss * ant_gain); 2686 | } 2687 | } 2688 | 2689 | pthread_mutex_lock(&plutotx.data_mutex); 2690 | for (isamp = 0; isamp < NUM_SAMPLES; isamp++) { 2691 | int64_t i_acc = 0; 2692 | int64_t q_acc = 0; 2693 | 2694 | for (i = 0; i < MAX_CHAN; i++) { 2695 | if (chan[i].prn > 0) { 2696 | #ifdef FLOAT_CARR_PHASE 2697 | iTable = (int) floor(chan[i].carr_phase * 512.0); 2698 | #else 2699 | iTable = (chan[i].carr_phase >> 16) & 0x1ff; // 9-bit index 2700 | #endif 2701 | ip = chan[i].dataBit * chan[i].codeCA * cosTable512[iTable] * gain[i]; 2702 | qp = chan[i].dataBit * chan[i].codeCA * sinTable512[iTable] * gain[i]; 2703 | 2704 | // Accumulate for all visible satellites 2705 | i_acc += ip; 2706 | q_acc += qp; 2707 | 2708 | // Update code phase 2709 | chan[i].code_phase += chan[i].f_code * delt; 2710 | 2711 | if (chan[i].code_phase >= CA_SEQ_LEN) { 2712 | chan[i].code_phase -= CA_SEQ_LEN; 2713 | 2714 | chan[i].icode++; 2715 | 2716 | if (chan[i].icode >= 20) // 20 C/A codes = 1 navigation data bit 2717 | { 2718 | chan[i].icode = 0; 2719 | chan[i].ibit++; 2720 | 2721 | if (chan[i].ibit >= 30) // 30 navigation data bits = 1 word 2722 | { 2723 | chan[i].ibit = 0; 2724 | chan[i].iword++; 2725 | /* 2726 | if (chan[i].iword>=N_DWRD) 2727 | fprintf(stderr, "\nWARNING: Subframe word buffer overflow.\n"); 2728 | */ 2729 | } 2730 | 2731 | // Set new navigation data bit 2732 | chan[i].dataBit = (int) ((chan[i].dwrd[chan[i].iword]>>(29 - chan[i].ibit)) & 0x1UL)*2 - 1; 2733 | } 2734 | } 2735 | 2736 | // Set current code chip 2737 | chan[i].codeCA = chan[i].ca[(int) chan[i].code_phase]*2 - 1; 2738 | 2739 | // Update carrier phase 2740 | #ifdef FLOAT_CARR_PHASE 2741 | chan[i].carr_phase += chan[i].f_carr * delt; 2742 | 2743 | if (chan[i].carr_phase >= 1.0) 2744 | chan[i].carr_phase -= 1.0; 2745 | else if (chan[i].carr_phase < 0.0) 2746 | chan[i].carr_phase += 1.0; 2747 | #else 2748 | chan[i].carr_phase += chan[i].carr_phasestep; 2749 | #endif 2750 | } 2751 | } 2752 | 2753 | // Store I/Q samples into buffer 2754 | iq_buff[isamp * 2] = (short) i_acc; 2755 | iq_buff[isamp * 2 + 1] = (short) q_acc; 2756 | } 2757 | pthread_cond_signal(&plutotx.data_cond); 2758 | pthread_cond_wait(&plutotx.data_cond, &plutotx.data_mutex); 2759 | pthread_mutex_unlock(&plutotx.data_mutex); 2760 | 2761 | // 2762 | // Update navigation message and channel allocation every 30 seconds 2763 | // 2764 | igrx = (int) (grx.sec * 10.0 + 0.5); 2765 | 2766 | if (igrx % 300 == 0) // Every 30 seconds 2767 | { 2768 | // Update navigation message 2769 | for (i = 0; i < MAX_CHAN; i++) { 2770 | if (chan[i].prn > 0) 2771 | generateNavMsg(grx, &chan[i], 0); 2772 | } 2773 | 2774 | // Refresh ephemeris and subframes 2775 | // Quick and dirty fix. Need more elegant way. 2776 | for (sv = 0; sv < MAX_SAT; sv++) { 2777 | if (eph[ieph + 1][sv].vflg == true) { 2778 | dt = subGpsTime(eph[ieph + 1][sv].toc, grx); 2779 | if (dt < SECONDS_IN_HOUR) { 2780 | ieph++; 2781 | 2782 | for (i = 0; i < MAX_CHAN; i++) { 2783 | // Generate new subframes if allocated 2784 | if (chan[i].prn != 0) 2785 | eph2sbf(eph[ieph][chan[i].prn - 1], ionoutc, chan[i].sbf); 2786 | } 2787 | } 2788 | break; 2789 | } 2790 | } 2791 | 2792 | // Update channel allocation 2793 | if (!staticLocationMode) { 2794 | allocateChannel(chan, eph[ieph], ionoutc, grx, xyz[iumd], elvmask); 2795 | } else { 2796 | allocateChannel(chan, eph[ieph], ionoutc, grx, xyz[0], elvmask); 2797 | } 2798 | } 2799 | // Update receiver time 2800 | grx = incGpsTime(grx, 0.1); 2801 | // update postition index 2802 | iumd++; 2803 | if (iumd >= numd) { 2804 | iumd = 0; 2805 | } 2806 | } 2807 | 2808 | exit_main_thread: 2809 | pthread_mutex_unlock(&plutotx.data_mutex); 2810 | plutotx.exit = true; 2811 | pthread_join(pluto_thread, NULL); /* Wait on Pluto TX thread exit */ 2812 | pthread_mutex_destroy(&plutotx.data_mutex); 2813 | 2814 | // Free I/Q buffers 2815 | if (iq_buff) { 2816 | free(iq_buff); 2817 | } 2818 | return (0); 2819 | } 2820 | -------------------------------------------------------------------------------- /plutogpssim.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of the pluto-gps-sim project at 3 | * https://github.com/mictronics/pluto-gps-sim.git 4 | * 5 | * Copyright © 2015-2018 Mictronics 6 | * Distributed under the MIT License. 7 | * 8 | */ 9 | #ifndef PLUTOGPSSIM_H 10 | #define PLUTOGPSSIM_H 11 | 12 | #define FLOAT_CARR_PHASE // For RKT simulation. Higher computational load, but smoother carrier phase. 13 | 14 | /*! \brief Maximum length of a line in a text file (RINEX, motion) */ 15 | #define MAX_CHAR (100) 16 | 17 | /*! \brief Maximum number of satellites in RINEX file */ 18 | #define MAX_SAT (32) 19 | 20 | /*! \brief Maximum number of channels we simulate */ 21 | #define MAX_CHAN (12) 22 | 23 | /*! \brief Maximum number of user motion points */ 24 | #ifndef USER_MOTION_SIZE 25 | #define USER_MOTION_SIZE (3000) // max duration at 10Hz 26 | #endif 27 | 28 | /*! \brief Number of subframes */ 29 | #define N_SBF (5) // 5 subframes per frame 30 | 31 | /*! \brief Number of words per subframe */ 32 | #define N_DWRD_SBF (10) // 10 word per subframe 33 | 34 | /*! \brief Number of words */ 35 | #define N_DWRD ((N_SBF+1)*N_DWRD_SBF) // Subframe word buffer size 36 | 37 | /*! \brief C/A code sequence length */ 38 | #define CA_SEQ_LEN (1023) 39 | 40 | #define SECONDS_IN_WEEK 604800.0 41 | #define SECONDS_IN_HALF_WEEK 302400.0 42 | #define SECONDS_IN_DAY 86400.0 43 | #define SECONDS_IN_HOUR 3600.0 44 | #define SECONDS_IN_MINUTE 60.0 45 | 46 | #define POW2_M5 0.03125 47 | #define POW2_M19 1.907348632812500e-6 48 | #define POW2_M29 1.862645149230957e-9 49 | #define POW2_M31 4.656612873077393e-10 50 | #define POW2_M33 1.164153218269348e-10 51 | #define POW2_M43 1.136868377216160e-13 52 | #define POW2_M55 2.775557561562891e-17 53 | 54 | #define POW2_M50 8.881784197001252e-016 55 | #define POW2_M30 9.313225746154785e-010 56 | #define POW2_M27 7.450580596923828e-009 57 | #define POW2_M24 5.960464477539063e-008 58 | 59 | // Conventional values employed in GPS ephemeris model (ICD-GPS-200) 60 | #define GM_EARTH 3.986005e14 61 | #define OMEGA_EARTH 7.2921151467e-5 62 | #define PI 3.1415926535898 63 | 64 | #define WGS84_RADIUS 6378137.0 65 | #define WGS84_ECCENTRICITY 0.0818191908426 66 | 67 | #define R2D 57.2957795131 68 | 69 | #define SPEED_OF_LIGHT 2.99792458e8 70 | #define LAMBDA_L1 0.190293672798365 71 | 72 | /*! \brief GPS L1 Carrier frequency */ 73 | #define CARR_FREQ (1575.42e6) 74 | /*! \brief C/A code frequency */ 75 | #define CODE_FREQ (1.023e6) 76 | #define CARR_TO_CODE (1.0/1540.0) 77 | 78 | #define EPHEM_ARRAY_SIZE (13) // for daily GPS broadcast ephemeris file (brdc) 79 | 80 | /*! \brief Structure representing GPS time */ 81 | typedef struct { 82 | int week; /*!< GPS week number (since January 1980) */ 83 | double sec; /*!< second inside the GPS \a week */ 84 | } gpstime_t; 85 | 86 | /*! \brief Structure representing UTC time */ 87 | typedef struct { 88 | int y; /*!< Calendar year */ 89 | int m; /*!< Calendar month */ 90 | int d; /*!< Calendar day */ 91 | int hh; /*!< Calendar hour */ 92 | int mm; /*!< Calendar minutes */ 93 | double sec; /*!< Calendar seconds */ 94 | } datetime_t; 95 | 96 | /*! \brief Structure representing ephemeris of a single satellite */ 97 | typedef struct { 98 | bool vflg; /*!< Valid Flag */ 99 | datetime_t t; 100 | gpstime_t toc; /*!< Time of Clock */ 101 | gpstime_t toe; /*!< Time of Ephemeris */ 102 | int iodc; /*!< Issue of Data, Clock */ 103 | int iode; /*!< Isuse of Data, Ephemeris */ 104 | double deltan; /*!< Delta-N (radians/sec) */ 105 | double cuc; /*!< Cuc (radians) */ 106 | double cus; /*!< Cus (radians) */ 107 | double cic; /*!< Correction to inclination cos (radians) */ 108 | double cis; /*!< Correction to inclination sin (radians) */ 109 | double crc; /*!< Correction to radius cos (meters) */ 110 | double crs; /*!< Correction to radius sin (meters) */ 111 | double ecc; /*!< e Eccentricity */ 112 | double sqrta; /*!< sqrt(A) (sqrt(m)) */ 113 | double m0; /*!< Mean anamoly (radians) */ 114 | double omg0; /*!< Longitude of the ascending node (radians) */ 115 | double inc0; /*!< Inclination (radians) */ 116 | double aop; 117 | double omgdot; /*!< Omega dot (radians/s) */ 118 | double idot; /*!< IDOT (radians/s) */ 119 | double af0; /*!< Clock offset (seconds) */ 120 | double af1; /*!< rate (sec/sec) */ 121 | double af2; /*!< acceleration (sec/sec^2) */ 122 | double tgd; /*!< Group delay L2 bias */ 123 | int svhlth; 124 | int codeL2; 125 | // Working variables follow 126 | double n; /*!< Mean motion (Average angular velocity) */ 127 | double sq1e2; /*!< sqrt(1-e^2) */ 128 | double A; /*!< Semi-major axis */ 129 | double omgkdot; /*!< OmegaDot-OmegaEdot */ 130 | } ephem_t; 131 | 132 | typedef struct { 133 | bool enable; 134 | bool vflg; 135 | double alpha0, alpha1, alpha2, alpha3; 136 | double beta0, beta1, beta2, beta3; 137 | double A0, A1; 138 | int dtls, tot, wnt; 139 | int dtlsf, dn, wnlsf; 140 | } ionoutc_t; 141 | 142 | typedef struct { 143 | gpstime_t g; 144 | double range; // pseudorange 145 | double rate; 146 | double d; // geometric distance 147 | double azel[2]; 148 | double iono_delay; 149 | } range_t; 150 | 151 | /*! \brief Structure representing a Channel */ 152 | typedef struct { 153 | int prn; /*< PRN Number */ 154 | int ca[CA_SEQ_LEN]; /*< C/A Sequence */ 155 | double f_carr; /*< Carrier frequency */ 156 | double f_code; /*< Code frequency */ 157 | #ifdef FLOAT_CARR_PHASE 158 | double carr_phase; 159 | #else 160 | unsigned int carr_phase; /*< Carrier phase */ 161 | int carr_phasestep; /*< Carrier phasestep */ 162 | #endif 163 | double code_phase; /*< Code phase */ 164 | gpstime_t g0; /*!< GPS time at start */ 165 | unsigned long sbf[5][N_DWRD_SBF]; /*!< current subframe */ 166 | unsigned long dwrd[N_DWRD]; /*!< Data words of sub-frame */ 167 | int iword; /*!< initial word */ 168 | int ibit; /*!< initial bit */ 169 | int icode; /*!< initial code */ 170 | int dataBit; /*!< current data bit */ 171 | int codeCA; /*!< current C/A code */ 172 | double azel[2]; 173 | range_t rho0; 174 | } channel_t; 175 | 176 | /* Structure represending a single GPS monitoring station. */ 177 | typedef struct { 178 | const char *id_v2; 179 | const char *id_v3; 180 | const char *name; 181 | } stations_t; 182 | 183 | /** 184 | * Stations providing Rinex v3 format. 185 | * 4-characters station ID 186 | * 9-characters station ID 187 | * Station name 188 | * Including Ionosphere data in Rinex file 189 | */ 190 | const stations_t stations_v3[] = { 191 | {"func", "FUNC00PRT", "Funchal"}, 192 | {"flrs", "FLRS00PRT", "Santa Cruz das Flore"}, 193 | {"pdel", "PDEL00PRT", "PONTA DELGADA"} 194 | }; 195 | 196 | /** 197 | * Stations providing Rinex v2 format. 198 | * 4-characters station ID 199 | * 9-characters station ID 200 | * Station name 201 | * Including Ionosphere data in Rinex file 202 | */ 203 | const stations_t stations_v2[] = { 204 | {"abmf", "ABMF00GLP", "Aeroport du Raizet"}, 205 | {"aggo", "AGGO00ARG", "AGGO"}, 206 | {"ajac", "AJAC00FRA", "Ajaccio"}, 207 | {"ankr", "ANKR00TUR", "Ankara"}, 208 | {"areg", "AREG00PER", "Arequipa"}, 209 | {"ascg", "ASCG00SHN", "Ascension"}, 210 | {"bogi", "BOGI00POL", "Borowa Gora"}, 211 | {"bor1", "BOR100POL", "Borowiec"}, 212 | {"brst", "BRST00FRA", "Brest"}, 213 | {"chpg", "CHPG00BRA", "Cachoeira Paulista"}, 214 | {"cibg", "CIBG00IDN", "Cibinong"}, 215 | {"cpvg", "CPVG00CPV", "CAP-VERT"}, 216 | {"djig", "DJIG00DJI", "Djibouti"}, 217 | {"dlf1", "DLF100NLD", "Delft"}, 218 | {"ffmj", "FFMJ00DEU", "Frankfurt/Main"}, 219 | {"ftna", "FTNA00WLF", "Futuna"}, 220 | {"gamb", "GAMB00PYF", "Rikitea"}, 221 | {"gamg", "GAMG00KOR", "Geochang"}, 222 | {"glps", "GLPS00ECU", "Galapagos Permanent Station"}, 223 | {"glsv", "GLSV00UKR", "Kiev/Golosiiv"}, 224 | {"gmsd", "GMSD00JPN", "GUTS Masda"}, 225 | {"gop6", "GOP600CZE", "Pecny, Ondrejov"}, 226 | {"gop7", "GOP700CZE", "Pecny, Ondrejov"}, 227 | {"gope", "GOPE00CZE", "Pecny, Ondrejov"}, 228 | {"grac", "GRAC00FRA", "Grasse"}, 229 | {"gras", "GRAS00FRA", "Observatoire de Calern - OCA"}, 230 | {"holb", "HOLB00CAN", "Holberg"}, 231 | {"hueg", "HUEG00DEU", "Huegelheim"}, 232 | {"ieng", "IENG00ITA", "Torino"}, 233 | {"ista", "ISTA00TUR", "Istanbul"}, 234 | {"izmi", "IZMI00TUR", "Izmir"}, 235 | {"jfng", "JFNG00CHN", "Juifeng"}, 236 | {"joz2", "JOZ200POL", "Jozefoslaw"}, 237 | {"joze", "JOZE00POL", "Jozefoslaw"}, 238 | {"kerg", "KERG00ATF", "Kerguelen Islands"}, 239 | {"kitg", "KITG00UZB", "Kitab"}, 240 | {"koug", "KOUG00GUF", "Kourou"}, 241 | {"krgg", "KRGG00ATF", "Kerguelen Islands"}, 242 | {"krs1", "KRS100TUR", "Kars"}, 243 | {"lama", "LAMA00POL", "Lamkowo"}, 244 | {"leij", "LEIJ00DEU", "Leipzig"}, 245 | {"lmmf", "LMMF00MTQ", "Aeroport Aime CESAIRE-LE LAMENTIN"}, 246 | {"lroc", "LROC00FRA", "La Rochelle"}, 247 | {"mad2", "MAD200ESP", "Madrid Deep Space Tracking Station"}, 248 | {"madr", "MADR00ESP", "Madrid Deep Space Tracking Station"}, 249 | {"mayg", "MAYG00MYT", "Dzaoudzi"}, 250 | {"mers", "MERS00TUR", "Mersin"}, 251 | {"mikl", "MIKL00UKR", "Mykolaiv"}, 252 | {"morp", "MORP00GBR", "Morpeth"}, 253 | {"nklg", "NKLG00GAB", "N'KOLTANG"}, 254 | {"nyal", "NYAL00NOR", "Ny-Alesund"}, 255 | {"nya1", "NYA100NOR", "Ny-Alesund"}, 256 | {"ohi2", "OHI200ATA", "O'Higgins"}, 257 | {"orid", "ORID00MKD", "Ohrid"}, 258 | {"owmg", "OWMG00NZL", "Chatham Island"}, 259 | {"polv", "POLV00UKR", "Poltava"}, 260 | {"ptbb", "PTBB00DEU", "Braunschweig"}, 261 | {"ptgg", "PTGG00PHL", "Manilla"}, 262 | {"rabt", "RABT00MAR", "Rabat, EMI"}, 263 | {"reun", "REUN00REU", "La Reunion - Observatoire Volcanologique"}, 264 | {"rgdg", "RGDG00ARG", "Rio Grande"}, 265 | {"riga", "RIGA00LVA", "RIGA permanent GPS"}, 266 | {"seyg", "SEYG00SYC", "Mahe"}, 267 | {"sofi", "SOFI00BGR", "Sofia"}, 268 | {"stj3", "STJ300CAN", "STJ3 CACS-GSD"}, 269 | {"sulp", "SULP00UKR", "Lviv Polytechnic"}, 270 | {"svtl", "SVTL00RUS", "Svetloe"}, 271 | {"tana", "TANA00ETH", "ILA, Bahir Dar University"}, 272 | {"thtg", "THTG00PYF", "Papeete Tahiti"}, 273 | {"thti", "THTI00PYF", "Tahiti"}, 274 | {"tit2", "TIT200DEU", "Titz / Jackerath"}, 275 | {"tlse", "TLSE00FRA", "Toulouse"}, 276 | {"tro1", "TRO100NOR", "Tromsoe"}, 277 | {"warn", "WARN00DEU", "Warnemuende"}, 278 | {"whit", "WHIT00CAN", "WHIT CACS-GSD"}, 279 | {"wroc", "WROC00POL", "Wroclaw"}, 280 | {"wtza", "WTZA00DEU", "Wettzell"}, 281 | {"yel2", "YEL200CAN", "Yellow Knife"}, 282 | {"zeck", "ZECK00RUS", "Zelenchukskaya"}, 283 | {"zim2", "ZIM200CHE", "Zimmerwald"}, 284 | {"zimm", "ZIMM00CHE", "Zimmerwald L+T 88"}, 285 | }; 286 | 287 | #endif 288 | --------------------------------------------------------------------------------