diff --git a/CMakeLists.txt b/CMakeLists.txt index f6114797b..c3994de90 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -271,6 +271,14 @@ set(CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} CACHE STRING "") # Append -O2 optimization flag for Debug builds set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O2") +# allow 'large' files in 32 bit builds +if(UNIX) + add_definitions( -D_LARGEFILE_SOURCE + -D_FILE_OFFSET_BITS=64 + -D_LARGE_FILES + ) +endif(UNIX) + ################################################################################ @@ -331,7 +339,6 @@ if(OS_IS_LINUX) endif(OS_IS_LINUX) - ################################################################################ # Googletest - https://github.com/google/googletest ################################################################################ @@ -508,9 +515,9 @@ if(NOT VOLK_GNSSSDR_FOUND) set(CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") set(C_FLAGS "${CMAKE_C_FLAGS} -std=c11") - if(CMAKE_CROSSCOMPILING) - set(VOLK_GNSSSDR_COMPILER "") - else(CMAKE_CROSSCOMPILING) + if(CMAKE_CROSSCOMPILING) + set(VOLK_GNSSSDR_COMPILER "") + else(CMAKE_CROSSCOMPILING) set(VOLK_GNSSSDR_COMPILER -DCMAKE_C_COMPILER=${CMAKE_C_COMPILER} -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER}) endif(CMAKE_CROSSCOMPILING) set(VOLK_GNSSSDR_CMAKE_ARGS ${VOLK_GNSSSDR_COMPILER} @@ -520,11 +527,11 @@ if(NOT VOLK_GNSSSDR_FOUND) -DCMAKE_CXX_FLAGS=${CXX_FLAGS} -DCMAKE_C_FLAGS=${C_FLAGS} -DCMAKE_INCLUDE_PATH=${Boost_INCLUDE_DIR} + -DENABLE_ORC=OFF ${STRIP_VOLK_GNSSSDR_PROFILE} ${USE_THIS_PYTHON} ) if(EXISTS $ENV{OECORE_TARGET_SYSROOT}) - set(VOLK_GNSSSDR_CMAKE_ARGS ${VOLK_GNSSSDR_CMAKE_ARGS} - -DENABLE_ORC=OFF + set(VOLK_GNSSSDR_CMAKE_ARGS ${VOLK_GNSSSDR_CMAKE_ARGS} -DCMAKE_TOOLCHAIN_FILE=${CMAKE_CURRENT_SOURCE_DIR}/cmake/Toolchains/oe-sdk_cross.cmake -DCROSSCOMPILE_MULTILIB=TRUE ) endif(EXISTS $ENV{OECORE_TARGET_SYSROOT}) @@ -672,7 +679,7 @@ ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/glog/glog-${glog_RELEASE}/configure") GROUP_EXECUTE WORLD_READ WORLD_EXECUTE) set(GLOG_CONFIGURE ${CMAKE_CURRENT_BINARY_DIR}/glog-${glog_RELEASE}/configure_with_gflags) - + # Ensure that aclocal and libtool are present if(OS_IS_LINUX) if(EXISTS "/usr/bin/libtoolize") @@ -882,28 +889,19 @@ endif(OS_IS_LINUX) find_package(Armadillo) if(NOT ARMADILLO_FOUND OR ENABLE_OWN_ARMADILLO) message(STATUS " Armadillo has not been found.") - message(STATUS " Armadillo will be downloaded and built automatically ") + message(STATUS " Armadillo will be downloaded and built automatically") message(STATUS " when doing 'make'. ") - - if(CMAKE_VERSION VERSION_LESS 3.1) - # ExternalProject in CMake > 3.1 cannot open .xz files - set(armadillo_RELEASE 6.700.7) - set(armadillo_MD5 "8116185e1d7391eed3bf6c500f81b4d8") - set(ARMA_FILE_EXTENSION "gz") - else(CMAKE_VERSION VERSION_LESS 3.1) - set(armadillo_RELEASE 7.800.1) - set(armadillo_MD5 "e094351771c40a6e06e1a9c1ffdfc2b8") - set(ARMA_FILE_EXTENSION "xz") - endif(CMAKE_VERSION VERSION_LESS 3.1) + set(armadillo_BRANCH 7.800.x) + set(armadillo_RELEASE ${armadillo_BRANCH}) ExternalProject_Add( armadillo-${armadillo_RELEASE} PREFIX ${CMAKE_CURRENT_BINARY_DIR}/armadillo-${armadillo_RELEASE} - URL http://sourceforge.net/projects/arma/files/armadillo-${armadillo_RELEASE}.tar.${ARMA_FILE_EXTENSION} - DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/download/armadillo-${armadillo_RELEASE} - URL_MD5 ${armadillo_MD5} - CMAKE_ARGS -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER} -DBUILD_SHARED_LIBS=OFF - BUILD_IN_SOURCE 1 + GIT_REPOSITORY https://github.com/conradsnicta/armadillo-code.git + GIT_TAG ${armadillo_BRANCH} + SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/armadillo/armadillo-${armadillo_RELEASE} + BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/armadillo-${armadillo_RELEASE} + CMAKE_ARGS -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER} -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS=-std=c++11 BUILD_COMMAND make UPDATE_COMMAND "" INSTALL_COMMAND "" @@ -911,7 +909,7 @@ if(NOT ARMADILLO_FOUND OR ENABLE_OWN_ARMADILLO) # Set up variables ExternalProject_Get_Property(armadillo-${armadillo_RELEASE} binary_dir) - set(ARMADILLO_INCLUDE_DIRS ${binary_dir}/include ) + set(ARMADILLO_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/armadillo/armadillo-${armadillo_RELEASE}/include ) find_library(LAPACK NAMES lapack HINTS /usr/lib /usr/local/lib /usr/lib64) if(OS_IS_MACOSX) find_library(BLAS blas) @@ -925,10 +923,6 @@ if(NOT ARMADILLO_FOUND OR ENABLE_OWN_ARMADILLO) endif(NOT GFORTRAN) set(ARMADILLO_LIBRARIES ${BLAS} ${LAPACK} ${GFORTRAN} ${binary_dir}/${CMAKE_FIND_LIBRARY_PREFIXES}armadillo.a) set(LOCAL_ARMADILLO true CACHE STRING "Armadillo downloaded and built automatically" FORCE) - # Save a copy at the thirdparty folder - file(COPY ${CMAKE_CURRENT_BINARY_DIR}/armadillo-${armadillo_RELEASE} - DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/armadillo - ) set(ARMADILLO_VERSION_STRING ${armadillo_RELEASE}) else(NOT ARMADILLO_FOUND OR ENABLE_OWN_ARMADILLO) set(armadillo_RELEASE ${ARMADILLO_VERSION_STRING}) @@ -1180,8 +1174,17 @@ if($ENV{RTLSDR_DRIVER}) endif($ENV{RTLSDR_DRIVER}) if(ENABLE_OSMOSDR) - message(STATUS "The driver for OsmoSDR and other front-ends (HackRF, bladeRF, Realtek's RTL2832U-based dongles, etc.) will be compiled." ) - message(STATUS "You can disable it with 'cmake -DENABLE_OSMOSDR=OFF ../'" ) + find_package(GrOsmoSDR) + if(GROSMOSDR_FOUND) + message(STATUS "The driver for OsmoSDR and other front-ends (HackRF, bladeRF, Realtek's RTL2832U-based dongles, etc.) will be compiled." ) + message(STATUS "You can disable it with 'cmake -DENABLE_OSMOSDR=OFF ../'" ) + else(GROSMOSDR_FOUND) + if(ENABLE_PACKAGING) + message(WARNING "gr-osmosdr has not been found. Source blocks depending on it will NOT be built.") + else(ENABLE_PACKAGING) + message(FATAL_ERROR "gr-osmosdr required to build gnss-sdr with the optional OSMOSDR driver") + endif(ENABLE_PACKAGING) + endif(GROSMOSDR_FOUND) else(ENABLE_OSMOSDR) message(STATUS "The (optional) driver for OsmoSDR and related front-ends is not enabled." ) message(STATUS "Enable it with 'cmake -DENABLE_OSMOSDR=ON ../' to add support for OsmoSDR and other front-ends (HackRF, bladeRF, Realtek's RTL2832U-based USB dongles, etc.)" ) @@ -1314,4 +1317,3 @@ add_custom_target(uninstall # Add subdirectories (in order of deps) ######################################################################## add_subdirectory(src) - diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md new file mode 100644 index 000000000..33e45b9d5 --- /dev/null +++ b/CODE_OF_CONDUCT.md @@ -0,0 +1,74 @@ +# Contributor Covenant Code of Conduct + +## Our Pledge + +In the interest of fostering an open and welcoming environment, we as +contributors and maintainers pledge to making participation in our project and +our community a harassment-free experience for everyone, regardless of age, body +size, disability, ethnicity, gender identity and expression, level of experience, +nationality, personal appearance, race, religion, or sexual identity and +orientation. + +## Our Standards + +Examples of behavior that contributes to creating a positive environment +include: + +* Using welcoming and inclusive language +* Being respectful of differing viewpoints and experiences +* Gracefully accepting constructive criticism +* Focusing on what is best for the community +* Showing empathy towards other community members + +Examples of unacceptable behavior by participants include: + +* The use of sexualized language or imagery and unwelcome sexual attention or +advances +* Trolling, insulting/derogatory comments, and personal or political attacks +* Public or private harassment +* Publishing others' private information, such as a physical or electronic + address, without explicit permission +* Other conduct which could reasonably be considered inappropriate in a + professional setting + +## Our Responsibilities + +Project maintainers are responsible for clarifying the standards of acceptable +behavior and are expected to take appropriate and fair corrective action in +response to any instances of unacceptable behavior. + +Project maintainers have the right and responsibility to remove, edit, or +reject comments, commits, code, wiki edits, issues, and other contributions +that are not aligned to this Code of Conduct, or to ban temporarily or +permanently any contributor for other behaviors that they deem inappropriate, +threatening, offensive, or harmful. + +## Scope + +This Code of Conduct applies both within project spaces and in public spaces +when an individual is representing the project or its community. Examples of +representing a project or community include using an official project e-mail +address, posting via an official social media account, or acting as an appointed +representative at an online or offline event. Representation of a project may be +further defined and clarified by project maintainers. + +## Enforcement + +Instances of abusive, harassing, or otherwise unacceptable behavior may be +reported by contacting the project team at carles.fernandez@cttc.es. All +complaints will be reviewed and investigated and will result in a response that +is deemed necessary and appropriate to the circumstances. The project team is +obligated to maintain confidentiality with regard to the reporter of an incident. +Further details of specific enforcement policies may be posted separately. + +Project maintainers who do not follow or enforce the Code of Conduct in good +faith may face temporary or permanent repercussions as determined by other +members of the project's leadership. + +## Attribution + +This Code of Conduct is adapted from the [Contributor Covenant][homepage], version 1.4, +available at [http://contributor-covenant.org/version/1/4][version] + +[homepage]: http://contributor-covenant.org +[version]: http://contributor-covenant.org/version/1/4/ diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 000000000..12cc1328f --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,176 @@ +# Contributing to GNSS-SDR + +:+1::tada: Thanks for taking the time to contribute! :tada::+1: + +Third-party contributions are essential for keeping GNSS-SDR +continuously improving. We simply cannot access the huge number of +platforms and myriad configurations for running GNSS-SDR. We want to +keep it as easy as possible to contribute changes that get things +working in your environment. There are a few guidelines that we need +contributors to follow so that we can have a chance of keeping on top of +things. + +The following is a set of guidelines for contributing to GNSS-SDR, which +is hosted in the [GNSS-SDR Organization](https://github.com/gnss-sdr) on +GitHub. These are just guidelines, not rules. Use your best judgment, +and feel free to propose changes to this document in a [pull +request](#how-to-submit-a-pull-request). + +## Code of Conduct + +This project adheres to the Contributor Covenant [code of +conduct](CODE_OF_CONDUCT.md). By participating, you are expected to +uphold this code. Please report unacceptable behavior. + +## Reporting an issue + +Have you found a bug in the code which is not in the [list of known +bugs](https://github.com/gnss-sdr/gnss-sdr/issues)? Do you have a +suggestion for improvement? Then by all means please [submit a new +issue](https://github.com/gnss-sdr/gnss-sdr/issues/new), and do not +hesitate to comment on existing [open +issues](https://github.com/gnss-sdr/gnss-sdr/issues). + +When filling a new issue, please remember to: + + * **Use a clear and descriptive title** for the issue to identify the +problem. + + * **Describe the exact steps which reproduce the problem** in as many +details as possible. For example, start by describing your computing +platform (Operating System and version, how did you installed GNSS-SDR +and its dependencies, what file or front-end are you using as a signal +source, etc.). You can also include the configuration file you are +using, or a dump of the terminal output you are getting. The more +information you provide, the more chances to get useful answers. + + * **Please be patient**. This organization is run on a volunteer basis, +so it can take some time to the Developer Team to reach your issue. +They will do their best to fix it as soon as possible. + + * If you opened an issue that is now solved, it is a good practice to +**close it**. + +The list of [open issues](https://github.com/gnss-sdr/gnss-sdr/issues) +can be a good starting point and a source of ideas if you are looking to +contribute to the source code. + + +## Contributing to the source code + +### Preliminaries + + 1. If you still have not done so, [create your personal account on +GitHub](https://github.com/join). + + 2. [Fork GNSS-SDR from +GitHub](https://github.com/gnss-sdr/gnss-sdr/fork). This will copy the +whole gnss-sdr repository to your personal account. + + 3. Then, go to your favourite working folder in your computer and +clone your forked repository by typing (replacing ```YOUR_USERNAME``` by +the actual username of your GitHub account): + + $ git clone https://github.com/YOUR_USERNAME/gnss-sdr + + 4. Your forked repository https://github.com/YOUR_USERNAME/gnss-sdr +will receive the default name of `origin`. You can also add the original +gnss-sdr repository, which is usually called `upstream`: + + $ cd gnss-sdr + $ git remote add upstream https://github.com/gnss-sdr/gnss-sdr.git + +To verify the new upstream repository you have specified for your fork, +type `git remote -v`. You should see the URL for your fork as `origin`, +and the URL for the original repository as `upstream`: + +``` +$ git remote -v +origin https://github.com/YOUR_USERNAME/gnss-sdr.git (fetch) +origin https://github.com/YOUR_USERNAME/gnss-sdr.git (push) +upstream https://github.com/gnss-sdr/gnss-sdr.git (fetch) +upstream https://github.com/gnss-sdr/gnss-sdr.git (push) +``` + +### Start working on your contribution + +Checkout the `next` branch of the git repository in order to get +synchronized with the latest development code: + +``` +$ git checkout next +$ git pull upstream next +``` + +When start working in a new improvement, please **always** branch off +from `next`. Open a new branch and start working on it: + +``` +$ git checkout -b my_feature +``` + +Now you can do changes, add files, do commits (please take a look at +[how to write good commit +messages](https://chris.beams.io/posts/git-commit/)!) and push them to +your repository: + +``` +$ git push origin my_feature +``` + +If there have been new pushes to the `next` branch of the `upstream` +repository since the last time you pulled from it, you might want to put +your commits on top of them (this is mandatory for pull requests): + +``` +$ git pull --rebase upstream next +``` + +### How to submit a pull request + +When the contribution is ready, you can [submit a pull +request](https://github.com/gnss-sdr/gnss-sdr/compare/). Head to your +GitHub repository, switch to your `my_feature` branch, and click the +_**Pull Request**_ button, which will do all the work for you. Code +comparison must be always to the `next` branch. + +Once a pull request is sent, the Developer Team can review the set of +changes, discuss potential modifications, and even push follow-up +commits if necessary. + +Some things that will increase the chance that your pull request is +accepted: + + * Avoid platform-dependent code. If your code require external + dependencies, they must be available as packages in [Debian OldStable](https://wiki.debian.org/DebianOldStable). + * Write tests. + * Follow our [coding style guide](http://gnss-sdr.org/coding-style/). + * Write a descriptive and detailed summary. Please consider that +reviewing pull requests is hard, so include as much information as +possible to make your pull request's intent clear. + +For more details about Git usage, please check out [our +tutorial](http://gnss-sdr.org/docs/tutorials/using-git/). + + +## Contributing to the website + +The content of http://gnss-sdr.org lives in a GitHub repository at +https://github.com/gnss-sdr/geniuss-place + +You can fork that repository, reproduce the entire website on your +computer using [Jekyll](https://jekyllrb.com/), do changes and submit +pull requests, just as explained above. For more details, please check +out [how to contribute](http://gnss-sdr.org/contribute/). + +Last but not the least, you can leave your comments on the website. + + +------ + + + +![GeNiuSS +contributes](http://gnss-sdr.org/assets/images/geniuss-contribute.png) + +Thanks for your contribution to GNSS-SDR! diff --git a/README.md b/README.md index 178dc1b60..7fe3332e3 100644 --- a/README.md +++ b/README.md @@ -129,9 +129,9 @@ or manually as explained below, and then please follow instructions on how to [d $ sudo apt-get install libopenblas-dev liblapack-dev # For Debian/Ubuntu/LinuxMint $ sudo yum install lapack-devel blas-devel # For Fedora/CentOS/RHEL $ sudo zypper install lapack-devel blas-devel # For OpenSUSE -$ wget http://sourceforge.net/projects/arma/files/armadillo-7.800.1.tar.xz -$ tar xvfz armadillo-7.800.1.tar.xz -$ cd armadillo-7.800.1 +$ wget http://sourceforge.net/projects/arma/files/armadillo-7.800.2.tar.xz +$ tar xvfz armadillo-7.800.2.tar.xz +$ cd armadillo-7.800.2 $ cmake . $ make $ sudo make install diff --git a/conf/gnss-sdr_Hybrid_byte_sim.conf b/conf/gnss-sdr_Hybrid_byte_sim.conf index c5c594e52..40120b2c3 100644 --- a/conf/gnss-sdr_Hybrid_byte_sim.conf +++ b/conf/gnss-sdr_Hybrid_byte_sim.conf @@ -17,7 +17,7 @@ SignalSource.implementation=File_Signal_Source ;#filename: path to file with the captured GNSS signal samples to be processed ;#SignalSource.filename=/home/javier/Descargas/rtlsdr_tcxo_l1/rtlsdr_tcxo_l1.bin ; <- PUT YOUR FILE HERE -SignalSource.filename=/home/javier/git/gnss-sdr/build/signal_out.bin ; <- PUT YOUR FILE HERE +SignalSource.filename=/Users/carlesfernandez/git/cttc/build/signal_out.bin ; <- PUT YOUR FILE HERE ;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. SignalSource.item_type=byte diff --git a/docs/changelog b/docs/changelog new file mode 100644 index 000000000..3d27ab215 --- /dev/null +++ b/docs/changelog @@ -0,0 +1,258 @@ +## [0.0.9](https://github.com/gnss-sdr/gnss-sdr/releases/tag/v0.0.9) + +DOI: https://doi.org/10.5281/zenodo.291371 + +This release has several improvements, addition of new features and bug fixes in many dimensions: + +### Improvements in Accuracy: + +- Major rewriting in the generation of pseudoranges. +- Fixed bug in Galileo E5a/I codes. +- Fixed bug in Galileo E1 correlator spacing. +- Fixed bug that was causing errors in receivers above the troposphere. +- Fixed 16-bit complex resampler. +- Improved time tracking algorithm. +- Added Bancroft's algorithm implementation for PVT initialization. + + +### Improvements in Availability: + +- Improved numerical stability of the PVT solution. The infamous bug that was causing apparently random error peaks has finally been fixed. + + +### Improvements in Efficiency: + +- VOLK_GNSSSDR: Added NEON,AVX and unaligned protokernels for volk_gnsssdr_32f_index_max_32 kernel. +- VOLK_GNSSSDR: Added volk_gnsssdr-config-info to the list of generated executables. + + +### Improvements in Flexibility: + +- Added maximum number of dwells in the Tong algorithm. + + +### Improvements in Interoperability: + +- Added six new Galileo satellites: FM7, FM10, FM11, FM12, FM13, FM14. +- The Hybrid_Observables and Hybrid_PVT implementations can now handle more types of GNSS signals. +- The RINEX printer can now print L2C and E5a observables and navigation files, including multiband configurations. +- Added RTCM 3.2 output to more receiver configurations. + + +### Improvements in Maintainability: + +- The VOLK_GNSSSDR library can now be built with Python 3. Switched dependencies for VOLK_GNSSDR: from (old, python2.7-only) python-cheetah templates to Python3 friendly python-mako and python-six. So, Python-cheetah dependency has been dropped, and python-mako and python-six have been added. +- If suitable versions of gflags, glog, armadillo or googletest are not found in the system, they will be downloaded and built at compile time (versions 2.2.0, 0.3.4, 7.600.2 and 1.8.0, respectively). +- Fixed more than 30 defects detected by Coverity Scan. +- Added CMake Python finder and module checker. +- Deleted files related to CPack. +- Fixes, updates and improvements in the documentation. +- Improvements in CMake scripts: General code cleaning and addition of comments. Improved user information in case of failure. Improved detection of dependencies in more processor architectures (e.g. aarch64). + + +### Improvements in Marketability: + +- Reduced time from a commit to deployment (see virtualization mechanisms in Portability). + + +### Improvements in Portability: + +- Now GNSS-SDR can be run in virtual environments through snap packages (see https://github.com/carlesfernandez/snapcraft-sandbox) and docker images (see https://github.com/carlesfernandez/docker-gnsssdr). +- Now GNSS-SDR is adapted to cross-compiling environments for embedded devices (see https://github.com/carlesfernandez/oe-gnss-sdr-manifest). +- BLAS and LAPACK libraries are not longer mandatory on ARM devices. + + +### Improvements in Scalability: + +- Fixed bug in acquisition with rata rates higher than 16 Msps in 4ms code periods. + + +### Improvements in Testability: + +- Major QA source code refactoring: they has been split into src/tests/unit-tests and src/tests/system-tests folders. They are optionally built with the ENABLE_UNIT_TESTING=ON (unit testing QA code), ENABLE_UNIT_TESTING_EXTRA=ON (unit tests that require extra files downloaded at configure time), ENABLE_SYSTEM_TESTING=ON (system tests, such as measurement of Time-To-First-Fix) and ENABLE_SYSTEM_TESTING_EXTRA=ON (extra system test requiring external tools, automatically downloaded and built at building time) configuration flags. The EXTRA options also download and build a custom software-defined signal generator and version 2.9 of GPSTk, if not already found on the system. Download and local link of version 2.9 can be forced by ENABLE_OWN_GPSTK=ON building configuration flag. Only ENABLE_UNIT_TESTING is set to ON by default. +- Unit tests added: CPU_multicorrelator_test and GPU_multicorrelator_test measure computer performance in multicorrelator setups. +- Unit tests added: GpsL1CADllPllTracking and GpsL1CATelemetryDecoderTest. +- System test added: ttff_gps_l1 performs a set of cold / assisted runs of the software receiver and computes statistics about the obtained Time To First Fix. +- System test added: obs_gps_l1_system_test uses an external software-defined signal generator to produce raw digital GNSS signal from a RINEX navigation file and a position (static or dynamic), processes it with GNSS-SDR, and then compares the RINEX observation file produced by the software receiver to that produced by the signal generator. +- Software Development Kit provided for embedded devices (see http://gnss-sdr.org/docs/tutorials/cross-compiling/). + +### Improvements in Usability: + +- Now the block factory automatically detects Channel input data type, so it is no longer required to specify Channel.input_type in the configuration. An error raises if Acquisition and Tracking Blocks are not configured with the same input data type. +- Block names changed from L2_M to L2C. +- Documentation available at http://gnss-sdr.org/docs/ +- Improved tools for compilation, execution and testing in embedded devices. + + +See the definitions of concepts and metrics at http://gnss-sdr.org/design-forces/ + + + + +## [0.0.8](https://github.com/gnss-sdr/gnss-sdr/releases/tag/v0.0.8) + +DOI: https://doi.org/10.5281/zenodo.57022 + +This is a maintenance and bug fix release with no relevant new features with respect to v0.0.7. The main changes are: + +- Fixed a bug that broke building when using latest VOLK release +- Updated PYBOMBS instructions +- Added Tests for FFT length +- Added Tests for CUDA-based tracking +- Added Tests for SIMD-based tracking +- Improved CUDA-based correlation. +- Updated documentation +- Fixed building in mips and powerpc architectures. +- gr-gn3s and gr-dbfcttc moved to its own repository. +- Improved package reproducibility +- VOLK_GNSSSDR: Fixed a bug in AVX2 puppet +- VOLK_GNSSSDR: can now be built using the C98 standard +- VOLK_GNSSSDR: Fixed a bug that broke building when linking to Boost in some configurations. +- VOLK_GNSSSDR: Added an option to trigger profiling at building time. +- VOLK_GNSSSDR: Fix the CMake-based check for posix_memalign. + + + + +## [0.0.7](https://github.com/gnss-sdr/gnss-sdr/releases/tag/v0.0.7) + +DOI: https://doi.org/10.5281/zenodo.51521 + +This release has several improvements, addition of new features and bug fixes: + +- Improvements in receiver design: Internal block communication has been redesigned to accommodate the addition of new signals, and now upstream and downstream communication within blocks is implemented through the GNU Radio block’s asynchronous message passing system, leading to a more scalable, more robust and cleaner design. +- Improvements in receiver design: Correlators have been rewritten to take full advantage of VOLK and VOLK_GNSSSDR, and they are of general use for any tracking block. Their API now admit an arbitrary number of correlators, spaced in an arbitrary manner, in 16ic and 32fc versions. +- Improvements in receiver design: Block adapters are now all managed by smart pointers, ensuring better memory management. +- Improvements in processing speed: The VOLK_GNSSSDR library has been rewritten, following current VOLK standards and adding a number of new kernels. This approach addresses both efficiency and portability. Now the library provides the key kernels for GNSS signal processing in 16ic and 32fc versions, including SSE2, SSE3, SSE4.1, AVX, AV2 and NEON implementations. Please execute volk_gnsssdr_profile and volk_profile to use the fastest implementation for your host machine. +- New source block: Two_Bit_Packed_File_Signal_Source. This block takes 2 bit samples that have been packed into bytes or shorts as input and generates a byte for each sample. +- Fixes in SUPL assistance (supl.nokia.com removed). +- Improvements in acquisition: Added a non CFAR PCPS acquisition algorithm based on the estimation of the post correlation noise floor. If enabled as an option in the acquisition configuration, it allows setting more stable thresholds in the presence of non-gaussian front-end noise (which is the usual behavior of front-ends.) +- Fixes in acquisition: Fixed mismatch between the config files and the acquisition code in the specification of the IF. Fixed a bug in the length of the FFT of local codes. +- Improvements in tracking sensitivity: Added configuration option to customize the extension of the GPS L1 CA correlation length after bit synchronization (options are: [1,2,4,5,10,20] ms). Only available in the GPS_L1_CA_DLL_PLL_C_Aid_Tracking implementation. +- New tracking block introduced: GPS_L1_CA_DLL_PLL_C_Aid_Tracking is a GPS L1 C/A carrier PLL and code DLL with optional carrier-aid feedback. It is available in both 32 bits gr_complex input samples and in 16 bits short int complex samples. The gr_complex version has also the capability to extend the coherent correlation period from 1ms to 20ms using telemetry symbol synchronization. +- Increased resolution in CN0 estimator internal variables. +- Fixed a bug in computation of GPS L1 C/A carrier phase observable. +- Fixed a bug in the internal state machine that was blocking the receiver after a few hours of usage. Now the receiver can work continually (tested for more than one week, no known limit). +- New tracking block introduced: GPS_L1_CA_DLL_PLL_Tracking_GPU is a GPS L1 C/A carrier PLL and code DLL that uses the CUDA-compatible GPU to compute carrier wipe off and correlation operations, alleviating the CPU load. +- Obsolete/buggy blocks removed: GPS_L1_CA_DLL_FLL_PLL_Tracking, GPS_L1_CA_DLL_PLL_Optim_Tracking. +- Added a RTCM printer and TCP server in PVT blocks (still experimental). The receiver is now able to stream data in real time, serving RTCM 3.2 messages to multiple clients. For instance, it can act as a Ntrip Source feeding a Ntrip Server, or to be used as data input in RTKLIB, obtaining Precise Point Positioning fixes in real-time. The TCP port, Station ID, and rate of MT1019/MT1045 and MSM can be configured. GPS_L1_CA_PVT serves MT1019 (GPS Ephemeris) and MSM7 (MT1077, full GPS pseudoranges, phase ranges, phase range rates and CNR - high resolution) messages, while GALILEO_E1_PVT serves MT1045 (Galileo ephemeris) and MSM7 (MT1097, full Galileo pseudoranges, phase ranges, phase range rates and CNR - high resolution). +- Added a GeoJSON printer. Basic (least-squares) position fixes can be now also stored in this format, in addition to KML. +- Obsolete block removed: output filter. +- QA code migrated to the new asynchronous message passing system. +- Improvements in documentation: update of README.md file, addition of documentation for the VOLK_GNSSSDR library, updated links to new ICDs. +- Improvements in documentation: Satellite identification updated to current constellation status. +- Updated and cleaner console output. Now Galileo satellites have the ‘E’ identifier in their PRN number. +- Several improvements in CMake scripts allow to build GNSS-SDR in Linux Debian (Jessie, Stretch and Sid), Ubuntu (from 12.04 to 16.04), including amd64, i386, armhf and arm64 architectures, and possibly in other GNU/Linux distributions, as well as in Mac OS X 10.9 to 10.11. It also works well with CMake 3.5 (some problems solved with VOLK_GNSSSDR as a sub-project). +- The software can link either against OpenSSL or against GnuTLS with openssl extensions, whatever it is available. This allows buildings in distributions such as Fedora or ArchLinux, while being compatible with binary distribution through Debian packages. +- Fixed a number of defects detected by Coverity Scan. +- Some fixes required by Debian licensing and packaging system. +- Added a CGRAN (http://www.cgran.org/) manifest +- Lots of code cleaning and fixes of typos and small bugs. + + + + +## [0.0.6](https://github.com/gnss-sdr/gnss-sdr/releases/tag/v0.0.6) + +This release has several improvements and bug fixes: + +- Added initial support to multi-band, multi-source configurations (multiple signal sources and signal conditioners). +- Updated configuration files to new notation. Old and new configuration notations still compatible. +- Added skeleton for mixed (multi-frequency and multi-system) observables block. +- Faster local carrier update (25% of improvement). +- Added initial support to GPS L2C real time tracking and decoding of CNAV message with NSL STEREO v2, Fraunhofer’s Flexiband, and USRPx front-ends (the latter requiring external clock). +- Added initial support to select the frontend clock reference source in UHD signal source (i.e. internal or external clock reference). +- Added 2 bits complex file source for GNSS-SDR GSoC 2015 signal sampler designed by Ajith Peter. +- Added a new rtl_tcp signal source, remote access to RTL2832U-based dongles via TCP. +- Always build front-end-cal, a calibration tool for some DVB-T receivers based on the Realtek's RTL2832U chipset. +- Fixed bug in UTC time computation for GPS signals. +- Updated satellite identification for GPS and Galileo. +- Defined ‘cbyte’ as a new input data type (std::complex) +- Adding a new data_type_adapter, from interleaved short to std::complex +- Adding a filter for complex short streams. +- Adding a fir_filter for std::complex (aka cbyte). It converts the data type to floats, filters, and converts back to cbyte. +- Added a resampler for cbytes and cshorts. +- First working version of a GPS tracking block implementation using CUDA with multi-GPU device support. +- Updating RINEX obs header when leap second is available. +- Updating RINEX nav file when IONO and UTC data are available. +- Include Signal Strength Indicator in RINEX observable files. +- Tests fixed. +- Fixed more than 200 code defects detected by Coverity Scan. +- Updated documentation. +- Updated documentation and CMake scripts for the GN3S v2 driver (Linux-only) +- Armadillo version automatically downloaded and built if it is not present in the system is now 5.400.3. +- Updated old links from googlecode to new links at GitHub for Google Test, gflags, glog and gperftools. +- gfortran is no longer a required package, but it is used if available. +- Added an option to remove logging. +- Enabled cross-compilation for ARM devices. +- Lots of code cleaning. + + + + +## [0.0.5](https://github.com/gnss-sdr/gnss-sdr/releases/tag/v0.0.5) + +This release has several improvements and bug fixes: + +- Now GNSS-SDR can be installed on the system with the usual ‘cmake ../ && make && sudo make install’. +- Added volk_gnsssdr library, a volk-like library implementing some specific kernels and ensuring portable executables. It comes with a ‘volk_gnsssdr_profile’ executable, in the fashion of volk_profile. Volk and volk_gnsssdr are compatible and can be mixed together. This is expected to enable faster execution of the software receiver in upcoming versions. +- The former ‘rtlsdr_signal_source’ has been replaced by a more general ‘osmosdr_signal_source’ compatible with all those front-ends accessible by the OsmoSDR driver (bladeRF, hackRF, etc.) in addition to RTL-based dongles. +- Added manpages when binaries gnss-sdr, volk_gnsssdr_profile and front-end-cal are installed. +- Now GNSS-SDR can be build on i386, amd64, armhf, armel and arm64 architectures. +- Now GNSS-SDR builds on Ubuntu 14.04 and 14.10, Debian jessie/sid and Mac OS X 10.9 and 10.10. +- Improved detection of dependencies, specially when installed as .deb packages. +- Added a ‘check' target with some minimal tests. +- Added support for interleaved I/Q byte-size sample files. +- Minor bug fixes, updated documentation and code cleaning. + + + + +## [0.0.4](https://github.com/gnss-sdr/gnss-sdr/releases/tag/v0.0.4) + +This release has several improvements and bug fixes: + +- Added hybrid processing GPS L1 C/A and Galileo E1B, providing position fixes make use of observables for both constellations. +- Added implementations of the QuickSync algorithm for GPS L1 C/A and Galileo E1 acquisition. +- Added processing blocks for Galileo E5a: Acquisition, Tracking, Telemetry_Decoder (experimental) +- New configuration files allow to configure GPS and Galileo channels in the same receiver. +- Added tropospheric corrections to GPS and Galileo PVT solution. +- Improved precision obtained by changing some variables from float to double. +- New building options: ENABLE_GN3S, ENABLE_RTLSDR and ENABLE_ARRAY and ENABLE_OPENCL. +- Improved documentation on how to enable optional drivers. +- Fixed bug in memory alignment that caused problems with high data rates. +- Added ENABLE_GENERIC_ARCH, an option to build the binary without detecting the SIMD instruction set present in the compiling machine, so it can be executed in other machines without those specific sets. +- Added ENABLE_GPERFTOOLS, which links the executable to tcmalloc and profiler if Gperftools is available on the system. +- Added carrier phase, Doppler shift and signal strength observables to the RINEX files. Static PPP solutions are available for GPS with RTKLIB via RINEX files. +- The executable now produces RINEX files version 3.02 of Galileo Observables, Navigation data, and mixed (GPS/Galileo) observables and nav data. RINEX 3.02 is the default version of RINEX files. +- Armadillo version updated to 4.400.2 +- Armadillo now uses OpenBLAS instead of BLAS if the former is available on the system. +- Some raw pointers have been changed to smart pointers. +- Minor bug fixes and code cleaning. + + + + +## [0.0.3](https://github.com/gnss-sdr/gnss-sdr/releases/tag/v0.0.3) + +This release has several improvements and bug fixes, completing the transition from Subversion to Git. The main changes are: + +- Created some missing directories lost in the SVN to Git transition. +- New C++11-ized block factory, flow graph and tests, resulting in better memory management and fewer segmentation faults. Several raw pointers converted to smart pointers. +- Reorganization of assistance data input and output. +- Fixed memory leak when talking to SUPL servers. +- Improved retrieval of assistance data. +- Fixing an error in a constant value related to Galileo. +- Inform users if the temporal folder is not /tmp. +- Fixes and additions to the documentation. +- README in markdown language so it looks better in Git repositories. +- Fixed a bug that prevented the update of all shared map structures (ephemeris, iono parameters, etc…). +- The configuration script now throws error if GCC is older than 4.7 or Boost is older than 1.45 +- Improved detection / downloading & building if missing of Gflags and Glog. +- Improved detection / downloading & building if missing of Armadillo and related dependencies. +- Fixes many warnings that appeared when using CMake 3.0. +- Improved detection of GTEST_DIR variable. +- Include header files in libraries so IDEs such as Xcode can display them. + + +Enjoy it! diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 893c4c01e..437acd35c 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -217,8 +217,8 @@ bool rtklib_solver::get_PVT(std::map gnss_observables_map, dou nav_data.n=valid_obs; for (int i=0; i< MAXSAT;i++) { - nav_data.lam[i][0]=CLIGHT/FREQ1; /* L1/E1 */ - nav_data.lam[i][1]=CLIGHT/FREQ2; /* L2 */ + nav_data.lam[i][0]=SPEED_OF_LIGHT/FREQ1; /* L1/E1 */ + nav_data.lam[i][1]=SPEED_OF_LIGHT/FREQ2; /* L2 */ } result=pntpos(obs_data, valid_obs, &nav_data, &rtklib_opt, &old_pvt_sol, NULL, NULL,rtklib_msg); diff --git a/src/algorithms/libs/rtklib/CMakeLists.txt b/src/algorithms/libs/rtklib/CMakeLists.txt index fd582ea9d..9a172fb53 100644 --- a/src/algorithms/libs/rtklib/CMakeLists.txt +++ b/src/algorithms/libs/rtklib/CMakeLists.txt @@ -1,4 +1,4 @@ -# Copyright (C) 2012-2015 (see AUTHORS file for a list of contributors) +# Copyright (C) 2012-2017 (see AUTHORS file for a list of contributors) # # This file is part of GNSS-SDR. # @@ -38,14 +38,13 @@ include_directories( ${GFlags_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} ) + file(GLOB RTKLIB_LIB_HEADERS "*.h") list(SORT RTKLIB_LIB_HEADERS) add_library(rtklib_lib ${RTKLIB_LIB_SOURCES} ${RTKLIB_LIB_HEADERS}) source_group(Headers FILES ${RTKLIB_LIB_HEADERS}) add_dependencies(rtklib_lib armadillo-${armadillo_RELEASE} glog-${glog_RELEASE}) -#set_property(SOURCE rtklib_rtkcmn.cc APPEND_STRING PROPERTY COMPILE_FLAGS " -Wno-missing-field-initializers ") - target_link_libraries( rtklib_lib ${Boost_LIBRARIES} @@ -55,6 +54,4 @@ target_link_libraries( ${BLAS} ${LAPACK} ) - -#MESSAGE( STATUS "*****************BLAS: " ${BLAS} ) - + diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index e8bb7bd76..b40ec2325 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -48,356 +48,243 @@ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RTKLIB_H_ -#define RTKLIB_H_ + +#ifndef GNSS_SDR_RTKLIB_H_ +#define GNSS_SDR_RTKLIB_H_ #include #include #include #include -#include +#include #include #include +#include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" +#include "gnss_obs_codes.h" -#define thread_t pthread_t -#define lock_t pthread_mutex_t -#define initlock(f) pthread_mutex_init(f,NULL) -#define lock(f) pthread_mutex_lock(f) -#define unlock(f) pthread_mutex_unlock(f) -#define FILEPATHSEP '/' -#define PI 3.1415926535897932 /* pi */ -#define D2R (PI/180.0) /* deg to rad */ -#define R2D (180.0/PI) /* rad to deg */ -#define CLIGHT 299792458.0 /* speed of light (m/s) */ -#define SC2RAD 3.1415926535898 /* semi-circle to radian (IS-GPS) */ -#define AU 149597870691.0 /* 1 AU (m) */ -#define AS2R (D2R/3600.0) /* arc sec to radian */ +const int FILEPATHSEP = '/'; -#define OMGE 7.2921151467E-5 /* earth angular velocity (IS-GPS) (rad/s) */ +const double RE_WGS84 = 6378137.0; //!< earth semimajor axis (WGS84) (m) +const double FE_WGS84 = (1.0 / 298.257223563); //!< earth flattening (WGS84) -#define RE_WGS84 6378137.0 /* earth semimajor axis (WGS84) (m) */ -#define FE_WGS84 (1.0/298.257223563) /* earth flattening (WGS84) */ +const double HION = 350000.0; //!< ionosphere height (m) -#define HION 350000.0 /* ionosphere height (m) */ +const unsigned int POLYCRC32 = 0xEDB88320u; //!< CRC32 polynomial +const unsigned int POLYCRC24Q = 0x1864CFBu; //!< CRC24Q polynomial -#define P2_5 0.03125 /* 2^-5 */ -#define P2_6 0.015625 /* 2^-6 */ -#define P2_11 4.882812500000000E-04 /* 2^-11 */ -#define P2_15 3.051757812500000E-05 /* 2^-15 */ -#define P2_17 7.629394531250000E-06 /* 2^-17 */ -#define P2_19 1.907348632812500E-06 /* 2^-19 */ -#define P2_20 9.536743164062500E-07 /* 2^-20 */ -#define P2_21 4.768371582031250E-07 /* 2^-21 */ -#define P2_23 1.192092895507810E-07 /* 2^-23 */ -#define P2_24 5.960464477539063E-08 /* 2^-24 */ -#define P2_27 7.450580596923828E-09 /* 2^-27 */ -#define P2_29 1.862645149230957E-09 /* 2^-29 */ -#define P2_30 9.313225746154785E-10 /* 2^-30 */ -#define P2_31 4.656612873077393E-10 /* 2^-31 */ -#define P2_32 2.328306436538696E-10 /* 2^-32 */ -#define P2_33 1.164153218269348E-10 /* 2^-33 */ -#define P2_35 2.910383045673370E-11 /* 2^-35 */ -#define P2_38 3.637978807091710E-12 /* 2^-38 */ -#define P2_39 1.818989403545856E-12 /* 2^-39 */ -#define P2_40 9.094947017729280E-13 /* 2^-40 */ -#define P2_43 1.136868377216160E-13 /* 2^-43 */ -#define P2_48 3.552713678800501E-15 /* 2^-48 */ -#define P2_50 8.881784197001252E-16 /* 2^-50 */ -#define P2_55 2.775557561562891E-17 /* 2^-55 */ +const unsigned int PMODE_SINGLE = 0; //!< positioning mode: single +const unsigned int PMODE_DGPS = 1; //!< positioning mode: DGPS/DGNSS +const unsigned int PMODE_KINEMA = 2; //!< positioning mode: kinematic +const unsigned int PMODE_STATIC = 3; //!< positioning mode: static +const unsigned int PMODE_MOVEB = 4; //!< positioning mode: moving-base +const unsigned int PMODE_FIXED = 5; //!< positioning mode: fixed +const unsigned int PMODE_PPP_KINEMA = 6; //!< positioning mode: PPP-kinemaric +const unsigned int PMODE_PPP_STATIC = 7; //!< positioning mode: PPP-static +const unsigned int PMODE_PPP_FIXED = 8; //!< positioning mode: PPP-fixed -#define POLYCRC32 0xEDB88320u /* CRC32 polynomial */ -#define POLYCRC24Q 0x1864CFBu /* CRC24Q polynomial */ +const unsigned int SOLF_LLH = 0; //!< solution format: lat/lon/height +const unsigned int SOLF_XYZ = 1; //!< solution format: x/y/z-ecef +const unsigned int SOLF_ENU = 2; //!< solution format: e/n/u-baseline +const unsigned int SOLF_NMEA = 3; //!< solution format: NMEA-183 +const unsigned int SOLF_STAT = 4; //!< solution format: solution status +const unsigned int SOLF_GSIF = 5; //!< solution format: GSI F1/F2 -#define PMODE_SINGLE 0 /* positioning mode: single */ -#define PMODE_DGPS 1 /* positioning mode: DGPS/DGNSS */ -#define PMODE_KINEMA 2 /* positioning mode: kinematic */ -#define PMODE_STATIC 3 /* positioning mode: static */ -#define PMODE_MOVEB 4 /* positioning mode: moving-base */ -#define PMODE_FIXED 5 /* positioning mode: fixed */ -#define PMODE_PPP_KINEMA 6 /* positioning mode: PPP-kinemaric */ -#define PMODE_PPP_STATIC 7 /* positioning mode: PPP-static */ -#define PMODE_PPP_FIXED 8 /* positioning mode: PPP-fixed */ +const unsigned int SOLQ_NONE = 0; //!< solution status: no solution +const unsigned int SOLQ_FIX = 1; //!< solution status: fix +const unsigned int SOLQ_FLOAT = 2; //!< solution status: float +const unsigned int SOLQ_SBAS = 3; //!< solution status: SBAS +const unsigned int SOLQ_DGPS = 4; //!< solution status: DGPS/DGNSS +const unsigned int SOLQ_SINGLE = 5; //!< solution status: single +const unsigned int SOLQ_PPP = 6; //!< solution status: PPP +const unsigned int SOLQ_DR = 7; //!< solution status: dead reckoning +const unsigned int MAXSOLQ = 7; //!< max number of solution status -#define SOLF_LLH 0 /* solution format: lat/lon/height */ -#define SOLF_XYZ 1 /* solution format: x/y/z-ecef */ -#define SOLF_ENU 2 /* solution format: e/n/u-baseline */ -#define SOLF_NMEA 3 /* solution format: NMEA-183 */ -#define SOLF_STAT 4 /* solution format: solution status */ -#define SOLF_GSIF 5 /* solution format: GSI F1/F2 */ +const unsigned int TIMES_GPST = 0; //!< time system: gps time +const unsigned int TIMES_UTC = 1; //!< time system: utc +const unsigned int TIMES_JST = 2; //!< time system: jst -#define SOLQ_NONE 0 /* solution status: no solution */ -#define SOLQ_FIX 1 /* solution status: fix */ -#define SOLQ_FLOAT 2 /* solution status: float */ -#define SOLQ_SBAS 3 /* solution status: SBAS */ -#define SOLQ_DGPS 4 /* solution status: DGPS/DGNSS */ -#define SOLQ_SINGLE 5 /* solution status: single */ -#define SOLQ_PPP 6 /* solution status: PPP */ -#define SOLQ_DR 7 /* solution status: dead reconing */ -#define MAXSOLQ 7 /* max number of solution status */ +const int MAXFREQ = 7; //!< max NFREQ -#define TIMES_GPST 0 /* time system: gps time */ -#define TIMES_UTC 1 /* time system: utc */ -#define TIMES_JST 2 /* time system: jst */ +const int MAXLEAPS = 64; //!< max number of leap seconds table +const double DTTOL = 0.005; //!< tolerance of time difference (s) -#define MAXFREQ 7 /* max NFREQ */ +const int NFREQ = 3; +const int NEXOBS = 0; //!< number of extended obs codes +const int MAXANT = 64; //!< max length of station name/antenna type -#define FREQ1 1.57542E9 /* L1/E1 frequency (Hz) */ -#define FREQ2 1.22760E9 /* L2 frequency (Hz) */ -#define FREQ5 1.17645E9 /* L5/E5a frequency (Hz) */ -#define FREQ6 1.27875E9 /* E6/LEX frequency (Hz) */ -#define FREQ7 1.20714E9 /* E5b frequency (Hz) */ -#define FREQ8 1.191795E9 /* E5a+b frequency (Hz) */ -#define FREQ9 2.492028E9 /* S frequency (Hz) */ -#define FREQ1_GLO 1.60200E9 /* GLONASS G1 base frequency (Hz) */ -#define DFRQ1_GLO 0.56250E6 /* GLONASS G1 bias frequency (Hz/n) */ -#define FREQ2_GLO 1.24600E9 /* GLONASS G2 base frequency (Hz) */ -#define DFRQ2_GLO 0.43750E6 /* GLONASS G2 bias frequency (Hz/n) */ -#define FREQ3_GLO 1.202025E9 /* GLONASS G3 frequency (Hz) */ -#define FREQ1_CMP 1.561098E9 /* BeiDou B1 frequency (Hz) */ -#define FREQ2_CMP 1.20714E9 /* BeiDou B2 frequency (Hz) */ -#define FREQ3_CMP 1.26852E9 /* BeiDou B3 frequency (Hz) */ +const int MINPRNGPS = 1; //!< min satellite PRN number of GPS +const int MAXPRNGPS = 32; //!< max satellite PRN number of GPS +const int NSATGPS = (MAXPRNGPS - MINPRNGPS + 1); //!< number of GPS satellites +const int NSYSGPS = 1; -#define MAXLEAPS 64 /* max number of leap seconds table */ -#define DTTOL 0.005 /* tolerance of time difference (s) */ - -#define NFREQ 3 -#define NEXOBS 0 /* number of extended obs codes */ -#define MAXANT 64 /* max length of station name/antenna type */ - -#define MINPRNGPS 1 /* min satellite PRN number of GPS */ -#define MAXPRNGPS 32 /* max satellite PRN number of GPS */ -#define NSATGPS (MAXPRNGPS-MINPRNGPS+1) /* number of GPS satellites */ -#define NSYSGPS 1 - -#define SYS_NONE 0x00 /* navigation system: none */ -#define SYS_GPS 0x01 /* navigation system: GPS */ -#define SYS_SBS 0x02 /* navigation system: SBAS */ -#define SYS_GLO 0x04 /* navigation system: GLONASS */ -#define SYS_GAL 0x08 /* navigation system: Galileo */ -#define SYS_QZS 0x10 /* navigation system: QZSS */ -#define SYS_CMP 0x20 /* navigation system: BeiDou */ -#define SYS_IRN 0x40 /* navigation system: IRNS */ -#define SYS_LEO 0x80 /* navigation system: LEO */ -#define SYS_ALL 0xFF /* navigation system: all */ - -#define CODE_NONE 0 /* obs code: none or unknown */ -#define CODE_L1C 1 /* obs code: L1C/A,G1C/A,E1C (GPS,GLO,GAL,QZS,SBS) */ -#define CODE_L1P 2 /* obs code: L1P,G1P (GPS,GLO) */ -#define CODE_L1W 3 /* obs code: L1 Z-track (GPS) */ -#define CODE_L1Y 4 /* obs code: L1Y (GPS) */ -#define CODE_L1M 5 /* obs code: L1M (GPS) */ -#define CODE_L1N 6 /* obs code: L1codeless (GPS) */ -#define CODE_L1S 7 /* obs code: L1C(D) (GPS,QZS) */ -#define CODE_L1L 8 /* obs code: L1C(P) (GPS,QZS) */ -#define CODE_L1E 9 /* (not used) */ -#define CODE_L1A 10 /* obs code: E1A (GAL) */ -#define CODE_L1B 11 /* obs code: E1B (GAL) */ -#define CODE_L1X 12 /* obs code: E1B+C,L1C(D+P) (GAL,QZS) */ -#define CODE_L1Z 13 /* obs code: E1A+B+C,L1SAIF (GAL,QZS) */ -#define CODE_L2C 14 /* obs code: L2C/A,G1C/A (GPS,GLO) */ -#define CODE_L2D 15 /* obs code: L2 L1C/A-(P2-P1) (GPS) */ -#define CODE_L2S 16 /* obs code: L2C(M) (GPS,QZS) */ -#define CODE_L2L 17 /* obs code: L2C(L) (GPS,QZS) */ -#define CODE_L2X 18 /* obs code: L2C(M+L),B1I+Q (GPS,QZS,CMP) */ -#define CODE_L2P 19 /* obs code: L2P,G2P (GPS,GLO) */ -#define CODE_L2W 20 /* obs code: L2 Z-track (GPS) */ -#define CODE_L2Y 21 /* obs code: L2Y (GPS) */ -#define CODE_L2M 22 /* obs code: L2M (GPS) */ -#define CODE_L2N 23 /* obs code: L2codeless (GPS) */ -#define CODE_L5I 24 /* obs code: L5/E5aI (GPS,GAL,QZS,SBS) */ -#define CODE_L5Q 25 /* obs code: L5/E5aQ (GPS,GAL,QZS,SBS) */ -#define CODE_L5X 26 /* obs code: L5/E5aI+Q/L5B+C (GPS,GAL,QZS,IRN,SBS) */ -#define CODE_L7I 27 /* obs code: E5bI,B2I (GAL,CMP) */ -#define CODE_L7Q 28 /* obs code: E5bQ,B2Q (GAL,CMP) */ -#define CODE_L7X 29 /* obs code: E5bI+Q,B2I+Q (GAL,CMP) */ -#define CODE_L6A 30 /* obs code: E6A (GAL) */ -#define CODE_L6B 31 /* obs code: E6B (GAL) */ -#define CODE_L6C 32 /* obs code: E6C (GAL) */ -#define CODE_L6X 33 /* obs code: E6B+C,LEXS+L,B3I+Q (GAL,QZS,CMP) */ -#define CODE_L6Z 34 /* obs code: E6A+B+C (GAL) */ -#define CODE_L6S 35 /* obs code: LEXS (QZS) */ -#define CODE_L6L 36 /* obs code: LEXL (QZS) */ -#define CODE_L8I 37 /* obs code: E5(a+b)I (GAL) */ -#define CODE_L8Q 38 /* obs code: E5(a+b)Q (GAL) */ -#define CODE_L8X 39 /* obs code: E5(a+b)I+Q (GAL) */ -#define CODE_L2I 40 /* obs code: B1I (BDS) */ -#define CODE_L2Q 41 /* obs code: B1Q (BDS) */ -#define CODE_L6I 42 /* obs code: B3I (BDS) */ -#define CODE_L6Q 43 /* obs code: B3Q (BDS) */ -#define CODE_L3I 44 /* obs code: G3I (GLO) */ -#define CODE_L3Q 45 /* obs code: G3Q (GLO) */ -#define CODE_L3X 46 /* obs code: G3I+Q (GLO) */ -#define CODE_L1I 47 /* obs code: B1I (BDS) */ -#define CODE_L1Q 48 /* obs code: B1Q (BDS) */ -#define CODE_L5A 49 /* obs code: L5A SPS (IRN) */ -#define CODE_L5B 50 /* obs code: L5B RS(D) (IRN) */ -#define CODE_L5C 51 /* obs code: L5C RS(P) (IRN) */ -#define CODE_L9A 52 /* obs code: SA SPS (IRN) */ -#define CODE_L9B 53 /* obs code: SB RS(D) (IRN) */ -#define CODE_L9C 54 /* obs code: SC RS(P) (IRN) */ -#define CODE_L9X 55 /* obs code: SB+C (IRN) */ -#define MAXCODE 55 /* max number of obs code */ +const int SYS_NONE = 0x00; //!< navigation system: none +const int SYS_GPS = 0x01; //!< navigation system: GPS +const int SYS_SBS = 0x02; //!< navigation system: SBAS +const int SYS_GLO = 0x04; //!< navigation system: GLONASS +const int SYS_GAL = 0x08; //!< navigation system: Galileo +const int SYS_QZS = 0x10; //!< navigation system: QZSS +const int SYS_BDS = 0x20; //!< navigation system: BeiDou +const int SYS_IRN = 0x40; //!< navigation system: IRNS +const int SYS_LEO = 0x80; //!< navigation system: LEO +const int SYS_ALL = 0xFF; //!< navigation system: all #ifdef ENAGLO -#define MINPRNGLO 1 /* min satellite slot number of GLONASS */ -#define MAXPRNGLO 27 /* max satellite slot number of GLONASS */ -#define NSATGLO (MAXPRNGLO-MINPRNGLO+1) /* number of GLONASS satellites */ -#define NSYSGLO 1 +const int MINPRNGLO = 1; //!< min satellite slot number of GLONASS +const int MAXPRNGLO = 27; //!< max satellite slot number of GLONASS +const int NSATGLO = (MAXPRNGLO - MINPRNGLO + 1); //!< number of GLONASS satellites +const int NSYSGLO = 1; #else -#define MINPRNGLO 0 -#define MAXPRNGLO 0 -#define NSATGLO 0 -#define NSYSGLO 0 +const int MINPRNGLO = 0; +const int MAXPRNGLO = 0; +const int NSATGLO = 0; +const int NSYSGLO = 0; #endif + #ifdef ENAGAL -#define MINPRNGAL 1 /* min satellite PRN number of Galileo */ -#define MAXPRNGAL 30 /* max satellite PRN number of Galileo */ -#define NSATGAL (MAXPRNGAL-MINPRNGAL+1) /* number of Galileo satellites */ -#define NSYSGAL 1 +const int MINPRNGAL = 1; //!< min satellite PRN number of Galileo +const int MAXPRNGAL = 30; //!< max satellite PRN number of Galileo +const int NSATGAL = (MAXPRNGAL - MINPRNGAL + 1); //!< number of Galileo satellites +const int NSYSGAL = 1; #else -#define MINPRNGAL 0 -#define MAXPRNGAL 0 -#define NSATGAL 0 -#define NSYSGAL 0 +const int MINPRNGAL = 0; +const int MAXPRNGAL = 0; +const int NSATGAL = 0; +const int NSYSGAL = 0; #endif + #ifdef ENAQZS -#define MINPRNQZS 193 /* min satellite PRN number of QZSS */ -#define MAXPRNQZS 199 /* max satellite PRN number of QZSS */ -#define MINPRNQZS_S 183 /* min satellite PRN number of QZSS SAIF */ -#define MAXPRNQZS_S 189 /* max satellite PRN number of QZSS SAIF */ -#define NSATQZS (MAXPRNQZS-MINPRNQZS+1) /* number of QZSS satellites */ -#define NSYSQZS 1 +const int MINPRNQZS = 193; //!< min satellite PRN number of QZSS +const int MAXPRNQZS = 199; //!< max satellite PRN number of QZSS +const int MINPRNQZS_S = 183; //!< min satellite PRN number of QZSS SAIF +const int MAXPRNQZS_S = 189; //!< max satellite PRN number of QZSS SAIF +const int NSATQZS = (MAXPRNQZS - MINPRNQZS + 1); //!< number of QZSS satellites +const int NSYSQZS = 1; #else -#define MINPRNQZS 0 -#define MAXPRNQZS 0 -#define MINPRNQZS_S 0 -#define MAXPRNQZS_S 0 -#define NSATQZS 0 -#define NSYSQZS 0 +const int MINPRNQZS = 0; +const int MAXPRNQZS = 0; +const int MINPRNQZS_S = 0; +const int MAXPRNQZS_S = 0; +const int NSATQZS = 0; +const int NSYSQZS = 0; #endif -#ifdef ENACMP -#define MINPRNCMP 1 /* min satellite sat number of BeiDou */ -#define MAXPRNCMP 35 /* max satellite sat number of BeiDou */ -#define NSATCMP (MAXPRNCMP-MINPRNCMP+1) /* number of BeiDou satellites */ -#define NSYSCMP 1 + +#ifdef ENABDS +const int MINPRNBDS = 1; //!< min satellite sat number of BeiDou +const int MAXPRNBDS = 35; //!< max satellite sat number of BeiDou +const int NSATBDS = (MAXPRNBDS - MINPRNCM + 1); //!< number of BeiDou satellites +const int NSYSBDS = 1; #else -#define MINPRNCMP 0 -#define MAXPRNCMP 0 -#define NSATCMP 0 -#define NSYSCMP 0 +const int MINPRNBDS = 0; +const int MAXPRNBDS = 0; +const int NSATBDS = 0; +const int NSYSBDS = 0; #endif + #ifdef ENAIRN -#define MINPRNIRN 1 /* min satellite sat number of IRNSS */ -#define MAXPRNIRN 7 /* max satellite sat number of IRNSS */ -#define NSATIRN (MAXPRNIRN-MINPRNIRN+1) /* number of IRNSS satellites */ -#define NSYSIRN 1 +const int MINPRNIRN = 1; //!< min satellite sat number of IRNSS +const int MAXPRNIRN = 7; //!< max satellite sat number of IRNSS +const int NSATIRN = (MAXPRNIRN - MINPRNIRN + 1); //!< number of IRNSS satellites +const int NSYSIRN = 1; #else -#define MINPRNIRN 0 -#define MAXPRNIRN 0 -#define NSATIRN 0 -#define NSYSIRN 0 +const int MINPRNIRN = 0; +const int MAXPRNIRN = 0; +const int NSATIRN = 0; +const int NSYSIRN = 0; #endif + #ifdef ENALEO -#define MINPRNLEO 1 /* min satellite sat number of LEO */ -#define MAXPRNLEO 10 /* max satellite sat number of LEO */ -#define NSATLEO (MAXPRNLEO-MINPRNLEO+1) /* number of LEO satellites */ -#define NSYSLEO 1 +const int MINPRNLEO = 1; //!< min satellite sat number of LEO +const int NSATLEO = 10; //!< max satellite sat number of LEO +const int NSATLEO = (MAXPRNLEO - MINPRNLEO + 1); //!< number of LEO satellites +const int NSYSLEO = 1; #else -#define MINPRNLEO 0 -#define MAXPRNLEO 0 -#define NSATLEO 0 -#define NSYSLEO 0 +const int MINPRNLEO = 0; +const int MAXPRNLEO = 0; +const int NSATLEO = 0; +const int NSYSLEO = 0; #endif -#define NSYS (NSYSGPS+NSYSGLO+NSYSGAL+NSYSQZS+NSYSCMP+NSYSIRN+NSYSLEO) /* number of systems */ +const int NSYS = (NSYSGPS + NSYSGLO + NSYSGAL + NSYSQZS + NSYSBDS + NSYSIRN + NSYSLEO); //!< number of systems -#define MINPRNSBS 120 /* min satellite PRN number of SBAS */ -#define MAXPRNSBS 142 /* max satellite PRN number of SBAS */ -#define NSATSBS (MAXPRNSBS-MINPRNSBS+1) /* number of SBAS satellites */ +const int MINPRNSBS = 120; //!< min satellite PRN number of SBAS +const int MAXPRNSBS = 142; //!< max satellite PRN number of SBAS +const int NSATSBS = (MAXPRNSBS - MINPRNSBS + 1); //!< number of SBAS satellites -#define MAXSAT (NSATGPS+NSATGLO+NSATGAL+NSATQZS+NSATCMP+NSATIRN+NSATSBS+NSATLEO) +const int MAXSAT = (NSATGPS + NSATGLO + NSATGAL + NSATQZS + NSATBDS + NSATIRN + NSATSBS + NSATLEO); -#define MAXNIGP 201 /* max number of IGP in SBAS band */ - - -#define MAXCODE 55 /* max number of obs code */ - -#define MAXSTA 255 +const int MAXSTA = 255; #ifndef MAXOBS -#define MAXOBS 64 /* max number of obs in an epoch */ +const int MAXOBS = 64; //!< max number of obs in an epoch #endif -#define MAXRCV 64 /* max receiver number (1 to MAXRCV) */ -#define MAXOBSTYPE 64 /* max number of obs type in RINEX */ -#define DTTOL 0.005 /* tolerance of time difference (s) */ -#define MAXDTOE 7200.0 /* max time difference to GPS Toe (s) */ -#define MAXDTOE_QZS 7200.0 /* max time difference to QZSS Toe (s) */ -#define MAXDTOE_GAL 10800.0 /* max time difference to Galileo Toe (s) */ -#define MAXDTOE_CMP 21600.0 /* max time difference to BeiDou Toe (s) */ -#define MAXDTOE_GLO 1800.0 /* max time difference to GLONASS Toe (s) */ -#define MAXDTOE_SBS 360.0 /* max time difference to SBAS Toe (s) */ -#define MAXDTOE_S 86400.0 /* max time difference to ephem toe (s) for other */ -#define MAXGDOP 300.0 /* max GDOP */ +const int MAXRCV = 64; //!< max receiver number (1 to MAXRCV) +const int MAXOBSTYPE = 64; //!< max number of obs type in RINEX +const double MAXDTOE = 7200.0; //!< max time difference to GPS Toe (s) +const double MAXDTOE_QZS = 7200.0; //!< max time difference to QZSS Toe (s) +const double MAXDTOE_GAL = 10800.0; //!< max time difference to Galileo Toe (s) +const double MAXDTOE_BDS = 21600.0; //!< max time difference to BeiDou Toe (s) +const double MAXDTOE_GLO = 1800.0; //!< max time difference to GLONASS Toe (s) +const double MAXDTOE_SBS = 360.0; //!< max time difference to SBAS Toe (s) +const double MAXDTOE_S = 86400.0; //!< max time difference to ephem toe (s) for other +const double MAXGDOP = 300.0; //!< max GDOP -#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */ -#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */ -#define MAXSBSURA 8 /* max URA of SBAS satellite */ -#define MAXBAND 10 /* max SBAS band of IGP */ -#define MAXNIGP 201 /* max number of IGP in SBAS band */ -#define MAXNGEO 4 /* max number of GEO satellites */ +const int MAXSBSURA = 8; //!< max URA of SBAS satellite +const int MAXBAND = 10; //!< max SBAS band of IGP +const int MAXNIGP = 201; //!< max number of IGP in SBAS band +const int MAXNGEO = 4; //!< max number of GEO satellites -#define MAXSOLMSG 8191 /* max length of solution message */ -#define MAXERRMSG 4096 /* max length of error/warning message */ +const int MAXSOLMSG = 8191; //!< max length of solution message +const int MAXERRMSG = 4096; //!< max length of error/warning message -#define IONOOPT_OFF 0 /* ionosphere option: correction off */ -#define IONOOPT_BRDC 1 /* ionosphere option: broadcast model */ -#define IONOOPT_SBAS 2 /* ionosphere option: SBAS model */ -#define IONOOPT_IFLC 3 /* ionosphere option: L1/L2 or L1/L5 iono-free LC */ -#define IONOOPT_EST 4 /* ionosphere option: estimation */ -#define IONOOPT_TEC 5 /* ionosphere option: IONEX TEC model */ -#define IONOOPT_QZS 6 /* ionosphere option: QZSS broadcast model */ -#define IONOOPT_LEX 7 /* ionosphere option: QZSS LEX ionospehre */ -#define IONOOPT_STEC 8 /* ionosphere option: SLANT TEC model */ +const unsigned int IONOOPT_OFF = 0; //!< ionosphere option: correction off +const unsigned int IONOOPT_BRDC = 1; //!< ionosphere option: broadcast model +const unsigned int IONOOPT_SBAS = 2; //!< ionosphere option: SBAS model +const unsigned int IONOOPT_IFLC = 3; //!< ionosphere option: L1/L2 or L1/L5 iono-free LC +const unsigned int IONOOPT_EST = 4; //!< ionosphere option: estimation +const unsigned int IONOOPT_TEC = 5; //!< ionosphere option: IONEX TEC model +const unsigned int IONOOPT_QZS = 6; //!< ionosphere option: QZSS broadcast model +const unsigned int IONOOPT_LEX = 7; //!< ionosphere option: QZSS LEX ionospehre +const unsigned int IONOOPT_STEC = 8; //!< ionosphere option: SLANT TEC model -#define TROPOPT_OFF 0 /* troposphere option: correction off */ -#define TROPOPT_SAAS 1 /* troposphere option: Saastamoinen model */ -#define TROPOPT_SBAS 2 /* troposphere option: SBAS model */ -#define TROPOPT_EST 3 /* troposphere option: ZTD estimation */ -#define TROPOPT_ESTG 4 /* troposphere option: ZTD+grad estimation */ -#define TROPOPT_ZTD 5 /* troposphere option: ZTD correction */ +const unsigned int TROPOPT_OFF = 0; //!< troposphere option: correction off +const unsigned int TROPOPT_SAAS = 1; //!< troposphere option: Saastamoinen model +const unsigned int TROPOPT_SBAS = 2; //!< troposphere option: SBAS model +const unsigned int TROPOPT_EST = 3; //!< troposphere option: ZTD estimation +const unsigned int TROPOPT_ESTG = 4; //!< troposphere option: ZTD+grad estimation +const unsigned int TROPOPT_ZTD = 5; //!< troposphere option: ZTD correction -#define EPHOPT_BRDC 0 /* ephemeris option: broadcast ephemeris */ -#define EPHOPT_PREC 1 /* ephemeris option: precise ephemeris */ -#define EPHOPT_SBAS 2 /* ephemeris option: broadcast + SBAS */ -#define EPHOPT_SSRAPC 3 /* ephemeris option: broadcast + SSR_APC */ -#define EPHOPT_SSRCOM 4 /* ephemeris option: broadcast + SSR_COM */ -#define EPHOPT_LEX 5 /* ephemeris option: QZSS LEX ephemeris */ +const unsigned int EPHOPT_BRDC = 0; //!< ephemeris option: broadcast ephemeris +const unsigned int EPHOPT_PREC = 1; //!< ephemeris option: precise ephemeris +const unsigned int EPHOPT_SBAS = 2; //!< ephemeris option: broadcast + SBAS +const unsigned int EPHOPT_SSRAPC = 3; //!< ephemeris option: broadcast + SSR_APC +const unsigned int EPHOPT_SSRCOM = 4; //!< ephemeris option: broadcast + SSR_COM +const unsigned int EPHOPT_LEX = 5; //!< ephemeris option: QZSS LEX ephemeris -#define EFACT_GPS 1.0 /* error factor: GPS */ -#define EFACT_GLO 1.5 /* error factor: GLONASS */ -#define EFACT_GAL 1.0 /* error factor: Galileo */ -#define EFACT_QZS 1.0 /* error factor: QZSS */ -#define EFACT_CMP 1.0 /* error factor: BeiDou */ -#define EFACT_IRN 1.5 /* error factor: IRNSS */ -#define EFACT_SBS 3.0 /* error factor: SBAS */ +const double EFACT_GPS = 1.0; //!< error factor: GPS +const double EFACT_GLO = 1.5; //!< error factor: GLONASS +const double EFACT_GAL = 1.0; //!< error factor: Galileo +const double EFACT_QZS = 1.0; //!< error factor: QZSS +const double EFACT_BDS = 1.0; //!< error factor: BeiDou +const double EFACT_IRN = 1.5; //!< error factor: IRNSS +const double EFACT_SBS = 3.0; //!< error factor: SBAS + +const int MAXEXFILE = 1024; //!< max number of expanded files +const double MAXSBSAGEF = 30.0; //!< max age of SBAS fast correction (s) +const double MAXSBSAGEL = 1800.0; //!< max age of SBAS long term corr (s) -#define MAXEXFILE 1024 /* max number of expanded files */ -#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */ -#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */ -#define MAXSBSURA 8 /* max URA of SBAS satellite */ -#define MAXBAND 10 /* max SBAS band of IGP */ -#define MAXNIGP 201 /* max number of IGP in SBAS band */ +typedef void fatalfunc_t(const char *); //!< fatal callback function type -typedef void fatalfunc_t(const char *); /* fatal callback function type */ typedef struct { /* time struct */ time_t time; /* time (s) expressed by standard time_t */ double sec; /* fraction of second under 1 s */ } gtime_t; + typedef struct { /* observation data record */ gtime_t time; /* receiver sampling time (GPST) */ unsigned char sat,rcv; /* satellite/receiver number */ @@ -409,11 +296,13 @@ typedef struct { /* observation data record */ float D[NFREQ+NEXOBS]; /* observation data doppler frequency (Hz) */ } obsd_t; + typedef struct { /* observation data */ int n,nmax; /* number of obervation data/allocated */ obsd_t *data; /* observation data records */ } obs_t; + typedef struct { /* earth rotation parameter data type */ double mjd; /* mjd (days) */ double xp,yp; /* pole offset (rad) */ @@ -422,11 +311,13 @@ typedef struct { /* earth rotation parameter data type */ double lod; /* length of day (s/day) */ } erpd_t; + typedef struct { /* earth rotation parameter type */ int n,nmax; /* number and max number of data */ erpd_t *data; /* earth rotation parameter data */ } erp_t; + typedef struct { /* antenna parameter type */ int sat; /* satellite number (0:receiver) */ char type[MAXANT]; /* antenna type */ @@ -437,11 +328,13 @@ typedef struct { /* antenna parameter type */ /* el=90,85,...,0 or nadir=0,1,2,3,... (deg) */ } pcv_t; + typedef struct { /* antenna parameters type */ int n,nmax; /* number of data/allocated */ pcv_t *pcv; /* antenna parameters data */ } pcvs_t; + typedef struct { /* almanac type */ int sat; /* satellite number */ int svh; /* sv health (0:ok) */ @@ -454,14 +347,15 @@ typedef struct { /* almanac type */ double f0,f1; /* SV clock parameters (af0,af1) */ } alm_t; + typedef struct { /* GPS/QZS/GAL broadcast ephemeris type */ int sat; /* satellite number */ int iode,iodc; /* IODE,IODC */ int sva; /* SV accuracy (URA index) */ int svh; /* SV health (0:ok) */ int week; /* GPS/QZS: gps week, GAL: galileo week */ - int code; /* GPS/QZS: code on L2, GAL/CMP: data sources */ - int flag; /* GPS/QZS: L2 P data flag, CMP: nav type */ + int code; /* GPS/QZS: code on L2, GAL/BDS: data sources */ + int flag; /* GPS/QZS: L2 P data flag, BDS: nav type */ gtime_t toe,toc,ttr; /* Toe,Toc,T_trans */ /* SV orbit parameters */ double A,e,i0,OMG0,omg,M0,deln,OMGd,idot; @@ -472,10 +366,11 @@ typedef struct { /* GPS/QZS/GAL broadcast ephemeris type */ double tgd[4]; /* group delay parameters */ /* GPS/QZS:tgd[0]=TGD */ /* GAL :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */ - /* CMP :tgd[0]=BGD1,tgd[1]=BGD2 */ + /* BDS :tgd[0]=BGD1,tgd[1]=BGD2 */ double Adot,ndot; /* Adot,ndot for CNAV */ } eph_t; + typedef struct { /* GLONASS broadcast ephemeris type */ int sat; /* satellite number */ int iode; /* IODE (0-6 bit of tb field) */ @@ -490,6 +385,7 @@ typedef struct { /* GLONASS broadcast ephemeris type */ double dtaun; /* delay between L1 and L2 (s) */ } geph_t; + typedef struct { /* precise ephemeris type */ gtime_t time; /* time (GPST) */ int index; /* ephemeris index for multiple files */ @@ -501,6 +397,7 @@ typedef struct { /* precise ephemeris type */ float vco[MAXSAT][3]; /* satellite velocity covariance (m^2) */ } peph_t; + typedef struct { /* precise clock type */ gtime_t time; /* time (GPST) */ int index; /* clock index for multiple files */ @@ -508,6 +405,7 @@ typedef struct { /* precise clock type */ float std[MAXSAT][1]; /* satellite clock std (s) */ } pclk_t; + typedef struct { /* SBAS ephemeris type */ int sat; /* satellite number */ gtime_t t0; /* reference epoch time (GPST) */ @@ -520,6 +418,7 @@ typedef struct { /* SBAS ephemeris type */ double af0,af1; /* satellite clock-offset/drift (s,s/s) */ } seph_t; + typedef struct { /* norad two line element data type */ char name [32]; /* common name */ char alias[32]; /* alias name */ @@ -541,11 +440,13 @@ typedef struct { /* norad two line element data type */ int rev; /* revolution number at epoch */ } tled_t; + typedef struct { /* norad two line element type */ int n,nmax; /* number/max number of two line element data */ tled_t *data; /* norad two line element data */ } tle_t; + typedef struct { /* TEC grid type */ gtime_t time; /* epoch time (GPST) */ int ndata[3]; /* TEC grid data size {nlat,nlon,nhgt} */ @@ -557,23 +458,27 @@ typedef struct { /* TEC grid type */ float *rms; /* RMS values (tecu) */ } tec_t; + typedef struct { /* satellite fcb data type */ gtime_t ts,te; /* start/end time (GPST) */ double bias[MAXSAT][3]; /* fcb value (cyc) */ double std [MAXSAT][3]; /* fcb std-dev (cyc) */ } fcbd_t; + typedef struct { /* SBAS message type */ int week,tow; /* receiption time */ int prn; /* SBAS satellite PRN number */ unsigned char msg[29]; /* SBAS message (226bit) padded by 0 */ } sbsmsg_t; + typedef struct { /* SBAS messages type */ int n,nmax; /* number of SBAS messages/allocated */ sbsmsg_t *msgs; /* SBAS messages */ } sbs_t; + typedef struct { /* SBAS fast correction type */ gtime_t t0; /* time of applicability (TOF) */ double prc; /* pseudorange correction (PRC) (m) */ @@ -584,6 +489,7 @@ typedef struct { /* SBAS fast correction type */ short ai; /* degradation factor indicator */ } sbsfcorr_t; + typedef struct { /* SBAS long term satellite error correction type */ gtime_t t0; /* correction time */ int iode; /* IODE (issue of date ephemeris) */ @@ -592,12 +498,14 @@ typedef struct { /* SBAS long term satellite error correction type */ double daf0,daf1; /* delta clock-offset/drift (s,s/s) */ } sbslcorr_t; + typedef struct { /* SBAS satellite correction type */ int sat; /* satellite number */ sbsfcorr_t fcorr; /* fast correction */ sbslcorr_t lcorr; /* long term correction */ } sbssatp_t; + typedef struct { /* SBAS satellite corrections type */ int iodp; /* IODP (issue of date mask) */ int nsat; /* number of satellites */ @@ -605,6 +513,7 @@ typedef struct { /* SBAS satellite corrections type */ sbssatp_t sat[MAXSAT]; /* satellite correction */ } sbssat_t; + typedef struct { /* SBAS ionospheric correction type */ gtime_t t0; /* correction time */ short lat,lon; /* latitude/longitude (deg) */ @@ -612,6 +521,7 @@ typedef struct { /* SBAS ionospheric correction type */ float delay; /* vertical delay estimate (m) */ } sbsigp_t; + typedef struct { /* IGP band type */ short x; /* longitude/latitude (deg) */ const short *y; /* latitudes/longitudes (deg) */ @@ -619,12 +529,14 @@ typedef struct { /* IGP band type */ unsigned char bite; /* IGP mask end bit */ } sbsigpband_t; + typedef struct { /* SBAS ionospheric corrections type */ int iodi; /* IODI (issue of date ionos corr) */ int nigp; /* number of igps */ sbsigp_t igp[MAXNIGP]; /* ionospheric correction */ } sbsion_t; + typedef struct { /* DGPS/GNSS correction type */ gtime_t t0; /* correction time */ double prc; /* pseudorange correction (PRC) (m) */ @@ -633,6 +545,7 @@ typedef struct { /* DGPS/GNSS correction type */ double udre; /* UDRE */ } dgps_t; + typedef struct { /* SSR correction type */ gtime_t t0[6]; /* epoch time (GPST) {eph,clk,hrclk,ura,bias,pbias} */ double udi[6]; /* SSR update interval (s) */ @@ -652,6 +565,7 @@ typedef struct { /* SSR correction type */ unsigned char update; /* update flag (0:no update,1:update) */ } ssr_t; + typedef struct { /* QZSS LEX message type */ int prn; /* satellite PRN number */ int type; /* message type */ @@ -662,11 +576,13 @@ typedef struct { /* QZSS LEX message type */ unsigned char msg[212]; /* LEX message data part 1695 bits */ } lexmsg_t; + typedef struct { /* QZSS LEX messages type */ int n,nmax; /* number of LEX messages and allocated */ lexmsg_t *msgs; /* LEX messages */ } lex_t; + typedef struct { /* QZSS LEX ephemeris type */ gtime_t toe; /* epoch time (GPST) */ gtime_t tof; /* message frame time (GPST) */ @@ -682,6 +598,7 @@ typedef struct { /* QZSS LEX ephemeris type */ double isc[8]; /* ISC */ } lexeph_t; + typedef struct { /* QZSS LEX ionosphere correction type */ gtime_t t0; /* epoch time (GPST) */ double tspan; /* valid time span (s) */ @@ -689,6 +606,7 @@ typedef struct { /* QZSS LEX ionosphere correction type */ double coef[3][2]; /* coefficients lat x lon (3 x 2) */ } lexion_t; + typedef struct { /* stec data type */ gtime_t time; /* time (GPST) */ unsigned char sat; /* satellite number */ @@ -698,12 +616,14 @@ typedef struct { /* stec data type */ unsigned char flag; /* fix flag */ } stec_t; + typedef struct { /* trop data type */ gtime_t time; /* time (GPST) */ double trp[3]; /* zenith tropos delay/gradient (m) */ float std[3]; /* std-dev (m) */ } trop_t; + typedef struct { /* ppp corrections type */ int nsta; /* number of stations */ char stas[MAXSTA][8]; /* station names */ @@ -714,6 +634,7 @@ typedef struct { /* ppp corrections type */ trop_t *trop[MAXSTA]; /* trop data */ } pppcorr_t; + typedef struct { /* navigation data type */ int n,nmax; /* number of broadcast ephemeris */ int ng,ngmax; /* number of glonass ephemeris */ @@ -761,6 +682,7 @@ typedef struct { /* navigation data type */ pppcorr_t pppcorr; /* ppp corrections */ } nav_t; + typedef struct { /* station parameter type */ char name [MAXANT]; /* marker name */ char marker [MAXANT]; /* marker number */ @@ -777,6 +699,7 @@ typedef struct { /* station parameter type */ double hgt; /* antenna height (m) */ } sta_t; + typedef struct { /* solution type */ gtime_t time; /* time (GPST) */ double rr[6]; /* position/velocity (m|m/s) */ @@ -793,6 +716,7 @@ typedef struct { /* solution type */ float thres; /* AR ratio threshold for valiation */ } sol_t; + typedef struct { /* solution buffer type */ int n,nmax; /* number of solution/max number of buffer */ int cyclic; /* cyclic buffer flag */ @@ -804,6 +728,7 @@ typedef struct { /* solution buffer type */ int nb; /* number of byte in message buffer */ } solbuf_t; + typedef struct { /* solution status type */ gtime_t time; /* time (GPST) */ unsigned char sat; /* satellite number */ @@ -819,11 +744,13 @@ typedef struct { /* solution status type */ unsigned short rejc; /* reject counter */ } solstat_t; + typedef struct { /* solution status buffer type */ int n,nmax; /* number of solution/max number of buffer */ solstat_t *data; /* solution status data */ } solstatbuf_t; + typedef struct { /* RTCM control struct type */ int staid; /* station id */ int stah; /* station health */ @@ -855,6 +782,7 @@ typedef struct { /* RTCM control struct type */ char opt[256]; /* RTCM dependent options */ } rtcm_t; + typedef struct { /* download url type */ char type[32]; /* data type */ char path[1024]; /* url path */ @@ -862,6 +790,7 @@ typedef struct { /* download url type */ double tint; /* time interval (s) */ } url_t; + typedef struct { /* option type */ const char *name; /* option name */ int format; /* option format (0:int,1:double,2:string,3:enum) */ @@ -869,6 +798,7 @@ typedef struct { /* option type */ const char *comment; /* option comment/enum labels/unit */ } opt_t; + typedef struct { /* extended receiver error model */ int ena[4]; /* model enabled */ double cerr[4][NFREQ*2]; /* code errors (m) */ @@ -877,11 +807,13 @@ typedef struct { /* extended receiver error model */ double gloicb [NFREQ]; /* glonass interchannel bias (m/fn) */ } exterr_t; + typedef struct { /* SNR mask type */ int ena[2]; /* enable flag {rover,base} */ double mask[NFREQ][9]; /* mask (dBHz) at 5,10,...85 deg */ } snrmask_t; + typedef struct { /* processing options type */ int mode; /* positioning mode (PMODE_???) */ int soltype; /* solution type (0:forward,1:backward,2:combined) */ @@ -944,6 +876,7 @@ typedef struct { /* processing options type */ char pppopt[256]; /* ppp option */ } prcopt_t; + typedef struct { /* solution options type */ int posf; /* solution format (SOLF_???) */ int times; /* time system (TIMES_???) */ @@ -966,7 +899,6 @@ typedef struct { /* solution options type */ } solopt_t; - typedef struct { /* satellite status type */ unsigned char sys; /* navigation system */ unsigned char vs; /* valid satellite flag single */ @@ -990,6 +922,7 @@ typedef struct { /* satellite status type */ double ph[2][NFREQ]; /* previous carrier-phase observable (cycle) */ } ssat_t; + typedef struct { /* ambiguity control type */ gtime_t epoch[4]; /* last epoch */ int n[4]; /* number of epochs */ @@ -999,6 +932,7 @@ typedef struct { /* ambiguity control type */ char flags[MAXSAT]; /* fix flags */ } ambc_t; + typedef struct { /* RTK control/result type */ sol_t sol; /* RTK solution */ double rb[6]; /* base position/velocity (ecef) (m|m/s) */ @@ -1014,6 +948,7 @@ typedef struct { /* RTK control/result type */ prcopt_t opt; /* processing options */ } rtk_t; + typedef struct half_cyc_tag { /* half-cycle correction list type */ unsigned char sat; /* satellite number */ unsigned char freq; /* frequency number (0:L1,1:L2,2:L5) */ @@ -1024,21 +959,23 @@ typedef struct half_cyc_tag { /* half-cycle correction list type */ } half_cyc_t; -const double chisqr[100]={ /* chi-sqr(n) (alpha=0.001) */ - 10.8,13.8,16.3,18.5,20.5,22.5,24.3,26.1,27.9,29.6, - 31.3,32.9,34.5,36.1,37.7,39.3,40.8,42.3,43.8,45.3, - 46.8,48.3,49.7,51.2,52.6,54.1,55.5,56.9,58.3,59.7, - 61.1,62.5,63.9,65.2,66.6,68.0,69.3,70.7,72.1,73.4, - 74.7,76.0,77.3,78.6,80.0,81.3,82.6,84.0,85.4,86.7, - 88.0,89.3,90.6,91.9,93.3,94.7,96.0,97.4,98.7,100 , - 101 ,102 ,103 ,104 ,105 ,107 ,108 ,109 ,110 ,112 , - 113 ,114 ,115 ,116 ,118 ,119 ,120 ,122 ,123 ,125 , - 126 ,127 ,128 ,129 ,131 ,132 ,133 ,134 ,135 ,137 , - 138 ,139 ,140 ,142 ,143 ,144 ,145 ,147 ,148 ,149 +const double chisqr[100] = { /* chi-sqr(n) (alpha=0.001) */ + 10.8, 13.8, 16.3, 18.5, 20.5, 22.5, 24.3, 26.1, 27.9, 29.6, + 31.3, 32.9, 34.5, 36.1, 37.7, 39.3, 40.8, 42.3, 43.8, 45.3, + 46.8, 48.3, 49.7, 51.2, 52.6, 54.1, 55.5, 56.9, 58.3, 59.7, + 61.1, 62.5, 63.9, 65.2, 66.6, 68.0, 69.3, 70.7, 72.1, 73.4, + 74.7, 76.0, 77.3, 78.6, 80.0, 81.3, 82.6, 84.0, 85.4, 86.7, + 88.0, 89.3, 90.6, 91.9, 93.3, 94.7, 96.0, 97.4, 98.7, 100 , + 101 , 102 , 103 , 104 , 105 , 107 , 108 , 109 , 110 , 112 , + 113 , 114 , 115 , 116 , 118 , 119 , 120 , 122 , 123 , 125 , + 126 , 127 , 128 , 129 , 131 , 132 , 133 , 134 , 135 , 137 , + 138 , 139 , 140 , 142 , 143 , 144 , 145 , 147 , 148 , 149 }; -const double lam_carr[MAXFREQ]={ /* carrier wave length (m) */ - CLIGHT/FREQ1,CLIGHT/FREQ2,CLIGHT/FREQ5,CLIGHT/FREQ6,CLIGHT/FREQ7, - CLIGHT/FREQ8,CLIGHT/FREQ9 + + +const double lam_carr[MAXFREQ] = { /* carrier wave length (m) */ + SPEED_OF_LIGHT / FREQ1, SPEED_OF_LIGHT / FREQ2, SPEED_OF_LIGHT / FREQ5, SPEED_OF_LIGHT / FREQ6, SPEED_OF_LIGHT / FREQ7, + SPEED_OF_LIGHT / FREQ8, SPEED_OF_LIGHT / FREQ9 }; #endif diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index 12d3108f2..96350a2c6 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -46,165 +46,171 @@ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/ + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + #include "rtklib_conversions.h" obsd_t obs_to_rtklib(Gnss_Synchro gnss_synchro, int week) { - obsd_t rtklib_obs; - rtklib_obs.D[0]=gnss_synchro.Carrier_Doppler_hz; - rtklib_obs.P[0]=gnss_synchro.Pseudorange_m; - rtklib_obs.L[0]=gnss_synchro.Carrier_phase_rads;//todo: check units - //rtklib_obs.SNR=gnss_synchro.CN0_dB_hz; - rtklib_obs.sat=gnss_synchro.PRN; - rtklib_obs.time=gpst2time(adjgpsweek(week),gnss_synchro.RX_time); + obsd_t rtklib_obs = {}; + rtklib_obs.D[0] = gnss_synchro.Carrier_Doppler_hz; + rtklib_obs.P[0] = gnss_synchro.Pseudorange_m; + rtklib_obs.L[0] = gnss_synchro.Carrier_phase_rads;//todo: check units + //rtklib_obs.SNR = gnss_synchro.CN0_dB_hz; + rtklib_obs.sat = gnss_synchro.PRN; + rtklib_obs.time = gpst2time(adjgpsweek(week), gnss_synchro.RX_time); //printf("OBS RX TIME [%i]: %s,%f\n\r",rtklib_obs.sat,time_str(rtklib_obs.time,3),rtklib_obs.time.sec); return rtklib_obs; } + + eph_t eph_to_rtklib(Galileo_Ephemeris gal_eph) { - eph_t rtklib_sat; + eph_t rtklib_sat = {}; rtklib_sat.sat = gal_eph.i_satellite_PRN; - rtklib_sat.A=gal_eph.A_1*gal_eph.A_1; - rtklib_sat.M0=gal_eph.M0_1; - rtklib_sat.deln=gal_eph.delta_n_3; - rtklib_sat.OMG0=gal_eph.OMEGA_0_2; - rtklib_sat.OMGd=gal_eph.OMEGA_dot_3; - rtklib_sat.omg=gal_eph.omega_2; - rtklib_sat.i0=gal_eph.i_0_2; - rtklib_sat.idot=gal_eph.iDot_2; - rtklib_sat.e=gal_eph.e_1; - rtklib_sat.Adot=0; //only in CNAV; - rtklib_sat.ndot=0; //only in CNAV; + rtklib_sat.A = gal_eph.A_1 * gal_eph.A_1; + rtklib_sat.M0 = gal_eph.M0_1; + rtklib_sat.deln = gal_eph.delta_n_3; + rtklib_sat.OMG0 = gal_eph.OMEGA_0_2; + rtklib_sat.OMGd = gal_eph.OMEGA_dot_3; + rtklib_sat.omg = gal_eph.omega_2; + rtklib_sat.i0 = gal_eph.i_0_2; + rtklib_sat.idot = gal_eph.iDot_2; + rtklib_sat.e = gal_eph.e_1; + rtklib_sat.Adot = 0; //only in CNAV; + rtklib_sat.ndot = 0; //only in CNAV; - rtklib_sat.week=adjgpsweek(gal_eph.WN_5); /* week of tow */ - rtklib_sat.cic=gal_eph.C_ic_4; - rtklib_sat.cis=gal_eph.C_is_4; - rtklib_sat.cuc=gal_eph.C_uc_3; - rtklib_sat.cus=gal_eph.C_us_3; - rtklib_sat.crc=gal_eph.C_rc_3; - rtklib_sat.crs=gal_eph.C_rs_3; - rtklib_sat.f0=gal_eph.af0_4; - rtklib_sat.f1=gal_eph.af1_4; - rtklib_sat.f2=gal_eph.af2_4; - rtklib_sat.tgd[0]=0; - rtklib_sat.tgd[1]=0; - rtklib_sat.tgd[2]=0; - rtklib_sat.tgd[3]=0; - rtklib_sat.toes=gal_eph.t0e_1; - rtklib_sat.toc=gpst2time(rtklib_sat.week,gal_eph.t0c_4); - rtklib_sat.ttr=gpst2time(rtklib_sat.week,gal_eph.TOW_5); + rtklib_sat.week = adjgpsweek(gal_eph.WN_5); /* week of tow */ + rtklib_sat.cic = gal_eph.C_ic_4; + rtklib_sat.cis = gal_eph.C_is_4; + rtklib_sat.cuc = gal_eph.C_uc_3; + rtklib_sat.cus = gal_eph.C_us_3; + rtklib_sat.crc = gal_eph.C_rc_3; + rtklib_sat.crs = gal_eph.C_rs_3; + rtklib_sat.f0 = gal_eph.af0_4; + rtklib_sat.f1 = gal_eph.af1_4; + rtklib_sat.f2 = gal_eph.af2_4; + rtklib_sat.tgd[0] = 0; + rtklib_sat.tgd[1] = 0; + rtklib_sat.tgd[2] = 0; + rtklib_sat.tgd[3] = 0; + rtklib_sat.toes = gal_eph.t0e_1; + rtklib_sat.toc = gpst2time(rtklib_sat.week, gal_eph.t0c_4); + rtklib_sat.ttr = gpst2time(rtklib_sat.week, gal_eph.TOW_5); /* adjustment for week handover */ double tow, toc; - tow=time2gpst(rtklib_sat.ttr,&rtklib_sat.week); - toc=time2gpst(rtklib_sat.toc,NULL); - if (rtklib_sat.toestow+302400.0) {rtklib_sat.week--; tow+=604800.0;} - rtklib_sat.toe=gpst2time(rtklib_sat.week,rtklib_sat.toes); - rtklib_sat.toc=gpst2time(rtklib_sat.week,toc); - rtklib_sat.ttr=gpst2time(rtklib_sat.week,tow); + tow = time2gpst(rtklib_sat.ttr, &rtklib_sat.week); + toc = time2gpst(rtklib_sat.toc, NULL); + if (rtklib_sat.toes < tow - 302400.0) {rtklib_sat.week++; tow -= 604800.0;} + else if (rtklib_sat.toes > tow + 302400.0) {rtklib_sat.week--; tow += 604800.0;} + rtklib_sat.toe = gpst2time(rtklib_sat.week, rtklib_sat.toes); + rtklib_sat.toc = gpst2time(rtklib_sat.week, toc); + rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); return rtklib_sat; } + eph_t eph_to_rtklib(Gps_Ephemeris gps_eph) { - eph_t rtklib_sat; + eph_t rtklib_sat = {}; rtklib_sat.sat = gps_eph.i_satellite_PRN; - rtklib_sat.A=gps_eph.d_sqrt_A*gps_eph.d_sqrt_A; - rtklib_sat.M0=gps_eph.d_M_0; - rtklib_sat.deln=gps_eph.d_Delta_n; - rtklib_sat.OMG0=gps_eph.d_OMEGA0; - rtklib_sat.OMGd=gps_eph.d_OMEGA_DOT; - rtklib_sat.omg=gps_eph.d_OMEGA; - rtklib_sat.i0=gps_eph.d_i_0; - rtklib_sat.idot=gps_eph.d_IDOT; - rtklib_sat.e=gps_eph.d_e_eccentricity; - rtklib_sat.Adot=0; //only in CNAV; - rtklib_sat.ndot=0; //only in CNAV; + rtklib_sat.A = gps_eph.d_sqrt_A * gps_eph.d_sqrt_A; + rtklib_sat.M0 = gps_eph.d_M_0; + rtklib_sat.deln = gps_eph.d_Delta_n; + rtklib_sat.OMG0 = gps_eph.d_OMEGA0; + rtklib_sat.OMGd = gps_eph.d_OMEGA_DOT; + rtklib_sat.omg = gps_eph.d_OMEGA; + rtklib_sat.i0 = gps_eph.d_i_0; + rtklib_sat.idot = gps_eph.d_IDOT; + rtklib_sat.e = gps_eph.d_e_eccentricity; + rtklib_sat.Adot = 0; //only in CNAV; + rtklib_sat.ndot = 0; //only in CNAV; - rtklib_sat.week=adjgpsweek(gps_eph.i_GPS_week); /* week of tow */ - rtklib_sat.cic=gps_eph.d_Cic; - rtklib_sat.cis=gps_eph.d_Cis; - rtklib_sat.cuc=gps_eph.d_Cuc; - rtklib_sat.cus=gps_eph.d_Cus; - rtklib_sat.crc=gps_eph.d_Crc; - rtklib_sat.crs=gps_eph.d_Crs; - rtklib_sat.f0=gps_eph.d_A_f0; - rtklib_sat.f1=gps_eph.d_A_f1; - rtklib_sat.f2=gps_eph.d_A_f2; - rtklib_sat.tgd[0]=gps_eph.d_TGD; - rtklib_sat.tgd[1]=0; - rtklib_sat.tgd[2]=0; - rtklib_sat.tgd[3]=0; - rtklib_sat.toes=gps_eph.d_Toe; - rtklib_sat.toc=gpst2time(rtklib_sat.week,gps_eph.d_Toc); - rtklib_sat.ttr=gpst2time(rtklib_sat.week,gps_eph.d_TOW); + rtklib_sat.week = adjgpsweek(gps_eph.i_GPS_week); /* week of tow */ + rtklib_sat.cic = gps_eph.d_Cic; + rtklib_sat.cis = gps_eph.d_Cis; + rtklib_sat.cuc = gps_eph.d_Cuc; + rtklib_sat.cus = gps_eph.d_Cus; + rtklib_sat.crc = gps_eph.d_Crc; + rtklib_sat.crs = gps_eph.d_Crs; + rtklib_sat.f0 = gps_eph.d_A_f0; + rtklib_sat.f1 = gps_eph.d_A_f1; + rtklib_sat.f2 = gps_eph.d_A_f2; + rtklib_sat.tgd[0] = gps_eph.d_TGD; + rtklib_sat.tgd[1] = 0; + rtklib_sat.tgd[2] = 0; + rtklib_sat.tgd[3] = 0; + rtklib_sat.toes = gps_eph.d_Toe; + rtklib_sat.toc = gpst2time(rtklib_sat.week, gps_eph.d_Toc); + rtklib_sat.ttr = gpst2time(rtklib_sat.week, gps_eph.d_TOW); /* adjustment for week handover */ double tow, toc; - tow=time2gpst(rtklib_sat.ttr,&rtklib_sat.week); - toc=time2gpst(rtklib_sat.toc,NULL); - if (rtklib_sat.toestow+302400.0) {rtklib_sat.week--; tow+=604800.0;} - rtklib_sat.toe=gpst2time(rtklib_sat.week,rtklib_sat.toes); - rtklib_sat.toc=gpst2time(rtklib_sat.week,toc); - rtklib_sat.ttr=gpst2time(rtklib_sat.week,tow); + tow = time2gpst(rtklib_sat.ttr, &rtklib_sat.week); + toc = time2gpst(rtklib_sat.toc, NULL); + if (rtklib_sat.toes < tow - 302400.0) {rtklib_sat.week++; tow -= 604800.0;} + else if (rtklib_sat.toes > tow + 302400.0) {rtklib_sat.week--; tow += 604800.0;} + rtklib_sat.toe = gpst2time(rtklib_sat.week, rtklib_sat.toes); + rtklib_sat.toc = gpst2time(rtklib_sat.week, toc); + rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); //printf("EPHEMERIS TIME [%i]: %s,%f\n\r",rtklib_sat.sat,time_str(rtklib_sat.toe,3),rtklib_sat.toe.sec); return rtklib_sat; } + + eph_t eph_to_rtklib(Gps_CNAV_Ephemeris gps_cnav_eph) { - eph_t rtklib_sat; + eph_t rtklib_sat = {}; rtklib_sat.sat = gps_cnav_eph.i_satellite_PRN; const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170 - rtklib_sat.A=A_REF + gps_cnav_eph.d_DELTA_A; - rtklib_sat.M0=gps_cnav_eph.d_M_0; - rtklib_sat.deln=gps_cnav_eph.d_Delta_n; - rtklib_sat.OMG0=gps_cnav_eph.d_OMEGA0; + rtklib_sat.A = A_REF + gps_cnav_eph.d_DELTA_A; + rtklib_sat.M0 = gps_cnav_eph.d_M_0; + rtklib_sat.deln = gps_cnav_eph.d_Delta_n; + rtklib_sat.OMG0 = gps_cnav_eph.d_OMEGA0; // Compute the angle between the ascending node and the Greenwich meridian const double OMEGA_DOT_REF = -2.6e-9; // semicircles / s, see IS-GPS-200H pp. 164 - double d_OMEGA_DOT = OMEGA_DOT_REF*GPS_L2_PI + gps_cnav_eph.d_DELTA_OMEGA_DOT; - rtklib_sat.OMGd=d_OMEGA_DOT; - rtklib_sat.omg=gps_cnav_eph.d_OMEGA; - rtklib_sat.i0=gps_cnav_eph.d_i_0; - rtklib_sat.idot=gps_cnav_eph.d_IDOT; - rtklib_sat.e=gps_cnav_eph.d_e_eccentricity; - rtklib_sat.Adot=gps_cnav_eph.d_A_DOT; //only in CNAV; - rtklib_sat.ndot=gps_cnav_eph.d_DELTA_DOT_N; //only in CNAV; + double d_OMEGA_DOT = OMEGA_DOT_REF * GPS_L2_PI + gps_cnav_eph.d_DELTA_OMEGA_DOT; + rtklib_sat.OMGd = d_OMEGA_DOT; + rtklib_sat.omg = gps_cnav_eph.d_OMEGA; + rtklib_sat.i0 = gps_cnav_eph.d_i_0; + rtklib_sat.idot = gps_cnav_eph.d_IDOT; + rtklib_sat.e = gps_cnav_eph.d_e_eccentricity; + rtklib_sat.Adot = gps_cnav_eph.d_A_DOT; //only in CNAV; + rtklib_sat.ndot = gps_cnav_eph.d_DELTA_DOT_N; //only in CNAV; - rtklib_sat.week=adjgpsweek(gps_cnav_eph.i_GPS_week); /* week of tow */ - rtklib_sat.cic=gps_cnav_eph.d_Cic; - rtklib_sat.cis=gps_cnav_eph.d_Cis; - rtklib_sat.cuc=gps_cnav_eph.d_Cuc; - rtklib_sat.cus=gps_cnav_eph.d_Cus; - rtklib_sat.crc=gps_cnav_eph.d_Crc; - rtklib_sat.crs=gps_cnav_eph.d_Crs; - rtklib_sat.f0=gps_cnav_eph.d_A_f0; - rtklib_sat.f1=gps_cnav_eph.d_A_f1; - rtklib_sat.f2=gps_cnav_eph.d_A_f2; - rtklib_sat.tgd[0]=gps_cnav_eph.d_TGD; - rtklib_sat.tgd[1]=0; - rtklib_sat.tgd[2]=0; - rtklib_sat.tgd[3]=0; - rtklib_sat.toes=gps_cnav_eph.d_Toe1; - rtklib_sat.toc=gpst2time(rtklib_sat.week,gps_cnav_eph.d_Toc); - rtklib_sat.ttr=gpst2time(rtklib_sat.week,gps_cnav_eph.d_TOW); + rtklib_sat.week = adjgpsweek(gps_cnav_eph.i_GPS_week); /* week of tow */ + rtklib_sat.cic = gps_cnav_eph.d_Cic; + rtklib_sat.cis = gps_cnav_eph.d_Cis; + rtklib_sat.cuc = gps_cnav_eph.d_Cuc; + rtklib_sat.cus = gps_cnav_eph.d_Cus; + rtklib_sat.crc = gps_cnav_eph.d_Crc; + rtklib_sat.crs = gps_cnav_eph.d_Crs; + rtklib_sat.f0 = gps_cnav_eph.d_A_f0; + rtklib_sat.f1 = gps_cnav_eph.d_A_f1; + rtklib_sat.f2 = gps_cnav_eph.d_A_f2; + rtklib_sat.tgd[0] = gps_cnav_eph.d_TGD; + rtklib_sat.tgd[1] = 0; + rtklib_sat.tgd[2] = 0; + rtklib_sat.tgd[3] = 0; + rtklib_sat.toes = gps_cnav_eph.d_Toe1; + rtklib_sat.toc = gpst2time(rtklib_sat.week,gps_cnav_eph.d_Toc); + rtklib_sat.ttr = gpst2time(rtklib_sat.week,gps_cnav_eph.d_TOW); /* adjustment for week handover */ double tow, toc; - tow=time2gpst(rtklib_sat.ttr,&rtklib_sat.week); - toc=time2gpst(rtklib_sat.toc,NULL); - if (rtklib_sat.toestow+302400.0) {rtklib_sat.week--; tow+=604800.0;} - rtklib_sat.toe=gpst2time(rtklib_sat.week,rtklib_sat.toes); - rtklib_sat.toc=gpst2time(rtklib_sat.week,toc); - rtklib_sat.ttr=gpst2time(rtklib_sat.week,tow); + tow = time2gpst(rtklib_sat.ttr, &rtklib_sat.week); + toc = time2gpst(rtklib_sat.toc, NULL); + if (rtklib_sat.toes < tow - 302400.0) {rtklib_sat.week++; tow -= 604800.0;} + else if (rtklib_sat.toes > tow + 302400.0) {rtklib_sat.week--; tow += 604800.0;} + rtklib_sat.toe = gpst2time(rtklib_sat.week, rtklib_sat.toes); + rtklib_sat.toc = gpst2time(rtklib_sat.week, toc); + rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); return rtklib_sat; - } diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.h b/src/algorithms/libs/rtklib/rtklib_conversions.h index 4a9fd2374..617557a62 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.h +++ b/src/algorithms/libs/rtklib/rtklib_conversions.h @@ -47,8 +47,10 @@ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/ -#ifndef SRC_ALGORITHMS_PVT_LIBS_RTKLIB_CONVERSIONS_H_ -#define SRC_ALGORITHMS_PVT_LIBS_RTKLIB_CONVERSIONS_H_ + + +#ifndef GNSS_SDR_RTKLIB_CONVERSIONS_H_ +#define GNSS_SDR_RTKLIB_CONVERSIONS_H_ #include "rtklib_rtkcmn.h" #include "gnss_synchro.h" @@ -62,4 +64,4 @@ eph_t eph_to_rtklib(Gps_CNAV_Ephemeris gps_cnav_eph); obsd_t obs_to_rtklib(Gnss_Synchro gnss_synchro, int week); -#endif /* SRC_ALGORITHMS_PVT_LIBS_RTKLIB_CONVERSIONS_H_ */ +#endif /* GNSS_SDR_RTKLIB_CONVERSIONS_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index 02e671fb2..fba5040da 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -47,751 +47,824 @@ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -* history : 2010/07/28 1.1 moved from rtkcmn.c -* added api: -* eph2clk(),geph2clk(),seph2clk(),satantoff() -* satposs() -* changed api: -* eph2pos(),geph2pos(),satpos() -* deleted api: -* satposv(),satposiode() -* 2010/08/26 1.2 add ephemeris option EPHOPT_LEX -* 2010/09/09 1.3 fix problem when precise clock outage -* 2011/01/12 1.4 add api alm2pos() -* change api satpos(),satposs() -* enable valid unhealthy satellites and output status -* fix bug on exception by glonass ephem computation -* 2013/01/10 1.5 support beidou (compass) -* use newton's method to solve kepler eq. -* update ssr correction algorithm -* 2013/03/20 1.6 fix problem on ssr clock relativitic correction -* 2013/09/01 1.7 support negative pseudorange -* fix bug on variance in case of ura ssr = 63 -* 2013/11/11 1.8 change constant MAXAGESSR 70.0 -> 90.0 -* 2014/10/24 1.9 fix bug on return of var_uraeph() if ura<0||15 1E-13 -* set MAX_ITER_KEPLER for alm2pos() -*-----------------------------------------------------------------------------*/ + * + */ #include "rtklib_ephemeris.h" -static const char rcsid[]="$Id:$"; -/* constants and macros ------------------------------------------------------*/ +/* constants ------------------------------------------------------*/ -#define SQR(x) ((x)*(x)) +const double RE_GLO = 6378136.0; /* radius of earth (m) ref [2] */ +const double MU_GPS = 3.9860050e14; /* gravitational constant ref [1] */ +const double MU_GLO = 3.9860044e14; /* gravitational constant ref [2] */ +const double MU_GAL = 3.986004418e14; /* earth gravitational constant ref [7] */ +const double MU_BDS = 3.986004418e14; /* earth gravitational constant ref [9] */ +const double J2_GLO = 1.0826257e-3; /* 2nd zonal harmonic of geopot ref [2] */ -#define RE_GLO 6378136.0 /* radius of earth (m) ref [2] */ -#define MU_GPS 3.9860050E14 /* gravitational constant ref [1] */ -#define MU_GLO 3.9860044E14 /* gravitational constant ref [2] */ -#define MU_GAL 3.986004418E14 /* earth gravitational constant ref [7] */ -#define MU_CMP 3.986004418E14 /* earth gravitational constant ref [9] */ -#define J2_GLO 1.0826257E-3 /* 2nd zonal harmonic of geopot ref [2] */ +const double OMGE_GLO = 7.292115e-5; /* earth angular velocity (rad/s) ref [2] */ +const double OMGE_GAL = 7.2921151467e-5; /* earth angular velocity (rad/s) ref [7] */ +const double OMGE_BDS = 7.292115e-5; /* earth angular velocity (rad/s) ref [9] */ -#define OMGE_GLO 7.292115E-5 /* earth angular velocity (rad/s) ref [2] */ -#define OMGE_GAL 7.2921151467E-5 /* earth angular velocity (rad/s) ref [7] */ -#define OMGE_CMP 7.292115E-5 /* earth angular velocity (rad/s) ref [9] */ +const double SIN_5 = -0.0871557427476582; /* sin(-5.0 deg) */ +const double COS_5 = 0.9961946980917456; /* cos(-5.0 deg) */ -#define SIN_5 -0.0871557427476582 /* sin(-5.0 deg) */ -#define COS_5 0.9961946980917456 /* cos(-5.0 deg) */ +const double ERREPH_GLO = 5.0; /* error of glonass ephemeris (m) */ +const double TSTEP = 60.0; /* integration step glonass ephemeris (s) */ +const double RTOL_KEPLER = 1e-13; /* relative tolerance for Kepler equation */ -#define ERREPH_GLO 5.0 /* error of glonass ephemeris (m) */ -#define TSTEP 60.0 /* integration step glonass ephemeris (s) */ -#define RTOL_KEPLER 1E-13 /* relative tolerance for Kepler equation */ +const double DEFURASSR = 0.15; /* default accurary of ssr corr (m) */ +const double MAXECORSSR = 10.0; /* max orbit correction of ssr (m) */ +const double MAXCCORSSR = 1e-6 * SPEED_OF_LIGHT; /* max clock correction of ssr (m) */ +const double MAXAGESSR = 90.0; /* max age of ssr orbit and clock (s) */ +const double MAXAGESSR_HRCLK = 10.0; /* max age of ssr high-rate clock (s) */ +const double STD_BRDCCLK = 30.0; /* error of broadcast clock (m) */ -#define DEFURASSR 0.15 /* default accurary of ssr corr (m) */ -#define MAXECORSSR 10.0 /* max orbit correction of ssr (m) */ -#define MAXCCORSSR (1E-6*CLIGHT) /* max clock correction of ssr (m) */ -#define MAXAGESSR 90.0 /* max age of ssr orbit and clock (s) */ -#define MAXAGESSR_HRCLK 10.0 /* max age of ssr high-rate clock (s) */ -#define STD_BRDCCLK 30.0 /* error of broadcast clock (m) */ +const int MAX_ITER_KEPLER = 30; /* max number of iteration of Kelpler */ -#define MAX_ITER_KEPLER 30 /* max number of iteration of Kelpler */ /* variance by ura ephemeris (ref [1] 20.3.3.3.1.1) --------------------------*/ double var_uraeph(int ura) { - const double ura_value[]={ - 2.4,3.4,4.85,6.85,9.65,13.65,24.0,48.0,96.0,192.0,384.0,768.0,1536.0, - 3072.0,6144.0 + const double ura_value[] = { + 2.4, 3.4, 4.85, 6.85, 9.65, 13.65, 24.0, 48.0, 96.0, 192.0, 384.0, 768.0, 1536.0, + 3072.0, 6144.0 }; - return ura<0||15=63) return SQR(5.4665); - std=(pow(3.0,(ura>>3)&7)*(1.0+(ura&7)/4.0)-1.0)*1E-3; - return SQR(std); + double std_; + if (ura <= 0) return std::pow(DEFURASSR, 2.0); + if (ura >= 63) return std::pow(5.4665, 2.0); + std_ = (std::pow((ura >> 3) & 7, 2.0) * (1.0 + (ura & 7) / 4.0) - 1.0) * 1e-3; + return std::pow(std_, 2.0); } + + /* almanac to satellite position and clock bias -------------------------------- -* compute satellite position and clock bias with almanac (gps, galileo, qzss) -* args : gtime_t time I time (gpst) -* alm_t *alm I almanac -* double *rs O satellite position (ecef) {x,y,z} (m) -* double *dts O satellite clock bias (s) -* return : none -* notes : see ref [1],[7],[8] -*-----------------------------------------------------------------------------*/ + * compute satellite position and clock bias with almanac (gps, galileo, qzss) + * args : gtime_t time I time (gpst) + * alm_t *alm I almanac + * double *rs O satellite position (ecef) {x,y,z} (m) + * double *dts O satellite clock bias (s) + * return : none + * notes : see ref [1],[7],[8] + *-----------------------------------------------------------------------------*/ void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts) { - double tk,M,E,Ek,sinE,cosE,u,r,i,O,x,y,sinO,cosO,cosi,mu; + double tk, M, E, Ek, sinE, cosE, u, r, i, O, x, y, sinO, cosO, cosi, mu; int n; - - trace(4,"alm2pos : time=%s sat=%2d\n",time_str(time,3),alm->sat); - - tk=timediff(time,alm->toa); - - if (alm->A<=0.0) { - rs[0]=rs[1]=rs[2]=*dts=0.0; - return; - } - mu=satsys(alm->sat,NULL)==SYS_GAL?MU_GAL:MU_GPS; - - M=alm->M0+sqrt(mu/(alm->A*alm->A*alm->A))*tk; - for (n=0,E=M,Ek=0.0;fabs(E-Ek)>RTOL_KEPLER&&ne*sin(E)-M)/(1.0-alm->e*cos(E)); - } - if (n>=MAX_ITER_KEPLER) { - trace(2,"alm2pos: kepler iteration overflow sat=%2d\n",alm->sat); - return; - } - sinE=sin(E); cosE=cos(E); - u=atan2(sqrt(1.0-alm->e*alm->e)*sinE,cosE-alm->e)+alm->omg; - r=alm->A*(1.0-alm->e*cosE); - i=alm->i0; - O=alm->OMG0+(alm->OMGd-OMGE)*tk-OMGE*alm->toas; - x=r*cos(u); y=r*sin(u); sinO=sin(O); cosO=cos(O); cosi=cos(i); - rs[0]=x*cosO-y*cosi*sinO; - rs[1]=x*sinO+y*cosi*cosO; - rs[2]=y*sin(i); - *dts=alm->f0+alm->f1*tk; + + trace(4, "alm2pos : time=%s sat=%2d\n", time_str(time, 3), alm->sat); + + tk = timediff(time, alm->toa); + + if (alm->A <= 0.0) + { + rs[0] = rs[1] = rs[2] = *dts = 0.0; + return; + } + mu = satsys(alm->sat, NULL) == SYS_GAL ? MU_GAL : MU_GPS; + + M = alm->M0 + sqrt(mu / (alm->A * alm->A * alm->A)) * tk; + for (n = 0, E = M, Ek = 0.0; fabs(E - Ek) > RTOL_KEPLER && n < MAX_ITER_KEPLER; n++) + { + Ek = E; + E -= (E - alm->e * sin(E) - M) / (1.0 - alm->e * cos(E)); + } + if (n >= MAX_ITER_KEPLER) + { + trace(2, "alm2pos: kepler iteration overflow sat=%2d\n", alm->sat); + return; + } + sinE = sin(E); + cosE = cos(E); + u = atan2(sqrt(1.0 - alm->e * alm->e) * sinE, cosE - alm->e) + alm->omg; + r = alm->A * (1.0 - alm->e*cosE); + i = alm->i0; + O = alm->OMG0 + (alm->OMGd - DEFAULT_OMEGA_EARTH_DOT) * tk - DEFAULT_OMEGA_EARTH_DOT * alm->toas; + x = r * cos(u); + y = r * sin(u); + sinO = sin(O); + cosO = cos(O); + cosi = cos(i); + rs[0] = x * cosO - y * cosi * sinO; + rs[1] = x * sinO + y * cosi * cosO; + rs[2] = y * sin(i); + *dts = alm->f0 + alm->f1 * tk; } + + /* broadcast ephemeris to satellite clock bias --------------------------------- -* compute satellite clock bias with broadcast ephemeris (gps, galileo, qzss) -* args : gtime_t time I time by satellite clock (gpst) -* eph_t *eph I broadcast ephemeris -* return : satellite clock bias (s) without relativeity correction -* notes : see ref [1],[7],[8] -* satellite clock does not include relativity correction and tdg -*-----------------------------------------------------------------------------*/ + * compute satellite clock bias with broadcast ephemeris (gps, galileo, qzss) + * args : gtime_t time I time by satellite clock (gpst) + * eph_t *eph I broadcast ephemeris + * return : satellite clock bias (s) without relativeity correction + * notes : see ref [1],[7],[8] + * satellite clock does not include relativity correction and tdg + *-----------------------------------------------------------------------------*/ double eph2clk(gtime_t time, const eph_t *eph) { double t; int i; - - trace(4,"eph2clk : time=%s sat=%2d\n",time_str(time,3),eph->sat); - - t=timediff(time,eph->toc); - - for (i=0;i<2;i++) { - t-=eph->f0+eph->f1*t+eph->f2*t*t; - } - return eph->f0+eph->f1*t+eph->f2*t*t; + + trace(4, "eph2clk : time=%s sat=%2d\n", time_str(time, 3), eph->sat); + + t = timediff(time, eph->toc); + + for (i = 0; i < 2; i++) + { + t -= eph->f0 + eph->f1 * t + eph->f2 * t * t; + } + return eph->f0 + eph->f1 * t + eph->f2 * t *t; } + + /* broadcast ephemeris to satellite position and clock bias -------------------- -* compute satellite position and clock bias with broadcast ephemeris (gps, -* galileo, qzss) -* args : gtime_t time I time (gpst) -* eph_t *eph I broadcast ephemeris -* double *rs O satellite position (ecef) {x,y,z} (m) -* double *dts O satellite clock bias (s) -* double *var O satellite position and clock variance (m^2) -* return : none -* notes : see ref [1],[7],[8] -* satellite clock includes relativity correction without code bias -* (tgd or bgd) -*-----------------------------------------------------------------------------*/ + * compute satellite position and clock bias with broadcast ephemeris (gps, + * galileo, qzss) + * args : gtime_t time I time (gpst) + * eph_t *eph I broadcast ephemeris + * double *rs O satellite position (ecef) {x,y,z} (m) + * double *dts O satellite clock bias (s) + * double *var O satellite position and clock variance (m^2) + * return : none + * notes : see ref [1],[7],[8] + * satellite clock includes relativity correction without code bias + * (tgd or bgd) + *-----------------------------------------------------------------------------*/ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, - double *var) + double *var) { - double tk,M,E,Ek,sinE,cosE,u,r,i,O,sin2u,cos2u,x,y,sinO,cosO,cosi,mu,omge; - double xg,yg,zg,sino,coso; - int n,sys,prn; - - trace(4,"eph2pos : time=%s sat=%2d\n",time_str(time,3),eph->sat); - - if (eph->A<=0.0) { - rs[0]=rs[1]=rs[2]=*dts=*var=0.0; - return; + double tk, M, E, Ek, sinE, cosE, u, r, i, O, sin2u, cos2u, x, y, sinO, cosO, cosi, mu, omge; + double xg, yg, zg, sino, coso; + int n, sys, prn; + + trace(4, "eph2pos : time=%s sat=%2d\n", time_str(time, 3), eph->sat); + + if (eph->A <= 0.0) + { + rs[0] = rs[1] = rs[2] = *dts = *var = 0.0; + return; + } + tk = timediff(time , eph->toe); + + switch ((sys = satsys(eph->sat, &prn))) + { + case SYS_GAL: mu = MU_GAL; omge = OMGE_GAL; break; + case SYS_BDS: mu = MU_BDS; omge = OMGE_BDS; break; + default: mu = MU_GPS; omge = DEFAULT_OMEGA_EARTH_DOT; break; } - tk=timediff(time,eph->toe); - - switch ((sys=satsys(eph->sat,&prn))) { - case SYS_GAL: mu=MU_GAL; omge=OMGE_GAL; break; - case SYS_CMP: mu=MU_CMP; omge=OMGE_CMP; break; - default: mu=MU_GPS; omge=OMGE; break; - } - M=eph->M0+(sqrt(mu/(eph->A*eph->A*eph->A))+eph->deln)*tk; - - for (n=0,E=M,Ek=0.0;fabs(E-Ek)>RTOL_KEPLER&&ne*sin(E)-M)/(1.0-eph->e*cos(E)); - } - if (n>=MAX_ITER_KEPLER) { - trace(2,"eph2pos: kepler iteration overflow sat=%2d\n",eph->sat); - return; - } - sinE=sin(E); cosE=cos(E); - - trace(4,"kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n",eph->sat,eph->e,n,E-Ek); - - u=atan2(sqrt(1.0-eph->e*eph->e)*sinE,cosE-eph->e)+eph->omg; - r=eph->A*(1.0-eph->e*cosE); - i=eph->i0+eph->idot*tk; - sin2u=sin(2.0*u); cos2u=cos(2.0*u); - u+=eph->cus*sin2u+eph->cuc*cos2u; - r+=eph->crs*sin2u+eph->crc*cos2u; - i+=eph->cis*sin2u+eph->cic*cos2u; - x=r*cos(u); y=r*sin(u); cosi=cos(i); - + M = eph->M0 + (sqrt(mu / (eph->A * eph->A * eph->A)) + eph->deln) * tk; + + for (n = 0, E = M, Ek = 0.0; fabs(E - Ek) > RTOL_KEPLER && n < MAX_ITER_KEPLER; n++) + { + Ek = E; + E -= (E - eph->e * sin(E) - M) / (1.0 - eph->e * cos(E)); + } + if (n >= MAX_ITER_KEPLER) + { + trace(2, "eph2pos: kepler iteration overflow sat=%2d\n", eph->sat); + return; + } + sinE = sin(E); + cosE = cos(E); + + trace(4, "kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n", eph->sat, eph->e, n, E - Ek); + + u = atan2(sqrt(1.0 - eph->e*eph->e) * sinE, cosE-eph->e) + eph->omg; + r = eph->A * (1.0 - eph->e * cosE); + i = eph->i0 + eph->idot * tk; + sin2u = sin(2.0 * u); cos2u = cos(2.0 * u); + u += eph->cus * sin2u + eph->cuc * cos2u; + r += eph->crs * sin2u + eph->crc * cos2u; + i += eph->cis * sin2u + eph->cic * cos2u; + x = r * cos(u); + y = r * sin(u); + cosi = cos(i); + /* beidou geo satellite (ref [9]) */ - if (sys==SYS_CMP&&prn<=5) { - O=eph->OMG0+eph->OMGd*tk-omge*eph->toes; - sinO=sin(O); cosO=cos(O); - xg=x*cosO-y*cosi*sinO; - yg=x*sinO+y*cosi*cosO; - zg=y*sin(i); - sino=sin(omge*tk); coso=cos(omge*tk); - rs[0]= xg*coso+yg*sino*COS_5+zg*sino*SIN_5; - rs[1]=-xg*sino+yg*coso*COS_5+zg*coso*SIN_5; - rs[2]=-yg*SIN_5+zg*COS_5; - } - else { - O=eph->OMG0+(eph->OMGd-omge)*tk-omge*eph->toes; - sinO=sin(O); cosO=cos(O); - rs[0]=x*cosO-y*cosi*sinO; - rs[1]=x*sinO+y*cosi*cosO; - rs[2]=y*sin(i); - } - tk=timediff(time,eph->toc); - *dts=eph->f0+eph->f1*tk+eph->f2*tk*tk; - + if (sys == SYS_BDS && prn <= 5) + { + O = eph->OMG0 + eph->OMGd * tk - omge * eph->toes; + sinO = sin(O); + cosO = cos(O); + xg = x * cosO - y * cosi * sinO; + yg = x * sinO + y * cosi * cosO; + zg = y * sin(i); + sino = sin(omge * tk); + coso = cos(omge * tk); + rs[0] = xg * coso + yg * sino * COS_5 + zg * sino * SIN_5; + rs[1] = -xg * sino + yg * coso * COS_5 + zg * coso * SIN_5; + rs[2] = -yg * SIN_5 + zg * COS_5; + } + else + { + O = eph->OMG0 + (eph->OMGd - omge) * tk - omge * eph->toes; + sinO = sin(O); + cosO = cos(O); + rs[0] = x * cosO - y * cosi * sinO; + rs[1] = x * sinO + y *cosi * cosO; + rs[2] = y * sin(i); + } + tk = timediff(time, eph->toc); + *dts = eph->f0 + eph->f1 * tk + eph->f2 * tk * tk; + /* relativity correction */ - *dts-=2.0*sqrt(mu*eph->A)*eph->e*sinE/SQR(CLIGHT); - + *dts -= 2.0 * sqrt(mu * eph->A) * eph-> e* sinE / std::pow(SPEED_OF_LIGHT, 2.0); + /* position and clock error variance */ - *var=var_uraeph(eph->sva); + *var = var_uraeph(eph->sva); } + + /* glonass orbit differential equations --------------------------------------*/ void deq(const double *x, double *xdot, const double *acc) { - double a,b,c,r2=dot(x,x,3),r3=r2*sqrt(r2),omg2=SQR(OMGE_GLO); - - if (r2<=0.0) { - xdot[0]=xdot[1]=xdot[2]=xdot[3]=xdot[4]=xdot[5]=0.0; - return; - } + double a, b, c, r2 = dot(x, x, 3), r3 = r2 * sqrt(r2), omg2 = std::pow(OMGE_GLO, 2.0); + + if (r2 <= 0.0) + { + xdot[0] = xdot[1] = xdot[2] = xdot[3] = xdot[4] = xdot[5] = 0.0; + return; + } /* ref [2] A.3.1.2 with bug fix for xdot[4],xdot[5] */ - a=1.5*J2_GLO*MU_GLO*SQR(RE_GLO)/r2/r3; /* 3/2*J2*mu*Ae^2/r^5 */ - b=5.0*x[2]*x[2]/r2; /* 5*z^2/r^2 */ - c=-MU_GLO/r3-a*(1.0-b); /* -mu/r^3-a(1-b) */ - xdot[0]=x[3]; xdot[1]=x[4]; xdot[2]=x[5]; - xdot[3]=(c+omg2)*x[0]+2.0*OMGE_GLO*x[4]+acc[0]; - xdot[4]=(c+omg2)*x[1]-2.0*OMGE_GLO*x[3]+acc[1]; - xdot[5]=(c-2.0*a)*x[2]+acc[2]; + a = 1.5 * J2_GLO * MU_GLO * std::pow(RE_GLO, 2.0) / r2 / r3; /* 3/2*J2*mu*Ae^2/r^5 */ + b = 5.0 * x[2] * x[2] / r2; /* 5*z^2/r^2 */ + c = -MU_GLO / r3 - a * (1.0 - b); /* -mu/r^3-a(1-b) */ + xdot[0] = x[3]; + xdot[1] = x[4]; xdot[2] = x[5]; + xdot[3] = (c + omg2) * x[0] + 2.0 * OMGE_GLO * x[4] + acc[0]; + xdot[4] = (c + omg2) * x[1] - 2.0 * OMGE_GLO * x[3] + acc[1]; + xdot[5] = (c - 2.0 * a) * x[2] + acc[2]; } + + /* glonass position and velocity by numerical integration --------------------*/ void glorbit(double t, double *x, const double *acc) { - double k1[6],k2[6],k3[6],k4[6],w[6]; + double k1[6], k2[6], k3[6], k4[6], w[6]; int i; - - deq(x,k1,acc); for (i=0;i<6;i++) w[i]=x[i]+k1[i]*t/2.0; - deq(w,k2,acc); for (i=0;i<6;i++) w[i]=x[i]+k2[i]*t/2.0; - deq(w,k3,acc); for (i=0;i<6;i++) w[i]=x[i]+k3[i]*t; - deq(w,k4,acc); - for (i=0;i<6;i++) x[i]+=(k1[i]+2.0*k2[i]+2.0*k3[i]+k4[i])*t/6.0; + + deq(x, k1, acc); for (i = 0; i< 6; i++) w[i] = x[i] + k1[i] * t / 2.0; + deq(w, k2, acc); for (i = 0; i < 6; i++) w[i] = x[i] + k2[i] * t / 2.0; + deq(w, k3, acc); for (i = 0; i < 6; i++) w[i] = x[i] + k3[i] * t; + deq(w, k4, acc); + for (i = 0; i < 6; i++) x[i] += (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]) * t / 6.0; } + + /* glonass ephemeris to satellite clock bias ----------------------------------- -* compute satellite clock bias with glonass ephemeris -* args : gtime_t time I time by satellite clock (gpst) -* geph_t *geph I glonass ephemeris -* return : satellite clock bias (s) -* notes : see ref [2] -*-----------------------------------------------------------------------------*/ + * compute satellite clock bias with glonass ephemeris + * args : gtime_t time I time by satellite clock (gpst) + * geph_t *geph I glonass ephemeris + * return : satellite clock bias (s) + * notes : see ref [2] + *-----------------------------------------------------------------------------*/ double geph2clk(gtime_t time, const geph_t *geph) { double t; int i; - - trace(4,"geph2clk: time=%s sat=%2d\n",time_str(time,3),geph->sat); - - t=timediff(time,geph->toe); - - for (i=0;i<2;i++) { - t-=-geph->taun+geph->gamn*t; - } - return -geph->taun+geph->gamn*t; + + trace(4, "geph2clk: time=%s sat=%2d\n", time_str(time, 3), geph->sat); + + t = timediff(time, geph->toe); + + for (i = 0; i < 2; i++) + { + t -= -geph->taun + geph->gamn * t; + } + return -geph->taun + geph->gamn * t; } + + /* glonass ephemeris to satellite position and clock bias ---------------------- -* compute satellite position and clock bias with glonass ephemeris -* args : gtime_t time I time (gpst) -* geph_t *geph I glonass ephemeris -* double *rs O satellite position {x,y,z} (ecef) (m) -* double *dts O satellite clock bias (s) -* double *var O satellite position and clock variance (m^2) -* return : none -* notes : see ref [2] -*-----------------------------------------------------------------------------*/ + * compute satellite position and clock bias with glonass ephemeris + * args : gtime_t time I time (gpst) + * geph_t *geph I glonass ephemeris + * double *rs O satellite position {x,y,z} (ecef) (m) + * double *dts O satellite clock bias (s) + * double *var O satellite position and clock variance (m^2) + * return : none + * notes : see ref [2] + *-----------------------------------------------------------------------------*/ void geph2pos(gtime_t time, const geph_t *geph, double *rs, double *dts, double *var) { - double t,tt,x[6]; + double t, tt, x[6]; int i; - - trace(4,"geph2pos: time=%s sat=%2d\n",time_str(time,3),geph->sat); - - t=timediff(time,geph->toe); - - *dts=-geph->taun+geph->gamn*t; - - for (i=0;i<3;i++) { - x[i ]=geph->pos[i]; - x[i+3]=geph->vel[i]; - } - for (tt=t<0.0?-TSTEP:TSTEP;fabs(t)>1E-9;t-=tt) { - if (fabs(t)acc); - } - for (i=0;i<3;i++) rs[i]=x[i]; - - *var=SQR(ERREPH_GLO); + + trace(4, "geph2pos: time=%s sat=%2d\n", time_str(time, 3), geph->sat); + + t = timediff(time, geph->toe); + + *dts = -geph->taun + geph->gamn * t; + + for (i = 0; i < 3; i++) + { + x[i ] = geph->pos[i]; + x[i+3] = geph->vel[i]; + } + for (tt = t < 0.0 ? - TSTEP : TSTEP; fabs(t) > 1e-9; t -= tt) + { + if (fabs(t) < TSTEP) tt = t; + glorbit(tt, x, geph->acc); + } + for (i = 0; i < 3; i++) rs[i] = x[i]; + + *var = std::pow(ERREPH_GLO, 2.0); } + + /* sbas ephemeris to satellite clock bias -------------------------------------- -* compute satellite clock bias with sbas ephemeris -* args : gtime_t time I time by satellite clock (gpst) -* seph_t *seph I sbas ephemeris -* return : satellite clock bias (s) -* notes : see ref [3] -*-----------------------------------------------------------------------------*/ + * compute satellite clock bias with sbas ephemeris + * args : gtime_t time I time by satellite clock (gpst) + * seph_t *seph I sbas ephemeris + * return : satellite clock bias (s) + * notes : see ref [3] + *-----------------------------------------------------------------------------*/ double seph2clk(gtime_t time, const seph_t *seph) { double t; int i; - - trace(4,"seph2clk: time=%s sat=%2d\n",time_str(time,3),seph->sat); - - t=timediff(time,seph->t0); - - for (i=0;i<2;i++) { - t-=seph->af0+seph->af1*t; - } - return seph->af0+seph->af1*t; + + trace(4, "seph2clk: time=%s sat=%2d\n", time_str(time, 3), seph->sat); + + t = timediff(time, seph->t0); + + for (i = 0; i < 2; i++) + { + t-=seph->af0 + seph->af1 * t; + } + return seph->af0 + seph->af1 * t; } + + /* sbas ephemeris to satellite position and clock bias ------------------------- -* compute satellite position and clock bias with sbas ephemeris -* args : gtime_t time I time (gpst) -* seph_t *seph I sbas ephemeris -* double *rs O satellite position {x,y,z} (ecef) (m) -* double *dts O satellite clock bias (s) -* double *var O satellite position and clock variance (m^2) -* return : none -* notes : see ref [3] -*-----------------------------------------------------------------------------*/ + * compute satellite position and clock bias with sbas ephemeris + * args : gtime_t time I time (gpst) + * seph_t *seph I sbas ephemeris + * double *rs O satellite position {x,y,z} (ecef) (m) + * double *dts O satellite clock bias (s) + * double *var O satellite position and clock variance (m^2) + * return : none + * notes : see ref [3] + *-----------------------------------------------------------------------------*/ void seph2pos(gtime_t time, const seph_t *seph, double *rs, double *dts, - double *var) + double *var) { double t; int i; - - trace(4,"seph2pos: time=%s sat=%2d\n",time_str(time,3),seph->sat); - - t=timediff(time,seph->t0); - - for (i=0;i<3;i++) { - rs[i]=seph->pos[i]+seph->vel[i]*t+seph->acc[i]*t*t/2.0; - } - *dts=seph->af0+seph->af1*t; - - *var=var_uraeph(seph->sva); + + trace(4, "seph2pos: time=%s sat=%2d\n", time_str(time, 3), seph->sat); + + t = timediff(time, seph->t0); + + for (i = 0; i < 3; i++) + { + rs[i] = seph->pos[i] + seph->vel[i] * t + seph->acc[i] * t * t / 2.0; + } + *dts = seph->af0 + seph->af1 * t; + + *var = var_uraeph(seph->sva); } + + /* select ephememeris --------------------------------------------------------*/ eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav) { - double t,tmax,tmin; - int i,j=-1; - - trace(4,"seleph : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode); - - switch (satsys(sat,NULL)) { - case SYS_QZS: tmax=MAXDTOE_QZS+1.0; break; - case SYS_GAL: tmax=MAXDTOE_GAL+1.0; break; - case SYS_CMP: tmax=MAXDTOE_CMP+1.0; break; - default: tmax=MAXDTOE+1.0; break; + double t, tmax, tmin; + int i, j = -1; + + trace(4, "seleph : time=%s sat=%2d iode=%d\n", time_str(time, 3), sat, iode); + + switch (satsys(sat, NULL)) + { + case SYS_QZS: tmax = MAXDTOE_QZS + 1.0; break; + case SYS_GAL: tmax = MAXDTOE_GAL + 1.0; break; + case SYS_BDS: tmax = MAXDTOE_BDS + 1.0; break; + default: tmax = MAXDTOE + 1.0; break; } - tmin=tmax+1.0; - - for (i=0;in;i++) { - if (nav->eph[i].sat!=sat) continue; - if (iode>=0&&nav->eph[i].iode!=iode) continue; - if ((t=fabs(timediff(nav->eph[i].toe,time)))>tmax) continue; - if (iode>=0) return nav->eph+i; - if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ - } - if (iode>=0||j<0) { - trace(3,"no broadcast ephemeris: %s sat=%2d iode=%3d\n",time_str(time,0), - sat,iode); - return NULL; - } - return nav->eph+j; + tmin = tmax + 1.0; + + for (i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + if (iode >= 0 && nav->eph[i].iode != iode) continue; + if ((t = fabs(timediff(nav->eph[i].toe, time))) > tmax) continue; + if (iode >= 0) return nav->eph + i; + if (t <= tmin) {j = i; tmin = t;} /* toe closest to time */ + } + if (iode >= 0 || j<0) + { + trace(3, "no broadcast ephemeris: %s sat=%2d iode=%3d\n", time_str(time, 0), + sat, iode); + return NULL; + } + return nav->eph + j; } + + /* select glonass ephememeris ------------------------------------------------*/ geph_t *selgeph(gtime_t time, int sat, int iode, const nav_t *nav) { - double t,tmax=MAXDTOE_GLO,tmin=tmax+1.0; - int i,j=-1; - - trace(4,"selgeph : time=%s sat=%2d iode=%2d\n",time_str(time,3),sat,iode); - - for (i=0;ing;i++) { - if (nav->geph[i].sat!=sat) continue; - if (iode>=0&&nav->geph[i].iode!=iode) continue; - if ((t=fabs(timediff(nav->geph[i].toe,time)))>tmax) continue; - if (iode>=0) return nav->geph+i; - if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ - } - if (iode>=0||j<0) { - trace(3,"no glonass ephemeris : %s sat=%2d iode=%2d\n",time_str(time,0), - sat,iode); - return NULL; - } - return nav->geph+j; + double t, tmax = MAXDTOE_GLO, tmin = tmax + 1.0; + int i, j = -1; + + trace(4, "selgeph : time=%s sat=%2d iode=%2d\n", time_str(time, 3), sat, iode); + + for (i = 0; i < nav->ng; i++) + { + if (nav->geph[i].sat != sat) continue; + if (iode >= 0 && nav->geph[i].iode != iode) continue; + if ((t = fabs(timediff(nav->geph[i].toe, time))) > tmax) continue; + if (iode >= 0) return nav->geph + i; + if (t <= tmin) {j = i; tmin = t;} /* toe closest to time */ + } + if (iode >= 0 || j < 0) + { + trace(3, "no glonass ephemeris : %s sat=%2d iode=%2d\n", time_str(time, 0), + sat, iode); + return NULL; + } + return nav->geph + j; } + + /* select sbas ephememeris ---------------------------------------------------*/ seph_t *selseph(gtime_t time, int sat, const nav_t *nav) { - double t,tmax=MAXDTOE_SBS,tmin=tmax+1.0; - int i,j=-1; - - trace(4,"selseph : time=%s sat=%2d\n",time_str(time,3),sat); - - for (i=0;ins;i++) { - if (nav->seph[i].sat!=sat) continue; - if ((t=fabs(timediff(nav->seph[i].t0,time)))>tmax) continue; - if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ - } - if (j<0) { - trace(3,"no sbas ephemeris : %s sat=%2d\n",time_str(time,0),sat); - return NULL; - } - return nav->seph+j; + double t, tmax = MAXDTOE_SBS, tmin = tmax + 1.0; + int i, j = -1; + + trace(4, "selseph : time=%s sat=%2d\n", time_str(time, 3), sat); + + for (i = 0; i < nav->ns; i++) + { + if (nav->seph[i].sat != sat) continue; + if ((t = fabs(timediff(nav->seph[i].t0, time))) > tmax) continue; + if (t <= tmin) {j = i; tmin = t;} /* toe closest to time */ + } + if (j < 0) + { + trace(3, "no sbas ephemeris : %s sat=%2d\n", time_str(time, 0), sat); + return NULL; + } + return nav->seph + j; } + + /* satellite clock with broadcast ephemeris ----------------------------------*/ int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav, - double *dts) + double *dts) { eph_t *eph; geph_t *geph; seph_t *seph; int sys; - - trace(4,"ephclk : time=%s sat=%2d\n",time_str(time,3),sat); - - sys=satsys(sat,NULL); - - if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) { - if (!(eph=seleph(teph,sat,-1,nav))) return 0; - *dts=eph2clk(time,eph); - } - else if (sys==SYS_GLO) { - if (!(geph=selgeph(teph,sat,-1,nav))) return 0; - *dts=geph2clk(time,geph); - } - else if (sys==SYS_SBS) { - if (!(seph=selseph(teph,sat,nav))) return 0; - *dts=seph2clk(time,seph); - } + + trace(4, "ephclk : time=%s sat=%2d\n", time_str(time, 3), sat); + + sys = satsys(sat, NULL); + + if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS) + { + if (!(eph = seleph(teph, sat, -1, nav))) return 0; + *dts = eph2clk(time, eph); + } + else if (sys == SYS_GLO) + { + if (!(geph = selgeph(teph, sat, -1, nav))) return 0; + *dts = geph2clk(time, geph); + } + else if (sys == SYS_SBS) + { + if (!(seph = selseph(teph, sat, nav))) return 0; + *dts = seph2clk(time, seph); + } else return 0; - + return 1; } + + /* satellite position and clock by broadcast ephemeris -----------------------*/ int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav, - int iode, double *rs, double *dts, double *var, int *svh) + int iode, double *rs, double *dts, double *var, int *svh) { eph_t *eph; geph_t *geph; seph_t *seph; - double rst[3],dtst[1],tt=1E-3; - int i,sys; - - trace(4,"ephpos : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode); - - sys=satsys(sat,NULL); - - *svh=-1; - - if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) { - if (!(eph=seleph(teph,sat,iode,nav))) return 0; - - eph2pos(time,eph,rs,dts,var); - time=timeadd(time,tt); - eph2pos(time,eph,rst,dtst,var); - *svh=eph->svh; - } - else if (sys==SYS_GLO) { - if (!(geph=selgeph(teph,sat,iode,nav))) return 0; - geph2pos(time,geph,rs,dts,var); - time=timeadd(time,tt); - geph2pos(time,geph,rst,dtst,var); - *svh=geph->svh; - } - else if (sys==SYS_SBS) { - if (!(seph=selseph(teph,sat,nav))) return 0; - - seph2pos(time,seph,rs,dts,var); - time=timeadd(time,tt); - seph2pos(time,seph,rst,dtst,var); - *svh=seph->svh; - } + double rst[3], dtst[1], tt = 1e-3; + int i, sys; + + trace(4, "ephpos : time=%s sat=%2d iode=%d\n", time_str(time, 3), sat, iode); + + sys = satsys(sat, NULL); + + *svh = -1; + + if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS) + { + if (!(eph = seleph(teph, sat, iode, nav))) return 0; + + eph2pos(time, eph, rs, dts, var); + time = timeadd(time, tt); + eph2pos(time, eph, rst, dtst, var); + *svh = eph->svh; + } + else if (sys == SYS_GLO) + { + if (!(geph = selgeph(teph, sat, iode, nav))) return 0; + geph2pos(time, geph, rs, dts, var); + time = timeadd(time, tt); + geph2pos(time, geph, rst, dtst, var); + *svh = geph->svh; + } + else if (sys == SYS_SBS) + { + if (!(seph = selseph(teph, sat, nav))) return 0; + + seph2pos(time, seph, rs, dts, var); + time = timeadd(time, tt); + seph2pos(time, seph, rst, dtst, var); + *svh = seph->svh; + } else return 0; - + /* satellite velocity and clock drift by differential approx */ - for (i=0;i<3;i++) rs[i+3]=(rst[i]-rs[i])/tt; - dts[1]=(dtst[0]-dts[0])/tt; - + for (i = 0; i < 3; i++) rs[i + 3] = (rst[i] - rs[i]) / tt; + dts[1] = (dtst[0] - dts[0]) / tt; + return 1; } + + /* satellite position and clock with sbas correction -------------------------*/ int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav, - double *rs, double *dts, double *var, int *svh) + double *rs, double *dts, double *var, int *svh) { const sbssatp_t *sbs; int i; - - trace(4,"satpos_sbas: time=%s sat=%2d\n",time_str(time,3),sat); - + + trace(4, "satpos_sbas: time=%s sat=%2d\n", time_str(time, 3), sat); + /* search sbas satellite correciton */ - for (i=0;isbssat.nsat;i++) { - sbs=nav->sbssat.sat+i; - if (sbs->sat==sat) break; - } - if (i>=nav->sbssat.nsat) { - trace(2,"no sbas correction for orbit: %s sat=%2d\n",time_str(time,0),sat); - ephpos(time,teph,sat,nav,-1,rs,dts,var,svh); - *svh=-1; - return 0; - } + for (i = 0; i < nav->sbssat.nsat; i++) + { + sbs = nav->sbssat.sat + i; + if (sbs->sat == sat) break; + } + if (i >= nav->sbssat.nsat) + { + trace(2, "no sbas correction for orbit: %s sat=%2d\n", time_str(time, 0), sat); + ephpos(time, teph, sat, nav, -1, rs, dts, var, svh); + *svh = -1; + return 0; + } /* satellite postion and clock by broadcast ephemeris */ - if (!ephpos(time,teph,sat,nav,sbs->lcorr.iode,rs,dts,var,svh)) return 0; - + if (!ephpos(time, teph, sat, nav, sbs->lcorr.iode, rs, dts, var, svh)) return 0; + /* sbas satellite correction (long term and fast) */ - if (sbssatcorr(time,sat,nav,rs,dts,var)) return 1; - *svh=-1; + if (sbssatcorr(time, sat, nav, rs, dts, var)) return 1; + *svh = -1; return 0; } + + /* satellite position and clock with ssr correction --------------------------*/ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav, - int opt, double *rs, double *dts, double *var, int *svh) + int opt, double *rs, double *dts, double *var, int *svh) { const ssr_t *ssr; eph_t *eph; - double t1,t2,t3,er[3],ea[3],ec[3],rc[3],deph[3],dclk,dant[3]={0},tk; - int i,sys; - - trace(4,"satpos_ssr: time=%s sat=%2d\n",time_str(time,3),sat); - - ssr=nav->ssr+sat-1; - - if (!ssr->t0[0].time) { - trace(2,"no ssr orbit correction: %s sat=%2d\n",time_str(time,0),sat); - return 0; - } - if (!ssr->t0[1].time) { - trace(2,"no ssr clock correction: %s sat=%2d\n",time_str(time,0),sat); - return 0; - } + double t1, t2, t3, er[3], ea[3], ec[3], rc[3], deph[3], dclk, dant[3] = {0}, tk; + int i, sys; + + trace(4, "satpos_ssr: time=%s sat=%2d\n", time_str(time, 3), sat); + + ssr = nav->ssr + sat - 1; + + if (!ssr->t0[0].time) + { + trace(2, "no ssr orbit correction: %s sat=%2d\n", time_str(time, 0), sat); + return 0; + } + if (!ssr->t0[1].time) + { + trace(2, "no ssr clock correction: %s sat=%2d\n", time_str(time, 0), sat); + return 0; + } /* inconsistency between orbit and clock correction */ - if (ssr->iod[0]!=ssr->iod[1]) { - trace(2,"inconsist ssr correction: %s sat=%2d iod=%d %d\n", - time_str(time,0),sat,ssr->iod[0],ssr->iod[1]); - *svh=-1; - return 0; - } - t1=timediff(time,ssr->t0[0]); - t2=timediff(time,ssr->t0[1]); - t3=timediff(time,ssr->t0[2]); - + if (ssr->iod[0] != ssr->iod[1]) + { + trace(2, "inconsist ssr correction: %s sat=%2d iod=%d %d\n", + time_str(time, 0), sat, ssr->iod[0], ssr->iod[1]); + *svh = -1; + return 0; + } + t1 = timediff(time, ssr->t0[0]); + t2 = timediff(time, ssr->t0[1]); + t3 = timediff(time, ssr->t0[2]); + /* ssr orbit and clock correction (ref [4]) */ - if (fabs(t1)>MAXAGESSR||fabs(t2)>MAXAGESSR) { - trace(2,"age of ssr error: %s sat=%2d t=%.0f %.0f\n",time_str(time,0), - sat,t1,t2); - *svh=-1; - return 0; - } - if (ssr->udi[0]>=1.0) t1-=ssr->udi[0]/2.0; - if (ssr->udi[1]>=1.0) t2-=ssr->udi[0]/2.0; - - for (i=0;i<3;i++) deph[i]=ssr->deph[i]+ssr->ddeph[i]*t1; - dclk=ssr->dclk[0]+ssr->dclk[1]*t2+ssr->dclk[2]*t2*t2; - + if (fabs(t1) > MAXAGESSR || fabs(t2) > MAXAGESSR) + { + trace(2, "age of ssr error: %s sat=%2d t=%.0f %.0f\n", time_str(time, 0), + sat, t1, t2); + *svh = -1; + return 0; + } + if (ssr->udi[0] >= 1.0) t1 -= ssr->udi[0] / 2.0; + if (ssr->udi[1] >= 1.0) t2 -= ssr->udi[0] / 2.0; + + for (i = 0; i < 3; i++) deph[i] = ssr->deph[i] + ssr->ddeph[i] * t1; + dclk = ssr->dclk[0] + ssr->dclk[1] * t2 + ssr->dclk[2] * t2 * t2; + /* ssr highrate clock correction (ref [4]) */ - if (ssr->iod[0]==ssr->iod[2]&&ssr->t0[2].time&&fabs(t3)hrclk; - } - if (norm(deph,3)>MAXECORSSR||fabs(dclk)>MAXCCORSSR) { - trace(3,"invalid ssr correction: %s deph=%.1f dclk=%.1f\n", - time_str(time,0),norm(deph,3),dclk); - *svh=-1; - return 0; - } + if (ssr->iod[0] == ssr->iod[2] && ssr->t0[2].time && fabs(t3) < MAXAGESSR_HRCLK) + { + dclk += ssr->hrclk; + } + if (norm(deph, 3) > MAXECORSSR || fabs(dclk) > MAXCCORSSR) + { + trace(3, "invalid ssr correction: %s deph=%.1f dclk=%.1f\n", + time_str(time, 0), norm(deph, 3), dclk); + *svh = -1; + return 0; + } /* satellite postion and clock by broadcast ephemeris */ - if (!ephpos(time,teph,sat,nav,ssr->iode,rs,dts,var,svh)) return 0; - + if (!ephpos(time, teph, sat, nav, ssr->iode, rs, dts, var, svh)) return 0; + /* satellite clock for gps, galileo and qzss */ - sys=satsys(sat,NULL); - if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) { - if (!(eph=seleph(teph,sat,ssr->iode,nav))) return 0; - - /* satellite clock by clock parameters */ - tk=timediff(time,eph->toc); - dts[0]=eph->f0+eph->f1*tk+eph->f2*tk*tk; - dts[1]=eph->f1+2.0*eph->f2*tk; - - /* relativity correction */ - dts[0]-=2.0*dot(rs,rs+3,3)/CLIGHT/CLIGHT; - } + sys = satsys(sat, NULL); + if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS) + { + if (!(eph = seleph(teph, sat, ssr->iode, nav))) return 0; + + /* satellite clock by clock parameters */ + tk=timediff(time, eph->toc); + dts[0] = eph->f0 + eph->f1*tk + eph->f2 * tk * tk; + dts[1] = eph->f1 + 2.0*eph->f2 * tk; + + /* relativity correction */ + dts[0] -= 2.0 * dot(rs, rs + 3, 3) / SPEED_OF_LIGHT / SPEED_OF_LIGHT; + } /* radial-along-cross directions in ecef */ - if (!normv3(rs+3,ea)) return 0; - cross3(rs,rs+3,rc); - if (!normv3(rc,ec)) { - *svh=-1; - return 0; - } - cross3(ea,ec,er); - + if (!normv3(rs + 3, ea)) return 0; + cross3(rs, rs + 3, rc); + if (!normv3(rc, ec)) + { + *svh = -1; + return 0; + } + cross3(ea, ec, er); + /* satellite antenna offset correction */ - if (opt) { - satantoff(time,rs,sat,nav,dant); - } - for (i=0;i<3;i++) { - rs[i]+=-(er[i]*deph[0]+ea[i]*deph[1]+ec[i]*deph[2])+dant[i]; - } - /* t_corr = t_sv - (dts(brdc) + dclk(ssr) / CLIGHT) (ref [10] eq.3.12-7) */ - dts[0]+=dclk/CLIGHT; - + if (opt) + { + satantoff(time, rs, sat, nav, dant); + } + for (i = 0; i < 3; i++) + { + rs[i] += -(er[i] * deph[0] + ea[i] * deph[1] + ec[i] * deph[2]) + dant[i]; + } + /* t_corr = t_sv - (dts(brdc) + dclk(ssr) / SPEED_OF_LIGHT) (ref [10] eq.3.12-7) */ + dts[0] += dclk / SPEED_OF_LIGHT; + /* variance by ssr ura */ - *var=var_urassr(ssr->ura); - - trace(5,"satpos_ssr: %s sat=%2d deph=%6.3f %6.3f %6.3f er=%6.3f %6.3f %6.3f dclk=%6.3f var=%6.3f\n", - time_str(time,2),sat,deph[0],deph[1],deph[2],er[0],er[1],er[2],dclk,*var); - + *var = var_urassr(ssr->ura); + + trace(5, "satpos_ssr: %s sat=%2d deph=%6.3f %6.3f %6.3f er=%6.3f %6.3f %6.3f dclk=%6.3f var=%6.3f\n", + time_str(time, 2), sat, deph[0], deph[1], deph[2], er[0], er[1], er[2], dclk, *var); + return 1; } + + /* satellite position and clock ------------------------------------------------ -* compute satellite position, velocity and clock -* args : gtime_t time I time (gpst) -* gtime_t teph I time to select ephemeris (gpst) -* int sat I satellite number -* nav_t *nav I navigation data -* int ephopt I ephemeris option (EPHOPT_???) -* double *rs O sat position and velocity (ecef) -* {x,y,z,vx,vy,vz} (m|m/s) -* double *dts O sat clock {bias,drift} (s|s/s) -* double *var O sat position and clock error variance (m^2) -* int *svh O sat health flag (-1:correction not available) -* return : status (1:ok,0:error) -* notes : satellite position is referenced to antenna phase center -* satellite clock does not include code bias correction (tgd or bgd) -*-----------------------------------------------------------------------------*/ + * compute satellite position, velocity and clock + * args : gtime_t time I time (gpst) + * gtime_t teph I time to select ephemeris (gpst) + * int sat I satellite number + * nav_t *nav I navigation data + * int ephopt I ephemeris option (EPHOPT_???) + * double *rs O sat position and velocity (ecef) + * {x,y,z,vx,vy,vz} (m|m/s) + * double *dts O sat clock {bias,drift} (s|s/s) + * double *var O sat position and clock error variance (m^2) + * int *svh O sat health flag (-1:correction not available) + * return : status (1:ok,0:error) + * notes : satellite position is referenced to antenna phase center + * satellite clock does not include code bias correction (tgd or bgd) + *-----------------------------------------------------------------------------*/ int satpos(gtime_t time, gtime_t teph, int sat, int ephopt, - const nav_t *nav, double *rs, double *dts, double *var, - int *svh) + const nav_t *nav, double *rs, double *dts, double *var, + int *svh) { - trace(4,"satpos : time=%s sat=%2d ephopt=%d\n",time_str(time,3),sat,ephopt); - - *svh=0; - - switch (ephopt) { - case EPHOPT_BRDC : return ephpos (time,teph,sat,nav,-1,rs,dts,var,svh); - case EPHOPT_SBAS : return satpos_sbas(time,teph,sat,nav, rs,dts,var,svh); - case EPHOPT_SSRAPC: return satpos_ssr (time,teph,sat,nav, 0,rs,dts,var,svh); - case EPHOPT_SSRCOM: return satpos_ssr (time,teph,sat,nav, 1,rs,dts,var,svh); - case EPHOPT_PREC : - if (!peph2pos(time,sat,nav,1,rs,dts,var)) break; else return 1; + trace(4, "satpos : time=%s sat=%2d ephopt=%d\n", time_str(time, 3), sat, ephopt); + + *svh = 0; + + switch (ephopt) + { + case EPHOPT_BRDC : return ephpos (time, teph, sat, nav, -1, rs, dts, var, svh); + case EPHOPT_SBAS : return satpos_sbas(time, teph, sat, nav, rs, dts, var, svh); + case EPHOPT_SSRAPC: return satpos_ssr (time, teph, sat, nav, 0, rs, dts, var, svh); + case EPHOPT_SSRCOM: return satpos_ssr (time, teph, sat, nav, 1, rs, dts, var, svh); + case EPHOPT_PREC : + if (!peph2pos(time, sat, nav, 1, rs, dts, var)) break; else return 1; //TODO: enable lex //case EPHOPT_LEX : - // if (!lexeph2pos(time,sat,nav,rs,dts,var)) break; else return 1; + // if (!lexeph2pos(time, sat, nav, rs, dts, var)) break; else return 1; } - *svh=-1; + *svh = -1; return 0; } + + /* satellite positions and clocks ---------------------------------------------- -* compute satellite positions, velocities and clocks -* args : gtime_t teph I time to select ephemeris (gpst) -* obsd_t *obs I observation data -* int n I number of observation data -* nav_t *nav I navigation data -* int ephopt I ephemeris option (EPHOPT_???) -* double *rs O satellite positions and velocities (ecef) -* double *dts O satellite clocks -* double *var O sat position and clock error variances (m^2) -* int *svh O sat health flag (-1:correction not available) -* return : none -* notes : rs [(0:2)+i*6]= obs[i] sat position {x,y,z} (m) -* rs [(3:5)+i*6]= obs[i] sat velocity {vx,vy,vz} (m/s) -* dts[(0:1)+i*2]= obs[i] sat clock {bias,drift} (s|s/s) -* var[i] = obs[i] sat position and clock error variance (m^2) -* svh[i] = obs[i] sat health flag -* if no navigation data, set 0 to rs[], dts[], var[] and svh[] -* satellite position and clock are values at signal transmission time -* satellite position is referenced to antenna phase center -* satellite clock does not include code bias correction (tgd or bgd) -* any pseudorange and broadcast ephemeris are always needed to get -* signal transmission time -*-----------------------------------------------------------------------------*/ + * compute satellite positions, velocities and clocks + * args : gtime_t teph I time to select ephemeris (gpst) + * obsd_t *obs I observation data + * int n I number of observation data + * nav_t *nav I navigation data + * int ephopt I ephemeris option (EPHOPT_???) + * double *rs O satellite positions and velocities (ecef) + * double *dts O satellite clocks + * double *var O sat position and clock error variances (m^2) + * int *svh O sat health flag (-1:correction not available) + * return : none + * notes : rs [(0:2)+i*6]= obs[i] sat position {x,y,z} (m) + * rs [(3:5)+i*6]= obs[i] sat velocity {vx,vy,vz} (m/s) + * dts[(0:1)+i*2]= obs[i] sat clock {bias,drift} (s|s/s) + * var[i] = obs[i] sat position and clock error variance (m^2) + * svh[i] = obs[i] sat health flag + * if no navigation data, set 0 to rs[], dts[], var[] and svh[] + * satellite position and clock are values at signal transmission time + * satellite position is referenced to antenna phase center + * satellite clock does not include code bias correction (tgd or bgd) + * any pseudorange and broadcast ephemeris are always needed to get + * signal transmission time + *-----------------------------------------------------------------------------*/ void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav, - int ephopt, double *rs, double *dts, double *var, int *svh) + int ephopt, double *rs, double *dts, double *var, int *svh) { - gtime_t time[MAXOBS]={}; - double dt,pr; - int i,j; - - trace(3,"satposs : teph=%s n=%d ephopt=%d\n",time_str(teph,3),n,ephopt); - - for (i=0;i=NFREQ) { - trace(2,"no pseudorange %s sat=%2d\n",time_str(obs[i].time,3),obs[i].sat); - continue; + gtime_t time[MAXOBS] = {}; + double dt, pr; + int i, j; + + trace(3, "satposs : teph=%s n=%d ephopt=%d\n", time_str(teph, 3), n, ephopt); + + for (i = 0; i < n && i < MAXOBS; i++) + { + for (j = 0; j < 6; j++) rs [j + i * 6] = 0.0; + for (j = 0; j < 2; j++) dts[j + i * 2] = 0.0; + var[i] = 0.0; + svh[i] = 0; + + /* search any pseudorange */ + for (j = 0, pr = 0.0; j < NFREQ; j++) if ((pr = obs[i].P[j]) != 0.0) break; + + if (j >= NFREQ) + { + trace(2, "no pseudorange %s sat=%2d\n", time_str(obs[i].time, 3), obs[i].sat); + continue; + } + /* transmission time by satellite clock */ + time[i] = timeadd(obs[i].time, -pr / SPEED_OF_LIGHT); + + /* satellite clock bias by broadcast ephemeris */ + if (!ephclk(time[i], teph, obs[i].sat, nav, &dt)) + { + trace(3, "no broadcast clock %s sat=%2d\n", time_str(time[i], 3), obs[i].sat); + continue; + } + time[i] = timeadd(time[i], -dt); + + /* satellite position and clock at transmission time */ + if (!satpos(time[i], teph, obs[i].sat, ephopt, nav, rs + i * 6, dts + i * 2, var + i, + svh + i)) + { + trace(3, "no ephemeris %s sat=%2d\n", time_str(time[i], 3), obs[i].sat); + continue; + } + /* if no precise clock available, use broadcast clock instead */ + if (dts[i * 2] == 0.0) + { + if (!ephclk(time[i], teph, obs[i].sat, nav, dts + i * 2)) continue; + dts[1 + i * 2] = 0.0; + *var = std::pow(STD_BRDCCLK, 2.0); + } } - /* transmission time by satellite clock */ - time[i]=timeadd(obs[i].time,-pr/CLIGHT); - - /* satellite clock bias by broadcast ephemeris */ - if (!ephclk(time[i],teph,obs[i].sat,nav,&dt)) { - trace(3,"no broadcast clock %s sat=%2d\n",time_str(time[i],3),obs[i].sat); - continue; + for (i = 0; i < n && i < MAXOBS; i++) + { + trace(4, "%s sat=%2d rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f svh=%02X\n", + time_str(time[i], 6), obs[i].sat, rs[i * 6], rs[1 + i * 6], rs[2 + i * 6], + dts[i * 2] * 1e9, var[i], svh[i]); } - time[i]=timeadd(time[i],-dt); - - /* satellite position and clock at transmission time */ - if (!satpos(time[i],teph,obs[i].sat,ephopt,nav,rs+i*6,dts+i*2,var+i, - svh+i)) { - trace(3,"no ephemeris %s sat=%2d\n",time_str(time[i],3),obs[i].sat); - continue; - } - /* if no precise clock available, use broadcast clock instead */ - if (dts[i*2]==0.0) { - if (!ephclk(time[i],teph,obs[i].sat,nav,dts+i*2)) continue; - dts[1+i*2]=0.0; - *var=SQR(STD_BRDCCLK); - } - } - for (i=0;i 90.0 -* 2014/10/24 1.9 fix bug on return of var_uraeph() if ura<0||15 1E-13 -* set MAX_ITER_KEPLER for alm2pos() -*-----------------------------------------------------------------------------*/ + * + *-----------------------------------------------------------------------------*/ -#ifndef RTKLIB_EPHEMERIS_H_ -#define RTKLIB_EPHEMERIS_H_ +#ifndef GNSS_SDR_RTKLIB_EPHEMERIS_H_ +#define GNSS_SDR_RTKLIB_EPHEMERIS_H_ #include "rtklib.h" #include "rtklib_rtkcmn.h" @@ -122,4 +96,4 @@ void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav, -#endif /* RTKLIB_EPHEMERIS_H_ */ +#endif /* GNSS_SDR_RTKLIB_EPHEMERIS_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_ionex.cc b/src/algorithms/libs/rtklib/rtklib_ionex.cc index 0fd99cb44..62f46f8fe 100644 --- a/src/algorithms/libs/rtklib/rtklib_ionex.cc +++ b/src/algorithms/libs/rtklib/rtklib_ionex.cc @@ -55,462 +55,536 @@ * Maps based on GPS Carrier Phase Data Routinely producted by CODE * Analysis Center, Proceeding of the IGS Analysis Center Workshop, 1996 * -* version : $Revision:$ $Date:$ -* history : 2011/03/29 1.0 new -* 2013/03/05 1.1 change api readtec() -* fix problem in case of lat>85deg or lat<-85deg -* 2014/02/22 1.2 fix problem on compiled as C++ *-----------------------------------------------------------------------------*/ + #include "rtklib_ionex.h" + /* get index -----------------------------------------------------------------*/ int getindex(double value, const double *range) { - if (range[2]==0.0) return 0; - if (range[1]>0.0&&(value 0.0 && (value < range[0] || range[1]nt>=nav->ntmax) { - nav->ntmax+=256; - if (!(nav_tec=(tec_t *)realloc(nav->tec,sizeof(tec_t)*nav->ntmax))) { - trace(1,"readionex malloc error ntmax=%d\n",nav->ntmax); - free(nav->tec); nav->tec=NULL; nav->nt=nav->ntmax=0; + tec_t *p, *nav_tec; + gtime_t time0 = {}; + int i, n, ndata[3]; + + trace(3, "addtec :\n"); + + ndata[0] = nitem(lats); + ndata[1] = nitem(lons); + ndata[2] = nitem(hgts); + if (ndata[0] <= 1 || ndata[1] <= 1 || ndata[2] <= 0) return NULL; + + if (nav->nt >= nav->ntmax) + { + nav->ntmax+=256; + if (!(nav_tec = (tec_t *)realloc(nav->tec, sizeof(tec_t)*nav->ntmax))) + { + trace(1, "readionex malloc error ntmax=%d\n", nav->ntmax); + free(nav->tec); nav->tec = NULL; nav->nt = nav->ntmax = 0; + return NULL; + } + nav->tec = nav_tec; + } + p = nav->tec+nav->nt; + p->time = time0; + p->rb = rb; + for (i = 0; i < 3; i++) + { + p->ndata[i] = ndata[i]; + p->lats[i] = lats[i]; + p->lons[i] = lons[i]; + p->hgts[i] = hgts[i]; + } + n = ndata[0] * ndata[1] * ndata[2]; + + if (!(p->data = (double *)malloc(sizeof(double) * n)) || + !(p->rms = (float *)malloc(sizeof(float ) * n))) + { return NULL; } - nav->tec=nav_tec; - } - p=nav->tec+nav->nt; - p->time=time0; - p->rb=rb; - for (i=0;i<3;i++) { - p->ndata[i]=ndata[i]; - p->lats[i]=lats[i]; - p->lons[i]=lons[i]; - p->hgts[i]=hgts[i]; - } - n=ndata[0]*ndata[1]*ndata[2]; - - if (!(p->data=(double *)malloc(sizeof(double)*n))|| - !(p->rms =(float *)malloc(sizeof(float )*n))) { - return NULL; - } - for (i=0;idata[i]=0.0; - p->rms [i]=0.0f; - } + for (i = 0; i < n; i++) + { + p->data[i] = 0.0; + p->rms [i] = 0.0f; + } nav->nt++; return p; } + + /* read ionex dcb aux data ----------------------------------------------------*/ void readionexdcb(FILE *fp, double *dcb, double *rms) { - int i,sat; - char buff[1024],id[32],*label; - - trace(3,"readionexdcb:\n"); - - for (i=0;int-1;i>=0;i--) { - if (fabs(timediff(time,nav->tec[i].time))>=1.0) continue; - p=nav->tec+i; - break; + tec_t *p = NULL; + gtime_t time = {}; + double lat, lon[3], hgt, x; + int i, j, k, n, m, index, type = 0; + char buff[1024], *label = buff + 60; + + trace(3, "readionexb:\n"); + + while (fgets(buff, sizeof(buff), fp)) + { + if (strlen(buff) < 60) continue; + + if (strstr(label, "START OF TEC MAP") == label) + { + if ((p = addtec(lats, lons, hgts, rb, nav))) type = 1; + } + else if (strstr(label, "END OF TEC MAP") == label) + { + type = 0; + p = NULL; + } + else if (strstr(label, "START OF RMS MAP") == label) + { + type = 2; + p = NULL; + } + else if (strstr(label, "END OF RMS MAP") == label) + { + type = 0; + p = NULL; + } + else if (strstr(label, "EPOCH OF CURRENT MAP") == label) + { + if (str2time(buff, 0, 36, &time)) + { + trace(2, "ionex epoch invalid: %-36.36s\n", buff); + continue; + } + if (type == 2) + { + for (i = nav->nt-1; i >= 0; i--) + { + if (fabs(timediff(time, nav->tec[i].time)) >= 1.0) continue; + p = nav->tec + i; + break; + } + } + else if (p) p->time = time; + } + else if (strstr(label, "LAT/LON1/LON2/DLON/H") == label && p) + { + lat = str2num(buff, 2, 6); + lon[0] = str2num(buff, 8, 6); + lon[1] = str2num(buff, 14, 6); + lon[2] = str2num(buff, 20, 6); + hgt = str2num(buff, 26, 6); + + i = getindex(lat, p->lats); + k = getindex(hgt, p->hgts); + n = nitem(lon); + + for (m = 0; m < n; m++) + { + if (m % 16 == 0 && !fgets(buff, sizeof(buff), fp)) break; + + j = getindex(lon[0] + lon[2] * m, p->lons); + if ((index = dataindex(i, j, k, p->ndata)) < 0) continue; + + if ((x = str2num(buff, m % 16 * 5, 5)) == 9999.0) continue; + + if (type == 1) p->data[index] = x * std::pow(10.0, nexp); + else p->rms[index] = (float)(x * std::pow(10.0, nexp)); + } } - } - else if (p) p->time=time; } - else if (strstr(label,"LAT/LON1/LON2/DLON/H")==label&&p) { - lat =str2num(buff, 2,6); - lon[0]=str2num(buff, 8,6); - lon[1]=str2num(buff,14,6); - lon[2]=str2num(buff,20,6); - hgt =str2num(buff,26,6); - - i=getindex(lat,p->lats); - k=getindex(hgt,p->hgts); - n=nitem(lon); - - for (m=0;mlons); - if ((index=dataindex(i,j,k,p->ndata))<0) continue; - - if ((x=str2num(buff,m%16*5,5))==9999.0) continue; - - if (type==1) p->data[index]=x*pow(10.0,nexp); - else p->rms[index]=(float)(x*pow(10.0,nexp)); - } - } - } return 1; } + + /* combine tec grid data -----------------------------------------------------*/ void combtec(nav_t *nav) { tec_t tmp; - int i,j,n=0; - - trace(3,"combtec : nav->nt=%d\n",nav->nt); - - for (i=0;int-1;i++) { - for (j=i+1;jnt;j++) { - if (timediff(nav->tec[j].time,nav->tec[i].time)<0.0) { - tmp=nav->tec[i]; - nav->tec[i]=nav->tec[j]; - nav->tec[j]=tmp; - } + int i, j, n = 0; + + trace(3, "combtec : nav->nt=%d\n", nav->nt); + + for (i = 0; i < nav->nt - 1; i++) + { + for (j = i + 1; j < nav->nt; j++) + { + if (timediff(nav->tec[j].time, nav->tec[i].time) < 0.0) + { + tmp = nav->tec[i]; + nav->tec[i] = nav->tec[j]; + nav->tec[j] = tmp; + } + } } - } - for (i=0;int;i++) { - if (i>0&&timediff(nav->tec[i].time,nav->tec[n-1].time)==0.0) { - free(nav->tec[n-1].data); - free(nav->tec[n-1].rms ); - nav->tec[n-1]=nav->tec[i]; - continue; + for (i = 0; i < nav->nt; i++) + { + if (i > 0 && timediff(nav->tec[i].time, nav->tec[n - 1].time) == 0.0) + { + free(nav->tec[n - 1].data); + free(nav->tec[n - 1].rms ); + nav->tec[n - 1] = nav->tec[i]; + continue; + } + nav->tec[n++] = nav->tec[i]; } - nav->tec[n++]=nav->tec[i]; - } - nav->nt=n; - - trace(4,"combtec : nav->nt=%d\n",nav->nt); + nav->nt = n; + + trace(4, "combtec : nav->nt=%d\n", nav->nt); } + + /* read ionex tec grid file ---------------------------------------------------- -* read ionex ionospheric tec grid file -* args : char *file I ionex tec grid file -* (wind-card * is expanded) -* nav_t *nav IO navigation data -* nav->nt, nav->ntmax and nav->tec are modified -* int opt I read option (1: no clear of tec data,0:clear) -* return : none -* notes : see ref [1] -*-----------------------------------------------------------------------------*/ + * read ionex ionospheric tec grid file + * args : char *file I ionex tec grid file + * (wind-card * is expanded) + * nav_t *nav IO navigation data + * nav->nt, nav->ntmax and nav->tec are modified + * int opt I read option (1: no clear of tec data,0:clear) + * return : none + * notes : see ref [1] + *-----------------------------------------------------------------------------*/ void readtec(const char *file, nav_t *nav, int opt) { FILE *fp; - double lats[3]={0},lons[3]={0},hgts[3]={0},rb=0.0,nexp=-1.0; - double dcb[MAXSAT]={0},rms[MAXSAT]={0}; - int i,n; + double lats[3] = {0}, lons[3] = {0}, hgts[3] = {0}, rb = 0.0, nexp = -1.0; + double dcb[MAXSAT] = {0}, rms[MAXSAT] = {0}; + int i, n; char *efiles[MAXEXFILE]; - - trace(3,"readtec : file=%s\n",file); - + + trace(3, "readtec : file=%s\n", file); + /* clear of tec grid data option */ - if (!opt) { - free(nav->tec); nav->tec=NULL; nav->nt=nav->ntmax=0; - } - for (i=0;i=0;i--) free(efiles[i]); - return; + if (!opt) + { + free(nav->tec); + nav->tec = NULL; + nav->nt = nav->ntmax = 0; + } + for (i = 0; i < MAXEXFILE; i++) + { + if (!(efiles[i] = (char *)malloc(1024))) + { + for (i--; i >= 0; i--) free(efiles[i]); + return; + } } - } /* expand wild card in file path */ - n=expath(file,efiles,MAXEXFILE); - - for (i=0;int>0) combtec(nav); - + if (nav->nt > 0) combtec(nav); + /* P1-P2 dcb */ - for (i=0;icbias[i][0]=CLIGHT*dcb[i]*1E-9; /* ns->m */ - } + for (i = 0; i < MAXSAT; i++) + { + nav->cbias[i][0] = SPEED_OF_LIGHT * dcb[i] * 1e-9; /* ns->m */ + } } + + /* interpolate tec grid data -------------------------------------------------*/ int interptec(const tec_t *tec, int k, const double *posp, double *value, - double *rms) + double *rms) { - double dlat,dlon,a,b,d[4]={0},r[4]={0}; - int i,j,n,index; - - trace(3,"interptec: k=%d posp=%.2f %.2f\n",k,posp[0]*R2D,posp[1]*R2D); - *value=*rms=0.0; - - if (tec->lats[2]==0.0||tec->lons[2]==0.0) return 0; - - dlat=posp[0]*R2D-tec->lats[0]; - dlon=posp[1]*R2D-tec->lons[0]; - if (tec->lons[2]>0.0) dlon-=floor( dlon/360)*360.0; /* 0<=dlon<360 */ - else dlon+=floor(-dlon/360)*360.0; /* -360lats[2]; - b=dlon/tec->lons[2]; - i=(int)floor(a); a-=i; - j=(int)floor(b); b-=j; - + double dlat, dlon, a, b, d[4] = {0}, r[4] = {0}; + int i, j, n, index; + + trace(3, "interptec: k=%d posp=%.2f %.2f\n", k, posp[0] * R2D, posp[1] * R2D); + *value = *rms = 0.0; + + if (tec->lats[2] == 0.0 || tec->lons[2] == 0.0) return 0; + + dlat = posp[0] * R2D - tec->lats[0]; + dlon = posp[1] * R2D - tec->lons[0]; + if (tec->lons[2] > 0.0) dlon -= floor( dlon / 360) * 360.0; /* 0 <= dlon<360 */ + else dlon += floor(-dlon / 360) * 360.0; /* -360lats[2]; + b = dlon / tec->lons[2]; + i = (int)floor(a); + a -= i; + j = (int)floor(b); + b -= j; + /* get gridded tec data */ - for (n=0;n<4;n++) { - if ((index=dataindex(i+(n%2),j+(n<2?0:1),k,tec->ndata))<0) continue; - d[n]=tec->data[index]; - r[n]=tec->rms [index]; - } - if (d[0]>0.0&&d[1]>0.0&&d[2]>0.0&&d[3]>0.0) { - - /* bilinear interpolation (inside of grid) */ - *value=(1.0-a)*(1.0-b)*d[0]+a*(1.0-b)*d[1]+(1.0-a)*b*d[2]+a*b*d[3]; - *rms =(1.0-a)*(1.0-b)*r[0]+a*(1.0-b)*r[1]+(1.0-a)*b*r[2]+a*b*r[3]; - } + for (n = 0; n < 4; n++) + { + if ((index = dataindex(i + (n % 2), j + (n < 2 ? 0 : 1), k, tec->ndata)) < 0) continue; + d[n] = tec->data[index]; + r[n] = tec->rms[index]; + } + if (d[0] > 0.0 && d[1] > 0.0 && d[2] > 0.0 && d[3] > 0.0) + { + /* bilinear interpolation (inside of grid) */ + *value = (1.0 - a) * (1.0 - b) * d[0] + a*(1.0 -b) * d[1] + (1.0 - a) * b * d[2] + a * b * d[3]; + *rms = (1.0 - a) * (1.0 - b) * r[0] + a*(1.0 - b) * r[1] + (1.0 - a) * b * r[2] + a * b * r[3]; + } /* nearest-neighbour extrapolation (outside of grid) */ - else if (a<=0.5&&b<=0.5&&d[0]>0.0) {*value=d[0]; *rms=r[0];} - else if (a> 0.5&&b<=0.5&&d[1]>0.0) {*value=d[1]; *rms=r[1];} - else if (a<=0.5&&b> 0.5&&d[2]>0.0) {*value=d[2]; *rms=r[2];} - else if (a> 0.5&&b> 0.5&&d[3]>0.0) {*value=d[3]; *rms=r[3];} - else { - i=0; - for (n=0;n<4;n++) if (d[n]>0.0) {i++; *value+=d[n]; *rms+=r[n];} - if(i==0) return 0; - *value/=i; *rms/=i; - } + else if (a <= 0.5 && b <= 0.5 && d[0] > 0.0) {*value = d[0]; *rms = r[0];} + else if (a > 0.5 && b <= 0.5 && d[1] > 0.0) {*value = d[1]; *rms = r[1];} + else if (a <= 0.5 && b > 0.5 && d[2] > 0.0) {*value = d[2]; *rms = r[2];} + else if (a > 0.5 && b > 0.5 && d[3] > 0.0) {*value = d[3]; *rms = r[3];} + else + { + i = 0; + for (n = 0; n < 4; n++) if (d[n] > 0.0) {i++; *value += d[n]; *rms += r[n];} + if(i == 0) return 0; + *value /= i; + *rms /= i; + } return 1; } + + /* ionosphere delay by tec grid data -----------------------------------------*/ int iondelay(gtime_t time, const tec_t *tec, const double *pos, - const double *azel, int opt, double *delay, double *var) + const double *azel, int opt, double *delay, double *var) { - const double fact=40.30E16/FREQ1/FREQ1; /* tecu->L1 iono (m) */ - double fs,posp[3]={0},vtec,rms,hion,rp; + const double fact = 40.30E16 / FREQ1 / FREQ1; /* tecu->L1 iono (m) */ + double fs, posp[3] = {0}, vtec, rms, hion, rp; int i; - - trace(3,"iondelay: time=%s pos=%.1f %.1f azel=%.1f %.1f\n",time_str(time,0), - pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,azel[1]*R2D); - - *delay=*var=0.0; - - for (i=0;indata[2];i++) { /* for a layer */ - - hion=tec->hgts[0]+tec->hgts[2]*i; - - /* ionospheric pierce point position */ - fs=ionppp(pos,azel,tec->rb,hion,posp); - - if (opt&2) { - /* modified single layer mapping function (M-SLM) ref [2] */ - rp=tec->rb/(tec->rb+hion)*sin(0.9782*(PI/2.0-azel[1])); - fs=1.0/sqrt(1.0-rp*rp); + + trace(3, "iondelay: time=%s pos=%.1f %.1f azel=%.1f %.1f\n", time_str(time, 0), + pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, azel[1]*R2D); + + *delay = *var = 0.0; + + for (i = 0;i < tec->ndata[2]; i++) + { /* for a layer */ + hion = tec->hgts[0] + tec->hgts[2] * i; + + /* ionospheric pierce point position */ + fs = ionppp(pos, azel, tec->rb, hion, posp); + + if (opt&2) + { + /* modified single layer mapping function (M-SLM) ref [2] */ + rp = tec->rb / (tec->rb + hion) * sin(0.9782 * (PI / 2.0 - azel[1])); + fs = 1.0 / sqrt(1.0 - rp * rp); + } + if (opt&1) + { + /* earth rotation correction (sun-fixed coordinate) */ + posp[1] += 2.0 * PI * timediff(time, tec->time) / 86400.0; + } + /* interpolate tec grid data */ + if (!interptec(tec, i, posp, &vtec, &rms)) return 0; + + *delay += fact * fs * vtec; + *var += fact * fact * fs * fs * rms * rms; } - if (opt&1) { - /* earth rotation correction (sun-fixed coordinate) */ - posp[1]+=2.0*PI*timediff(time,tec->time)/86400.0; - } - /* interpolate tec grid data */ - if (!interptec(tec,i,posp,&vtec,&rms)) return 0; - - *delay+=fact*fs*vtec; - *var+=fact*fact*fs*fs*rms*rms; - } - trace(4,"iondelay: delay=%7.2f std=%6.2f\n",*delay,sqrt(*var)); - + trace(4, "iondelay: delay=%7.2f std=%6.2f\n", *delay, sqrt(*var)); + return 1; } + + /* ionosphere model by tec grid data ------------------------------------------- -* compute ionospheric delay by tec grid data -* args : gtime_t time I time (gpst) -* nav_t *nav I navigation data -* double *pos I receiver position {lat,lon,h} (rad,m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* int opt I model option -* bit0: 0:earth-fixed,1:sun-fixed -* bit1: 0:single-layer,1:modified single-layer -* double *delay O ionospheric delay (L1) (m) -* double *var O ionospheric dealy (L1) variance (m^2) -* return : status (1:ok,0:error) -* notes : before calling the function, read tec grid data by calling readtec() -* return ok with delay=0 and var=VAR_NOTEC if elnt;i++) { - if (timediff(nav->tec[i].time,time)>0.0) break; - } - if (i==0||i>=nav->nt) { - trace(2,"%s: tec grid out of period\n",time_str(time,0)); - return 0; - } - if ((tt=timediff(nav->tec[i].time,nav->tec[i-1].time))==0.0) { - trace(2,"tec grid time interval error\n"); - return 0; - } + double dels[2], vars[2], a, tt; + int i, stat[2]; + + trace(3, "iontec : time=%s pos=%.1f %.1f azel=%.1f %.1f\n", time_str(time, 0), + pos[0] * R2D, pos[1] * R2D, azel[0] * R2D, azel[1] * R2D); + + if (azel[1] < MIN_EL || pos[2] < MIN_HGT) + { + *delay = 0.0; + *var = VAR_NOTEC; + return 1; + } + for (i = 0; i < nav->nt; i++) + { + if (timediff(nav->tec[i].time, time) > 0.0) break; + } + if (i == 0 || i >= nav->nt) + { + trace(2, "%s: tec grid out of period\n", time_str(time, 0)); + return 0; + } + if ((tt = timediff(nav->tec[i].time, nav->tec[i-1].time)) == 0.0) + { + trace(2, "tec grid time interval error\n"); + return 0; + } /* ionospheric delay by tec grid data */ - stat[0]=iondelay(time,nav->tec+i-1,pos,azel,opt,dels ,vars ); - stat[1]=iondelay(time,nav->tec+i ,pos,azel,opt,dels+1,vars+1); - - if (!stat[0]&&!stat[1]) { - trace(2,"%s: tec grid out of area pos=%6.2f %7.2f azel=%6.1f %5.1f\n", - time_str(time,0),pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,azel[1]*R2D); - return 0; - } - if (stat[0]&&stat[1]) { /* linear interpolation by time */ - a=timediff(time,nav->tec[i-1].time)/tt; - *delay=dels[0]*(1.0-a)+dels[1]*a; - *var =vars[0]*(1.0-a)+vars[1]*a; - } - else if (stat[0]) { /* nearest-neighbour extrapolation by time */ - *delay=dels[0]; - *var =vars[0]; - } - else { - *delay=dels[1]; - *var =vars[1]; - } - trace(3,"iontec : delay=%5.2f std=%5.2f\n",*delay,sqrt(*var)); + stat[0] = iondelay(time, nav->tec+i-1, pos, azel, opt, dels , vars ); + stat[1] = iondelay(time, nav->tec+i , pos, azel, opt, dels+1, vars+1); + + if (!stat[0] && !stat[1]) + { + trace(2, "%s: tec grid out of area pos=%6.2f %7.2f azel=%6.1f %5.1f\n", + time_str(time, 0), pos[0] * R2D, pos[1] * R2D, azel[0] * R2D, azel[1] * R2D); + return 0; + } + if (stat[0] && stat[1]) + { /* linear interpolation by time */ + a = timediff(time, nav->tec[i-1].time) / tt; + *delay = dels[0] * (1.0 - a) + dels[1] * a; + *var = vars[0] * (1.0 - a) + vars[1] * a; + } + else if (stat[0]) + { /* nearest-neighbour extrapolation by time */ + *delay = dels[0]; + *var = vars[0]; + } + else + { + *delay = dels[1]; + *var = vars[1]; + } + trace(3, "iontec : delay=%5.2f std=%5.2f\n", *delay, sqrt(*var)); return 1; } diff --git a/src/algorithms/libs/rtklib/rtklib_ionex.h b/src/algorithms/libs/rtklib/rtklib_ionex.h index 155b92ff9..a30ba5b10 100644 --- a/src/algorithms/libs/rtklib/rtklib_ionex.h +++ b/src/algorithms/libs/rtklib/rtklib_ionex.h @@ -1,5 +1,5 @@ /*! - * \file rtklib_ionex.cc + * \file rtklib_ionex.h * \brief ionex functions * \authors
    *
  • 2007-2013, T. Takasu @@ -47,29 +47,24 @@ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -* -* references: -* [1] S.Schear, W.Gurtner and J.Feltens, IONEX: The IONosphere Map EXchange -* Format Version 1, February 25, 1998 -* [2] S.Schaer, R.Markus, B.Gerhard and A.S.Timon, Daily Global Ionosphere -* Maps based on GPS Carrier Phase Data Routinely producted by CODE -* Analysis Center, Proceeding of the IGS Analysis Center Workshop, 1996 -* -* version : $Revision:$ $Date:$ -* history : 2011/03/29 1.0 new -* 2013/03/05 1.1 change api readtec() -* fix problem in case of lat>85deg or lat<-85deg -* 2014/02/22 1.2 fix problem on compiled as C++ -*-----------------------------------------------------------------------------*/ -#ifndef RTKLIB_IONEX_H_ -#define RTKLIB_IONEX_H_ + * + * References: + * [1] S.Schear, W.Gurtner and J.Feltens, IONEX: The IONosphere Map EXchange + * Format Version 1, February 25, 1998 + * [2] S.Schaer, R.Markus, B.Gerhard and A.S.Timon, Daily Global Ionosphere + * Maps based on GPS Carrier Phase Data Routinely producted by CODE + * Analysis Center, Proceeding of the IGS Analysis Center Workshop, 1996 + * + *-----------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_IONEX_H_ +#define GNSS_SDR_RTKLIB_IONEX_H_ #include "rtklib_rtkcmn.h" -#define SQR(x) ((x)*(x)) -#define VAR_NOTEC SQR(30.0) /* variance of no tec */ -#define MIN_EL 0.0 /* min elevation angle (rad) */ -#define MIN_HGT -1000.0 /* min user height (m) */ +const double VAR_NOTEC = 30.0 * 30.0; /* variance of no tec */ +const double MIN_EL = 0.0; /* min elevation angle (rad) */ +const double MIN_HGT = -1000.0; /* min user height (m) */ int getindex(double value, const double *range); @@ -92,4 +87,4 @@ int iondelay(gtime_t time, const tec_t *tec, const double *pos, int iontec(gtime_t time, const nav_t *nav, const double *pos, const double *azel, int opt, double *delay, double *var); -#endif /* SRC_ALGORITHMS_PVT_LIBS_RTKLIB_IONEX_H_ */ +#endif /* GNSS_SDR_RTKLIB_IONEX_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 1e45c07bc..92086497c 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -47,658 +47,742 @@ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -* history : 2010/07/28 1.0 moved from rtkcmn.c -* changed api: -* pntpos() -* deleted api: -* pntvel() -* 2011/01/12 1.1 add option to include unhealthy satellite -* reject duplicated observation data -* changed api: ionocorr() -* 2011/11/08 1.2 enable snr mask for single-mode (rtklib_2.4.1_p3) -* 2012/12/25 1.3 add variable snr mask -* 2014/05/26 1.4 support galileo and beidou -* 2015/03/19 1.5 fix bug on ionosphere correction for GLO and BDS -*-----------------------------------------------------------------------------*/ + * + *-----------------------------------------------------------------------------*/ + #include "rtklib_pntpos.h" /* pseudorange measurement error variance ------------------------------------*/ double varerr(const prcopt_t *opt, double el, int sys) { - double fact,varr; - fact=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:EFACT_GPS); - varr=SQR(opt->err[0])*(SQR(opt->err[1])+SQR(opt->err[2])/sin(el)); - if (opt->ionoopt==IONOOPT_IFLC) varr*=SQR(3.0); /* iono-free */ - return SQR(fact)*varr; + double fact, varr; + fact = sys == SYS_GLO ? EFACT_GLO : (sys == SYS_SBS ? EFACT_SBS : EFACT_GPS); + varr = std::pow(opt->err[0], 2.0) * (std::pow(opt->err[1], 2.0) + std::pow(opt->err[2], 2.0) / sin(el)); + if (opt->ionoopt == IONOOPT_IFLC) varr *= std::pow(2, 3.0); /* iono-free */ + return std::pow(fact, 2.0) * varr; } + + /* get tgd parameter (m) -----------------------------------------------------*/ double gettgd(int sat, const nav_t *nav) { int i; - for (i=0;in;i++) { - if (nav->eph[i].sat!=sat) continue; - return CLIGHT*nav->eph[i].tgd[0]; - } + for (i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * nav->eph[i].tgd[0]; + } return 0.0; } + + /* psendorange with code bias correction -------------------------------------*/ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, - int iter, const prcopt_t *opt, double *var) + int iter, const prcopt_t *opt, double *var) { - const double *lam=nav->lam[obs->sat-1]; - double PC,P1,P2,P1_P2,P1_C1,P2_C2,gamma; - int i=0,j=1,sys; - - *var=0.0; - - if (!(sys=satsys(obs->sat,NULL))) - { - trace(4,"prange: satsys NULL\n"); - return 0.0; - } + const double *lam = nav->lam[obs->sat - 1]; + double PC, P1, P2, P1_P2, P1_C1, P2_C2, gamma; + int i = 0, j = 1, sys; - - /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ - if (NFREQ>=3&&(sys&(SYS_GAL|SYS_SBS))) j=2; - - if (NFREQ<2||lam[i]==0.0||lam[j]==0.0) + *var = 0.0; + + if (!(sys = satsys(obs->sat, NULL))) { - trace(4,"prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n"); - printf("i: %d j:%d, lam[i]: %f lam[j] %f\n",i,j,lam[i],lam[j]); + trace(4, "prange: satsys NULL\n"); return 0.0; } - + + + /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ + if (NFREQ >= 3 && (sys & (SYS_GAL | SYS_SBS))) j = 2; + + if (NFREQ<2 || lam[i] == 0.0 || lam[j] == 0.0) + { + trace(4, "prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n"); + printf("i: %d j:%d, lam[i]: %f lam[j] %f\n", i, j, lam[i], lam[j]); + return 0.0; + } + /* test snr mask */ - if (iter>0) { - if (testsnr(0,i,azel[1],obs->SNR[i]*0.25,&opt->snrmask)) { - trace(4,"snr mask: %s sat=%2d el=%.1f snr=%.1f\n", - time_str(obs->time,0),obs->sat,azel[1]*R2D,obs->SNR[i]*0.25); - return 0.0; - } - if (opt->ionoopt==IONOOPT_IFLC) { - if (testsnr(0,j,azel[1],obs->SNR[j]*0.25,&opt->snrmask)) + if (iter>0) + { + if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask)) { - trace(4,"prange: testsnr error\n"); + trace(4, "snr mask: %s sat=%2d el=%.1f snr=%.1f\n", + time_str(obs->time, 0), obs->sat, azel[1] * R2D, obs->SNR[i] * 0.25); return 0.0; } + if (opt->ionoopt == IONOOPT_IFLC) + { + if (testsnr(0, j, azel[1], obs->SNR[j] * 0.25, &opt->snrmask)) + { + trace(4, "prange: testsnr error\n"); + return 0.0; + } + } } - } - gamma=SQR(lam[j])/SQR(lam[i]); /* f1^2/f2^2 */ - P1=obs->P[i]; - P2=obs->P[j]; - P1_P2=nav->cbias[obs->sat-1][0]; - P1_C1=nav->cbias[obs->sat-1][1]; - P2_C2=nav->cbias[obs->sat-1][2]; - + gamma = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); /* f1^2/f2^2 */ + P1 = obs->P[i]; + P2 = obs->P[j]; + P1_P2 = nav->cbias[obs->sat-1][0]; + P1_C1 = nav->cbias[obs->sat-1][1]; + P2_C2 = nav->cbias[obs->sat-1][2]; + /* if no P1-P2 DCB, use TGD instead */ - if (P1_P2==0.0&&(sys&(SYS_GPS|SYS_GAL|SYS_QZS))) { - P1_P2=(1.0-gamma)*gettgd(obs->sat,nav); - } - if (opt->ionoopt==IONOOPT_IFLC) { /* dual-frequency */ - - if (P1==0.0||P2==0.0) return 0.0; - if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */ - if (obs->code[j]==CODE_L2C) P2+=P2_C2; /* C2->P2 */ - - /* iono-free combination */ - PC=(gamma*P1-P2)/(gamma-1.0); - } - else { /* single-frequency */ - - if (P1==0.0) return 0.0; - if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */ - PC=P1-P1_P2/(1.0-gamma); - } - if (opt->sateph==EPHOPT_SBAS) PC-=P1_C1; /* sbas clock based C1 */ - - *var=SQR(ERR_CBIAS); - + if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS))) + { + P1_P2 = (1.0 - gamma) * gettgd(obs->sat, nav); + } + if (opt->ionoopt == IONOOPT_IFLC) + { /* dual-frequency */ + + if (P1 == 0.0 || P2 == 0.0) return 0.0; + if (obs->code[i] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ + if (obs->code[j] == CODE_L2C) P2 += P2_C2; /* C2->P2 */ + + /* iono-free combination */ + PC = (gamma * P1 - P2) / (gamma - 1.0); + } + else + { /* single-frequency */ + + if (P1 == 0.0) return 0.0; + if (obs->code[i] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ + PC = P1 - P1_P2 / (1.0 - gamma); + } + if (opt->sateph == EPHOPT_SBAS) PC -= P1_C1; /* sbas clock based C1 */ + + *var = std::pow(ERR_CBIAS, 2.0); + return PC; } + + /* ionospheric correction ------------------------------------------------------ -* compute ionospheric correction -* args : gtime_t time I time -* nav_t *nav I navigation data -* int sat I satellite number -* double *pos I receiver position {lat,lon,h} (rad|m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* int ionoopt I ionospheric correction option (IONOOPT_???) -* double *ion O ionospheric delay (L1) (m) -* double *var O ionospheric delay (L1) variance (m^2) -* return : status(1:ok,0:error) -*-----------------------------------------------------------------------------*/ + * compute ionospheric correction + * args : gtime_t time I time + * nav_t *nav I navigation data + * int sat I satellite number + * double *pos I receiver position {lat,lon,h} (rad|m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * int ionoopt I ionospheric correction option (IONOOPT_???) + * double *ion O ionospheric delay (L1) (m) + * double *var O ionospheric delay (L1) variance (m^2) + * return : status(1:ok,0:error) + *-----------------------------------------------------------------------------*/ int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos, - const double *azel, int ionoopt, double *ion, double *var) + const double *azel, int ionoopt, double *ion, double *var) { - trace(4,"ionocorr: time=%s opt=%d sat=%2d pos=%.3f %.3f azel=%.3f %.3f\n", - time_str(time,3),ionoopt,sat,pos[0]*R2D,pos[1]*R2D,azel[0]*R2D, - azel[1]*R2D); - + trace(4, "ionocorr: time=%s opt=%d sat=%2d pos=%.3f %.3f azel=%.3f %.3f\n", + time_str(time, 3), ionoopt, sat, pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, + azel[1]*R2D); + /* broadcast model */ - if (ionoopt==IONOOPT_BRDC) { - *ion=ionmodel(time,nav->ion_gps,pos,azel); - *var=SQR(*ion*ERR_BRDCI); - return 1; - } + if (ionoopt == IONOOPT_BRDC) + { + *ion = ionmodel(time, nav->ion_gps, pos, azel); + *var = std::pow(*ion * ERR_BRDCI, 2.0); + return 1; + } /* sbas ionosphere model */ - if (ionoopt==IONOOPT_SBAS) { - return sbsioncorr(time,nav,pos,azel,ion,var); - } + if (ionoopt == IONOOPT_SBAS) + { + return sbsioncorr(time, nav, pos, azel, ion, var); + } /* ionex tec model */ - if (ionoopt==IONOOPT_TEC) { - return iontec(time,nav,pos,azel,1,ion,var); - } + if (ionoopt == IONOOPT_TEC) + { + return iontec(time, nav, pos, azel, 1, ion, var); + } /* qzss broadcast model */ - if (ionoopt==IONOOPT_QZS&&norm(nav->ion_qzs,8)>0.0) { - *ion=ionmodel(time,nav->ion_qzs,pos,azel); - *var=SQR(*ion*ERR_BRDCI); - return 1; - } + if (ionoopt == IONOOPT_QZS && norm(nav->ion_qzs, 8)>0.0) + { + *ion = ionmodel(time, nav->ion_qzs, pos, azel); + *var = std::pow(*ion * ERR_BRDCI, 2.0); + return 1; + } /* lex ionosphere model */ - //if (ionoopt==IONOOPT_LEX) { - // return lexioncorr(time,nav,pos,azel,ion,var); + //if (ionoopt == IONOOPT_LEX) { + // return lexioncorr(time, nav, pos, azel, ion, var); //} - *ion=0.0; - *var=ionoopt==IONOOPT_OFF?SQR(ERR_ION):0.0; + *ion = 0.0; + *var = ionoopt == IONOOPT_OFF ? std::pow(ERR_ION, 2.0) : 0.0; return 1; } + + /* tropospheric correction ----------------------------------------------------- -* compute tropospheric correction -* args : gtime_t time I time -* nav_t *nav I navigation data -* double *pos I receiver position {lat,lon,h} (rad|m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* int tropopt I tropospheric correction option (TROPOPT_???) -* double *trp O tropospheric delay (m) -* double *var O tropospheric delay variance (m^2) -* return : status(1:ok,0:error) -*-----------------------------------------------------------------------------*/ -int tropcorr(gtime_t time, const nav_t *nav, const double *pos, - const double *azel, int tropopt, double *trp, double *var) + * compute tropospheric correction + * args : gtime_t time I time + * nav_t *nav I navigation data + * double *pos I receiver position {lat,lon,h} (rad|m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * int tropopt I tropospheric correction option (TROPOPT_???) + * double *trp O tropospheric delay (m) + * double *var O tropospheric delay variance (m^2) + * return : status(1:ok,0:error) + *-----------------------------------------------------------------------------*/ +int tropcorr(gtime_t time, const nav_t *nav __attribute__((unused)), const double *pos, + const double *azel, int tropopt, double *trp, double *var) { - trace(4,"tropcorr: time=%s opt=%d pos=%.3f %.3f azel=%.3f %.3f\n", - time_str(time,3),tropopt,pos[0]*R2D,pos[1]*R2D,azel[0]*R2D, - azel[1]*R2D); - + trace(4, "tropcorr: time=%s opt=%d pos=%.3f %.3f azel=%.3f %.3f\n", + time_str(time, 3), tropopt, pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, + azel[1]*R2D); + /* saastamoinen model */ - if (tropopt==TROPOPT_SAAS||tropopt==TROPOPT_EST||tropopt==TROPOPT_ESTG) { - *trp=tropmodel(time,pos,azel,REL_HUMI); - *var=SQR(ERR_SAAS/(sin(azel[1])+0.1)); - return 1; - } + if (tropopt == TROPOPT_SAAS || tropopt == TROPOPT_EST || tropopt == TROPOPT_ESTG) + { + *trp = tropmodel(time, pos, azel, REL_HUMI); + *var = std::pow(ERR_SAAS / (sin(azel[1]) + 0.1), 2.0); + return 1; + } /* sbas troposphere model */ - if (tropopt==TROPOPT_SBAS) { - *trp=sbstropcorr(time,pos,azel,var); - return 1; - } + if (tropopt == TROPOPT_SBAS) + { + *trp = sbstropcorr(time, pos, azel, var); + return 1; + } /* no correction */ - *trp=0.0; - *var=tropopt==TROPOPT_OFF?SQR(ERR_TROP):0.0; + *trp = 0.0; + *var = tropopt == TROPOPT_OFF ? std::pow(ERR_TROP, 2.0) : 0.0; return 1; } + + /* pseudorange residuals -----------------------------------------------------*/ int rescode(int iter, const obsd_t *obs, int n, const double *rs, - const double *dts, const double *vare, const int *svh, - const nav_t *nav, const double *x, const prcopt_t *opt, - double *v, double *H, double *var, double *azel, int *vsat, - double *resp, int *ns) + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const double *x, const prcopt_t *opt, + double *v, double *H, double *var, double *azel, int *vsat, + double *resp, int *ns) { - double r,dion,dtrp,vmeas,vion,vtrp,rr[3],pos[3],dtr,e[3],P,lam_L1; - int i,j,nv=0,sys,mask[4]={0}; - - trace(3,"resprng : n=%d\n",n); - - for (i=0;i<3;i++) rr[i]=x[i]; dtr=x[3]; - - ecef2pos(rr,pos); - - for (i=*ns=0;ielmin) + { + trace(4, "geodist / satazel error\n"); + continue; + } + + /* psudorange with code bias correction */ + if ((P = prange(obs+i, nav, azel+i*2, iter, opt, &vmeas)) == 0.0) + { + trace(4, "prange error\n"); + continue; + } + + /* excluded satellite? */ + if (satexclude(obs[i].sat, svh[i], opt)) + { + trace(4, "satexclude error\n"); + continue; + } + + /* ionospheric corrections */ + if (!ionocorr(obs[i].time, nav, obs[i].sat, pos, azel+i*2, + iter>0 ? opt->ionoopt : IONOOPT_BRDC, &dion, &vion)) + { + trace(4, "ionocorr error\n"); + continue; + } + + /* GPS-L1 -> L1/B1 */ + if ((lam_L1 = nav->lam[obs[i].sat - 1][0]) > 0.0) + { + dion *= std::pow(lam_L1 / lam_carr[0], 2.0); + } + /* tropospheric corrections */ + if (!tropcorr(obs[i].time, nav, pos, azel + i*2, + iter > 0 ? opt->tropopt : TROPOPT_SAAS, &dtrp, &vtrp)) + { + trace(4, "tropocorr error\n"); + continue; + } + /* pseudorange residual */ + v[nv] = P - (r + dtr - SPEED_OF_LIGHT * dts[i*2] + dion + dtrp); + + /* design matrix */ + for (j = 0; j < NX; j++) H[j + nv * NX] = j < 3 ? - e[j] : (j == 3 ? 1.0 : 0.0); + + /* time system and receiver bias offset correction */ + if (sys == SYS_GLO) {v[nv] -= x[4]; H[4+nv*NX] = 1.0; mask[1] = 1;} + else if (sys == SYS_GAL) {v[nv] -= x[5]; H[5+nv*NX] = 1.0; mask[2] = 1;} + else if (sys == SYS_BDS) {v[nv] -= x[6]; H[6+nv*NX] = 1.0; mask[3] = 1;} + else mask[0] = 1; + + vsat[i] = 1; + resp[i] = v[nv]; + (*ns)++; + + /* error variance */ + var[nv++] = varerr(opt, azel[1+i*2], sys) + vare[i] + vmeas + vion + vtrp; + + trace(4, "sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n", obs[i].sat, + azel[i*2] * R2D, azel[1+i*2] * R2D, resp[i], sqrt(var[nv-1])); } - /* geometric distance/azimuth/elevation angle */ - if ((r=geodist(rs+i*6,rr,e))<=0.0|| - satazel(pos,e,azel+i*2)elmin) - { - trace(4,"geodist / satazel error\n"); - continue; - } - - /* psudorange with code bias correction */ - if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0) - { - trace(4,"prange error\n"); - continue; - } - - /* excluded satellite? */ - if (satexclude(obs[i].sat,svh[i],opt)) - { - trace(4,"satexclude error\n"); - continue; - } - - /* ionospheric corrections */ - if (!ionocorr(obs[i].time,nav,obs[i].sat,pos,azel+i*2, - iter>0?opt->ionoopt:IONOOPT_BRDC,&dion,&vion)) - { - trace(4,"ionocorr error\n"); - continue; - } - - /* GPS-L1 -> L1/B1 */ - if ((lam_L1=nav->lam[obs[i].sat-1][0])>0.0) { - dion*=SQR(lam_L1/lam_carr[0]); - } - /* tropospheric corrections */ - if (!tropcorr(obs[i].time,nav,pos,azel+i*2, - iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) { - trace(4,"tropocorr error\n"); - continue; - } - /* pseudorange residual */ - v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp); - - /* design matrix */ - for (j=0;jnx&&vv>chisqr[nv-nx-1]) { - sprintf(msg,"chi-square error nv=%d vv=%.1f cs=%.1f",nv,vv,chisqr[nv-nx-1]); - return 0; - } + vv = dot(v, v, nv); + if (nv > nx && vv > chisqr[nv-nx-1]) + { + sprintf(msg, "chi-square error nv=%d vv=%.1f cs=%.1f", nv, vv, chisqr[nv-nx-1]); + return 0; + } /* large gdop check */ - for (i=ns=0;ielmin,dop); - if (dop[0]<=0.0||dop[0]>opt->maxgdop) { - sprintf(msg,"gdop error nv=%d gdop=%.1f",nv,dop[0]); - return 0; - } + for (i = ns = 0; i < n; i++) + { + if (!vsat[i]) continue; + azels[ ns*2] = azel[ i*2]; + azels[1+ns*2] = azel[1+i*2]; + ns++; + } + dops(ns, azels, opt->elmin, dop); + if (dop[0] <= 0.0 || dop[0] > opt->maxgdop) + { + sprintf(msg, "gdop error nv=%d gdop=%.1f", nv, dop[0]); + return 0; + } return 1; } + + /* estimate receiver position ------------------------------------------------*/ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, - const double *vare, const int *svh, const nav_t *nav, - const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, - double *resp, char *msg) + const double *vare, const int *svh, const nav_t *nav, + const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, + double *resp, char *msg) { - double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig; - int i,j,k,info,stat,nv,ns; - - trace(3,"estpos : n=%d\n",n); - - v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1); - - for (i=0;i<3;i++) x[i]=sol->rr[i]; - - for (i=0;irr[i]; + + for (i = 0; i < MAXITR; i++) + { + /* pseudorange residuals */ + nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, opt, v, H, var, azel, vsat, resp, &ns); + + if (nv < NX) + { + sprintf(msg, "lack of valid sats ns=%d", nv); + break; + } + /* weight by variance */ + for (j = 0;j < nv; j++) + { + sig = sqrt(var[j]); + v[j] /= sig; + for (k = 0; k < NX; k++) H[k + j * NX] /= sig; + } + /* least square estimation */ + if ((info = lsq(H, v, NX, nv, dx, Q))) + { + sprintf(msg, "lsq error info=%d", info); + break; + } + for (j = 0; j < NX; j++) x[j] += dx[j]; + + if (norm(dx, NX) < 1e-4) + { + sol->type = 0; + sol->time = timeadd(obs[0].time, -x[3] / SPEED_OF_LIGHT); + sol->dtr[0] = x[3] / SPEED_OF_LIGHT; /* receiver clock bias (s) */ + sol->dtr[1] = x[4] / SPEED_OF_LIGHT; /* glo-gps time offset (s) */ + sol->dtr[2] = x[5] / SPEED_OF_LIGHT; /* gal-gps time offset (s) */ + sol->dtr[3] = x[6] / SPEED_OF_LIGHT; /* bds-gps time offset (s) */ + for (j = 0; j < 6; j++) sol->rr[j] = j < 3 ? x[j] : 0.0; + for (j = 0; j < 3; j++) sol->qr[j] = (float)Q[j + j * NX]; + sol->qr[3] = (float)Q[1]; /* cov xy */ + sol->qr[4] = (float)Q[2 + NX]; /* cov yz */ + sol->qr[5] = (float)Q[2]; /* cov zx */ + sol->ns = (unsigned char)ns; + sol->age = sol->ratio = 0.0; + + /* validate solution */ + if ((stat = valsol(azel, vsat, n, opt, v, nv, NX, msg))) + { + sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE; + } + free(v); + free(H); + free(var); + + return stat; + } } - /* weight by variance */ - for (j=0;jtype=0; - sol->time=timeadd(obs[0].time,-x[3]/CLIGHT); - sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */ - sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */ - sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */ - sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */ - for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0; - for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX]; - sol->qr[3]=(float)Q[1]; /* cov xy */ - sol->qr[4]=(float)Q[2+NX]; /* cov yz */ - sol->qr[5]=(float)Q[2]; /* cov zx */ - sol->ns=(unsigned char)ns; - sol->age=sol->ratio=0.0; - - /* validate solution */ - if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) { - sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE; - } - free(v); free(H); free(var); - - return stat; - } - } - if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i); - - free(v); free(H); free(var); - + if (i >= MAXITR) sprintf(msg, "iteration divergent i=%d", i); + + free(v); + free(H); + free(var); + return 0; } + + /* raim fde (failure detection and exclution) -------------------------------*/ -int raim_fde(const obsd_t *obs, int n, const double *rs, - const double *dts, const double *vare, const int *svh, - const nav_t *nav, const prcopt_t *opt, sol_t *sol, - double *azel, int *vsat, double *resp, char *msg) +int raim_fde(const obsd_t *obs, int n, const double *rs, + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const prcopt_t *opt, sol_t *sol, + double *azel, int *vsat, double *resp, char *msg) { obsd_t *obs_e; - sol_t sol_e={}; - char tstr[32],name[16],msg_e[128]; - double *rs_e,*dts_e,*vare_e,*azel_e,*resp_e,rms_e,rms=100.0; - int i,j,k,nvsat,stat=0,*svh_e,*vsat_e,sat=0; - - trace(3,"raim_fde: %s n=%2d\n",time_str(obs[0].time,0),n); - - if (!(obs_e=(obsd_t *)malloc(sizeof(obsd_t)*n))) return 0; - rs_e = mat(6,n); dts_e = mat(2,n); vare_e=mat(1,n); azel_e=zeros(2,n); - svh_e=imat(1,n); vsat_e=imat(1,n); resp_e=mat(1,n); - - for (i=0;i rms) continue; + + /* save result */ + for (j = k = 0; j < n; j++) + { + if (j == i) continue; + matcpy(azel + 2 * j, azel_e + 2 * k, 2, 1); + vsat[j] = vsat_e[k]; + resp[j] = resp_e[k++]; + } + stat = 1; + *sol = sol_e; + sat = obs[i].sat; + rms = rms_e; + vsat[i] = 0; + strcpy(msg, msg_e); } - /* estimate receiver position without a satellite */ - if (!estpos(obs_e,n-1,rs_e,dts_e,vare_e,svh_e,nav,opt,&sol_e,azel_e, - vsat_e,resp_e,msg_e)) { - trace(3,"raim_fde: exsat=%2d (%s)\n",obs[i].sat,msg); - continue; + if (stat) + { + time2str(obs[0].time, tstr, 2); satno2id(sat, name); + trace(2, "%s: %s excluded by raim\n", tstr + 11, name); } - for (j=nvsat=0,rms_e=0.0;jrms) continue; - - /* save result */ - for (j=k=0;jlam[obs[i].sat-1][0]; - - if (obs[i].D[0]==0.0||lam==0.0||!vsat[i]||norm(rs+3+i*6,3)<=0.0) { - continue; + double lam, rate, pos[3], E[9], a[3], e[3], vs[3], cosel; + int i, j, nv = 0; + + trace(3, "resdop : n=%d\n", n); + + ecef2pos(rr, pos); + xyz2enu(pos, E); + + for (i = 0; i < n && i < MAXOBS; i++) + { + lam = nav->lam[obs[i].sat-1][0]; + + if (obs[i].D[0] == 0.0 || lam == 0.0 || !vsat[i] || norm(rs + 3 + i * 6, 3) <= 0.0) + { + continue; + } + /* line-of-sight vector in ecef */ + cosel = cos(azel[1+i*2]); + a[0] = sin(azel[i*2]) * cosel; + a[1] = cos(azel[i*2]) * cosel; + a[2] = sin(azel[1+i*2]); + matmul("TN", 3, 1, 3, 1.0, E, a, 0.0, e); + + /* satellite velocity relative to receiver in ecef */ + for (j = 0; j < 3; j++) vs[j] = rs[j+3+i*6] - x[j]; + + /* range rate with earth rotation correction */ + rate = dot(vs, e, 3) + DEFAULT_OMEGA_EARTH_DOT / SPEED_OF_LIGHT * (rs[4 + i * 6] * rr[0] + rs[1 + i * 6] * x[0]- + rs[3 + i * 6] * rr[1] - rs[i * 6] * x[1]); + + /* doppler residual */ + v[nv] =- lam * obs[i].D[0] - (rate + x[3] - SPEED_OF_LIGHT * dts[1 + i *2]); + + /* design matrix */ + for (j = 0; j < 4; j++) H[j + nv * 4] = j < 3 ? - e[j] : 1.0; + + nv++; } - /* line-of-sight vector in ecef */ - cosel=cos(azel[1+i*2]); - a[0]=sin(azel[i*2])*cosel; - a[1]=cos(azel[i*2])*cosel; - a[2]=sin(azel[1+i*2]); - matmul("TN",3,1,3,1.0,E,a,0.0,e); - - /* satellite velocity relative to receiver in ecef */ - for (j=0;j<3;j++) vs[j]=rs[j+3+i*6]-x[j]; - - /* range rate with earth rotation correction */ - rate=dot(vs,e,3)+OMGE/CLIGHT*(rs[4+i*6]*rr[0]+rs[1+i*6]*x[0]- - rs[3+i*6]*rr[1]-rs[ i*6]*x[1]); - - /* doppler residual */ - v[nv]=-lam*obs[i].D[0]-(rate+x[3]-CLIGHT*dts[1+i*2]); - - /* design matrix */ - for (j=0;j<4;j++) H[j+nv*4]=j<3?-e[j]:1.0; - - nv++; - } return nv; } + + /* estimate receiver velocity ------------------------------------------------*/ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, - const nav_t *nav, const prcopt_t *opt, sol_t *sol, - const double *azel, const int *vsat) + const nav_t *nav, const prcopt_t *opt __attribute__((unused)), sol_t *sol, + const double *azel, const int *vsat) { - double x[4]={0},dx[4],Q[16],*v,*H; - int i,j,nv; - - trace(3,"estvel : n=%d\n",n); - - v=mat(n,1); H=mat(4,n); - - for (i=0;irr,x,azel,vsat,v,H))<4) { - break; - } - /* least square estimation */ - if (lsq(H,v,4,nv,dx,Q)) break; - - for (j=0;j<4;j++) x[j]+=dx[j]; - - if (norm(dx,4)<1E-6) { - for (i=0;i<3;i++) sol->rr[i+3]=x[i]; - break; - } - } - free(v); free(H); -} -/* single-point positioning ---------------------------------------------------- -* compute receiver position, velocity, clock bias by single-point positioning -* with pseudorange and doppler observables -* args : obsd_t *obs I observation data -* int n I number of observation data -* nav_t *nav I navigation data -* prcopt_t *opt I processing options -* sol_t *sol IO solution -* double *azel IO azimuth/elevation angle (rad) (NULL: no output) -* ssat_t *ssat IO satellite status (NULL: no output) -* char *msg O error message for error exit -* return : status(1:ok,0:error) -* notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and -* receiver bias are negligible (only involving glonass-gps time offset -* and receiver bias) -*-----------------------------------------------------------------------------*/ -int pntpos(const obsd_t *obs, int n, const nav_t *nav, - const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, - char *msg) -{ -// int k=0; -// for (k=0;kn;k++) -// { -// printf("NAV[%i]: sat %i, %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f \r\n", -// k, -// nav->eph[k].sat, -// nav->eph[k].A, -// nav->eph[k].Adot, -// nav->eph[k].M0, -// nav->eph[k].OMG0, -// nav->eph[k].OMGd, -// nav->eph[k].cic, -// nav->eph[k].cis, -// nav->eph[k].code, -// nav->eph[k].crc, -// nav->eph[k].crs, -// nav->eph[k].cuc, -// nav->eph[k].cus, -// nav->eph[k].deln, -// nav->eph[k].e, -// nav->eph[k].f0, -// nav->eph[k].f1, -// nav->eph[k].f2, -// nav->eph[k].fit, -// nav->eph[k].flag, -// nav->eph[k].i0, -// nav->eph[k].idot, -// nav->eph[k].iodc, -// nav->eph[k].iode, -// nav->eph[k].ndot, -// nav->eph[k].omg, -// nav->eph[k].sat, -// nav->eph[k].sva, -// nav->eph[k].svh, -// nav->eph[k].tgd[0], -// nav->eph[k].toc.sec, -// nav->eph[k].toe.sec, -// nav->eph[k].toes, -// nav->eph[k].ttr.sec, -// nav->eph[k].week); -// } + double x[4] = {0}, dx[4], Q[16], *v, *H; + int i, j, nv; - prcopt_t opt_=*opt; - double *rs,*dts,*var,*azel_,*resp; - int i,stat,vsat[MAXOBS]={0},svh[MAXOBS]; - - sol->stat=SOLQ_NONE; - - if (n<=0) {strcpy(msg,"no observation data"); return 0;} - - trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n); - - sol->time=obs[0].time; msg[0]='\0'; - - rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n); - - if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */ + trace(3, "estvel : n=%d\n", n); + + v = mat(n, 1); + H = mat(4, n); + + for (i = 0; i < MAXITR; i++) + { + /* doppler residuals */ + if ((nv = resdop(obs, n, rs, dts, nav, sol->rr, x, azel, vsat, v, H)) < 4) + { + break; + } + /* least square estimation */ + if (lsq(H, v, 4, nv, dx, Q)) break; + + for (j = 0; j < 4; j++) x[j] += dx[j]; + + if (norm(dx, 4) < 1e-6) + { + for (i = 0; i < 3; i++) sol->rr[i+3] = x[i]; + break; + } + } + free(v); + free(H); +} + + +/* single-point positioning ---------------------------------------------------- + * compute receiver position, velocity, clock bias by single-point positioning + * with pseudorange and doppler observables + * args : obsd_t *obs I observation data + * int n I number of observation data + * nav_t *nav I navigation data + * prcopt_t *opt I processing options + * sol_t *sol IO solution + * double *azel IO azimuth/elevation angle (rad) (NULL: no output) + * ssat_t *ssat IO satellite status (NULL: no output) + * char *msg O error message for error exit + * return : status(1:ok,0:error) + * notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and + * receiver bias are negligible (only involving glonass-gps time offset + * and receiver bias) + *-----------------------------------------------------------------------------*/ +int pntpos(const obsd_t *obs, int n, const nav_t *nav, + const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, + char *msg) +{ + // int k = 0; + // for (k = 0;kn;k++) + // { + // printf("NAV[%i]: sat %i, %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f \r\n", + // k, + // nav->eph[k].sat, + // nav->eph[k].A, + // nav->eph[k].Adot, + // nav->eph[k].M0, + // nav->eph[k].OMG0, + // nav->eph[k].OMGd, + // nav->eph[k].cic, + // nav->eph[k].cis, + // nav->eph[k].code, + // nav->eph[k].crc, + // nav->eph[k].crs, + // nav->eph[k].cuc, + // nav->eph[k].cus, + // nav->eph[k].deln, + // nav->eph[k].e, + // nav->eph[k].f0, + // nav->eph[k].f1, + // nav->eph[k].f2, + // nav->eph[k].fit, + // nav->eph[k].flag, + // nav->eph[k].i0, + // nav->eph[k].idot, + // nav->eph[k].iodc, + // nav->eph[k].iode, + // nav->eph[k].ndot, + // nav->eph[k].omg, + // nav->eph[k].sat, + // nav->eph[k].sva, + // nav->eph[k].svh, + // nav->eph[k].tgd[0], + // nav->eph[k].toc.sec, + // nav->eph[k].toe.sec, + // nav->eph[k].toes, + // nav->eph[k].ttr.sec, + // nav->eph[k].week); + // } + + prcopt_t opt_ = *opt; + double *rs, *dts, *var, *azel_, *resp; + int i, stat, vsat[MAXOBS] = {0}, svh[MAXOBS]; + + sol->stat = SOLQ_NONE; + + if (n <= 0) {strcpy(msg, "no observation data"); return 0;} + + trace(3, "pntpos : tobs=%s n=%d\n", time_str(obs[0].time, 3), n); + + sol->time = obs[0].time; + msg[0] = '\0'; + + rs = mat(6, n); + dts = mat(2, n); + var = mat(1, n); + azel_ = zeros(2, n); + resp = mat(1, n); + + if (opt_.mode != PMODE_SINGLE) + { /* for precise positioning */ #if 0 - opt_.sateph =EPHOPT_BRDC; + opt_.sateph = EPHOPT_BRDC; #endif - opt_.ionoopt=IONOOPT_BRDC; - opt_.tropopt=TROPOPT_SAAS; - } + opt_.ionoopt = IONOOPT_BRDC; + opt_.tropopt = TROPOPT_SAAS; + } /* satellite positons, velocities and clocks */ - satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh); - + satposs(sol->time, obs, n, nav, opt_.sateph, rs, dts, var, svh); + /* estimate receiver position with pseudorange */ - stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); - + stat = estpos(obs, n, rs, dts, var, svh, nav, &opt_, sol, azel_, vsat, resp, msg); + /* raim fde */ - if (!stat&&n>=6&&opt->posopt[4]) { - stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); - } + if (!stat && n >= 6 && opt->posopt[4]) + { + stat = raim_fde(obs, n, rs, dts, var, svh, nav, &opt_, sol, azel_, vsat, resp, msg); + } /* estimate receiver velocity with doppler */ - if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat); - - if (azel) { - for (i=0;ine>=nav->nemax) { - nav->nemax+=256; - if (!(nav_peph=(peph_t *)realloc(nav->peph,sizeof(peph_t)*nav->nemax))) { - trace(1,"readsp3b malloc error n=%d\n",nav->nemax); - free(nav->peph); nav->peph=NULL; nav->ne=nav->nemax=0; - return 0; + + if (nav->ne >= nav->nemax) + { + nav->nemax += 256; + if (!(nav_peph = (peph_t *)realloc(nav->peph, sizeof(peph_t)*nav->nemax))) + { + trace(1, "readsp3b malloc error n=%d\n", nav->nemax); + free(nav->peph); + nav->peph = NULL; + nav->ne = nav->nemax = 0; + return 0; + } + nav->peph = nav_peph; } - nav->peph=nav_peph; - } - nav->peph[nav->ne++]=*peph; + nav->peph[nav->ne++] = *peph; return 1; } + + /* read sp3 body -------------------------------------------------------------*/ -void readsp3b(FILE *fp, char type, int *sats, int ns, double *bfact, - char *tsys, int index, int opt, nav_t *nav) +void readsp3b(FILE *fp, char type, int *sats __attribute__((unused)), int ns, double *bfact, + char *tsys, int index, int opt, nav_t *nav) { peph_t peph; gtime_t time; - double val,std,base; - int i,j,sat,sys,prn,n=ns*(type=='P'?1:2),pred_o,pred_c,v; + double val, std, base; + int i, j, sat, sys, prn, n = ns*(type == 'P' ? 1 : 2), pred_o, pred_c, v; char buff[1024]; - - trace(3,"readsp3b: type=%c ns=%d index=%d opt=%d\n",type,ns,index,opt); - - while (fgets(buff,sizeof(buff),fp)) { - - if (!strncmp(buff,"EOF",3)) break; - - if (buff[0]!='*'||str2time(buff,3,28,&time)) { - trace(2,"sp3 invalid epoch %31.31s\n",buff); - continue; - } - if (!strcmp(tsys,"UTC")) time=utc2gpst(time); /* utc->gpst */ - peph.time =time; - peph.index=index; - - for (i=0;i=76&&buff[75]=='P'; - pred_o=strlen(buff)>=80&&buff[79]=='P'; - } - for (j=0;j<4;j++) { - - /* read option for predicted value */ - if (j< 3&&(opt&1)&& pred_o) continue; - if (j< 3&&(opt&2)&&!pred_o) continue; - if (j==3&&(opt&1)&& pred_c) continue; - if (j==3&&(opt&2)&&!pred_c) continue; - - val=str2num(buff, 4+j*14,14); - std=str2num(buff,61+j* 3,j<3?2:3); - - if (buff[0]=='P') { /* position */ - if (val!=0.0&&fabs(val-999999.999999)>=1E-6) { - peph.pos[sat-1][j]=val*(j<3?1000.0:1E-6); - v=1; /* valid epoch */ - } - if ((base=bfact[j<3?0:1])>0.0&&std>0.0) { - peph.std[sat-1][j]=(float)(pow(base,std)*(j<3?1E-3:1E-12)); - } + + trace(3, "readsp3b: type=%c ns=%d index=%d opt=%d\n", type, ns, index, opt); + + while (fgets(buff, sizeof(buff), fp)) + { + if (!strncmp(buff, "EOF", 3)) break; + + if (buff[0] != '*' || str2time(buff, 3, 28, &time)) + { + trace(2, "sp3 invalid epoch %31.31s\n", buff); + continue; } - else if (v) { /* velocity */ - if (val!=0.0&&fabs(val-999999.999999)>=1E-6) { - peph.vel[sat-1][j]=val*(j<3?0.1:1E-10); - } - if ((base=bfact[j<3?0:1])>0.0&&std>0.0) { - peph.vst[sat-1][j]=(float)(pow(base,std)*(j<3?1E-7:1E-16)); - } + if (!strcmp(tsys, "UTC")) time = utc2gpst(time); /* utc->gpst */ + peph.time = time; + peph.index = index; + + for (i = 0; i < MAXSAT; i++) + { + for (j = 0; j < 4; j++) + { + peph.pos[i][j] = 0.0; + peph.std[i][j] = 0.0f; + peph.vel[i][j] = 0.0; + peph.vst[i][j] = 0.0f; + } + for (j = 0; j < 3; j++) + { + peph.cov[i][j] = 0.0f; + peph.vco[i][j] = 0.0f; + } + } + for (i = pred_o = pred_c = v = 0;i= 76 && buff[75] == 'P'; + pred_o = strlen(buff) >= 80 && buff[79] == 'P'; + } + for (j = 0;j<4;j++) + { + /* read option for predicted value */ + if (j< 3 && (opt&1) && pred_o) continue; + if (j< 3 && (opt&2) && !pred_o) continue; + if (j == 3 && (opt&1) && pred_c) continue; + if (j == 3 && (opt&2) && !pred_c) continue; + + val = str2num(buff, 4 + j * 14, 14); + std = str2num(buff, 61 + j * 3, j < 3 ? 2 : 3); + + if (buff[0] == 'P') { /* position */ + if (val != 0.0 && fabs(val - 999999.999999) >= 1e-6) + { + peph.pos[sat-1][j] = val*(j < 3 ? 1000.0 : 1e-6); + v = 1; /* valid epoch */ + } + if ((base = bfact[j < 3 ? 0 : 1]) > 0.0 && std > 0.0) + { + peph.std[sat-1][j] = (float)(std::pow(base, std)*(j < 3 ? 1e-3 : 1e-12)); + } + } + else if (v) { /* velocity */ + if (val != 0.0 && fabs(val - 999999.999999) >= 1e-6) + { + peph.vel[sat-1][j] = val*(j < 3 ? 0.1 : 1e-10); + } + if ((base = bfact[j < 3 ? 0 : 1]) > 0.0 && std > 0.0) + { + peph.vst[sat-1][j] = (float)(std::pow(base, std)*(j < 3 ? 1e-7 : 1e-16)); + } + } + } + } + if (v) + { + if (!addpeph(nav, &peph)) return; } - } } - if (v) { - if (!addpeph(nav,&peph)) return; - } - } } + + /* compare precise ephemeris -------------------------------------------------*/ -int cmppeph(const void *p1, const void *p2) +int cmppeph(const void *p1, const void *p2) { - peph_t *q1=(peph_t *)p1,*q2=(peph_t *)p2; - double tt=timediff(q1->time,q2->time); - return tt<-1E-9?-1:(tt>1E-9?1:q1->index-q2->index); + peph_t *q1 = (peph_t *)p1, *q2 = (peph_t *)p2; + double tt = timediff(q1->time, q2->time); + return tt < -1e-9 ? -1 : (tt > 1e-9 ? 1 : q1->index - q2->index); } + + /* combine precise ephemeris -------------------------------------------------*/ -void combpeph(nav_t *nav, int opt) +void combpeph(nav_t *nav, int opt) { - int i,j,k,m; - - trace(3,"combpeph: ne=%d\n",nav->ne); - - qsort(nav->peph,nav->ne,sizeof(peph_t),cmppeph); - + int i, j, k, m; + + trace(3, "combpeph: ne=%d\n", nav->ne); + + qsort(nav->peph, nav->ne, sizeof(peph_t), cmppeph); + if (opt&4) return; - - for (i=0,j=1;jne;j++) { - - if (fabs(timediff(nav->peph[i].time,nav->peph[j].time))<1E-9) { - - for (k=0;kpeph[j].pos[k],4)<=0.0) continue; - for (m=0;m<4;m++) nav->peph[i].pos[k][m]=nav->peph[j].pos[k][m]; - for (m=0;m<4;m++) nav->peph[i].std[k][m]=nav->peph[j].std[k][m]; - for (m=0;m<4;m++) nav->peph[i].vel[k][m]=nav->peph[j].vel[k][m]; - for (m=0;m<4;m++) nav->peph[i].vst[k][m]=nav->peph[j].vst[k][m]; - } + + for (i = 0, j = 1; j < nav->ne; j++) + { + if (fabs(timediff(nav->peph[i].time, nav->peph[j].time)) < 1e-9) + { + for (k = 0; k < MAXSAT; k++) + { + if (norm(nav->peph[j].pos[k], 4) <= 0.0) continue; + for (m = 0;m < 4; m++) nav->peph[i].pos[k][m] = nav->peph[j].pos[k][m]; + for (m = 0;m < 4; m++) nav->peph[i].std[k][m] = nav->peph[j].std[k][m]; + for (m = 0;m < 4; m++) nav->peph[i].vel[k][m] = nav->peph[j].vel[k][m]; + for (m = 0;m < 4; m++) nav->peph[i].vst[k][m] = nav->peph[j].vst[k][m]; + } + } + else if (++i < j) nav->peph[i] = nav->peph[j]; } - else if (++ipeph[i]=nav->peph[j]; - } - nav->ne=i+1; - - trace(4,"combpeph: ne=%d\n",nav->ne); + nav->ne = i+1; + + trace(4, "combpeph: ne=%d\n", nav->ne); } + + /* read sp3 precise ephemeris file --------------------------------------------- -* read sp3 precise ephemeris/clock files and set them to navigation data -* args : char *file I sp3-c precise ephemeris file -* (wind-card * is expanded) -* nav_t *nav IO navigation data -* int opt I options (1: only observed + 2: only predicted + -* 4: not combined) -* return : none -* notes : see ref [1] -* precise ephemeris is appended and combined -* nav->peph and nav->ne must by properly initialized before calling the -* function -* only files with extensions of .sp3, .SP3, .eph* and .EPH* are read -*-----------------------------------------------------------------------------*/ + * read sp3 precise ephemeris/clock files and set them to navigation data + * args : char *file I sp3-c precise ephemeris file + * (wind-card * is expanded) + * nav_t *nav IO navigation data + * int opt I options (1: only observed + 2: only predicted + + * 4: not combined) + * return : none + * notes : see ref [1] + * precise ephemeris is appended and combined + * nav->peph and nav->ne must by properly initialized before calling the + * function + * only files with extensions of .sp3, .SP3, .eph* and .EPH* are read + *-----------------------------------------------------------------------------*/ void readsp3(const char *file, nav_t *nav, int opt) { FILE *fp; - gtime_t time={}; - double bfact[2]={}; - int i,j,n,ns,sats[MAXSAT]={}; - char *efiles[MAXEXFILE],*ext,type=' ',tsys[4]=""; - - trace(3,"readpephs: file=%s\n",file); - - for (i=0;i=0;i--) free(efiles[i]); - return; + gtime_t time = {}; + double bfact[2] = {}; + int i, j, n, ns, sats[MAXSAT] = {}; + char *efiles[MAXEXFILE], *ext, type = ' ', tsys[4] = ""; + + trace(3, "readpephs: file=%s\n", file); + + for (i = 0; i < MAXEXFILE; i++) + { + if (!(efiles[i] = (char *)malloc(1024))) + { + for (i--; i >= 0; i--) free(efiles[i]); + return; + } } - } /* expand wild card in file path */ - n=expath(file,efiles,MAXEXFILE); - - for (i=j=0;ine>0) combpeph(nav,opt); + if (nav->ne > 0) combpeph(nav, opt); } + + /* read satellite antenna parameters ------------------------------------------- -* read satellite antenna parameters -* args : char *file I antenna parameter file -* gtime_t time I time -* nav_t *nav IO navigation data -* return : status (1:ok,0:error) -* notes : only support antex format for the antenna parameter file -*-----------------------------------------------------------------------------*/ + * read satellite antenna parameters + * args : char *file I antenna parameter file + * gtime_t time I time + * nav_t *nav IO navigation data + * return : status (1:ok,0:error) + * notes : only support antex format for the antenna parameter file + *-----------------------------------------------------------------------------*/ int readsap(const char *file, gtime_t time, nav_t *nav) { - pcvs_t pcvs={}; - pcv_t pcv0={},*pcv; + pcvs_t pcvs = {}; + pcv_t pcv0 = {}, *pcv; int i; - - trace(3,"readsap : file=%s time=%s\n",file,time_str(time,0)); - - if (!readpcv(file,&pcvs)) return 0; - - for (i=0;ipcvs[i]=pcv?*pcv:pcv0; - } + + trace(3, "readsap : file=%s time=%s\n", file, time_str(time, 0)); + + if (!readpcv(file, &pcvs)) return 0; + + for (i = 0; i < MAXSAT; i++) + { + pcv = searchpcv(i + 1, "", time, &pcvs); + nav->pcvs[i] = pcv ? *pcv : pcv0; + } free(pcvs.pcv); return 1; } + + /* read dcb parameters file --------------------------------------------------*/ int readdcbf(const char *file, nav_t *nav, const sta_t *sta) { FILE *fp; double cbias; - char buff[256],str1[32],str2[32]=""; - int i,j,sat,type=0; - - trace(3,"readdcbf: file=%s\n",file); - - if (!(fp=fopen(file,"r"))) { - trace(2,"dcb parameters file open error: %s\n",file); - return 0; - } - while (fgets(buff,sizeof(buff),fp)) { - - if (strstr(buff,"DIFFERENTIAL (P1-P2) CODE BIASES")) type=1; - else if (strstr(buff,"DIFFERENTIAL (P1-C1) CODE BIASES")) type=2; - else if (strstr(buff,"DIFFERENTIAL (P2-C2) CODE BIASES")) type=3; - - if (!type||sscanf(buff,"%s %s",str1,str2)<1) continue; - - if ((cbias=str2num(buff,26,9))==0.0) continue; - - if (sta&&(!strcmp(str1,"G")||!strcmp(str1,"R"))) { /* receiver dcb */ - for (i=0;irbias[i][j][type-1]=cbias*1E-9*CLIGHT; /* ns -> m */ - } + char buff[256], str1[32], str2[32] = ""; + int i, j, sat, type = 0; + + trace(3, "readdcbf: file=%s\n", file); + + if (!(fp = fopen(file, "r"))) + { + trace(2, "dcb parameters file open error: %s\n", file); + return 0; } - else if ((sat=satid2no(str1))) { /* satellite dcb */ - nav->cbias[sat-1][type-1]=cbias*1E-9*CLIGHT; /* ns -> m */ + while (fgets(buff, sizeof(buff), fp)) + { + if (strstr(buff, "DIFFERENTIAL (P1-P2) CODE BIASES")) type = 1; + else if (strstr(buff, "DIFFERENTIAL (P1-C1) CODE BIASES")) type = 2; + else if (strstr(buff, "DIFFERENTIAL (P2-C2) CODE BIASES")) type = 3; + + if (!type || sscanf(buff, "%s %s", str1, str2) < 1) continue; + + if ((cbias = str2num(buff, 26, 9)) == 0.0) continue; + + if (sta && (!strcmp(str1, "G") || !strcmp(str1, "R"))) + { /* receiver dcb */ + for (i = 0; i < MAXRCV; i++) + { + if (!strcmp(sta[i].name, str2)) break; + } + if (i < MAXRCV) + { + j = !strcmp(str1, "G") ? 0 : 1; + nav->rbias[i][j][type-1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* ns -> m */ + } + } + else if ((sat = satid2no(str1))) + { /* satellite dcb */ + nav->cbias[sat-1][type-1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* ns -> m */ + } } - } fclose(fp); - + return 1; } + + /* read dcb parameters --------------------------------------------------------- -* read differential code bias (dcb) parameters -* args : char *file I dcb parameters file (wild-card * expanded) -* nav_t *nav IO navigation data -* sta_t *sta I station info data to inport receiver dcb -* (NULL: no use) -* return : status (1:ok,0:error) -* notes : currently only p1-c1 bias of code *.dcb file -*-----------------------------------------------------------------------------*/ + * read differential code bias (dcb) parameters + * args : char *file I dcb parameters file (wild-card * expanded) + * nav_t *nav IO navigation data + * sta_t *sta I station info data to inport receiver dcb + * (NULL: no use) + * return : status (1:ok,0:error) + * notes : currently only p1-c1 bias of code *.dcb file + *-----------------------------------------------------------------------------*/ int readdcb(const char *file, nav_t *nav, const sta_t *sta) { - int i,j,n; - char *efiles[MAXEXFILE]={}; - - trace(3,"readdcb : file=%s\n",file); - - for (i=0;icbias[i][j]=0.0; - } - for (i=0;i=0;i--) free(efiles[i]); - return 0; + int i, j, n; + char *efiles[MAXEXFILE] = {}; + + trace(3, "readdcb : file=%s\n", file); + + for (i = 0;i < MAXSAT; i++) for (j = 0; j < 3; j++) + { + nav->cbias[i][j] = 0.0; } - } - n=expath(file,efiles,MAXEXFILE); - - for (i=0;i= 0; i--) free(efiles[i]); + return 0; + } + } + n = expath(file, efiles, MAXEXFILE); + + for (i = 0; i < n; i++) + { + readdcbf(efiles[i], nav, sta); + } + for (i = 0; i < MAXEXFILE;i ++) free(efiles[i]); + return 1; } + + /* add satellite fcb ---------------------------------------------------------*/ int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat, - const double *bias, const double *std) + const double *bias, const double *std) { fcbd_t *nav_fcb; - int i,j; - - if (nav->nf>0&&fabs(timediff(ts,nav->fcb[nav->nf-1].ts))<=1e-3) { - for (i=0;i<3;i++) { - nav->fcb[nav->nf-1].bias[sat-1][i]=bias[i]; - nav->fcb[nav->nf-1].std [sat-1][i]=std [i]; + int i, j; + + if (nav->nf > 0 && fabs(timediff(ts, nav->fcb[nav->nf-1].ts)) <= 1e-3) + { + for (i = 0; i < 3; i++) + { + nav->fcb[nav->nf-1].bias[sat-1][i] = bias[i]; + nav->fcb[nav->nf-1].std [sat-1][i] = std [i]; + } + return 1; } - return 1; - } - if (nav->nf>=nav->nfmax) { - nav->nfmax=nav->nfmax<=0?2048:nav->nfmax*2; - if (!(nav_fcb=(fcbd_t *)realloc(nav->fcb,sizeof(fcbd_t)*nav->nfmax))) { - free(nav->fcb); nav->nf=nav->nfmax=0; - return 0; + if (nav->nf >= nav->nfmax) + { + nav->nfmax = nav->nfmax <= 0 ? 2048 : nav->nfmax * 2; + if (!(nav_fcb = (fcbd_t *)realloc(nav->fcb, sizeof(fcbd_t)*nav->nfmax))) + { + free(nav->fcb); + nav->nf = nav->nfmax = 0; + return 0; + } + nav->fcb = nav_fcb; } - nav->fcb=nav_fcb; - } - for (i=0;ifcb[nav->nf].bias[i][j]=nav->fcb[nav->nf].std[i][j]=0.0; - } - for (i=0;i<3;i++) { - nav->fcb[nav->nf].bias[sat-1][i]=bias[i]; - nav->fcb[nav->nf].std [sat-1][i]=std [i]; - } - nav->fcb[nav->nf ].ts=ts; - nav->fcb[nav->nf++].te=te; + for (i = 0; i < MAXSAT; i++) for (j = 0; j < 3; j++) + { + nav->fcb[nav->nf].bias[i][j] = nav->fcb[nav->nf].std[i][j] = 0.0; + } + for (i = 0; i < 3; i++) + { + nav->fcb[nav->nf].bias[sat-1][i] = bias[i]; + nav->fcb[nav->nf].std [sat-1][i] = std [i]; + } + nav->fcb[nav->nf ].ts = ts; + nav->fcb[nav->nf++].te = te; return 1; } + + /* read satellite fcb file ---------------------------------------------------*/ int readfcbf(const char *file, nav_t *nav) { FILE *fp; - gtime_t ts,te; - double ep1[6],ep2[6],bias[3]={},std[3]={}; - char buff[1024],str[32],*p; + gtime_t ts, te; + double ep1[6], ep2[6], bias[3] = {}, std[3] = {}; + char buff[1024], str[32], *p; int sat; - - trace(3,"readfcbf: file=%s\n",file); - - if (!(fp=fopen(file,"r"))) { - trace(2,"fcb parameters file open error: %s\n",file); - return 0; - } - while (fgets(buff,sizeof(buff),fp)) { - if ((p=strchr(buff,'#'))) *p='\0'; - if (sscanf(buff,"%lf/%lf/%lf %lf:%lf:%lf %lf/%lf/%lf %lf:%lf:%lf %s" - "%lf %lf %lf %lf %lf %lf",ep1,ep1+1,ep1+2,ep1+3,ep1+4,ep1+5, - ep2,ep2+1,ep2+2,ep2+3,ep2+4,ep2+5,str,bias,std,bias+1,std+1, - bias+2,std+2)<17) continue; - if (!(sat=satid2no(str))) continue; - ts=epoch2time(ep1); - te=epoch2time(ep2); - if (!addfcb(nav,ts,te,sat,bias,std)) return 0; - } + + trace(3, "readfcbf: file=%s\n", file); + + if (!(fp = fopen(file, "r"))) + { + trace(2, "fcb parameters file open error: %s\n", file); + return 0; + } + while (fgets(buff, sizeof(buff), fp)) + { + if ((p = strchr(buff, '#'))) *p = '\0'; + if (sscanf(buff, "%lf/%lf/%lf %lf:%lf:%lf %lf/%lf/%lf %lf:%lf:%lf %s" + "%lf %lf %lf %lf %lf %lf", ep1, ep1+1, ep1+2, ep1+3, ep1+4, ep1+5, + ep2, ep2+1, ep2+2, ep2+3, ep2+4, ep2+5, str, bias, std, bias+1, std+1, + bias+2, std+2) < 17) continue; + if (!(sat = satid2no(str))) continue; + ts = epoch2time(ep1); + te = epoch2time(ep2); + if (!addfcb(nav, ts, te, sat, bias, std)) return 0; + } fclose(fp); return 1; } + + /* compare satellite fcb -----------------------------------------------------*/ int cmpfcb(const void *p1, const void *p2) { - fcbd_t *q1=(fcbd_t *)p1,*q2=(fcbd_t *)p2; - double tt=timediff(q1->ts,q2->ts); - return tt<-1E-3?-1:(tt>1E-3?1:0); + fcbd_t *q1 = (fcbd_t *)p1, *q2 = (fcbd_t *)p2; + double tt = timediff(q1->ts, q2->ts); + return tt < -1e-3 ? -1 : (tt > 1e-3 ? 1 : 0); } + + /* read satellite fcb data ----------------------------------------------------- -* read satellite fractional cycle bias (dcb) parameters -* args : char *file I fcb parameters file (wild-card * expanded) -* nav_t *nav IO navigation data -* return : status (1:ok,0:error) -* notes : fcb data appended to navigation data -*-----------------------------------------------------------------------------*/ + * read satellite fractional cycle bias (dcb) parameters + * args : char *file I fcb parameters file (wild-card * expanded) + * nav_t *nav IO navigation data + * return : status (1:ok,0:error) + * notes : fcb data appended to navigation data + *-----------------------------------------------------------------------------*/ int readfcb(const char *file, nav_t *nav) { - char *efiles[MAXEXFILE]={}; - int i,n; - - trace(3,"readfcb : file=%s\n",file); - - for (i=0;i=0;i--) free(efiles[i]); - return 0; + char *efiles[MAXEXFILE] = {}; + int i, n; + + trace(3, "readfcb : file=%s\n", file); + + for (i = 0; i < MAXEXFILE; i++) + { + if (!(efiles[i] = (char *)malloc(1024))) + { + for (i--; i >= 0; i--) free(efiles[i]); + return 0; + } + } + n = expath(file, efiles, MAXEXFILE); + + for (i = 0; i < n; i++) + { + readfcbf(efiles[i], nav); + } + for (i = 0; i < MAXEXFILE; i++) free(efiles[i]); + + if (nav->nf > 1) + { + qsort(nav->fcb, nav->nf, sizeof(fcbd_t), cmpfcb); } - } - n=expath(file,efiles,MAXEXFILE); - - for (i=0;inf>1) { - qsort(nav->fcb,nav->nf,sizeof(fcbd_t),cmpfcb); - } return 1; } + + /* polynomial interpolation by Neville's algorithm ---------------------------*/ double interppol(const double *x, double *y, int n) { - int i,j; - - for (j=1;jnepeph[0].time)<-MAXDTE|| - timediff(time,nav->peph[nav->ne-1].time)>MAXDTE) { - trace(3,"no prec ephem %s sat=%2d\n",time_str(time,0),sat); - return 0; - } - /* binary search */ - for (i=0,j=nav->ne-1;ipeph[k].time,time)<0.0) i=k+1; else j=k; - } - index=i<=0?0:i-1; - - /* polynomial interpolation for orbit */ - i=index-(NMAX+1)/2; - if (i<0) i=0; else if (i+NMAX>=nav->ne) i=nav->ne-NMAX-1; - - for (j=0;j<=NMAX;j++) { - t[j]=timediff(nav->peph[i+j].time,time); - if (norm(nav->peph[i+j].pos[sat-1],3)<=0.0) { - trace(3,"prec ephem outage %s sat=%2d\n",time_str(time,0),sat); + double t[NMAX+1], p[3][NMAX+1], c[2], *pos, std = 0.0, s[3], sinl, cosl; + int i, j, k, index; + + trace(4, "pephpos : time=%s sat=%2d\n", time_str(time, 3), sat); + + rs[0] = rs[1] = rs[2] = dts[0] = 0.0; + + if (nav->ne < NMAX + 1 || + timediff(time, nav->peph[0].time) < -MAXDTE || + timediff(time, nav->peph[nav->ne-1].time) > MAXDTE) + { + trace(3, "no prec ephem %s sat=%2d\n", time_str(time, 0), sat); return 0; } - } - for (j=0;j<=NMAX;j++) { - pos=nav->peph[i+j].pos[sat-1]; + /* binary search */ + for (i = 0, j = nav->ne - 1; i < j;) + { + k = (i + j) / 2; + if (timediff(nav->peph[k].time, time) < 0.0) i = k + 1; else j = k; + } + index = i <= 0 ? 0 : i-1; + + /* polynomial interpolation for orbit */ + i = index - (NMAX + 1) / 2; + if (i < 0) i = 0; else if (i + NMAX >= nav->ne) i = nav->ne - NMAX - 1; + + for (j = 0; j <= NMAX; j++) + { + t[j] = timediff(nav->peph[i+j].time, time); + if (norm(nav->peph[i+j].pos[sat-1], 3) <= 0.0) + { + trace(3, "prec ephem outage %s sat=%2d\n", time_str(time, 0), sat); + return 0; + } + } + for (j = 0;j<=NMAX;j++) + { + pos = nav->peph[i+j].pos[sat-1]; #if 0 - p[0][j]=pos[0]; - p[1][j]=pos[1]; + p[0][j] = pos[0]; + p[1][j] = pos[1]; #else - /* correciton for earh rotation ver.2.4.0 */ - sinl=sin(OMGE*t[j]); - cosl=cos(OMGE*t[j]); - p[0][j]=cosl*pos[0]-sinl*pos[1]; - p[1][j]=sinl*pos[0]+cosl*pos[1]; + /* correciton for earh rotation ver.2.4.0 */ + sinl = sin(DEFAULT_OMEGA_EARTH_DOT * t[j]); + cosl = cos(DEFAULT_OMEGA_EARTH_DOT * t[j]); + p[0][j] = cosl * pos[0] - sinl * pos[1]; + p[1][j] = sinl * pos[0] + cosl * pos[1]; #endif - p[2][j]=pos[2]; - } - for (i=0;i<3;i++) { - rs[i]=interppol(t,p[i],NMAX+1); - } - if (vare) { - for (i=0;i<3;i++) s[i]=nav->peph[index].std[sat-1][i]; - std=norm(s,3); - - /* extrapolation error for orbit */ - if (t[0 ]>0.0) std+=EXTERR_EPH*SQR(t[0 ])/2.0; - else if (t[NMAX]<0.0) std+=EXTERR_EPH*SQR(t[NMAX])/2.0; - *vare=SQR(std); - } + p[2][j] = pos[2]; + } + for (i = 0; i < 3; i++) + { + rs[i] = interppol(t, p[i], NMAX + 1); + } + if (vare) + { + for (i = 0; i < 3; i++) s[i] = nav->peph[index].std[sat-1][i]; + std = norm(s, 3); + + /* extrapolation error for orbit */ + if (t[0 ] > 0.0) std += EXTERR_EPH * std::pow(t[0 ], 2.0) / 2.0; + else if (t[NMAX] < 0.0) std += EXTERR_EPH * std::pow(t[NMAX], 2.0) / 2.0; + *vare = std::pow(std, 2.0); + } /* linear interpolation for clock */ - t[0]=timediff(time,nav->peph[index ].time); - t[1]=timediff(time,nav->peph[index+1].time); - c[0]=nav->peph[index ].pos[sat-1][3]; - c[1]=nav->peph[index+1].pos[sat-1][3]; - - if (t[0]<=0.0) { - if ((dts[0]=c[0])!=0.0) { - std=nav->peph[index].std[sat-1][3]*CLIGHT-EXTERR_CLK*t[0]; + t[0] = timediff(time, nav->peph[index ].time); + t[1] = timediff(time, nav->peph[index+1].time); + c[0] = nav->peph[index ].pos[sat-1][3]; + c[1] = nav->peph[index+1].pos[sat-1][3]; + + if (t[0] <= 0.0) + { + if ((dts[0] = c[0]) != 0.0) + { + std = nav->peph[index].std[sat-1][3] * SPEED_OF_LIGHT - EXTERR_CLK * t[0]; + } } - } - else if (t[1]>=0.0) { - if ((dts[0]=c[1])!=0.0) { - std=nav->peph[index+1].std[sat-1][3]*CLIGHT+EXTERR_CLK*t[1]; + else if (t[1] >= 0.0) + { + if ((dts[0] = c[1]) != 0.0) + { + std = nav->peph[index+1].std[sat-1][3] * SPEED_OF_LIGHT + EXTERR_CLK * t[1]; + } } - } - else if (c[0]!=0.0&&c[1]!=0.0) { - dts[0]=(c[1]*t[0]-c[0]*t[1])/(t[0]-t[1]); - i=t[0]<-t[1]?0:1; - std=nav->peph[index+i].std[sat-1][3]+EXTERR_CLK*fabs(t[i]); - } - else { - dts[0]=0.0; - } - if (varc) *varc=SQR(std); + else if (c[0] != 0.0 && c[1] != 0.0) + { + dts[0] = (c[1] * t[0] - c[0] * t[1]) / (t[0] - t[1]); + i = t[0] < -t[1] ? 0: 1; + std = nav->peph[index+i].std[sat-1][3] + EXTERR_CLK * fabs(t[i]); + } + else + { + dts[0] = 0.0; + } + if (varc) *varc = std::pow(std, 2.0); return 1; } + + /* satellite clock by precise clock ------------------------------------------*/ int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts, - double *varc) + double *varc) { - double t[2],c[2],std; - int i,j,k,index; - - trace(4,"pephclk : time=%s sat=%2d\n",time_str(time,3),sat); - - if (nav->nc<2|| - timediff(time,nav->pclk[0].time)<-MAXDTE|| - timediff(time,nav->pclk[nav->nc-1].time)>MAXDTE) { - trace(3,"no prec clock %s sat=%2d\n",time_str(time,0),sat); - return 1; - } + double t[2], c[2], std; + int i, j, k, index; + + trace(4, "pephclk : time=%s sat=%2d\n", time_str(time, 3), sat); + + if (nav->nc < 2 || + timediff(time, nav->pclk[0].time) < -MAXDTE || + timediff(time, nav->pclk[nav->nc-1].time) > MAXDTE) + { + trace(3, "no prec clock %s sat=%2d\n", time_str(time, 0), sat); + return 1; + } /* binary search */ - for (i=0,j=nav->nc-1;ipclk[k].time,time)<0.0) i=k+1; else j=k; - } - index=i<=0?0:i-1; - + for (i = 0, j = nav->nc - 1; i < j;) + { + k = (i + j) / 2; + if (timediff(nav->pclk[k].time, time) < 0.0) i = k + 1; else j = k; + } + index = i<=0?0:i-1; + /* linear interpolation for clock */ - t[0]=timediff(time,nav->pclk[index ].time); - t[1]=timediff(time,nav->pclk[index+1].time); - c[0]=nav->pclk[index ].clk[sat-1][0]; - c[1]=nav->pclk[index+1].clk[sat-1][0]; - - if (t[0]<=0.0) { - if ((dts[0]=c[0])==0.0) return 0; - std=nav->pclk[index].std[sat-1][0]*CLIGHT-EXTERR_CLK*t[0]; - } - else if (t[1]>=0.0) { - if ((dts[0]=c[1])==0.0) return 0; - std=nav->pclk[index+1].std[sat-1][0]*CLIGHT+EXTERR_CLK*t[1]; - } - else if (c[0]!=0.0&&c[1]!=0.0) { - dts[0]=(c[1]*t[0]-c[0]*t[1])/(t[0]-t[1]); - i=t[0]<-t[1]?0:1; - std=nav->pclk[index+i].std[sat-1][0]*CLIGHT+EXTERR_CLK*fabs(t[i]); - } - else { - trace(3,"prec clock outage %s sat=%2d\n",time_str(time,0),sat); - return 0; - } - if (varc) *varc=SQR(std); + t[0] = timediff(time, nav->pclk[index ].time); + t[1] = timediff(time, nav->pclk[index+1].time); + c[0] = nav->pclk[index ].clk[sat-1][0]; + c[1] = nav->pclk[index+1].clk[sat-1][0]; + + if (t[0] <= 0.0) + { + if ((dts[0] = c[0]) == 0.0) return 0; + std = nav->pclk[index].std[sat-1][0] * SPEED_OF_LIGHT - EXTERR_CLK * t[0]; + } + else if (t[1] >= 0.0) + { + if ((dts[0] = c[1]) == 0.0) return 0; + std = nav->pclk[index+1].std[sat-1][0] * SPEED_OF_LIGHT + EXTERR_CLK * t[1]; + } + else if (c[0] != 0.0 && c[1] != 0.0) + { + dts[0] = (c[1]*t[0]-c[0]*t[1]) / (t[0] - t[1]); + i = t[0] < -t[1] ? 0 : 1; + std = nav->pclk[index+i].std[sat-1][0] * SPEED_OF_LIGHT + EXTERR_CLK * fabs(t[i]); + } + else + { + trace(3, "prec clock outage %s sat=%2d\n", time_str(time, 0), sat); + return 0; + } + if (varc) *varc = std::pow(std, 2.0); return 1; } + + /* satellite antenna phase center offset --------------------------------------- -* compute satellite antenna phase center offset in ecef -* args : gtime_t time I time (gpst) -* double *rs I satellite position and velocity (ecef) -* {x,y,z,vx,vy,vz} (m|m/s) -* int sat I satellite number -* nav_t *nav I navigation data -* double *dant I satellite antenna phase center offset (ecef) -* {dx,dy,dz} (m) (iono-free LC value) -* return : none -*-----------------------------------------------------------------------------*/ + * compute satellite antenna phase center offset in ecef + * args : gtime_t time I time (gpst) + * double *rs I satellite position and velocity (ecef) + * {x,y,z,vx,vy,vz} (m|m/s) + * int sat I satellite number + * nav_t *nav I navigation data + * double *dant I satellite antenna phase center offset (ecef) + * {dx,dy,dz} (m) (iono-free LC value) + * return : none + *-----------------------------------------------------------------------------*/ void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav, - double *dant) + double *dant) { - const double *lam=nav->lam[sat-1]; - const pcv_t *pcv=nav->pcvs+sat-1; - double ex[3],ey[3],ez[3],es[3],r[3],rsun[3],gmst,erpv[5]={}; - double gamma,C1,C2,dant1,dant2; - int i,j=0,k=1; - - trace(4,"satantoff: time=%s sat=%2d\n",time_str(time,3),sat); - + const double *lam = nav->lam[sat-1]; + const pcv_t *pcv = nav->pcvs+sat-1; + double ex[3], ey[3], ez[3], es[3], r[3], rsun[3], gmst, erpv[5] = {}; + double gamma, C1, C2, dant1, dant2; + int i, j = 0, k = 1; + + trace(4, "satantoff: time=%s sat=%2d\n", time_str(time, 3), sat); + /* sun position in ecef */ - sunmoonpos(gpst2utc(time),erpv,rsun,NULL,&gmst); - + sunmoonpos(gpst2utc(time), erpv, rsun, NULL, &gmst); + /* unit vectors of satellite fixed coordinates */ - for (i=0;i<3;i++) r[i]=-rs[i]; - if (!normv3(r,ez)) return; - for (i=0;i<3;i++) r[i]=rsun[i]-rs[i]; - if (!normv3(r,es)) return; - cross3(ez,es,r); - if (!normv3(r,ey)) return; - cross3(ey,ez,ex); - - if (NFREQ>=3&&(satsys(sat,NULL)&(SYS_GAL|SYS_SBS))) k=2; - - if (NFREQ<2||lam[j]==0.0||lam[k]==0.0) return; - - gamma=SQR(lam[k])/SQR(lam[j]); - C1=gamma/(gamma-1.0); - C2=-1.0 /(gamma-1.0); - + for (i = 0; i < 3; i++) r[i] = -rs[i]; + if (!normv3(r, ez)) return; + for (i = 0; i < 3; i++) r[i] = rsun[i]-rs[i]; + if (!normv3(r, es)) return; + cross3(ez, es, r); + if (!normv3(r, ey)) return; + cross3(ey, ez, ex); + + if (NFREQ >= 3 && (satsys(sat, NULL) & (SYS_GAL | SYS_SBS))) k = 2; + + if (NFREQ < 2 || lam[j] == 0.0 || lam[k] == 0.0) return; + + gamma = std::pow(lam[k], 2.0) / std::pow(lam[j], 2.0); + C1 = gamma / (gamma - 1.0); + C2 = -1.0 / (gamma - 1.0); + /* iono-free LC */ - for (i=0;i<3;i++) { - dant1=pcv->off[j][0]*ex[i]+pcv->off[j][1]*ey[i]+pcv->off[j][2]*ez[i]; - dant2=pcv->off[k][0]*ex[i]+pcv->off[k][1]*ey[i]+pcv->off[k][2]*ez[i]; - dant[i]=C1*dant1+C2*dant2; - } + for (i = 0; i < 3; i++) + { + dant1 = pcv->off[j][0] * ex[i] + pcv->off[j][1] * ey[i] + pcv->off[j][2] * ez[i]; + dant2 = pcv->off[k][0] * ex[i] + pcv->off[k][1] * ey[i] + pcv->off[k][2] * ez[i]; + dant[i] = C1 * dant1 + C2 * dant2; + } } + + /* satellite position/clock by precise ephemeris/clock ------------------------- -* compute satellite position/clock with precise ephemeris/clock -* args : gtime_t time I time (gpst) -* int sat I satellite number -* nav_t *nav I navigation data -* int opt I sat postion option -* (0: center of mass, 1: antenna phase center) -* double *rs O sat position and velocity (ecef) -* {x,y,z,vx,vy,vz} (m|m/s) -* double *dts O sat clock {bias,drift} (s|s/s) -* double *var IO sat position and clock error variance (m) -* (NULL: no output) -* return : status (1:ok,0:error or data outage) -* notes : clock includes relativistic correction but does not contain code bias -* before calling the function, nav->peph, nav->ne, nav->pclk and -* nav->nc must be set by calling readsp3(), readrnx() or readrnxt() -* if precise clocks are not set, clocks in sp3 are used instead -*-----------------------------------------------------------------------------*/ + * compute satellite position/clock with precise ephemeris/clock + * args : gtime_t time I time (gpst) + * int sat I satellite number + * nav_t *nav I navigation data + * int opt I sat postion option + * (0: center of mass, 1: antenna phase center) + * double *rs O sat position and velocity (ecef) + * {x,y,z,vx,vy,vz} (m|m/s) + * double *dts O sat clock {bias,drift} (s|s/s) + * double *var IO sat position and clock error variance (m) + * (NULL: no output) + * return : status (1:ok,0:error or data outage) + * notes : clock includes relativistic correction but does not contain code bias + * before calling the function, nav->peph, nav->ne, nav->pclk and + * nav->nc must be set by calling readsp3(), readrnx() or readrnxt() + * if precise clocks are not set, clocks in sp3 are used instead + *-----------------------------------------------------------------------------*/ int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt, - double *rs, double *dts, double *var) + double *rs, double *dts, double *var) { - double rss[3],rst[3],dtss[1],dtst[1],dant[3]={},vare=0.0,varc=0.0,tt=1E-3; + double rss[3], rst[3], dtss[1], dtst[1], dant[3] = {}, vare = 0.0, varc = 0.0, tt = 1e-3; int i; - - trace(4,"peph2pos: time=%s sat=%2d opt=%d\n",time_str(time,3),sat,opt); - - if (sat<=0||MAXSAT strtok_r() in expath() for thread-safe -* add bdsmodear in procopt_default -* 2015/03/19 1.30 fix bug on interpolation of erp values in geterp() -* add leap second insertion before 2015/07/01 00:00 -* add api read_leaps() -* 2015/05/31 1.31 delte api windupcorr() -* 2015/08/08 1.32 add compile option CPUTIME_IN_GPST -* add api add_fatal() -* support usno leapsec.dat for api read_leaps() -* 2016/01/23 1.33 enable septentrio -* 2016/02/05 1.34 support GLONASS for savenav(), loadnav() -* 2016/06/11 1.35 delete trace() in reppath() to avoid deadlock -* 2016/07/01 1.36 support IRNSS -* add leap second before 2017/1/1 00:00:00 -* 2016/07/29 1.37 rename api compress() -> rtk_uncompress() -* rename api crc16() -> rtk_crc16() -* rename api crc24q() -> rtk_crc24q() -* rename api crc32() -> rtk_crc32() -* 2016/08/20 1.38 fix type incompatibility in win64 environment -* change constant _POSIX_C_SOURCE 199309 -> 199506 -* 2016/08/21 1.39 fix bug on week overflow in time2gpst()/gpst2time() -* 2016/09/05 1.40 fix bug on invalid nav data read in readnav() -* 2016/09/17 1.41 suppress warnings -* 2016/09/19 1.42 modify api deg2dms() to consider numerical error -*-----------------------------------------------------------------------------*/ + *----------------------------------------------------------------------------*/ + #include #include #include @@ -181,194 +59,205 @@ #include #include "rtklib_rtkcmn.h" -const double gpst0[]={1980,1, 6,0,0,0}; /* gps time reference */ -const double gst0 []={1999,8,22,0,0,0}; /* galileo system time reference */ -const double bdt0 []={2006,1, 1,0,0,0}; /* beidou time reference */ +const double gpst0[] = {1980, 1, 6, 0, 0, 0}; /* gps time reference */ +const double gst0 [] = {1999, 8, 22, 0, 0, 0}; /* galileo system time reference */ +const double bdt0 [] = {2006, 1, 1, 0, 0, 0}; /* beidou time reference */ -double leaps[MAXLEAPS+1][7]={ /* leap seconds (y,m,d,h,m,s,utc-gpst) */ - {2017,1,1,0,0,0,-18}, - {2015,7,1,0,0,0,-17}, - {2012,7,1,0,0,0,-16}, - {2009,1,1,0,0,0,-15}, - {2006,1,1,0,0,0,-14}, - {1999,1,1,0,0,0,-13}, - {1997,7,1,0,0,0,-12}, - {1996,1,1,0,0,0,-11}, - {1994,7,1,0,0,0,-10}, - {1993,7,1,0,0,0, -9}, - {1992,7,1,0,0,0, -8}, - {1991,1,1,0,0,0, -7}, - {1990,1,1,0,0,0, -6}, - {1988,1,1,0,0,0, -5}, - {1985,7,1,0,0,0, -4}, - {1983,7,1,0,0,0, -3}, - {1982,7,1,0,0,0, -2}, - {1981,7,1,0,0,0, -1}, - {} +double leaps[MAXLEAPS+1][7] = { /* leap seconds (y,m,d,h,m,s,utc-gpst) */ + {2017, 1, 1, 0, 0, 0, -18}, + {2015, 7, 1, 0, 0, 0, -17}, + {2012, 7, 1, 0, 0, 0, -16}, + {2009, 1, 1, 0, 0, 0, -15}, + {2006, 1, 1, 0, 0, 0, -14}, + {1999, 1, 1, 0, 0, 0, -13}, + {1997, 7, 1, 0, 0, 0, -12}, + {1996, 1, 1, 0, 0, 0, -11}, + {1994, 7, 1, 0, 0, 0, -10}, + {1993, 7, 1, 0, 0, 0, -9}, + {1992, 7, 1, 0, 0, 0, -8}, + {1991, 1, 1, 0, 0, 0, -7}, + {1990, 1, 1, 0, 0, 0, -6}, + {1988, 1, 1, 0, 0, 0, -5}, + {1985, 7, 1, 0, 0, 0, -4}, + {1983, 7, 1, 0, 0, 0, -3}, + {1982, 7, 1, 0, 0, 0, -2}, + {1981, 7, 1, 0, 0, 0, -1}, + {} }; -const prcopt_t prcopt_default={ /* defaults processing options */ - PMODE_SINGLE,0,2,SYS_GPS, /* mode,soltype,nf,navsys */ - 15.0*D2R,{}, /* elmin,snrmask */ - 0,1,1,1, /* sateph,modear,glomodear,bdsmodear */ - 5,0,10,1, /* maxout,minlock,minfix,armaxiter */ - 0,0,0,0, /* estion,esttrop,dynamics,tidecorr */ - 1,0,0,0,0, /* niter,codesmooth,intpref,sbascorr,sbassatsel */ - 0,0, /* rovpos,refpos */ - {100.0,100.0}, /* eratio[] */ - {100.0,0.003,0.003,0.0,1.0}, /* err[] */ - {30.0,0.03,0.3}, /* std[] */ - {1E-4,1E-3,1E-4,1E-1,1E-2,0.0}, /* prn[] */ - 5E-12, /* sclkstab */ - {3.0,0.9999,0.25,0.1,0.05}, /* thresar */ - 0.0,0.0,0.05, /* elmaskar,almaskhold,thresslip */ - 30.0,30.0,30.0, /* maxtdif,maxinno,maxgdop */ - {},{},{}, /* baseline,ru,rb */ - {"",""}, /* anttype */ - {},{},{}, /* antdel,pcv,exsats */ - {},{},{},{},{},{},{},{},{},{} - }; -const solopt_t solopt_default={ /* defaults solution output options */ - SOLF_LLH,TIMES_GPST,1,3, /* posf,times,timef,timeu */ - 0,1,0,0,0,0, /* degf,outhead,outopt,datum,height,geoid */ - 0,0,0, /* solstatic,sstat,trace */ - {0.0,0.0}, /* nmeaintv */ - " ","" /* separator/program name */ +const prcopt_t prcopt_default = { /* defaults processing options */ + PMODE_SINGLE, 0, 2, SYS_GPS, /* mode, soltype, nf, navsys */ + 15.0*D2R, {}, /* elmin, snrmask */ + 0, 1, 1, 1, /* sateph, modear, glomodear, bdsmodear */ + 5, 0, 10, 1, /* maxout, minlock, minfix, armaxiter */ + 0, 0, 0, 0, /* estion, esttrop, dynamics, tidecorr */ + 1, 0, 0, 0, 0, /* niter, codesmooth, intpref, sbascorr, sbassatsel */ + 0, 0, /* rovpos, refpos */ + {100.0, 100.0}, /* eratio[] */ + {100.0, 0.003, 0.003, 0.0, 1.0}, /* err[] */ + {30.0, 0.03, 0.3}, /* std[] */ + {1e-4, 1e-3, 1e-4, 1e-1, 1e-2, 0.0}, /* prn[] */ + 5E-12, /* sclkstab */ + {3.0, 0.9999, 0.25, 0.1, 0.05}, /* thresar */ + 0.0, 0.0, 0.05, /* elmaskar, almaskhold, thresslip */ + 30.0, 30.0, 30.0, /* maxtdif, maxinno, maxgdop */ + {}, {}, {}, /* baseline, ru, rb */ + {"",""}, /* anttype */ + {}, {}, {}, /* antdel, pcv, exsats */ + {}, {}, {}, {}, {}, {}, {}, {}, {}, {} }; -const char *formatstrs[32]={ /* stream format strings */ - "RTCM 2", /* 0 */ - "RTCM 3", /* 1 */ - "NovAtel OEM6", /* 2 */ - "NovAtel OEM3", /* 3 */ - "u-blox", /* 4 */ - "Superstar II", /* 5 */ - "Hemisphere", /* 6 */ - "SkyTraq", /* 7 */ - "GW10", /* 8 */ - "Javad", /* 9 */ - "NVS BINR", /* 10 */ - "BINEX", /* 11 */ - "Trimble RT17", /* 12 */ - "Septentrio", /* 13 */ - "CMR/CMR+", /* 14 */ - "LEX Receiver", /* 15 */ - "RINEX", /* 16 */ - "SP3", /* 17 */ - "RINEX CLK", /* 18 */ - "SBAS", /* 19 */ - "NMEA 0183", /* 20 */ - NULL -}; -char obscodes[][3]={ /* observation code strings */ - "" ,"1C","1P","1W","1Y", "1M","1N","1S","1L","1E", /* 0- 9 */ - "1A","1B","1X","1Z","2C", "2D","2S","2L","2X","2P", /* 10-19 */ - "2W","2Y","2M","2N","5I", "5Q","5X","7I","7Q","7X", /* 20-29 */ - "6A","6B","6C","6X","6Z", "6S","6L","8L","8Q","8X", /* 30-39 */ - "2I","2Q","6I","6Q","3I", "3Q","3X","1I","1Q","5A", /* 40-49 */ - "5B","5C","9A","9B","9C", "9X","" ,"" ,"" ,"" /* 50-59 */ -}; -unsigned char obsfreqs[]={ - /* 1:L1/E1, 2:L2/B1, 3:L5/E5a/L3, 4:L6/LEX/B3, 5:E5b/B2, 6:E5(a+b), 7:S */ - 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, /* 0- 9 */ - 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, /* 10-19 */ - 2, 2, 2, 2, 3, 3, 3, 5, 5, 5, /* 20-29 */ - 4, 4, 4, 4, 4, 4, 4, 6, 6, 6, /* 30-39 */ - 2, 2, 4, 4, 3, 3, 3, 1, 1, 3, /* 40-49 */ - 3, 3, 7, 7, 7, 7, 0, 0, 0, 0 /* 50-59 */ -}; -char codepris[7][MAXFREQ][16]={ /* code priority table */ - /* L1/E1 L2/B1 L5/E5a/L3 L6/LEX/B3 E5b/B2 E5(a+b) S */ - {"CPYWMNSL","PYWCMNDSLX","IQX" ,"" ,"" ,"" ,"" }, /* GPS */ - {"PC" ,"PC" ,"IQX" ,"" ,"" ,"" ,"" }, /* GLO */ - {"CABXZ" ,"" ,"IQX" ,"ABCXZ" ,"IQX" ,"IQX" ,"" }, /* GAL */ - {"CSLXZ" ,"SLX" ,"IQX" ,"SLX" ,"" ,"" ,"" }, /* QZS */ - {"C" ,"" ,"IQX" ,"" ,"" ,"" ,"" }, /* SBS */ - {"IQX" ,"IQX" ,"IQX" ,"IQX" ,"IQX" ,"" ,"" }, /* BDS */ - {"" ,"" ,"ABCX" ,"" ,"" ,"" ,"ABCX"} /* IRN */ +const solopt_t solopt_default = { /* defaults solution output options */ + SOLF_LLH, TIMES_GPST, 1, 3, /* posf, times, timef, timeu */ + 0, 1, 0, 0, 0, 0, /* degf, outhead, outopt, datum, height, geoid */ + 0, 0, 0, /* solstatic, sstat, trace */ + {0.0, 0.0}, /* nmeaintv */ + " ", "", 0 /* separator/program name */ }; -fatalfunc_t *fatalfunc=NULL; /* fatal callback function */ + + +const char *formatstrs[32] = { /* stream format strings */ + "RTCM 2", /* 0 */ + "RTCM 3", /* 1 */ + "NovAtel OEM6", /* 2 */ + "NovAtel OEM3", /* 3 */ + "u-blox", /* 4 */ + "Superstar II", /* 5 */ + "Hemisphere", /* 6 */ + "SkyTraq", /* 7 */ + "GW10", /* 8 */ + "Javad", /* 9 */ + "NVS BINR", /* 10 */ + "BINEX", /* 11 */ + "Trimble RT17", /* 12 */ + "Septentrio", /* 13 */ + "CMR/CMR+", /* 14 */ + "LEX Receiver", /* 15 */ + "RINEX", /* 16 */ + "SP3", /* 17 */ + "RINEX CLK", /* 18 */ + "SBAS", /* 19 */ + "NMEA 0183", /* 20 */ + NULL +}; + + +char obscodes[][3] = { /* observation code strings */ + "" ,"1C","1P","1W","1Y", "1M","1N","1S","1L","1E", /* 0- 9 */ + "1A","1B","1X","1Z","2C", "2D","2S","2L","2X","2P", /* 10-19 */ + "2W","2Y","2M","2N","5I", "5Q","5X","7I","7Q","7X", /* 20-29 */ + "6A","6B","6C","6X","6Z", "6S","6L","8L","8Q","8X", /* 30-39 */ + "2I","2Q","6I","6Q","3I", "3Q","3X","1I","1Q","5A", /* 40-49 */ + "5B","5C","9A","9B","9C", "9X","" ,"" ,"" ,"" /* 50-59 */ +}; + + +unsigned char obsfreqs[] = { + /* 1:L1/E1, 2:L2/B1, 3:L5/E5a/L3, 4:L6/LEX/B3, 5:E5b/B2, 6:E5(a+b), 7:S */ + 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, /* 0- 9 */ + 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, /* 10-19 */ + 2, 2, 2, 2, 3, 3, 3, 5, 5, 5, /* 20-29 */ + 4, 4, 4, 4, 4, 4, 4, 6, 6, 6, /* 30-39 */ + 2, 2, 4, 4, 3, 3, 3, 1, 1, 3, /* 40-49 */ + 3, 3, 7, 7, 7, 7, 0, 0, 0, 0 /* 50-59 */ +}; + + +char codepris[7][MAXFREQ][16] = { /* code priority table */ + + /* L1/E1 L2/B1 L5/E5a/L3 L6/LEX/B3 E5b/B2 E5(a+b) S */ + {"CPYWMNSL", "PYWCMNDSLX", "IQX" , "" , "" , "" , "" }, /* GPS */ + {"PC" , "PC" , "IQX" , "" , "" , "" , "" }, /* GLO */ + {"CABXZ" , "" , "IQX" , "ABCXZ" , "IQX" , "IQX" , "" }, /* GAL */ + {"CSLXZ" , "SLX" , "IQX" , "SLX" , "" , "" , "" }, /* QZS */ + {"C" , "" , "IQX" , "" , "" , "" , "" }, /* SBS */ + {"IQX" , "IQX" , "IQX" , "IQX" , "IQX" , "" , "" }, /* BDS */ + {"" , "" , "ABCX" , "" , "" , "" , "ABCX"} /* IRN */ +}; + + +fatalfunc_t *fatalfunc = NULL; /* fatal callback function */ /* crc tables generated by util/gencrc ---------------------------------------*/ -const unsigned short tbl_CRC16[]={ - 0x0000,0x1021,0x2042,0x3063,0x4084,0x50A5,0x60C6,0x70E7, - 0x8108,0x9129,0xA14A,0xB16B,0xC18C,0xD1AD,0xE1CE,0xF1EF, - 0x1231,0x0210,0x3273,0x2252,0x52B5,0x4294,0x72F7,0x62D6, - 0x9339,0x8318,0xB37B,0xA35A,0xD3BD,0xC39C,0xF3FF,0xE3DE, - 0x2462,0x3443,0x0420,0x1401,0x64E6,0x74C7,0x44A4,0x5485, - 0xA56A,0xB54B,0x8528,0x9509,0xE5EE,0xF5CF,0xC5AC,0xD58D, - 0x3653,0x2672,0x1611,0x0630,0x76D7,0x66F6,0x5695,0x46B4, - 0xB75B,0xA77A,0x9719,0x8738,0xF7DF,0xE7FE,0xD79D,0xC7BC, - 0x48C4,0x58E5,0x6886,0x78A7,0x0840,0x1861,0x2802,0x3823, - 0xC9CC,0xD9ED,0xE98E,0xF9AF,0x8948,0x9969,0xA90A,0xB92B, - 0x5AF5,0x4AD4,0x7AB7,0x6A96,0x1A71,0x0A50,0x3A33,0x2A12, - 0xDBFD,0xCBDC,0xFBBF,0xEB9E,0x9B79,0x8B58,0xBB3B,0xAB1A, - 0x6CA6,0x7C87,0x4CE4,0x5CC5,0x2C22,0x3C03,0x0C60,0x1C41, - 0xEDAE,0xFD8F,0xCDEC,0xDDCD,0xAD2A,0xBD0B,0x8D68,0x9D49, - 0x7E97,0x6EB6,0x5ED5,0x4EF4,0x3E13,0x2E32,0x1E51,0x0E70, - 0xFF9F,0xEFBE,0xDFDD,0xCFFC,0xBF1B,0xAF3A,0x9F59,0x8F78, - 0x9188,0x81A9,0xB1CA,0xA1EB,0xD10C,0xC12D,0xF14E,0xE16F, - 0x1080,0x00A1,0x30C2,0x20E3,0x5004,0x4025,0x7046,0x6067, - 0x83B9,0x9398,0xA3FB,0xB3DA,0xC33D,0xD31C,0xE37F,0xF35E, - 0x02B1,0x1290,0x22F3,0x32D2,0x4235,0x5214,0x6277,0x7256, - 0xB5EA,0xA5CB,0x95A8,0x8589,0xF56E,0xE54F,0xD52C,0xC50D, - 0x34E2,0x24C3,0x14A0,0x0481,0x7466,0x6447,0x5424,0x4405, - 0xA7DB,0xB7FA,0x8799,0x97B8,0xE75F,0xF77E,0xC71D,0xD73C, - 0x26D3,0x36F2,0x0691,0x16B0,0x6657,0x7676,0x4615,0x5634, - 0xD94C,0xC96D,0xF90E,0xE92F,0x99C8,0x89E9,0xB98A,0xA9AB, - 0x5844,0x4865,0x7806,0x6827,0x18C0,0x08E1,0x3882,0x28A3, - 0xCB7D,0xDB5C,0xEB3F,0xFB1E,0x8BF9,0x9BD8,0xABBB,0xBB9A, - 0x4A75,0x5A54,0x6A37,0x7A16,0x0AF1,0x1AD0,0x2AB3,0x3A92, - 0xFD2E,0xED0F,0xDD6C,0xCD4D,0xBDAA,0xAD8B,0x9DE8,0x8DC9, - 0x7C26,0x6C07,0x5C64,0x4C45,0x3CA2,0x2C83,0x1CE0,0x0CC1, - 0xEF1F,0xFF3E,0xCF5D,0xDF7C,0xAF9B,0xBFBA,0x8FD9,0x9FF8, - 0x6E17,0x7E36,0x4E55,0x5E74,0x2E93,0x3EB2,0x0ED1,0x1EF0 +const unsigned short tbl_CRC16[] = { + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, + 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, + 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, + 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, + 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, + 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, + 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, + 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, + 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, + 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, + 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, + 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, + 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, + 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, + 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, + 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, + 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, + 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, + 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, + 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, + 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, + 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, + 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, + 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, + 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, + 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, + 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, + 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, + 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, + 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0 }; -const unsigned int tbl_CRC24Q[]={ - 0x000000,0x864CFB,0x8AD50D,0x0C99F6,0x93E6E1,0x15AA1A,0x1933EC,0x9F7F17, - 0xA18139,0x27CDC2,0x2B5434,0xAD18CF,0x3267D8,0xB42B23,0xB8B2D5,0x3EFE2E, - 0xC54E89,0x430272,0x4F9B84,0xC9D77F,0x56A868,0xD0E493,0xDC7D65,0x5A319E, - 0x64CFB0,0xE2834B,0xEE1ABD,0x685646,0xF72951,0x7165AA,0x7DFC5C,0xFBB0A7, - 0x0CD1E9,0x8A9D12,0x8604E4,0x00481F,0x9F3708,0x197BF3,0x15E205,0x93AEFE, - 0xAD50D0,0x2B1C2B,0x2785DD,0xA1C926,0x3EB631,0xB8FACA,0xB4633C,0x322FC7, - 0xC99F60,0x4FD39B,0x434A6D,0xC50696,0x5A7981,0xDC357A,0xD0AC8C,0x56E077, - 0x681E59,0xEE52A2,0xE2CB54,0x6487AF,0xFBF8B8,0x7DB443,0x712DB5,0xF7614E, - 0x19A3D2,0x9FEF29,0x9376DF,0x153A24,0x8A4533,0x0C09C8,0x00903E,0x86DCC5, - 0xB822EB,0x3E6E10,0x32F7E6,0xB4BB1D,0x2BC40A,0xAD88F1,0xA11107,0x275DFC, - 0xDCED5B,0x5AA1A0,0x563856,0xD074AD,0x4F0BBA,0xC94741,0xC5DEB7,0x43924C, - 0x7D6C62,0xFB2099,0xF7B96F,0x71F594,0xEE8A83,0x68C678,0x645F8E,0xE21375, - 0x15723B,0x933EC0,0x9FA736,0x19EBCD,0x8694DA,0x00D821,0x0C41D7,0x8A0D2C, - 0xB4F302,0x32BFF9,0x3E260F,0xB86AF4,0x2715E3,0xA15918,0xADC0EE,0x2B8C15, - 0xD03CB2,0x567049,0x5AE9BF,0xDCA544,0x43DA53,0xC596A8,0xC90F5E,0x4F43A5, - 0x71BD8B,0xF7F170,0xFB6886,0x7D247D,0xE25B6A,0x641791,0x688E67,0xEEC29C, - 0x3347A4,0xB50B5F,0xB992A9,0x3FDE52,0xA0A145,0x26EDBE,0x2A7448,0xAC38B3, - 0x92C69D,0x148A66,0x181390,0x9E5F6B,0x01207C,0x876C87,0x8BF571,0x0DB98A, - 0xF6092D,0x7045D6,0x7CDC20,0xFA90DB,0x65EFCC,0xE3A337,0xEF3AC1,0x69763A, - 0x578814,0xD1C4EF,0xDD5D19,0x5B11E2,0xC46EF5,0x42220E,0x4EBBF8,0xC8F703, - 0x3F964D,0xB9DAB6,0xB54340,0x330FBB,0xAC70AC,0x2A3C57,0x26A5A1,0xA0E95A, - 0x9E1774,0x185B8F,0x14C279,0x928E82,0x0DF195,0x8BBD6E,0x872498,0x016863, - 0xFAD8C4,0x7C943F,0x700DC9,0xF64132,0x693E25,0xEF72DE,0xE3EB28,0x65A7D3, - 0x5B59FD,0xDD1506,0xD18CF0,0x57C00B,0xC8BF1C,0x4EF3E7,0x426A11,0xC426EA, - 0x2AE476,0xACA88D,0xA0317B,0x267D80,0xB90297,0x3F4E6C,0x33D79A,0xB59B61, - 0x8B654F,0x0D29B4,0x01B042,0x87FCB9,0x1883AE,0x9ECF55,0x9256A3,0x141A58, - 0xEFAAFF,0x69E604,0x657FF2,0xE33309,0x7C4C1E,0xFA00E5,0xF69913,0x70D5E8, - 0x4E2BC6,0xC8673D,0xC4FECB,0x42B230,0xDDCD27,0x5B81DC,0x57182A,0xD154D1, - 0x26359F,0xA07964,0xACE092,0x2AAC69,0xB5D37E,0x339F85,0x3F0673,0xB94A88, - 0x87B4A6,0x01F85D,0x0D61AB,0x8B2D50,0x145247,0x921EBC,0x9E874A,0x18CBB1, - 0xE37B16,0x6537ED,0x69AE1B,0xEFE2E0,0x709DF7,0xF6D10C,0xFA48FA,0x7C0401, - 0x42FA2F,0xC4B6D4,0xC82F22,0x4E63D9,0xD11CCE,0x575035,0x5BC9C3,0xDD8538 + + +const unsigned int tbl_CRC24Q[] = { + 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, + 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, + 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, + 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, + 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, + 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, + 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, + 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, + 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, + 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC, + 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, + 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, + 0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, + 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, + 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5, + 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C, + 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, + 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, + 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, + 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, + 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A, + 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, + 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, + 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, + 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61, + 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, + 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, + 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, + 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, + 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, + 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, + 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538 }; extern "C" { - void dgemm_(char *, char *, int *, int *, int *, double *, double *, - int *, double *, int *, double *, double *, int *); - extern void dgetrf_(int *, int *, double *, int *, int *, int *); - extern void dgetri_(int *, double *, int *, int *, double *, int *, int *); - extern void dgetrs_(char *, int *, int *, double *, int *, int *, double *, - int *, int *); + void dgemm_(char *, char *, int *, int *, int *, double *, double *, int *, double *, int *, double *, double *, int *); + extern void dgetrf_(int *, int *, double *, int *, int *, int *); + extern void dgetri_(int *, double *, int *, int *, double *, int *, int *); + extern void dgetrs_(char *, int *, int *, double *, int *, int *, double *, int *, int *); } @@ -377,601 +266,693 @@ extern "C" { #ifdef IERS_MODEL extern int gmf_(double *mjd, double *lat, double *lon, double *hgt, double *zd, - double *gmfh, double *gmfw); + double *gmfh, double *gmfw); #endif + /* fatal error ---------------------------------------------------------------*/ void fatalerr(const char *format, ...) { char msg[1024]; va_list ap; - va_start(ap,format); vsprintf(msg,format,ap); va_end(ap); - fprintf(stderr,"%s",msg); + va_start(ap, format); vsprintf(msg, format, ap); va_end(ap); + fprintf(stderr, "%s", msg); exit(-9); } + /* satellite system+prn/slot number to satellite number ------------------------ -* convert satellite system+prn/slot number to satellite number -* args : int sys I satellite system (SYS_GPS,SYS_GLO,...) -* int prn I satellite prn/slot number -* return : satellite number (0:error) -*-----------------------------------------------------------------------------*/ + * convert satellite system+prn/slot number to satellite number + * args : int sys I satellite system (SYS_GPS,SYS_GLO,...) + * int prn I satellite prn/slot number + * return : satellite number (0:error) + *-----------------------------------------------------------------------------*/ int satno(int sys, int prn) { - if (prn<=0) return 0; - switch (sys) { - case SYS_GPS: - if (prnexsats[sat-1]==1) return 1; /* excluded satellite */ - if (opt->exsats[sat-1]==2) return 0; /* included satellite */ - if (!(sys&opt->navsys)) return 1; /* unselected sat sys */ - } - if (sys==SYS_QZS) svh&=0xFE; /* mask QZSS LEX health */ - if (svh) { - trace(3,"unhealthy satellite: sat=%3d svh=%02X\n",sat,svh); - return 1; - } + + if (opt) + { + if (opt->exsats[sat-1] == 1) return 1; /* excluded satellite */ + if (opt->exsats[sat-1] == 2) return 0; /* included satellite */ + if (!(sys&opt->navsys)) return 1; /* unselected sat sys */ + } + if (sys == SYS_QZS) svh&=0xFE; /* mask QZSS LEX health */ + if (svh) + { + trace(3, "unhealthy satellite: sat=%3d svh=%02X\n", sat, svh); + return 1; + } return 0; } + + /* test SNR mask --------------------------------------------------------------- -* test SNR mask -* args : int base I rover or base-station (0:rover,1:base station) -* int freq I frequency (0:L1,1:L2,2:L3,...) -* double el I elevation angle (rad) -* double snr I C/N0 (dBHz) -* snrmask_t *mask I SNR mask -* return : status (1:masked,0:unmasked) -*-----------------------------------------------------------------------------*/ + * test SNR mask + * args : int base I rover or base-station (0:rover,1:base station) + * int freq I frequency (0:L1,1:L2,2:L3,...) + * double el I elevation angle (rad) + * double snr I C/N0 (dBHz) + * snrmask_t *mask I SNR mask + * return : status (1:masked,0:unmasked) + *-----------------------------------------------------------------------------*/ int testsnr(int base, int freq, double el, double snr, - const snrmask_t *mask) + const snrmask_t *mask) { - double minsnr,a; + double minsnr, a; int i; - - if (!mask->ena[base]||freq<0||freq>=NFREQ) return 0; - - a=(el*R2D+5.0)/10.0; - i=(int)floor(a); a-=i; - if (i<1) minsnr=mask->mask[freq][0]; - else if (i>8) minsnr=mask->mask[freq][8]; - else minsnr=(1.0-a)*mask->mask[freq][i-1]+a*mask->mask[freq][i]; - + + if (!mask->ena[base] || freq<0 || freq >= NFREQ) return 0; + + a = (el*R2D+5.0)/10.0; + i = (int)floor(a); a -= i; + if (i<1) minsnr = mask->mask[freq][0]; + else if (i>8) minsnr = mask->mask[freq][8]; + else minsnr = (1.0-a)*mask->mask[freq][i-1]+a*mask->mask[freq][i]; + return snr>(7-i%8))&1u); + for (i = pos; i>(7-i%8))&1u); return bits; } + + int getbits(const unsigned char *buff, int pos, int len) { - unsigned int bits=getbitu(buff,pos,len); - if (len<=0||32<=len||!(bits&(1u<<(len-1)))) return (int)bits; + unsigned int bits = getbitu(buff, pos, len); + if (len <= 0 || 32 <= len || !(bits&(1u<<(len-1)))) return (int)bits; return (int)(bits|(~0u<>=1) { - if (data&mask) buff[i/8]|=1u<<(7-i%8); else buff[i/8]&=~(1u<<(7-i%8)); - } + if (len <= 0 || 32>=1) + { + if (data&mask) buff[i/8]|=1u<<(7-i%8); else buff[i/8]&=~(1u<<(7-i%8)); + } } + + void setbits(unsigned char *buff, int pos, int len, int data) { if (data<0) data|=1<<(len-1); else data&=~(1<<(len-1)); /* set sign bit */ - setbitu(buff,pos,len,(unsigned int)data); + setbitu(buff, pos, len, (unsigned int)data); } + + /* crc-32 parity --------------------------------------------------------------- -* compute crc-32 parity for novatel raw -* args : unsigned char *buff I data -* int len I data length (bytes) -* return : crc-32 parity -* notes : see NovAtel OEMV firmware manual 1.7 32-bit CRC -*-----------------------------------------------------------------------------*/ + * compute crc-32 parity for novatel raw + * args : unsigned char *buff I data + * int len I data length (bytes) + * return : crc-32 parity + * notes : see NovAtel OEMV firmware manual 1.7 32-bit CRC + *-----------------------------------------------------------------------------*/ unsigned int rtk_crc32(const unsigned char *buff, int len) { - unsigned int crc=0; - int i,j; - - trace(4,"rtk_crc32: len=%d\n",len); - - for (i=0;i>1)^POLYCRC32; else crc>>=1; + unsigned int crc = 0; + int i, j; + + trace(4, "rtk_crc32: len=%d\n", len); + + for (i = 0; i>1)^POLYCRC32; else crc>>=1; + } } - } return crc; } + + /* crc-24q parity -------------------------------------------------------------- -* compute crc-24q parity for sbas, rtcm3 -* args : unsigned char *buff I data -* int len I data length (bytes) -* return : crc-24Q parity -* notes : see reference [2] A.4.3.3 Parity -*-----------------------------------------------------------------------------*/ + * compute crc-24q parity for sbas, rtcm3 + * args : unsigned char *buff I data + * int len I data length (bytes) + * return : crc-24Q parity + * notes : see reference [2] A.4.3.3 Parity + *-----------------------------------------------------------------------------*/ unsigned int rtk_crc24q(const unsigned char *buff, int len) { - unsigned int crc=0; + unsigned int crc = 0; int i; - - trace(4,"rtk_crc24q: len=%d\n",len); - - for (i=0;i>16)^buff[i]]; + + trace(4, "rtk_crc24q: len=%d\n", len); + + for (i = 0; i>16)^buff[i]]; return crc; } + + /* crc-16 parity --------------------------------------------------------------- -* compute crc-16 parity for binex, nvs -* args : unsigned char *buff I data -* int len I data length (bytes) -* return : crc-16 parity -* notes : see reference [10] A.3. -*-----------------------------------------------------------------------------*/ + * compute crc-16 parity for binex, nvs + * args : unsigned char *buff I data + * int len I data length (bytes) + * return : crc-16 parity + * notes : see reference [10] A.3. + *-----------------------------------------------------------------------------*/ unsigned short rtk_crc16(const unsigned char *buff, int len) { - unsigned short crc=0; + unsigned short crc = 0; int i; - - trace(4,"rtk_crc16: len=%d\n",len); - - for (i=0;i>8)^buff[i])&0xFF]; - } + + trace(4, "rtk_crc16: len=%d\n", len); + + for (i = 0; i>8)^buff[i])&0xFF]; + } return crc; } + + /* decode navigation data word ------------------------------------------------- -* check party and decode navigation data word -* args : unsigned int word I navigation data word (2+30bit) -* (previous word D29*-30* + current word D1-30) -* unsigned char *data O decoded navigation data without parity -* (8bitx3) -* return : status (1:ok,0:parity error) -* notes : see reference [1] 20.3.5.2 user parity algorithm -*-----------------------------------------------------------------------------*/ + * check party and decode navigation data word + * args : unsigned int word I navigation data word (2+30bit) + * (previous word D29*-30* + current word D1-30) + * unsigned char *data O decoded navigation data without parity + * (8bitx3) + * return : status (1:ok,0:parity error) + * notes : see reference [1] 20.3.5.2 user parity algorithm + *-----------------------------------------------------------------------------*/ int decode_word(unsigned int word, unsigned char *data) { - const unsigned int hamming[]={ - 0xBB1F3480,0x5D8F9A40,0xAEC7CD00,0x5763E680,0x6BB1F340,0x8B7A89C0 + const unsigned int hamming[] = { + 0xBB1F3480, 0x5D8F9A40, 0xAEC7CD00, 0x5763E680, 0x6BB1F340, 0x8B7A89C0 }; - unsigned int parity=0,w; + unsigned int parity = 0, w; int i; - - trace(5,"decodeword: word=%08x\n",word); - + + trace(5, "decodeword: word=%08x\n", word); + if (word&0x40000000) word^=0x3FFFFFC0; - - for (i=0;i<6;i++) { - parity<<=1; - for (w=(word&hamming[i])>>6;w;w>>=1) parity^=w&1; - } - if (parity!=(word&0x3F)) return 0; - - for (i=0;i<3;i++) data[i]=(unsigned char)(word>>(22-i*8)); + + for (i = 0; i<6; i++) + { + parity<<= 1; + for (w = (word&hamming[i])>>6; w; w>>=1) parity^=w&1; + } + if (parity != (word&0x3F)) return 0; + + for (i = 0; i<3; i++) data[i] = (unsigned char)(word>>(22-i*8)); return 1; } + + /* new matrix ------------------------------------------------------------------ -* allocate memory of matrix -* args : int n,m I number of rows and columns of matrix -* return : matrix pointer (if n<=0 or m<=0, return NULL) -*-----------------------------------------------------------------------------*/ + * allocate memory of matrix + * args : int n,m I number of rows and columns of matrix + * return : matrix pointer (if n<=0 or m<=0, return NULL) + *-----------------------------------------------------------------------------*/ double *mat(int n, int m) { double *p; - - if (n<=0||m<=0) return NULL; - if (!(p=(double *)malloc(sizeof(double)*n*m))) { - fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m); - } + + if (n <= 0 || m <= 0) return NULL; + if (!(p = (double *)malloc(sizeof(double)*n*m))) + { + fatalerr("matrix memory allocation error: n=%d,m=%d\n", n, m); + } return p; } + + /* new integer matrix ---------------------------------------------------------- -* allocate memory of integer matrix -* args : int n,m I number of rows and columns of matrix -* return : matrix pointer (if n<=0 or m<=0, return NULL) -*-----------------------------------------------------------------------------*/ + * allocate memory of integer matrix + * args : int n,m I number of rows and columns of matrix + * return : matrix pointer (if n <= 0 or m <= 0, return NULL) + *-----------------------------------------------------------------------------*/ int *imat(int n, int m) { int *p; - - if (n<=0||m<=0) return NULL; - if (!(p=(int *)malloc(sizeof(int)*n*m))) { - fatalerr("integer matrix memory allocation error: n=%d,m=%d\n",n,m); - } + + if (n <= 0 || m <= 0) return NULL; + if (!(p = (int *)malloc(sizeof(int)*n*m))) + { + fatalerr("integer matrix memory allocation error: n=%d,m=%d\n", n, m); + } return p; } + + /* zero matrix ----------------------------------------------------------------- -* generate new zero matrix -* args : int n,m I number of rows and columns of matrix -* return : matrix pointer (if n<=0 or m<=0, return NULL) -*-----------------------------------------------------------------------------*/ + * generate new zero matrix + * args : int n,m I number of rows and columns of matrix + * return : matrix pointer (if n <= 0 or m <= 0, return NULL) + *-----------------------------------------------------------------------------*/ double *zeros(int n, int m) { double *p; - + #if NOCALLOC - if ((p=mat(n,m))) for (n=n*m-1;n>=0;n--) p[n]=0.0; + if ((p = mat(n, m))) for (n = n*m-1; n >= 0; n--) p[n] = 0.0; #else - if (n<=0||m<=0) return NULL; - if (!(p=(double *)calloc(sizeof(double),n*m))) { - fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m); + if (n <= 0 || m <= 0) return NULL; + if (!(p = (double *)calloc(sizeof(double), n*m))) { + fatalerr("matrix memory allocation error: n=%d,m=%d\n", n, m); } #endif return p; } + + /* identity matrix ------------------------------------------------------------- -* generate new identity matrix -* args : int n I number of rows and columns of matrix -* return : matrix pointer (if n<=0, return NULL) -*-----------------------------------------------------------------------------*/ + * generate new identity matrix + * args : int n I number of rows and columns of matrix + * return : matrix pointer (if n <= 0, return NULL) + *-----------------------------------------------------------------------------*/ double *eye(int n) { double *p; int i; - - if ((p=zeros(n,n))) for (i=0;i=0) c+=a[n]*b[n]; + double c = 0.0; + + while (--n >= 0) c += a[n]*b[n]; return c; } + + /* euclid norm ----------------------------------------------------------------- -* euclid norm of vector -* args : double *a I vector a (n x 1) -* int n I size of vector a -* return : || a || -*-----------------------------------------------------------------------------*/ + * euclid norm of vector + * args : double *a I vector a (n x 1) + * int n I size of vector a + * return : || a || + *-----------------------------------------------------------------------------*/ double norm(const double *a, int n) { - return sqrt(dot(a,a,n)); + return sqrt(dot(a, a, n)); } + + /* outer product of 3d vectors ------------------------------------------------- -* outer product of 3d vectors -* args : double *a,*b I vector a,b (3 x 1) -* double *c O outer product (a x b) (3 x 1) -* return : none -*-----------------------------------------------------------------------------*/ + * outer product of 3d vectors + * args : double *a,*b I vector a,b (3 x 1) + * double *c O outer product (a x b) (3 x 1) + * return : none + *-----------------------------------------------------------------------------*/ void cross3(const double *a, const double *b, double *c) { - c[0]=a[1]*b[2]-a[2]*b[1]; - c[1]=a[2]*b[0]-a[0]*b[2]; - c[2]=a[0]*b[1]-a[1]*b[0]; + c[0] = a[1]*b[2]-a[2]*b[1]; + c[1] = a[2]*b[0]-a[0]*b[2]; + c[2] = a[0]*b[1]-a[1]*b[0]; } + + /* normalize 3d vector --------------------------------------------------------- -* normalize 3d vector -* args : double *a I vector a (3 x 1) -* double *b O normlized vector (3 x 1) || b || = 1 -* return : status (1:ok,0:error) -*-----------------------------------------------------------------------------*/ + * normalize 3d vector + * args : double *a I vector a (3 x 1) + * double *b O normlized vector (3 x 1) || b || = 1 + * return : status (1:ok,0:error) + *-----------------------------------------------------------------------------*/ int normv3(const double *a, double *b) { double r; - if ((r=norm(a,3))<=0.0) return 0; - b[0]=a[0]/r; - b[1]=a[1]/r; - b[2]=a[2]/r; + if ((r = norm(a, 3)) <= 0.0) return 0; + b[0] = a[0]/r; + b[1] = a[1]/r; + b[2] = a[2]/r; return 1; } + + /* copy matrix ----------------------------------------------------------------- -* copy matrix -* args : double *A O destination matrix A (n x m) -* double *B I source matrix B (n x m) -* int n,m I number of rows and columns of matrix -* return : none -*-----------------------------------------------------------------------------*/ + * copy matrix + * args : double *A O destination matrix A (n x m) + * double *B I source matrix B (n x m) + * int n,m I number of rows and columns of matrix + * return : none + *-----------------------------------------------------------------------------*/ void matcpy(double *A, const double *B, int n, int m) { - memcpy(A,B,sizeof(double)*n*m); + memcpy(A, B, sizeof(double)*n*m); } + /* matrix routines -----------------------------------------------------------*/ /* multiply matrix (wrapper of blas dgemm) ------------------------------------- -* multiply matrix by matrix (C=alpha*A*B+beta*C) -* args : char *tr I transpose flags ("N":normal,"T":transpose) -* int n,k,m I size of (transposed) matrix A,B -* double alpha I alpha -* double *A,*B I (transposed) matrix A (n x m), B (m x k) -* double beta I beta -* double *C IO matrix C (n x k) -* return : none -*-----------------------------------------------------------------------------*/ + * multiply matrix by matrix (C=alpha*A*B+beta*C) + * args : char *tr I transpose flags ("N":normal,"T":transpose) + * int n,k,m I size of (transposed) matrix A,B + * double alpha I alpha + * double *A,*B I (transposed) matrix A (n x m), B (m x k) + * double beta I beta + * double *C IO matrix C (n x k) + * return : none + *-----------------------------------------------------------------------------*/ void matmul(const char *tr, int n, int k, int m, double alpha, - const double *A, const double *B, double beta, double *C) + const double *A, const double *B, double beta, double *C) { - int lda=tr[0]=='T'?m:n,ldb=tr[1]=='T'?k:m; - - dgemm_((char *)tr,(char *)tr+1,&n,&k,&m,&alpha,(double *)A,&lda,(double *)B, - &ldb,&beta,C,&n); + int lda = tr[0] == 'T' ? m : n, ldb = tr[1] == 'T' ? k : m; + + dgemm_((char *)tr, (char *)tr+1, &n, &k, &m, &alpha, (double *)A, &lda, (double *)B, + &ldb, &beta, C, &n); } + + /* inverse of matrix ----------------------------------------------------------- -* inverse of matrix (A=A^-1) -* args : double *A IO matrix (n x n) -* int n I size of matrix A -* return : status (0:ok,0>:error) -*-----------------------------------------------------------------------------*/ + * inverse of matrix (A=A^-1) + * args : double *A IO matrix (n x n) + * int n I size of matrix A + * return : status (0:ok,0>:error) + *-----------------------------------------------------------------------------*/ int matinv(double *A, int n) { double *work; - int info,lwork=n*16,*ipiv=imat(n,1); - - work=mat(lwork,1); - dgetrf_(&n,&n,A,&n,ipiv,&info); - if (!info) dgetri_(&n,A,&n,ipiv,work,&lwork,&info); + int info, lwork = n*16, *ipiv = imat(n, 1); + + work = mat(lwork, 1); + dgetrf_(&n, &n, A, &n, ipiv, &info); + if (!info) dgetri_(&n, A, &n, ipiv, work, &lwork, &info); free(ipiv); free(work); return info; } + + /* solve linear equation ------------------------------------------------------- -* solve linear equation (X=A\Y or X=A'\Y) -* args : char *tr I transpose flag ("N":normal,"T":transpose) -* double *A I input matrix A (n x n) -* double *Y I input matrix Y (n x m) -* int n,m I size of matrix A,Y -* double *X O X=A\Y or X=A'\Y (n x m) -* return : status (0:ok,0>:error) -* notes : matirix stored by column-major order (fortran convention) -* X can be same as Y -*-----------------------------------------------------------------------------*/ + * solve linear equation (X=A\Y or X=A'\Y) + * args : char *tr I transpose flag ("N":normal,"T":transpose) + * double *A I input matrix A (n x n) + * double *Y I input matrix Y (n x m) + * int n,m I size of matrix A,Y + * double *X O X=A\Y or X=A'\Y (n x m) + * return : status (0:ok,0>:error) + * notes : matirix stored by column-major order (fortran convention) + * X can be same as Y + *-----------------------------------------------------------------------------*/ int solve(const char *tr, const double *A, const double *Y, int n, - int m, double *X) + int m, double *X) { - double *B=mat(n,n); - int info,*ipiv=imat(n,1); - - matcpy(B,A,n,n); - matcpy(X,Y,n,m); - dgetrf_(&n,&n,B,&n,ipiv,&info); - if (!info) dgetrs_((char *)tr,&n,&m,B,&n,ipiv,X,&n,&info); + double *B = mat(n, n); + int info, *ipiv = imat(n, 1); + + matcpy(B, A, n, n); + matcpy(X, Y, n, m); + dgetrf_(&n, &n, B, &n, ipiv, &info); + if (!info) dgetrs_((char *)tr, &n, &m, B, &n, ipiv, X, &n, &info); free(ipiv); free(B); return info; } @@ -980,1749 +961,1969 @@ int solve(const char *tr, const double *A, const double *Y, int n, /* end of matrix routines ----------------------------------------------------*/ /* least square estimation ----------------------------------------------------- -* least square estimation by solving normal equation (x=(A*A')^-1*A*y) -* args : double *A I transpose of (weighted) design matrix (n x m) -* double *y I (weighted) measurements (m x 1) -* int n,m I number of parameters and measurements (n<=m) -* double *x O estmated parameters (n x 1) -* double *Q O esimated parameters covariance matrix (n x n) -* return : status (0:ok,0>:error) -* notes : for weighted least square, replace A and y by A*w and w*y (w=W^(1/2)) -* matirix stored by column-major order (fortran convention) -*-----------------------------------------------------------------------------*/ + * least square estimation by solving normal equation (x=(A*A')^-1*A*y) + * args : double *A I transpose of (weighted) design matrix (n x m) + * double *y I (weighted) measurements (m x 1) + * int n,m I number of parameters and measurements (n <= m) + * double *x O estmated parameters (n x 1) + * double *Q O esimated parameters covariance matrix (n x n) + * return : status (0:ok,0>:error) + * notes : for weighted least square, replace A and y by A*w and w*y (w=W^(1/2)) + * matirix stored by column-major order (fortran convention) + *-----------------------------------------------------------------------------*/ int lsq(const double *A, const double *y, int n, int m, double *x, - double *Q) + double *Q) { double *Ay; int info; - + if (m0.0) ix[k++]=i; - x_=mat(k,1); xp_=mat(k,1); P_=mat(k,k); Pp_=mat(k,k); H_=mat(k,m); - for (i=0;i0.0) ix[k++] = i; + x_ = mat(k, 1); xp_ = mat(k, 1); P_ = mat(k, k); Pp_ = mat(k, k); H_ = mat(k, m); + for (i = 0; i:error) -* notes : see reference [4] 5.2 -* matirix stored by column-major order (fortran convention) -*-----------------------------------------------------------------------------*/ + * combine forward and backward filters by fixed-interval smoother as follows: + * + * xs=Qs*(Qf^-1*xf+Qb^-1*xb), Qs=(Qf^-1+Qb^-1)^-1) + * + * args : double *xf I forward solutions (n x 1) + * args : double *Qf I forward solutions covariance matrix (n x n) + * double *xb I backward solutions (n x 1) + * double *Qb I backward solutions covariance matrix (n x n) + * int n I number of solutions + * double *xs O smoothed solutions (n x 1) + * double *Qs O smoothed solutions covariance matrix (n x n) + * return : status (0:ok,0>:error) + * notes : see reference [4] 5.2 + * matirix stored by column-major order (fortran convention) + *-----------------------------------------------------------------------------*/ int smoother(const double *xf, const double *Qf, const double *xb, - const double *Qb, int n, double *xs, double *Qs) + const double *Qb, int n, double *xs, double *Qs) { - double *invQf=mat(n,n),*invQb=mat(n,n),*xx=mat(n,1); - int i,info=-1; - - matcpy(invQf,Qf,n,n); - matcpy(invQb,Qb,n,n); - if (!matinv(invQf,n)&&!matinv(invQb,n)) { - for (i=0;i=0;s++) *p++=*s=='d'||*s=='D'?'E':*s; *p='\0'; - return sscanf(str,"%lf",&value)==1?value:0.0; + char str[256], *p = str; + + if (i<0 || (int)strlen(s)= 0; s++) *p++=*s == 'd' || *s == 'D' ? 'E' : *s; *p = '\0'; + return sscanf(str, "%lf", &value) == 1 ? value : 0.0; } + + /* string to time -------------------------------------------------------------- -* convert substring in string to gtime_t struct -* args : char *s I string ("... yyyy mm dd hh mm ss ...") -* int i,n I substring position and width -* gtime_t *t O gtime_t struct -* return : status (0:ok,0>:error) -*-----------------------------------------------------------------------------*/ + * convert substring in string to gtime_t struct + * args : char *s I string ("... yyyy mm dd hh mm ss ...") + * int i,n I substring position and width + * gtime_t *t O gtime_t struct + * return : status (0:ok,0>:error) + *-----------------------------------------------------------------------------*/ int str2time(const char *s, int i, int n, gtime_t *t) { double ep[6]; - char str[256],*p=str; - - if (i<0||(int)strlen(s)=0;) *p++=*s++; *p='\0'; - if (sscanf(str,"%lf %lf %lf %lf %lf %lf",ep,ep+1,ep+2,ep+3,ep+4,ep+5)<6) + char str[256], *p = str; + + if (i<0 || (int)strlen(s)= 0;) *p++=*s++; *p = '\0'; + if (sscanf(str, "%lf %lf %lf %lf %lf %lf", ep, ep+1, ep+2, ep+3, ep+4, ep+5)<6) return -1; - if (ep[0]<100.0) ep[0]+=ep[0]<80.0?2000.0:1900.0; - *t=epoch2time(ep); + if (ep[0]<100.0) ep[0] += ep[0]<80.0 ? 2000.0 : 1900.0; + *t = epoch2time(ep); return 0; } + + /* convert calendar day/time to time ------------------------------------------- -* convert calendar day/time to gtime_t struct -* args : double *ep I day/time {year,month,day,hour,min,sec} -* return : gtime_t struct -* notes : proper in 1970-2037 or 1970-2099 (64bit time_t) -*-----------------------------------------------------------------------------*/ + * convert calendar day/time to gtime_t struct + * args : double *ep I day/time {year,month,day,hour,min,sec} + * return : gtime_t struct + * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) + *-----------------------------------------------------------------------------*/ gtime_t epoch2time(const double *ep) { - const int doy[]={1,32,60,91,121,152,182,213,244,274,305,335}; - gtime_t time={}; - int days,sec,year=(int)ep[0],mon=(int)ep[1],day=(int)ep[2]; - - if (year<1970||2099=3?1:0); - sec=(int)floor(ep[5]); - time.time=(time_t)days*86400+(int)ep[3]*3600+(int)ep[4]*60+sec; - time.sec=ep[5]-sec; + days = (year-1970)*365+(year-1969)/4+doy[mon-1]+day-2+(year%4 == 0 && mon >= 3 ? 1 : 0); + sec = (int)floor(ep[5]); + time.time = (time_t)days*86400+(int)ep[3]*3600+(int)ep[4]*60+sec; + time.sec = ep[5]-sec; return time; } + + /* time to calendar day/time --------------------------------------------------- -* convert gtime_t struct to calendar day/time -* args : gtime_t t I gtime_t struct -* double *ep O day/time {year,month,day,hour,min,sec} -* return : none -* notes : proper in 1970-2037 or 1970-2099 (64bit time_t) -*-----------------------------------------------------------------------------*/ + * convert gtime_t struct to calendar day/time + * args : gtime_t t I gtime_t struct + * double *ep O day/time {year,month,day,hour,min,sec} + * return : none + * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) + *-----------------------------------------------------------------------------*/ void time2epoch(gtime_t t, double *ep) { - const int mday[]={ /* # of days in a month */ - 31,28,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31, - 31,29,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31 + const int mday[] = { /* # of days in a month */ + 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, + 31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; - int days,sec,mon,day; - + int days, sec, mon, day; + /* leap year if year%4==0 in 1901-2099 */ - days=(int)(t.time/86400); - sec=(int)(t.time-(time_t)days*86400); - for (day=days%1461,mon=0;mon<48;mon++) { - if (day>=mday[mon]) day-=mday[mon]; else break; - } - ep[0]=1970+days/1461*4+mon/12; ep[1]=mon%12+1; ep[2]=day+1; - ep[3]=sec/3600; ep[4]=sec%3600/60; ep[5]=sec%60+t.sec; + days = (int)(t.time/86400); + sec = (int)(t.time-(time_t)days*86400); + for (day = days%1461, mon = 0; mon<48; mon++) + { + if (day >= mday[mon]) day -= mday[mon]; else break; + } + ep[0] = 1970+days/1461*4+mon/12; ep[1] = mon%12+1; ep[2] = day+1; + ep[3] = sec/3600; ep[4] = sec%3600/60; ep[5] = sec%60+t.sec; } + + /* gps time to time ------------------------------------------------------------ -* convert week and tow in gps time to gtime_t struct -* args : int week I week number in gps time -* double sec I time of week in gps time (s) -* return : gtime_t struct -*-----------------------------------------------------------------------------*/ + * convert week and tow in gps time to gtime_t struct + * args : int week I week number in gps time + * double sec I time of week in gps time (s) + * return : gtime_t struct + *-----------------------------------------------------------------------------*/ gtime_t gpst2time(int week, double sec) { - gtime_t t=epoch2time(gpst0); - - if (sec<-1E9||1E9tm_year+1900; ep[1]=tt->tm_mon+1; ep[2]=tt->tm_mday; - ep[3]=tt->tm_hour; ep[4]=tt->tm_min; ep[5]=tt->tm_sec+tv.tv_usec*1E-6; - } - time=epoch2time(ep); - + + if (!gettimeofday(&tv, NULL) && (tt = gmtime(&tv.tv_sec))) + { + ep[0] = tt->tm_year+1900; ep[1] = tt->tm_mon+1; ep[2] = tt->tm_mday; + ep[3] = tt->tm_hour; ep[4] = tt->tm_min; ep[5] = tt->tm_sec+tv.tv_usec*1e-6; + } + time = epoch2time(ep); + #ifdef CPUTIME_IN_GPST /* cputime operated in gpst */ - time=gpst2utc(time); + time = gpst2utc(time); #endif return time; } + + /* set current time in utc ----------------------------------------------------- -* set current time in utc -* args : gtime_t I current time in utc -* return : none -* notes : just set time offset between cpu time and current time -* the time offset is reflected to only timeget() -* not reentrant -*----------------------------------------------------------------------------- + * set current time in utc + * args : gtime_t I current time in utc + * return : none + * notes : just set time offset between cpu time and current time + * the time offset is reflected to only timeget() + * not reentrant + *----------------------------------------------------------------------------- void timeset(gtime_t t) { timeoffset_+=timediff(t,timeget()); } -*/ + */ /* read leap seconds table by text -------------------------------------------*/ int read_leaps_text(FILE *fp) { - char buff[256],*p; - int i,n=0,ep[6],ls; - + char buff[256], *p; + int i, n = 0, ep[6], ls; + rewind(fp); - - while (fgets(buff,sizeof(buff),fp)&&n=13) continue; - ls[n][0]=y; - ls[n][1]=m; - ls[n][2]=d; - ls[n++][6]=(char)(19.0-tai_utc); - } - for (i=0;i= 13) continue; + ls[n][0] = y; + ls[n][1] = m; + ls[n][2] = d; + ls[n++][6] = (char)(19.0-tai_utc); + } + for (i = 0; i0;i++) { - tu=timeadd(t,leaps[i][6]); - if (timediff(tu,epoch2time(leaps[i]))>=0.0) return tu; - } + + for (i = 0; leaps[i][0]>0; i++) + { + tu = timeadd(t, leaps[i][6]); + if (timediff(tu, epoch2time(leaps[i])) >= 0.0) return tu; + } return t; } + + /* utc to gpstime -------------------------------------------------------------- -* convert utc to gpstime considering leap seconds -* args : gtime_t t I time expressed in utc -* return : time expressed in gpstime -* notes : ignore slight time offset under 100 ns -*-----------------------------------------------------------------------------*/ + * convert utc to gpstime considering leap seconds + * args : gtime_t t I time expressed in utc + * return : time expressed in gpstime + * notes : ignore slight time offset under 100 ns + *-----------------------------------------------------------------------------*/ gtime_t utc2gpst(gtime_t t) { int i; - - for (i=0;leaps[i][0]>0;i++) { - if (timediff(t,epoch2time(leaps[i]))>=0.0) return timeadd(t,-leaps[i][6]); - } + + for (i = 0; leaps[i][0]>0; i++) + { + if (timediff(t, epoch2time(leaps[i])) >= 0.0) return timeadd(t, -leaps[i][6]); + } return t; } + + /* gpstime to bdt -------------------------------------------------------------- -* convert gpstime to bdt (beidou navigation satellite system time) -* args : gtime_t t I time expressed in gpstime -* return : time expressed in bdt -* notes : ref [8] 3.3, 2006/1/1 00:00 BDT = 2006/1/1 00:00 UTC -* no leap seconds in BDT -* ignore slight time offset under 100 ns -*-----------------------------------------------------------------------------*/ + * convert gpstime to bdt (beidou navigation satellite system time) + * args : gtime_t t I time expressed in gpstime + * return : time expressed in bdt + * notes : ref [8] 3.3, 2006/1/1 00:00 BDT = 2006/1/1 00:00 UTC + * no leap seconds in BDT + * ignore slight time offset under 100 ns + *-----------------------------------------------------------------------------*/ gtime_t gpst2bdt(gtime_t t) { - return timeadd(t,-14.0); + return timeadd(t, -14.0); } + + /* bdt to gpstime -------------------------------------------------------------- -* convert bdt (beidou navigation satellite system time) to gpstime -* args : gtime_t t I time expressed in bdt -* return : time expressed in gpstime -* notes : see gpst2bdt() -*-----------------------------------------------------------------------------*/ + * convert bdt (beidou navigation satellite system time) to gpstime + * args : gtime_t t I time expressed in bdt + * return : time expressed in gpstime + * notes : see gpst2bdt() + *-----------------------------------------------------------------------------*/ gtime_t bdt2gpst(gtime_t t) { - return timeadd(t,14.0); + return timeadd(t, 14.0); } + + /* time to day and sec -------------------------------------------------------*/ double time2sec(gtime_t time, gtime_t *day) { - double ep[6],sec; - time2epoch(time,ep); - sec=ep[3]*3600.0+ep[4]*60.0+ep[5]; - ep[3]=ep[4]=ep[5]=0.0; - *day=epoch2time(ep); + double ep[6], sec; + time2epoch(time, ep); + sec = ep[3]*3600.0+ep[4]*60.0+ep[5]; + ep[3] = ep[4] = ep[5] = 0.0; + *day = epoch2time(ep); return sec; } + + /* utc to gmst ----------------------------------------------------------------- -* convert utc to gmst (Greenwich mean sidereal time) -* args : gtime_t t I time expressed in utc -* double ut1_utc I UT1-UTC (s) -* return : gmst (rad) -*-----------------------------------------------------------------------------*/ + * convert utc to gmst (Greenwich mean sidereal time) + * args : gtime_t t I time expressed in utc + * double ut1_utc I UT1-UTC (s) + * return : gmst (rad) + *-----------------------------------------------------------------------------*/ double utc2gmst(gtime_t t, double ut1_utc) { - const double ep2000[]={2000,1,1,12,0,0}; - gtime_t tut,tut0; - double ut,t1,t2,t3,gmst0,gmst; - - tut=timeadd(t,ut1_utc); - ut=time2sec(tut,&tut0); - t1=timediff(tut0,epoch2time(ep2000))/86400.0/36525.0; - t2=t1*t1; t3=t2*t1; - gmst0=24110.54841+8640184.812866*t1+0.093104*t2-6.2E-6*t3; - gmst=gmst0+1.002737909350795*ut; - - return fmod(gmst,86400.0)*PI/43200.0; /* 0 <= gmst <= 2*PI */ + const double ep2000[] = {2000, 1, 1, 12, 0, 0}; + gtime_t tut, tut0; + double ut, t1, t2, t3, gmst0, gmst; + + tut = timeadd(t, ut1_utc); + ut = time2sec(tut, &tut0); + t1 = timediff(tut0, epoch2time(ep2000))/86400.0/36525.0; + t2 = t1*t1; t3 = t2*t1; + gmst0 = 24110.54841+8640184.812866*t1+0.093104*t2-6.2E-6*t3; + gmst = gmst0+1.002737909350795*ut; + + return fmod(gmst, 86400.0)*PI/43200.0; /* 0 <= gmst <= 2*PI */ } + + /* time to string -------------------------------------------------------------- -* convert gtime_t struct to string -* args : gtime_t t I gtime_t struct -* char *s O string ("yyyy/mm/dd hh:mm:ss.ssss") -* int n I number of decimals -* return : none -*-----------------------------------------------------------------------------*/ + * convert gtime_t struct to string + * args : gtime_t t I gtime_t struct + * char *s O string ("yyyy/mm/dd hh:mm:ss.ssss") + * int n I number of decimals + * return : none + *-----------------------------------------------------------------------------*/ void time2str(gtime_t t, char *s, int n) { double ep[6]; - - if (n<0) n=0; else if (n>12) n=12; - if (1.0-t.sec<0.5/pow(10.0,n)) {t.time++; t.sec=0.0;}; - time2epoch(t,ep); - sprintf(s,"%04.0f/%02.0f/%02.0f %02.0f:%02.0f:%0*.*f",ep[0],ep[1],ep[2], - ep[3],ep[4],n<=0?2:n+3,n<=0?0:n,ep[5]); + + if (n<0) n = 0; else if (n>12) n = 12; + if (1.0-t.sec<0.5/pow(10.0, n)) {t.time++; t.sec = 0.0;}; + time2epoch(t, ep); + sprintf(s, "%04.0f/%02.0f/%02.0f %02.0f:%02.0f:%0*.*f", ep[0], ep[1], ep[2], + ep[3], ep[4], n <= 0 ? 2:n+3, n <= 0 ? 0:n, ep[5]); } + + /* get time string ------------------------------------------------------------- -* get time string -* args : gtime_t t I gtime_t struct -* int n I number of decimals -* return : time string -* notes : not reentrant, do not use multiple in a function -*-----------------------------------------------------------------------------*/ + * get time string + * args : gtime_t t I gtime_t struct + * int n I number of decimals + * return : time string + * notes : not reentrant, do not use multiple in a function + *-----------------------------------------------------------------------------*/ char *time_str(gtime_t t, int n) { static char buff[64]; - time2str(t,buff,n); + time2str(t, buff, n); return buff; } + + /* time to day of year --------------------------------------------------------- -* convert time to day of year -* args : gtime_t t I gtime_t struct -* return : day of year (days) -*-----------------------------------------------------------------------------*/ + * convert time to day of year + * args : gtime_t t I gtime_t struct + * return : day of year (days) + *-----------------------------------------------------------------------------*/ double time2doy(gtime_t t) { double ep[6]; - - time2epoch(t,ep); - ep[1]=ep[2]=1.0; ep[3]=ep[4]=ep[5]=0.0; - return timediff(t,epoch2time(ep))/86400.0+1.0; + + time2epoch(t, ep); + ep[1] = ep[2] = 1.0; ep[3] = ep[4] = ep[5] = 0.0; + return timediff(t, epoch2time(ep))/86400.0+1.0; } + + /* adjust gps week number ------------------------------------------------------ -* adjust gps week number using cpu time -* args : int week I not-adjusted gps week number -* return : adjusted gps week number -*-----------------------------------------------------------------------------*/ + * adjust gps week number using cpu time + * args : int week I not-adjusted gps week number + * return : adjusted gps week number + *-----------------------------------------------------------------------------*/ int adjgpsweek(int week) { int w; - (void)time2gpst(utc2gpst(timeget()),&w); - if (w<1560) w=1560; /* use 2009/12/1 if time is earlier than 2009/12/1 */ + (void)time2gpst(utc2gpst(timeget()), &w); + if (w<1560) w = 1560; /* use 2009/12/1 if time is earlier than 2009/12/1 */ return week+(w-week+512)/1024*1024; } + + /* get tick time --------------------------------------------------------------- -* get current tick in ms -* args : none -* return : current tick in ms -*-----------------------------------------------------------------------------*/ + * get current tick in ms + * args : none + * return : current tick in ms + *-----------------------------------------------------------------------------*/ unsigned int tickget(void) { + struct timespec tp = {}; + struct timeval tv = {}; - struct timespec tp={}; - struct timeval tv={}; - #ifdef CLOCK_MONOTONIC_RAW /* linux kernel > 2.6.28 */ - if (!clock_gettime(CLOCK_MONOTONIC_RAW,&tp)) { - return tp.tv_sec*1000u+tp.tv_nsec/1000000u; - } - else { - gettimeofday(&tv,NULL); - return tv.tv_sec*1000u+tv.tv_usec/1000u; - } + if (!clock_gettime(CLOCK_MONOTONIC_RAW, &tp)) + { + return tp.tv_sec*1000u+tp.tv_nsec/1000000u; + } + else + { + gettimeofday(&tv, NULL); + return tv.tv_sec*1000u+tv.tv_usec/1000u; + } #else - gettimeofday(&tv,NULL); + gettimeofday(&tv, NULL); return tv.tv_sec*1000u+tv.tv_usec/1000u; #endif } + + /* sleep ms -------------------------------------------------------------------- -* sleep ms -* args : int ms I miliseconds to sleep (<0:no sleep) -* return : none -*-----------------------------------------------------------------------------*/ + * sleep ms + * args : int ms I miliseconds to sleep (<0:no sleep) + * return : none + *-----------------------------------------------------------------------------*/ void sleepms(int ms) { - struct timespec ts; - if (ms<=0) return; - ts.tv_sec=(time_t)(ms/1000); - ts.tv_nsec=(long)(ms%1000*1000000); - nanosleep(&ts,NULL); + if (ms <= 0) return; + ts.tv_sec = (time_t)(ms/1000); + ts.tv_nsec = (long)(ms%1000*1000000); + nanosleep(&ts, NULL); } + + /* convert degree to deg-min-sec ----------------------------------------------- -* convert degree to degree-minute-second -* args : double deg I degree -* double *dms O degree-minute-second {deg,min,sec} -* int ndec I number of decimals of second -* return : none -*-----------------------------------------------------------------------------*/ + * convert degree to degree-minute-second + * args : double deg I degree + * double *dms O degree-minute-second {deg, min, sec} + * int ndec I number of decimals of second + * return : none + *-----------------------------------------------------------------------------*/ void deg2dms(double deg, double *dms, int ndec) { - double sign=deg<0.0?-1.0:1.0,a=fabs(deg); - double unit=pow(0.1,ndec); - dms[0]=floor(a); a=(a-dms[0])*60.0; - dms[1]=floor(a); a=(a-dms[1])*60.0; - dms[2]=floor(a/unit+0.5)*unit; - if (dms[2]>=60.0) { - dms[2]=0.0; - dms[1]+=1.0; - if (dms[1]>=60.0) { - dms[1]=0.0; - dms[0]+=1.0; + double sign = deg<0.0 ? -1.0 : 1.0, a = fabs(deg); + double unit = pow(0.1, ndec); + dms[0] = floor(a); a = (a-dms[0])*60.0; + dms[1] = floor(a); a = (a-dms[1])*60.0; + dms[2] = floor(a/unit+0.5)*unit; + if (dms[2] >= 60.0) + { + dms[2] = 0.0; + dms[1] += 1.0; + if (dms[1] >= 60.0) + { + dms[1] = 0.0; + dms[0] += 1.0; + } } - } dms[0]*=sign; } + + /* convert deg-min-sec to degree ----------------------------------------------- -* convert degree-minute-second to degree -* args : double *dms I degree-minute-second {deg,min,sec} -* return : degree -*-----------------------------------------------------------------------------*/ + * convert degree-minute-second to degree + * args : double *dms I degree-minute-second {deg,min,sec} + * return : degree + *-----------------------------------------------------------------------------*/ double dms2deg(const double *dms) { - double sign=dms[0]<0.0?-1.0:1.0; + double sign = dms[0]<0.0 ? -1.0 : 1.0; return sign*(fabs(dms[0])+dms[1]/60.0+dms[2]/3600.0); } + + /* transform ecef to geodetic postion ------------------------------------------ -* transform ecef position to geodetic position -* args : double *r I ecef position {x,y,z} (m) -* double *pos O geodetic position {lat,lon,h} (rad,m) -* return : none -* notes : WGS84, ellipsoidal height -*-----------------------------------------------------------------------------*/ + * transform ecef position to geodetic position + * args : double *r I ecef position {x,y,z} (m) + * double *pos O geodetic position {lat,lon,h} (rad,m) + * return : none + * notes : WGS84, ellipsoidal height + *-----------------------------------------------------------------------------*/ void ecef2pos(const double *r, double *pos) { - double e2=FE_WGS84*(2.0-FE_WGS84),r2=dot(r,r,2),z,zk,v=RE_WGS84,sinp; - - for (z=r[2],zk=0.0;fabs(z-zk)>=1E-4;) { - zk=z; - sinp=z/sqrt(r2+z*z); - v=RE_WGS84/sqrt(1.0-e2*sinp*sinp); - z=r[2]+v*e2*sinp; - } - pos[0]=r2>1E-12?atan(z/sqrt(r2)):(r[2]>0.0?PI/2.0:-PI/2.0); - pos[1]=r2>1E-12?atan2(r[1],r[0]):0.0; - pos[2]=sqrt(r2+z*z)-v; + double e2 = FE_WGS84*(2.0-FE_WGS84), r2 = dot(r, r, 2), z, zk, v = RE_WGS84, sinp; + + for (z = r[2], zk = 0.0; fabs(z-zk) >= 1e-4;) + { + zk = z; + sinp = z/sqrt(r2+z*z); + v = RE_WGS84/sqrt(1.0-e2*sinp*sinp); + z = r[2]+v*e2*sinp; + } + pos[0] = r2>1e-12 ? atan(z/sqrt(r2)) : (r[2]>0.0 ? PI/2.0 : -PI/2.0); + pos[1] = r2>1e-12 ? atan2(r[1], r[0]) : 0.0; + pos[2] = sqrt(r2+z*z)-v; } + + /* transform geodetic to ecef position ----------------------------------------- -* transform geodetic position to ecef position -* args : double *pos I geodetic position {lat,lon,h} (rad,m) -* double *r O ecef position {x,y,z} (m) -* return : none -* notes : WGS84, ellipsoidal height -*-----------------------------------------------------------------------------*/ + * transform geodetic position to ecef position + * args : double *pos I geodetic position {lat, lon,h} (rad,m) + * double *r O ecef position {x,y,z} (m) + * return : none + * notes : WGS84, ellipsoidal height + *-----------------------------------------------------------------------------*/ void pos2ecef(const double *pos, double *r) { - double sinp=sin(pos[0]),cosp=cos(pos[0]),sinl=sin(pos[1]),cosl=cos(pos[1]); - double e2=FE_WGS84*(2.0-FE_WGS84),v=RE_WGS84/sqrt(1.0-e2*sinp*sinp); - - r[0]=(v+pos[2])*cosp*cosl; - r[1]=(v+pos[2])*cosp*sinl; - r[2]=(v*(1.0-e2)+pos[2])*sinp; + double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]); + double e2 = FE_WGS84*(2.0-FE_WGS84), v = RE_WGS84/sqrt(1.0-e2*sinp*sinp); + + r[0] = (v+pos[2])*cosp*cosl; + r[1] = (v+pos[2])*cosp*sinl; + r[2] = (v*(1.0-e2)+pos[2])*sinp; } + + /* ecef to local coordinate transfromation matrix ------------------------------ -* compute ecef to local coordinate transfromation matrix -* args : double *pos I geodetic position {lat,lon} (rad) -* double *E O ecef to local coord transformation matrix (3x3) -* return : none -* notes : matirix stored by column-major order (fortran convention) -*-----------------------------------------------------------------------------*/ + * compute ecef to local coordinate transfromation matrix + * args : double *pos I geodetic position {lat,lon} (rad) + * double *E O ecef to local coord transformation matrix (3x3) + * return : none + * notes : matirix stored by column-major order (fortran convention) + *-----------------------------------------------------------------------------*/ void xyz2enu(const double *pos, double *E) { - double sinp=sin(pos[0]),cosp=cos(pos[0]),sinl=sin(pos[1]),cosl=cos(pos[1]); - - E[0]=-sinl; E[3]=cosl; E[6]=0.0; - E[1]=-sinp*cosl; E[4]=-sinp*sinl; E[7]=cosp; - E[2]=cosp*cosl; E[5]=cosp*sinl; E[8]=sinp; + double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]); + + E[0] = -sinl; E[3] = cosl; E[6] = 0.0; + E[1] = -sinp*cosl; E[4] = -sinp*sinl; E[7] = cosp; + E[2] = cosp*cosl; E[5] = cosp*sinl; E[8] = sinp; } + + /* transform ecef vector to local tangental coordinate ------------------------- -* transform ecef vector to local tangental coordinate -* args : double *pos I geodetic position {lat,lon} (rad) -* double *r I vector in ecef coordinate {x,y,z} -* double *e O vector in local tangental coordinate {e,n,u} -* return : none -*-----------------------------------------------------------------------------*/ + * transform ecef vector to local tangental coordinate + * args : double *pos I geodetic position {lat,lon} (rad) + * double *r I vector in ecef coordinate {x,y,z} + * double *e O vector in local tangental coordinate {e,n,u} + * return : none + *-----------------------------------------------------------------------------*/ void ecef2enu(const double *pos, const double *r, double *e) { double E[9]; - + xyz2enu(pos,E); - matmul("NN",3,1,3,1.0,E,r,0.0,e); + matmul("NN", 3, 1, 3, 1.0, E, r, 0.0, e); } + + /* transform local vector to ecef coordinate ----------------------------------- -* transform local tangental coordinate vector to ecef -* args : double *pos I geodetic position {lat,lon} (rad) -* double *e I vector in local tangental coordinate {e,n,u} -* double *r O vector in ecef coordinate {x,y,z} -* return : none -*-----------------------------------------------------------------------------*/ + * transform local tangental coordinate vector to ecef + * args : double *pos I geodetic position {lat,lon} (rad) + * double *e I vector in local tangental coordinate {e,n,u} + * double *r O vector in ecef coordinate {x,y,z} + * return : none + *-----------------------------------------------------------------------------*/ void enu2ecef(const double *pos, const double *e, double *r) { double E[9]; - - xyz2enu(pos,E); - matmul("TN",3,1,3,1.0,E,e,0.0,r); + + xyz2enu(pos, E); + matmul("TN", 3, 1, 3, 1.0, E, e, 0.0, r); } + + /* transform covariance to local tangental coordinate -------------------------- -* transform ecef covariance to local tangental coordinate -* args : double *pos I geodetic position {lat,lon} (rad) -* double *P I covariance in ecef coordinate -* double *Q O covariance in local tangental coordinate -* return : none -*-----------------------------------------------------------------------------*/ + * transform ecef covariance to local tangental coordinate + * args : double *pos I geodetic position {lat, lon} (rad) + * double *P I covariance in ecef coordinate + * double *Q O covariance in local tangental coordinate + * return : none + *-----------------------------------------------------------------------------*/ void covenu(const double *pos, const double *P, double *Q) { - double E[9],EP[9]; - - xyz2enu(pos,E); - matmul("NN",3,3,3,1.0,E,P,0.0,EP); - matmul("NT",3,3,3,1.0,EP,E,0.0,Q); + double E[9], EP[9]; + + xyz2enu(pos, E); + matmul("NN", 3, 3, 3, 1.0, E, P, 0.0, EP); + matmul("NT", 3, 3, 3, 1.0, EP, E, 0.0, Q); } + + /* transform local enu coordinate covariance to xyz-ecef ----------------------- -* transform local enu covariance to xyz-ecef coordinate -* args : double *pos I geodetic position {lat,lon} (rad) -* double *Q I covariance in local enu coordinate -* double *P O covariance in xyz-ecef coordinate -* return : none -*-----------------------------------------------------------------------------*/ + * transform local enu covariance to xyz-ecef coordinate + * args : double *pos I geodetic position {lat,lon} (rad) + * double *Q I covariance in local enu coordinate + * double *P O covariance in xyz-ecef coordinate + * return : none + *-----------------------------------------------------------------------------*/ void covecef(const double *pos, const double *Q, double *P) { - double E[9],EQ[9]; - - xyz2enu(pos,E); - matmul("TN",3,3,3,1.0,E,Q,0.0,EQ); - matmul("NN",3,3,3,1.0,EQ,E,0.0,P); + double E[9], EQ[9]; + + xyz2enu(pos, E); + matmul("TN", 3, 3, 3, 1.0, E, Q, 0.0, EQ); + matmul("NN", 3, 3, 3, 1.0, EQ, E, 0.0, P); } /* astronomical arguments: f={l,l',F,D,OMG} (rad) ----------------------------*/ void ast_args(double t, double *f) { - static const double fc[][5]={ /* coefficients for iau 1980 nutation */ - { 134.96340251, 1717915923.2178, 31.8792, 0.051635, -0.00024470}, - { 357.52910918, 129596581.0481, -0.5532, 0.000136, -0.00001149}, - { 93.27209062, 1739527262.8478, -12.7512, -0.001037, 0.00000417}, - { 297.85019547, 1602961601.2090, -6.3706, 0.006593, -0.00003169}, - { 125.04455501, -6962890.2665, 7.4722, 0.007702, -0.00005939} + static const double fc[][5] = { /* coefficients for iau 1980 nutation */ + { 134.96340251, 1717915923.2178, 31.8792, 0.051635, -0.00024470}, + { 357.52910918, 129596581.0481, -0.5532, 0.000136, -0.00001149}, + { 93.27209062, 1739527262.8478, -12.7512, -0.001037, 0.00000417}, + { 297.85019547, 1602961601.2090, -6.3706, 0.006593, -0.00003169}, + { 125.04455501, -6962890.2665, 7.4722, 0.007702, -0.00005939} }; double tt[4]; - int i,j; - - for (tt[0]=t,i=1;i<4;i++) tt[i]=tt[i-1]*t; - for (i=0;i<5;i++) { - f[i]=fc[i][0]*3600.0; - for (j=0;j<4;j++) f[i]+=fc[i][j+1]*tt[j]; - f[i]=fmod(f[i]*AS2R,2.0*PI); - } + int i, j; + + for (tt[0] = t, i = 1; i<4; i++) tt[i] = tt[i-1]*t; + for (i = 0; i<5; i++) + { + f[i] = fc[i][0]*3600.0; + for (j = 0; j<4; j++) f[i] += fc[i][j+1]*tt[j]; + f[i] = fmod(f[i]*AS2R, 2.0*PI); + } } + + /* iau 1980 nutation ---------------------------------------------------------*/ void nut_iau1980(double t, const double *f, double *dpsi, double *deps) { - static const double nut[106][10]={ - { 0, 0, 0, 0, 1, -6798.4, -171996, -174.2, 92025, 8.9}, - { 0, 0, 2, -2, 2, 182.6, -13187, -1.6, 5736, -3.1}, - { 0, 0, 2, 0, 2, 13.7, -2274, -0.2, 977, -0.5}, - { 0, 0, 0, 0, 2, -3399.2, 2062, 0.2, -895, 0.5}, - { 0, -1, 0, 0, 0, -365.3, -1426, 3.4, 54, -0.1}, - { 1, 0, 0, 0, 0, 27.6, 712, 0.1, -7, 0.0}, - { 0, 1, 2, -2, 2, 121.7, -517, 1.2, 224, -0.6}, - { 0, 0, 2, 0, 1, 13.6, -386, -0.4, 200, 0.0}, - { 1, 0, 2, 0, 2, 9.1, -301, 0.0, 129, -0.1}, - { 0, -1, 2, -2, 2, 365.2, 217, -0.5, -95, 0.3}, - { -1, 0, 0, 2, 0, 31.8, 158, 0.0, -1, 0.0}, - { 0, 0, 2, -2, 1, 177.8, 129, 0.1, -70, 0.0}, - { -1, 0, 2, 0, 2, 27.1, 123, 0.0, -53, 0.0}, - { 1, 0, 0, 0, 1, 27.7, 63, 0.1, -33, 0.0}, - { 0, 0, 0, 2, 0, 14.8, 63, 0.0, -2, 0.0}, - { -1, 0, 2, 2, 2, 9.6, -59, 0.0, 26, 0.0}, - { -1, 0, 0, 0, 1, -27.4, -58, -0.1, 32, 0.0}, - { 1, 0, 2, 0, 1, 9.1, -51, 0.0, 27, 0.0}, - { -2, 0, 0, 2, 0, -205.9, -48, 0.0, 1, 0.0}, - { -2, 0, 2, 0, 1, 1305.5, 46, 0.0, -24, 0.0}, - { 0, 0, 2, 2, 2, 7.1, -38, 0.0, 16, 0.0}, - { 2, 0, 2, 0, 2, 6.9, -31, 0.0, 13, 0.0}, - { 2, 0, 0, 0, 0, 13.8, 29, 0.0, -1, 0.0}, - { 1, 0, 2, -2, 2, 23.9, 29, 0.0, -12, 0.0}, - { 0, 0, 2, 0, 0, 13.6, 26, 0.0, -1, 0.0}, - { 0, 0, 2, -2, 0, 173.3, -22, 0.0, 0, 0.0}, - { -1, 0, 2, 0, 1, 27.0, 21, 0.0, -10, 0.0}, - { 0, 2, 0, 0, 0, 182.6, 17, -0.1, 0, 0.0}, - { 0, 2, 2, -2, 2, 91.3, -16, 0.1, 7, 0.0}, - { -1, 0, 0, 2, 1, 32.0, 16, 0.0, -8, 0.0}, - { 0, 1, 0, 0, 1, 386.0, -15, 0.0, 9, 0.0}, - { 1, 0, 0, -2, 1, -31.7, -13, 0.0, 7, 0.0}, - { 0, -1, 0, 0, 1, -346.6, -12, 0.0, 6, 0.0}, - { 2, 0, -2, 0, 0, -1095.2, 11, 0.0, 0, 0.0}, - { -1, 0, 2, 2, 1, 9.5, -10, 0.0, 5, 0.0}, - { 1, 0, 2, 2, 2, 5.6, -8, 0.0, 3, 0.0}, - { 0, -1, 2, 0, 2, 14.2, -7, 0.0, 3, 0.0}, - { 0, 0, 2, 2, 1, 7.1, -7, 0.0, 3, 0.0}, - { 1, 1, 0, -2, 0, -34.8, -7, 0.0, 0, 0.0}, - { 0, 1, 2, 0, 2, 13.2, 7, 0.0, -3, 0.0}, - { -2, 0, 0, 2, 1, -199.8, -6, 0.0, 3, 0.0}, - { 0, 0, 0, 2, 1, 14.8, -6, 0.0, 3, 0.0}, - { 2, 0, 2, -2, 2, 12.8, 6, 0.0, -3, 0.0}, - { 1, 0, 0, 2, 0, 9.6, 6, 0.0, 0, 0.0}, - { 1, 0, 2, -2, 1, 23.9, 6, 0.0, -3, 0.0}, - { 0, 0, 0, -2, 1, -14.7, -5, 0.0, 3, 0.0}, - { 0, -1, 2, -2, 1, 346.6, -5, 0.0, 3, 0.0}, - { 2, 0, 2, 0, 1, 6.9, -5, 0.0, 3, 0.0}, - { 1, -1, 0, 0, 0, 29.8, 5, 0.0, 0, 0.0}, - { 1, 0, 0, -1, 0, 411.8, -4, 0.0, 0, 0.0}, - { 0, 0, 0, 1, 0, 29.5, -4, 0.0, 0, 0.0}, - { 0, 1, 0, -2, 0, -15.4, -4, 0.0, 0, 0.0}, - { 1, 0, -2, 0, 0, -26.9, 4, 0.0, 0, 0.0}, - { 2, 0, 0, -2, 1, 212.3, 4, 0.0, -2, 0.0}, - { 0, 1, 2, -2, 1, 119.6, 4, 0.0, -2, 0.0}, - { 1, 1, 0, 0, 0, 25.6, -3, 0.0, 0, 0.0}, - { 1, -1, 0, -1, 0, -3232.9, -3, 0.0, 0, 0.0}, - { -1, -1, 2, 2, 2, 9.8, -3, 0.0, 1, 0.0}, - { 0, -1, 2, 2, 2, 7.2, -3, 0.0, 1, 0.0}, - { 1, -1, 2, 0, 2, 9.4, -3, 0.0, 1, 0.0}, - { 3, 0, 2, 0, 2, 5.5, -3, 0.0, 1, 0.0}, - { -2, 0, 2, 0, 2, 1615.7, -3, 0.0, 1, 0.0}, - { 1, 0, 2, 0, 0, 9.1, 3, 0.0, 0, 0.0}, - { -1, 0, 2, 4, 2, 5.8, -2, 0.0, 1, 0.0}, - { 1, 0, 0, 0, 2, 27.8, -2, 0.0, 1, 0.0}, - { -1, 0, 2, -2, 1, -32.6, -2, 0.0, 1, 0.0}, - { 0, -2, 2, -2, 1, 6786.3, -2, 0.0, 1, 0.0}, - { -2, 0, 0, 0, 1, -13.7, -2, 0.0, 1, 0.0}, - { 2, 0, 0, 0, 1, 13.8, 2, 0.0, -1, 0.0}, - { 3, 0, 0, 0, 0, 9.2, 2, 0.0, 0, 0.0}, - { 1, 1, 2, 0, 2, 8.9, 2, 0.0, -1, 0.0}, - { 0, 0, 2, 1, 2, 9.3, 2, 0.0, -1, 0.0}, - { 1, 0, 0, 2, 1, 9.6, -1, 0.0, 0, 0.0}, - { 1, 0, 2, 2, 1, 5.6, -1, 0.0, 1, 0.0}, - { 1, 1, 0, -2, 1, -34.7, -1, 0.0, 0, 0.0}, - { 0, 1, 0, 2, 0, 14.2, -1, 0.0, 0, 0.0}, - { 0, 1, 2, -2, 0, 117.5, -1, 0.0, 0, 0.0}, - { 0, 1, -2, 2, 0, -329.8, -1, 0.0, 0, 0.0}, - { 1, 0, -2, 2, 0, 23.8, -1, 0.0, 0, 0.0}, - { 1, 0, -2, -2, 0, -9.5, -1, 0.0, 0, 0.0}, - { 1, 0, 2, -2, 0, 32.8, -1, 0.0, 0, 0.0}, - { 1, 0, 0, -4, 0, -10.1, -1, 0.0, 0, 0.0}, - { 2, 0, 0, -4, 0, -15.9, -1, 0.0, 0, 0.0}, - { 0, 0, 2, 4, 2, 4.8, -1, 0.0, 0, 0.0}, - { 0, 0, 2, -1, 2, 25.4, -1, 0.0, 0, 0.0}, - { -2, 0, 2, 4, 2, 7.3, -1, 0.0, 1, 0.0}, - { 2, 0, 2, 2, 2, 4.7, -1, 0.0, 0, 0.0}, - { 0, -1, 2, 0, 1, 14.2, -1, 0.0, 0, 0.0}, - { 0, 0, -2, 0, 1, -13.6, -1, 0.0, 0, 0.0}, - { 0, 0, 4, -2, 2, 12.7, 1, 0.0, 0, 0.0}, - { 0, 1, 0, 0, 2, 409.2, 1, 0.0, 0, 0.0}, - { 1, 1, 2, -2, 2, 22.5, 1, 0.0, -1, 0.0}, - { 3, 0, 2, -2, 2, 8.7, 1, 0.0, 0, 0.0}, - { -2, 0, 2, 2, 2, 14.6, 1, 0.0, -1, 0.0}, - { -1, 0, 0, 0, 2, -27.3, 1, 0.0, -1, 0.0}, - { 0, 0, -2, 2, 1, -169.0, 1, 0.0, 0, 0.0}, - { 0, 1, 2, 0, 1, 13.1, 1, 0.0, 0, 0.0}, - { -1, 0, 4, 0, 2, 9.1, 1, 0.0, 0, 0.0}, - { 2, 1, 0, -2, 0, 131.7, 1, 0.0, 0, 0.0}, - { 2, 0, 0, 2, 0, 7.1, 1, 0.0, 0, 0.0}, - { 2, 0, 2, -2, 1, 12.8, 1, 0.0, -1, 0.0}, - { 2, 0, -2, 0, 1, -943.2, 1, 0.0, 0, 0.0}, - { 1, -1, 0, -2, 0, -29.3, 1, 0.0, 0, 0.0}, - { -1, 0, 0, 1, 1, -388.3, 1, 0.0, 0, 0.0}, - { -1, -1, 0, 2, 1, 35.0, 1, 0.0, 0, 0.0}, - { 0, 1, 0, 1, 0, 27.3, 1, 0.0, 0, 0.0} + static const double nut[106][10] = { + { 0, 0, 0, 0, 1, -6798.4, -171996, -174.2, 92025, 8.9}, + { 0, 0, 2, -2, 2, 182.6, -13187, -1.6, 5736, -3.1}, + { 0, 0, 2, 0, 2, 13.7, -2274, -0.2, 977, -0.5}, + { 0, 0, 0, 0, 2, -3399.2, 2062, 0.2, -895, 0.5}, + { 0, -1, 0, 0, 0, -365.3, -1426, 3.4, 54, -0.1}, + { 1, 0, 0, 0, 0, 27.6, 712, 0.1, -7, 0.0}, + { 0, 1, 2, -2, 2, 121.7, -517, 1.2, 224, -0.6}, + { 0, 0, 2, 0, 1, 13.6, -386, -0.4, 200, 0.0}, + { 1, 0, 2, 0, 2, 9.1, -301, 0.0, 129, -0.1}, + { 0, -1, 2, -2, 2, 365.2, 217, -0.5, -95, 0.3}, + { -1, 0, 0, 2, 0, 31.8, 158, 0.0, -1, 0.0}, + { 0, 0, 2, -2, 1, 177.8, 129, 0.1, -70, 0.0}, + { -1, 0, 2, 0, 2, 27.1, 123, 0.0, -53, 0.0}, + { 1, 0, 0, 0, 1, 27.7, 63, 0.1, -33, 0.0}, + { 0, 0, 0, 2, 0, 14.8, 63, 0.0, -2, 0.0}, + { -1, 0, 2, 2, 2, 9.6, -59, 0.0, 26, 0.0}, + { -1, 0, 0, 0, 1, -27.4, -58, -0.1, 32, 0.0}, + { 1, 0, 2, 0, 1, 9.1, -51, 0.0, 27, 0.0}, + { -2, 0, 0, 2, 0, -205.9, -48, 0.0, 1, 0.0}, + { -2, 0, 2, 0, 1, 1305.5, 46, 0.0, -24, 0.0}, + { 0, 0, 2, 2, 2, 7.1, -38, 0.0, 16, 0.0}, + { 2, 0, 2, 0, 2, 6.9, -31, 0.0, 13, 0.0}, + { 2, 0, 0, 0, 0, 13.8, 29, 0.0, -1, 0.0}, + { 1, 0, 2, -2, 2, 23.9, 29, 0.0, -12, 0.0}, + { 0, 0, 2, 0, 0, 13.6, 26, 0.0, -1, 0.0}, + { 0, 0, 2, -2, 0, 173.3, -22, 0.0, 0, 0.0}, + { -1, 0, 2, 0, 1, 27.0, 21, 0.0, -10, 0.0}, + { 0, 2, 0, 0, 0, 182.6, 17, -0.1, 0, 0.0}, + { 0, 2, 2, -2, 2, 91.3, -16, 0.1, 7, 0.0}, + { -1, 0, 0, 2, 1, 32.0, 16, 0.0, -8, 0.0}, + { 0, 1, 0, 0, 1, 386.0, -15, 0.0, 9, 0.0}, + { 1, 0, 0, -2, 1, -31.7, -13, 0.0, 7, 0.0}, + { 0, -1, 0, 0, 1, -346.6, -12, 0.0, 6, 0.0}, + { 2, 0, -2, 0, 0, -1095.2, 11, 0.0, 0, 0.0}, + { -1, 0, 2, 2, 1, 9.5, -10, 0.0, 5, 0.0}, + { 1, 0, 2, 2, 2, 5.6, -8, 0.0, 3, 0.0}, + { 0, -1, 2, 0, 2, 14.2, -7, 0.0, 3, 0.0}, + { 0, 0, 2, 2, 1, 7.1, -7, 0.0, 3, 0.0}, + { 1, 1, 0, -2, 0, -34.8, -7, 0.0, 0, 0.0}, + { 0, 1, 2, 0, 2, 13.2, 7, 0.0, -3, 0.0}, + { -2, 0, 0, 2, 1, -199.8, -6, 0.0, 3, 0.0}, + { 0, 0, 0, 2, 1, 14.8, -6, 0.0, 3, 0.0}, + { 2, 0, 2, -2, 2, 12.8, 6, 0.0, -3, 0.0}, + { 1, 0, 0, 2, 0, 9.6, 6, 0.0, 0, 0.0}, + { 1, 0, 2, -2, 1, 23.9, 6, 0.0, -3, 0.0}, + { 0, 0, 0, -2, 1, -14.7, -5, 0.0, 3, 0.0}, + { 0, -1, 2, -2, 1, 346.6, -5, 0.0, 3, 0.0}, + { 2, 0, 2, 0, 1, 6.9, -5, 0.0, 3, 0.0}, + { 1, -1, 0, 0, 0, 29.8, 5, 0.0, 0, 0.0}, + { 1, 0, 0, -1, 0, 411.8, -4, 0.0, 0, 0.0}, + { 0, 0, 0, 1, 0, 29.5, -4, 0.0, 0, 0.0}, + { 0, 1, 0, -2, 0, -15.4, -4, 0.0, 0, 0.0}, + { 1, 0, -2, 0, 0, -26.9, 4, 0.0, 0, 0.0}, + { 2, 0, 0, -2, 1, 212.3, 4, 0.0, -2, 0.0}, + { 0, 1, 2, -2, 1, 119.6, 4, 0.0, -2, 0.0}, + { 1, 1, 0, 0, 0, 25.6, -3, 0.0, 0, 0.0}, + { 1, -1, 0, -1, 0, -3232.9, -3, 0.0, 0, 0.0}, + { -1, -1, 2, 2, 2, 9.8, -3, 0.0, 1, 0.0}, + { 0, -1, 2, 2, 2, 7.2, -3, 0.0, 1, 0.0}, + { 1, -1, 2, 0, 2, 9.4, -3, 0.0, 1, 0.0}, + { 3, 0, 2, 0, 2, 5.5, -3, 0.0, 1, 0.0}, + { -2, 0, 2, 0, 2, 1615.7, -3, 0.0, 1, 0.0}, + { 1, 0, 2, 0, 0, 9.1, 3, 0.0, 0, 0.0}, + { -1, 0, 2, 4, 2, 5.8, -2, 0.0, 1, 0.0}, + { 1, 0, 0, 0, 2, 27.8, -2, 0.0, 1, 0.0}, + { -1, 0, 2, -2, 1, -32.6, -2, 0.0, 1, 0.0}, + { 0, -2, 2, -2, 1, 6786.3, -2, 0.0, 1, 0.0}, + { -2, 0, 0, 0, 1, -13.7, -2, 0.0, 1, 0.0}, + { 2, 0, 0, 0, 1, 13.8, 2, 0.0, -1, 0.0}, + { 3, 0, 0, 0, 0, 9.2, 2, 0.0, 0, 0.0}, + { 1, 1, 2, 0, 2, 8.9, 2, 0.0, -1, 0.0}, + { 0, 0, 2, 1, 2, 9.3, 2, 0.0, -1, 0.0}, + { 1, 0, 0, 2, 1, 9.6, -1, 0.0, 0, 0.0}, + { 1, 0, 2, 2, 1, 5.6, -1, 0.0, 1, 0.0}, + { 1, 1, 0, -2, 1, -34.7, -1, 0.0, 0, 0.0}, + { 0, 1, 0, 2, 0, 14.2, -1, 0.0, 0, 0.0}, + { 0, 1, 2, -2, 0, 117.5, -1, 0.0, 0, 0.0}, + { 0, 1, -2, 2, 0, -329.8, -1, 0.0, 0, 0.0}, + { 1, 0, -2, 2, 0, 23.8, -1, 0.0, 0, 0.0}, + { 1, 0, -2, -2, 0, -9.5, -1, 0.0, 0, 0.0}, + { 1, 0, 2, -2, 0, 32.8, -1, 0.0, 0, 0.0}, + { 1, 0, 0, -4, 0, -10.1, -1, 0.0, 0, 0.0}, + { 2, 0, 0, -4, 0, -15.9, -1, 0.0, 0, 0.0}, + { 0, 0, 2, 4, 2, 4.8, -1, 0.0, 0, 0.0}, + { 0, 0, 2, -1, 2, 25.4, -1, 0.0, 0, 0.0}, + { -2, 0, 2, 4, 2, 7.3, -1, 0.0, 1, 0.0}, + { 2, 0, 2, 2, 2, 4.7, -1, 0.0, 0, 0.0}, + { 0, -1, 2, 0, 1, 14.2, -1, 0.0, 0, 0.0}, + { 0, 0, -2, 0, 1, -13.6, -1, 0.0, 0, 0.0}, + { 0, 0, 4, -2, 2, 12.7, 1, 0.0, 0, 0.0}, + { 0, 1, 0, 0, 2, 409.2, 1, 0.0, 0, 0.0}, + { 1, 1, 2, -2, 2, 22.5, 1, 0.0, -1, 0.0}, + { 3, 0, 2, -2, 2, 8.7, 1, 0.0, 0, 0.0}, + { -2, 0, 2, 2, 2, 14.6, 1, 0.0, -1, 0.0}, + { -1, 0, 0, 0, 2, -27.3, 1, 0.0, -1, 0.0}, + { 0, 0, -2, 2, 1, -169.0, 1, 0.0, 0, 0.0}, + { 0, 1, 2, 0, 1, 13.1, 1, 0.0, 0, 0.0}, + { -1, 0, 4, 0, 2, 9.1, 1, 0.0, 0, 0.0}, + { 2, 1, 0, -2, 0, 131.7, 1, 0.0, 0, 0.0}, + { 2, 0, 0, 2, 0, 7.1, 1, 0.0, 0, 0.0}, + { 2, 0, 2, -2, 1, 12.8, 1, 0.0, -1, 0.0}, + { 2, 0, -2, 0, 1, -943.2, 1, 0.0, 0, 0.0}, + { 1, -1, 0, -2, 0, -29.3, 1, 0.0, 0, 0.0}, + { -1, 0, 0, 1, 1, -388.3, 1, 0.0, 0, 0.0}, + { -1, -1, 0, 2, 1, 35.0, 1, 0.0, 0, 0.0}, + { 0, 1, 0, 1, 0, 27.3, 1, 0.0, 0, 0.0} }; double ang; - int i,j; - - *dpsi=*deps=0.0; - - for (i=0;i<106;i++) { - ang=0.0; - for (j=0;j<5;j++) ang+=nut[i][j]*f[j]; - *dpsi+=(nut[i][6]+nut[i][7]*t)*sin(ang); - *deps+=(nut[i][8]+nut[i][9]*t)*cos(ang); - } - *dpsi*=1E-4*AS2R; /* 0.1 mas -> rad */ - *deps*=1E-4*AS2R; + int i, j; + + *dpsi = *deps = 0.0; + + for (i = 0; i<106; i++) + { + ang = 0.0; + for (j = 0; j<5; j++) ang += nut[i][j]*f[j]; + *dpsi += (nut[i][6]+nut[i][7]*t)*sin(ang); + *deps += (nut[i][8]+nut[i][9]*t)*cos(ang); + } + *dpsi*=1e-4*AS2R; /* 0.1 mas -> rad */ + *deps*=1e-4*AS2R; } + + /* eci to ecef transformation matrix ------------------------------------------- -* compute eci to ecef transformation matrix -* args : gtime_t tutc I time in utc -* double *erpv I erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) -* double *U O eci to ecef transformation matrix (3 x 3) -* double *gmst IO greenwich mean sidereal time (rad) -* (NULL: no output) -* return : none -* note : see ref [3] chap 5 -* not thread-safe -*-----------------------------------------------------------------------------*/ + * compute eci to ecef transformation matrix + * args : gtime_t tutc I time in utc + * double *erpv I erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) + * double *U O eci to ecef transformation matrix (3 x 3) + * double *gmst IO greenwich mean sidereal time (rad) + * (NULL: no output) + * return : none + * note : see ref [3] chap 5 + * not thread-safe + *-----------------------------------------------------------------------------*/ void eci2ecef(gtime_t tutc, const double *erpv, double *U, double *gmst) { - const double ep2000[]={2000,1,1,12,0,0}; + const double ep2000[] = {2000, 1, 1, 12, 0, 0}; static gtime_t tutc_; - static double U_[9],gmst_; + static double U_[9], gmst_; gtime_t tgps; - double eps,ze,th,z,t,t2,t3,dpsi,deps,gast,f[5]; - double R1[9],R2[9],R3[9],R[9],W[9],N[9],P[9],NP[9]; + double eps, ze, th, z, t, t2, t3, dpsi, deps, gast, f[5]; + double R1[9], R2[9], R3[9], R[9], W[9], N[9], P[9], NP[9]; int i; - - trace(4,"eci2ecef: tutc=%s\n",time_str(tutc,3)); - - if (fabs(timediff(tutc,tutc_))<0.01) { /* read cache */ - for (i=0;i<9;i++) U[i]=U_[i]; - if (gmst) *gmst=gmst_; - return; - } - tutc_=tutc; - + + trace(4, "eci2ecef: tutc=%s\n", time_str(tutc, 3)); + + if (fabs(timediff(tutc, tutc_))<0.01) + { /* read cache */ + for (i = 0; i<9; i++) U[i] = U_[i]; + if (gmst) *gmst = gmst_; + return; + } + tutc_ = tutc; + /* terrestrial time */ - tgps=utc2gpst(tutc_); - t=(timediff(tgps,epoch2time(ep2000))+19.0+32.184)/86400.0/36525.0; - t2=t*t; t3=t2*t; - + tgps = utc2gpst(tutc_); + t = (timediff(tgps, epoch2time(ep2000))+19.0+32.184)/86400.0/36525.0; + t2 = t*t; t3 = t2*t; + /* astronomical arguments */ - ast_args(t,f); - + ast_args(t, f); + /* iau 1976 precession */ - ze=(2306.2181*t+0.30188*t2+0.017998*t3)*AS2R; - th=(2004.3109*t-0.42665*t2-0.041833*t3)*AS2R; - z =(2306.2181*t+1.09468*t2+0.018203*t3)*AS2R; - eps=(84381.448-46.8150*t-0.00059*t2+0.001813*t3)*AS2R; - Rz(-z,R1); Ry(th,R2); Rz(-ze,R3); - matmul("NN",3,3,3,1.0,R1,R2,0.0,R); - matmul("NN",3,3,3,1.0,R, R3,0.0,P); /* P=Rz(-z)*Ry(th)*Rz(-ze) */ - + ze = (2306.2181*t+0.30188*t2+0.017998*t3)*AS2R; + th = (2004.3109*t-0.42665*t2-0.041833*t3)*AS2R; + z = (2306.2181*t+1.09468*t2+0.018203*t3)*AS2R; + eps = (84381.448-46.8150*t-0.00059*t2+0.001813*t3)*AS2R; + Rz(-z, R1); Ry(th, R2); Rz(-ze, R3); + matmul("NN", 3, 3, 3, 1.0, R1, R2, 0.0, R); + matmul("NN", 3, 3, 3, 1.0, R, R3, 0.0, P); /* P=Rz(-z)*Ry(th)*Rz(-ze) */ + /* iau 1980 nutation */ - nut_iau1980(t,f,&dpsi,&deps); - Rx(-eps-deps,R1); Rz(-dpsi,R2); Rx(eps,R3); - matmul("NN",3,3,3,1.0,R1,R2,0.0,R); - matmul("NN",3,3,3,1.0,R ,R3,0.0,N); /* N=Rx(-eps)*Rz(-dspi)*Rx(eps) */ - + nut_iau1980(t, f, &dpsi, &deps); + Rx(-eps-deps, R1); Rz(-dpsi, R2); Rx(eps, R3); + matmul("NN", 3, 3, 3, 1.0, R1, R2, 0.0, R); + matmul("NN", 3, 3, 3, 1.0, R , R3, 0.0, N); /* N=Rx(-eps)*Rz(-dspi)*Rx(eps) */ + /* greenwich aparent sidereal time (rad) */ - gmst_=utc2gmst(tutc_,erpv[2]); - gast=gmst_+dpsi*cos(eps); - gast+=(0.00264*sin(f[4])+0.000063*sin(2.0*f[4]))*AS2R; - + gmst_ = utc2gmst(tutc_, erpv[2]); + gast = gmst_+dpsi*cos(eps); + gast += (0.00264*sin(f[4])+0.000063*sin(2.0*f[4]))*AS2R; + /* eci to ecef transformation matrix */ - Ry(-erpv[0],R1); Rx(-erpv[1],R2); Rz(gast,R3); - matmul("NN",3,3,3,1.0,R1,R2,0.0,W ); - matmul("NN",3,3,3,1.0,W ,R3,0.0,R ); /* W=Ry(-xp)*Rx(-yp) */ - matmul("NN",3,3,3,1.0,N ,P ,0.0,NP); - matmul("NN",3,3,3,1.0,R ,NP,0.0,U_); /* U=W*Rz(gast)*N*P */ - - for (i=0;i<9;i++) U[i]=U_[i]; - if (gmst) *gmst=gmst_; - - trace(5,"gmst=%.12f gast=%.12f\n",gmst_,gast); - trace(5,"P=\n"); tracemat(5,P,3,3,15,12); - trace(5,"N=\n"); tracemat(5,N,3,3,15,12); - trace(5,"W=\n"); tracemat(5,W,3,3,15,12); - trace(5,"U=\n"); tracemat(5,U,3,3,15,12); + Ry(-erpv[0], R1); Rx(-erpv[1], R2); Rz(gast, R3); + matmul("NN", 3, 3, 3, 1.0, R1, R2, 0.0, W ); + matmul("NN", 3, 3, 3, 1.0, W , R3, 0.0, R ); /* W=Ry(-xp)*Rx(-yp) */ + matmul("NN", 3, 3, 3, 1.0, N , P , 0.0, NP); + matmul("NN", 3, 3, 3, 1.0, R , NP, 0.0, U_); /* U=W*Rz(gast)*N*P */ + + for (i = 0; i<9; i++) U[i] = U_[i]; + if (gmst) *gmst = gmst_; + + trace(5, "gmst=%.12f gast=%.12f\n", gmst_, gast); + trace(5, "P=\n"); //tracemat(5, P, 3, 3, 15, 12); + trace(5, "N=\n"); //tracemat(5, N, 3, 3, 15, 12); + trace(5, "W=\n"); //tracemat(5, W, 3, 3, 15, 12); + trace(5, "U=\n"); //tracemat(5, U, 3, 3, 15, 12); } + + /* decode antenna parameter field --------------------------------------------*/ int decodef(char *p, int n, double *v) { int i; - - for (i=0;inmax<=pcvs->n) { - pcvs->nmax+=256; - if (!(pcvs_pcv=(pcv_t *)realloc(pcvs->pcv,sizeof(pcv_t)*pcvs->nmax))) { - trace(1,"addpcv: memory allocation error\n"); - free(pcvs->pcv); pcvs->pcv=NULL; pcvs->n=pcvs->nmax=0; - return; + + if (pcvs->nmax <= pcvs->n) + { + pcvs->nmax += 256; + if (!(pcvs_pcv = (pcv_t *)realloc(pcvs->pcv, sizeof(pcv_t)*pcvs->nmax))) + { + trace(1, "addpcv: memory allocation error\n"); + free(pcvs->pcv); pcvs->pcv = NULL; pcvs->n = pcvs->nmax = 0; + return; + } + pcvs->pcv = pcvs_pcv; } - pcvs->pcv=pcvs_pcv; - } - pcvs->pcv[pcvs->n++]=*pcv; + pcvs->pcv[pcvs->n++] = *pcv; } + + /* read ngs antenna parameter file -------------------------------------------*/ int readngspcv(const char *file, pcvs_t *pcvs) { FILE *fp; - static const pcv_t pcv0={}; + static const pcv_t pcv0 = {}; pcv_t pcv; double neu[3]; - int n=0; + int n = 0; char buff[256]; - - if (!(fp=fopen(file,"r"))) { - trace(2,"ngs pcv file open error: %s\n",file); - return 0; - } - while (fgets(buff,sizeof(buff),fp)) { - - if (strlen(buff)>=62&&buff[61]=='|') continue; - - if (buff[0]!=' ') n=0; /* start line */ - if (++n==1) { - pcv=pcv0; - strncpy(pcv.type,buff,61); pcv.type[61]='\0'; + + if (!(fp = fopen(file, "r"))) + { + trace(2, "ngs pcv file open error: %s\n", file); + return 0; } - else if (n==2) { - if (decodef(buff,3,neu)<3) continue; - pcv.off[0][0]=neu[1]; - pcv.off[0][1]=neu[0]; - pcv.off[0][2]=neu[2]; + while (fgets(buff, sizeof(buff), fp)) + { + if (strlen(buff) >= 62 && buff[61] == '|') continue; + + if (buff[0] != ' ') n = 0; /* start line */ + if (++n == 1) + { + pcv = pcv0; + strncpy(pcv.type, buff, 61); pcv.type[61] = '\0'; + } + else if (n == 2) + { + if (decodef(buff, 3, neu)<3) continue; + pcv.off[0][0] = neu[1]; + pcv.off[0][1] = neu[0]; + pcv.off[0][2] = neu[2]; + } + else if (n == 3) decodef(buff, 10, pcv.var[0]); + else if (n == 4) decodef(buff, 9, pcv.var[0]+10); + else if (n == 5) + { + if (decodef(buff, 3, neu)<3) continue;; + pcv.off[1][0] = neu[1]; + pcv.off[1][1] = neu[0]; + pcv.off[1][2] = neu[2]; + } + else if (n == 6) decodef(buff, 10, pcv.var[1]); + else if (n == 7) + { + decodef(buff, 9, pcv.var[1]+10); + addpcv(&pcv, pcvs); + } } - else if (n==3) decodef(buff,10,pcv.var[0]); - else if (n==4) decodef(buff,9,pcv.var[0]+10); - else if (n==5) { - if (decodef(buff,3,neu)<3) continue;; - pcv.off[1][0]=neu[1]; - pcv.off[1][1]=neu[0]; - pcv.off[1][2]=neu[2]; - } - else if (n==6) decodef(buff,10,pcv.var[1]); - else if (n==7) { - decodef(buff,9,pcv.var[1]+10); - addpcv(&pcv,pcvs); - } - } fclose(fp); - + return 1; } + + /* read antex file ----------------------------------------------------------*/ int readantex(const char *file, pcvs_t *pcvs) { FILE *fp; - static const pcv_t pcv0={}; + static const pcv_t pcv0 = {}; pcv_t pcv; double neu[3]; - int i,f,freq=0,state=0,freqs[]={1,2,5,6,7,8,0}; + int i, f, freq = 0, state = 0, freqs[] = {1, 2, 5, 6, 7, 8, 0}; char buff[256]; - - trace(3,"readantex: file=%s\n",file); - - if (!(fp=fopen(file,"r"))) { - trace(2,"antex pcv file open error: %s\n",file); - return 0; - } - while (fgets(buff,sizeof(buff),fp)) { - - if (strlen(buff)<60||strstr(buff+60,"COMMENT")) continue; - - if (strstr(buff+60,"START OF ANTENNA")) { - pcv=pcv0; - state=1; + + trace(3, "readantex: file=%s\n", file); + + if (!(fp = fopen(file, "r"))) + { + trace(2, "antex pcv file open error: %s\n", file); + return 0; } - if (strstr(buff+60,"END OF ANTENNA")) { - addpcv(&pcv,pcvs); - state=0; + while (fgets(buff, sizeof(buff), fp)) + { + if (strlen(buff)<60 || strstr(buff+60, "COMMENT")) continue; + + if (strstr(buff+60, "START OF ANTENNA")) + { + pcv = pcv0; + state = 1; + } + if (strstr(buff+60, "END OF ANTENNA")) + { + addpcv(&pcv, pcvs); + state = 0; + } + if (!state) continue; + + if (strstr(buff+60, "TYPE / SERIAL NO")) + { + strncpy(pcv.type, buff , 20); pcv.type[20] = '\0'; + strncpy(pcv.code, buff+20, 20); pcv.code[20] = '\0'; + if (!strncmp(pcv.code+3, " ", 8)) + { + pcv.sat = satid2no(pcv.code); + } + } + else if (strstr(buff+60, "VALID FROM")) + { + if (!str2time(buff, 0, 43, &pcv.ts)) continue; + } + else if (strstr(buff+60, "VALID UNTIL")) + { + if (!str2time(buff, 0, 43, &pcv.te)) continue; + } + else if (strstr(buff+60, "START OF FREQUENCY")) + { + if (sscanf(buff+4, "%d", &f)<1) continue; + for (i = 0; in;i++) { - pcv=pcvs->pcv+i; - trace(4,"sat=%2d type=%20s code=%s off=%8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n", - pcv->sat,pcv->type,pcv->code,pcv->off[0][0],pcv->off[0][1], - pcv->off[0][2],pcv->off[1][0],pcv->off[1][1],pcv->off[1][2]); - } + int i, stat; + + trace(3, "readpcv: file=%s\n", file); + + if (!(ext = strrchr(file, '.'))) + { + ext = ""; + } + + if (!strcmp(ext, ".atx") || !strcmp(ext, ".ATX")) + { + stat = readantex(file, pcvs); + } + else + { + stat = readngspcv(file, pcvs); + } + for (i = 0; in; i++) + { + pcv = pcvs->pcv+i; + trace(4, "sat=%2d type=%20s code=%s off=%8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n", + pcv->sat, pcv->type, pcv->code, pcv->off[0][0], pcv->off[0][1], + pcv->off[0][2], pcv->off[1][0], pcv->off[1][1], pcv->off[1][2]); + } return stat; } + + /* search antenna parameter ---------------------------------------------------- -* read satellite antenna phase center position -* args : int sat I satellite number (0: receiver antenna) -* char *type I antenna type for receiver antenna -* gtime_t time I time to search parameters -* pcvs_t *pcvs IO antenna parameters -* return : antenna parameter (NULL: no antenna) -*-----------------------------------------------------------------------------*/ + * read satellite antenna phase center position + * args : int sat I satellite number (0: receiver antenna) + * char *type I antenna type for receiver antenna + * gtime_t time I time to search parameters + * pcvs_t *pcvs IO antenna parameters + * return : antenna parameter (NULL: no antenna) + *-----------------------------------------------------------------------------*/ pcv_t *searchpcv(int sat, const char *type, gtime_t time, - const pcvs_t *pcvs) + const pcvs_t *pcvs) { pcv_t *pcv; - char buff[MAXANT],*types[2],*p; - int i,j,n=0; - - trace(3,"searchpcv: sat=%2d type=%s\n",sat,type); - + char buff[MAXANT], *types[2], *p; + int i, j, n = 0; + + trace(3, "searchpcv: sat=%2d type=%s\n", sat, type); + if (sat) { /* search satellite antenna */ - for (i=0;in;i++) { - pcv=pcvs->pcv+i; - if (pcv->sat!=sat) continue; - if (pcv->ts.time!=0&&timediff(pcv->ts,time)>0.0) continue; - if (pcv->te.time!=0&&timediff(pcv->te,time)<0.0) continue; - return pcv; - } + for (i = 0; in; i++) + { + pcv = pcvs->pcv+i; + if (pcv->sat != sat) continue; + if (pcv->ts.time != 0 && timediff(pcv->ts, time)>0.0) continue; + if (pcv->te.time != 0 && timediff(pcv->te, time)<0.0) continue; + return pcv; + } } - else { - strcpy(buff,type); - for (p=strtok(buff," ");p&&n<2;p=strtok(NULL," ")) types[n++]=p; - if (n<=0) return NULL; - - /* search receiver antenna with radome at first */ - for (i=0;in;i++) { - pcv=pcvs->pcv+i; - for (j=0;jtype,types[j])) break; - if (j>=n) return pcv; + else + { + strcpy(buff, type); + for (p = strtok(buff, " "); p && n<2; p = strtok(NULL, " ")) types[n++] = p; + if (n <= 0) return NULL; + + /* search receiver antenna with radome at first */ + for (i = 0; in; i++) + { + pcv = pcvs->pcv+i; + for (j = 0; jtype, types[j])) break; + if (j >= n) return pcv; + } + /* search receiver antenna without radome */ + for (i = 0; in; i++) + { + pcv = pcvs->pcv+i; + if (strstr(pcv->type, types[0]) != pcv->type) continue; + + trace(2, "pcv without radome is used type=%s\n", type); + return pcv; + } } - /* search receiver antenna without radome */ - for (i=0;in;i++) { - pcv=pcvs->pcv+i; - if (strstr(pcv->type,types[0])!=pcv->type) continue; - - trace(2,"pcv without radome is used type=%s\n",type); - return pcv; - } - } return NULL; } + + /* read station positions ------------------------------------------------------ -* read positions from station position file -* args : char *file I station position file containing -* lat(deg) lon(deg) height(m) name in a line -* char *rcvs I station name -* double *pos O station position {lat,lon,h} (rad/m) -* (all 0 if search error) -* return : none -*-----------------------------------------------------------------------------*/ + * read positions from station position file + * args : char *file I station position file containing + * lat(deg) lon(deg) height(m) name in a line + * char *rcvs I station name + * double *pos O station position {lat,lon,h} (rad/m) + * (all 0 if search error) + * return : none + *-----------------------------------------------------------------------------*/ void readpos(const char *file, const char *rcv, double *pos) { static double poss[2048][3]; static char stas[2048][16]; FILE *fp; - int i,j,len,np=0; - char buff[256],str[256]; - - trace(3,"readpos: file=%s\n",file); - - if (!(fp=fopen(file,"r"))) { - fprintf(stderr,"reference position file open error : %s\n",file); - return; - } - while (np<2048&&fgets(buff,sizeof(buff),fp)) { - if (buff[0]=='%'||buff[0]=='#') continue; - if (sscanf(buff,"%lf %lf %lf %s",&poss[np][0],&poss[np][1],&poss[np][2], - str)<4) continue; - strncpy(stas[np],str,15); stas[np++][15]='\0'; - } + int i, j, len, np = 0; + char buff[256], str[256]; + + trace(3, "readpos: file=%s\n", file); + + if (!(fp = fopen(file, "r"))) + { + fprintf(stderr, "reference position file open error : %s\n", file); + return; + } + while (np<2048 && fgets(buff, sizeof(buff), fp)) + { + if (buff[0] == '%' || buff[0] == '#') continue; + if (sscanf(buff, "%lf %lf %lf %s", &poss[np][0], &poss[np][1], &poss[np][2], + str)<4) continue; + strncpy(stas[np], str, 15); stas[np++][15] = '\0'; + } fclose(fp); - len=(int)strlen(rcv); - for (i=0;in>=erp->nmax) { - erp->nmax=erp->nmax<=0?128:erp->nmax*2; - erp_data=(erpd_t *)realloc(erp->data,sizeof(erpd_t)*erp->nmax); - if (!erp_data) { - free(erp->data); erp->data=NULL; erp->n=erp->nmax=0; - fclose(fp); - return 0; + while (fgets(buff, sizeof(buff), fp)) + { + if (sscanf(buff, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", + v, v+1, v+2, v+3, v+4, v+5, v+6, v+7, v+8, v+9, v+10, v+11, v+12, v+13)<5) { + continue; } - erp->data=erp_data; + if (erp->n >= erp->nmax) + { + erp->nmax = erp->nmax <= 0 ? 128 : erp->nmax*2; + erp_data = (erpd_t *)realloc(erp->data, sizeof(erpd_t)*erp->nmax); + if (!erp_data) + { + free(erp->data); erp->data = NULL; erp->n = erp->nmax = 0; + fclose(fp); + return 0; + } + erp->data = erp_data; + } + erp->data[erp->n].mjd = v[0]; + erp->data[erp->n].xp = v[1]*1e-6*AS2R; + erp->data[erp->n].yp = v[2]*1e-6*AS2R; + erp->data[erp->n].ut1_utc = v[3]*1e-7; + erp->data[erp->n].lod = v[4]*1e-7; + erp->data[erp->n].xpr = v[12]*1e-6*AS2R; + erp->data[erp->n++].ypr = v[13]*1e-6*AS2R; } - erp->data[erp->n].mjd=v[0]; - erp->data[erp->n].xp=v[1]*1E-6*AS2R; - erp->data[erp->n].yp=v[2]*1E-6*AS2R; - erp->data[erp->n].ut1_utc=v[3]*1E-7; - erp->data[erp->n].lod=v[4]*1E-7; - erp->data[erp->n].xpr=v[12]*1E-6*AS2R; - erp->data[erp->n++].ypr=v[13]*1E-6*AS2R; - } fclose(fp); return 1; } + + + /* get earth rotation parameter values ----------------------------------------- -* get earth rotation parameter values -* args : erp_t *erp I earth rotation parameters -* gtime_t time I time (gpst) -* double *erpv O erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) -* return : status (1:ok,0:error) -*-----------------------------------------------------------------------------*/ + * get earth rotation parameter values + * args : erp_t *erp I earth rotation parameters + * gtime_t time I time (gpst) + * double *erpv O erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) + * return : status (1:ok,0:error) + *-----------------------------------------------------------------------------*/ int geterp(const erp_t *erp, gtime_t time, double *erpv) { - const double ep[]={2000,1,1,12,0,0}; - double mjd,day,a; - int i,j,k; - - trace(4,"geterp:\n"); - - if (erp->n<=0) return 0; - - mjd=51544.5+(timediff(gpst2utc(time),epoch2time(ep)))/86400.0; - - if (mjd<=erp->data[0].mjd) { - day=mjd-erp->data[0].mjd; - erpv[0]=erp->data[0].xp +erp->data[0].xpr*day; - erpv[1]=erp->data[0].yp +erp->data[0].ypr*day; - erpv[2]=erp->data[0].ut1_utc-erp->data[0].lod*day; - erpv[3]=erp->data[0].lod; - return 1; - } - if (mjd>=erp->data[erp->n-1].mjd) { - day=mjd-erp->data[erp->n-1].mjd; - erpv[0]=erp->data[erp->n-1].xp +erp->data[erp->n-1].xpr*day; - erpv[1]=erp->data[erp->n-1].yp +erp->data[erp->n-1].ypr*day; - erpv[2]=erp->data[erp->n-1].ut1_utc-erp->data[erp->n-1].lod*day; - erpv[3]=erp->data[erp->n-1].lod; - return 1; - } - for (j=0,k=erp->n-1;jdata[i].mjd) k=i; else j=i; - } - if (erp->data[j].mjd==erp->data[j+1].mjd) { - a=0.5; - } - else { - a=(mjd-erp->data[j].mjd)/(erp->data[j+1].mjd-erp->data[j].mjd); - } - erpv[0]=(1.0-a)*erp->data[j].xp +a*erp->data[j+1].xp; - erpv[1]=(1.0-a)*erp->data[j].yp +a*erp->data[j+1].yp; - erpv[2]=(1.0-a)*erp->data[j].ut1_utc+a*erp->data[j+1].ut1_utc; - erpv[3]=(1.0-a)*erp->data[j].lod +a*erp->data[j+1].lod; + const double ep[] = {2000, 1, 1, 12, 0, 0}; + double mjd, day, a; + int i, j, k; + + trace(4, "geterp:\n"); + + if (erp->n <= 0) return 0; + + mjd = 51544.5+(timediff(gpst2utc(time), epoch2time(ep)))/86400.0; + + if (mjd <= erp->data[0].mjd) + { + day = mjd-erp->data[0].mjd; + erpv[0] = erp->data[0].xp +erp->data[0].xpr*day; + erpv[1] = erp->data[0].yp +erp->data[0].ypr*day; + erpv[2] = erp->data[0].ut1_utc-erp->data[0].lod*day; + erpv[3] = erp->data[0].lod; + return 1; + } + if (mjd >= erp->data[erp->n-1].mjd) + { + day = mjd-erp->data[erp->n-1].mjd; + erpv[0] = erp->data[erp->n-1].xp +erp->data[erp->n-1].xpr*day; + erpv[1] = erp->data[erp->n-1].yp +erp->data[erp->n-1].ypr*day; + erpv[2] = erp->data[erp->n-1].ut1_utc-erp->data[erp->n-1].lod*day; + erpv[3] = erp->data[erp->n-1].lod; + return 1; + } + for (j = 0, k = erp->n-1; jdata[i].mjd) k = i; else j = i; + } + if (erp->data[j].mjd == erp->data[j+1].mjd) + { + a = 0.5; + } + else + { + a = (mjd-erp->data[j].mjd)/(erp->data[j+1].mjd-erp->data[j].mjd); + } + erpv[0] = (1.0-a)*erp->data[j].xp +a*erp->data[j+1].xp; + erpv[1] = (1.0-a)*erp->data[j].yp +a*erp->data[j+1].yp; + erpv[2] = (1.0-a)*erp->data[j].ut1_utc+a*erp->data[j+1].ut1_utc; + erpv[3] = (1.0-a)*erp->data[j].lod +a*erp->data[j+1].lod; return 1; } + /* compare ephemeris ---------------------------------------------------------*/ int cmpeph(const void *p1, const void *p2) { - eph_t *q1=(eph_t *)p1,*q2=(eph_t *)p2; - return q1->ttr.time!=q2->ttr.time?(int)(q1->ttr.time-q2->ttr.time): - (q1->toe.time!=q2->toe.time?(int)(q1->toe.time-q2->toe.time): - q1->sat-q2->sat); + eph_t *q1 = (eph_t *)p1, *q2 = (eph_t *)p2; + return q1->ttr.time != q2->ttr.time ? (int)(q1->ttr.time-q2->ttr.time) : + (q1->toe.time != q2->toe.time ? (int)(q1->toe.time-q2->toe.time) : + q1->sat-q2->sat); } + + /* sort and unique ephemeris -------------------------------------------------*/ void uniqeph(nav_t *nav) { eph_t *nav_eph; int i,j; - - trace(3,"uniqeph: n=%d\n",nav->n); - - if (nav->n<=0) return; - - qsort(nav->eph,nav->n,sizeof(eph_t),cmpeph); - - for (i=1,j=0;in;i++) { - if (nav->eph[i].sat!=nav->eph[j].sat|| - nav->eph[i].iode!=nav->eph[j].iode) { - nav->eph[++j]=nav->eph[i]; + + trace(3, "uniqeph: n=%d\n", nav->n); + + if (nav->n <= 0) return; + + qsort(nav->eph, nav->n, sizeof(eph_t), cmpeph); + + for (i = 1, j = 0; in; i++) + { + if (nav->eph[i].sat != nav->eph[j].sat || + nav->eph[i].iode != nav->eph[j].iode) + { + nav->eph[++j] = nav->eph[i]; + } } - } - nav->n=j+1; - - if (!(nav_eph=(eph_t *)realloc(nav->eph,sizeof(eph_t)*nav->n))) { - trace(1,"uniqeph malloc error n=%d\n",nav->n); - free(nav->eph); nav->eph=NULL; nav->n=nav->nmax=0; - return; - } - nav->eph=nav_eph; - nav->nmax=nav->n; - - trace(4,"uniqeph: n=%d\n",nav->n); + nav->n = j+1; + + if (!(nav_eph = (eph_t *)realloc(nav->eph, sizeof(eph_t)*nav->n))) + { + trace(1, "uniqeph malloc error n=%d\n", nav->n); + free(nav->eph); nav->eph = NULL; nav->n = nav->nmax = 0; + return; + } + nav->eph = nav_eph; + nav->nmax = nav->n; + + trace(4, "uniqeph: n=%d\n", nav->n); } + + /* compare glonass ephemeris -------------------------------------------------*/ int cmpgeph(const void *p1, const void *p2) { - geph_t *q1=(geph_t *)p1,*q2=(geph_t *)p2; - return q1->tof.time!=q2->tof.time?(int)(q1->tof.time-q2->tof.time): - (q1->toe.time!=q2->toe.time?(int)(q1->toe.time-q2->toe.time): - q1->sat-q2->sat); + geph_t *q1 = (geph_t *)p1, *q2 = (geph_t *)p2; + return q1->tof.time != q2->tof.time ? (int)(q1->tof.time-q2->tof.time) : + (q1->toe.time != q2->toe.time ? (int)(q1->toe.time-q2->toe.time) : + q1->sat-q2->sat); } + + /* sort and unique glonass ephemeris -----------------------------------------*/ void uniqgeph(nav_t *nav) { geph_t *nav_geph; - int i,j; - - trace(3,"uniqgeph: ng=%d\n",nav->ng); - - if (nav->ng<=0) return; - - qsort(nav->geph,nav->ng,sizeof(geph_t),cmpgeph); - - for (i=j=0;ing;i++) { - if (nav->geph[i].sat!=nav->geph[j].sat|| - nav->geph[i].toe.time!=nav->geph[j].toe.time|| - nav->geph[i].svh!=nav->geph[j].svh) { - nav->geph[++j]=nav->geph[i]; + int i, j; + + trace(3, "uniqgeph: ng=%d\n", nav->ng); + + if (nav->ng <= 0) return; + + qsort(nav->geph, nav->ng, sizeof(geph_t), cmpgeph); + + for (i = j = 0; ing; i++) + { + if (nav->geph[i].sat != nav->geph[j].sat || + nav->geph[i].toe.time != nav->geph[j].toe.time || + nav->geph[i].svh != nav->geph[j].svh) + { + nav->geph[++j] = nav->geph[i]; + } } - } - nav->ng=j+1; - - if (!(nav_geph=(geph_t *)realloc(nav->geph,sizeof(geph_t)*nav->ng))) { - trace(1,"uniqgeph malloc error ng=%d\n",nav->ng); - free(nav->geph); nav->geph=NULL; nav->ng=nav->ngmax=0; - return; - } - nav->geph=nav_geph; - nav->ngmax=nav->ng; - - trace(4,"uniqgeph: ng=%d\n",nav->ng); + nav->ng = j+1; + + if (!(nav_geph = (geph_t *)realloc(nav->geph, sizeof(geph_t)*nav->ng))) + { + trace(1, "uniqgeph malloc error ng=%d\n", nav->ng); + free(nav->geph); nav->geph = NULL; nav->ng = nav->ngmax = 0; + return; + } + nav->geph = nav_geph; + nav->ngmax = nav->ng; + + trace(4, "uniqgeph: ng=%d\n", nav->ng); } + + /* compare sbas ephemeris ----------------------------------------------------*/ int cmpseph(const void *p1, const void *p2) { - seph_t *q1=(seph_t *)p1,*q2=(seph_t *)p2; - return q1->tof.time!=q2->tof.time?(int)(q1->tof.time-q2->tof.time): - (q1->t0.time!=q2->t0.time?(int)(q1->t0.time-q2->t0.time): - q1->sat-q2->sat); + seph_t *q1 = (seph_t *)p1, *q2 = (seph_t *)p2; + return q1->tof.time != q2->tof.time ? (int)(q1->tof.time-q2->tof.time) : + (q1->t0.time != q2->t0.time ? (int)(q1->t0.time-q2->t0.time) : + q1->sat-q2->sat); } + + /* sort and unique sbas ephemeris --------------------------------------------*/ void uniqseph(nav_t *nav) { seph_t *nav_seph; - int i,j; - - trace(3,"uniqseph: ns=%d\n",nav->ns); - - if (nav->ns<=0) return; - - qsort(nav->seph,nav->ns,sizeof(seph_t),cmpseph); - - for (i=j=0;ins;i++) { - if (nav->seph[i].sat!=nav->seph[j].sat|| - nav->seph[i].t0.time!=nav->seph[j].t0.time) { - nav->seph[++j]=nav->seph[i]; + int i, j; + + trace(3, "uniqseph: ns=%d\n", nav->ns); + + if (nav->ns <= 0) return; + + qsort(nav->seph, nav->ns, sizeof(seph_t), cmpseph); + + for (i = j = 0; ins; i++) + { + if (nav->seph[i].sat != nav->seph[j].sat || + nav->seph[i].t0.time != nav->seph[j].t0.time) + { + nav->seph[++j] = nav->seph[i]; + } } - } - nav->ns=j+1; - - if (!(nav_seph=(seph_t *)realloc(nav->seph,sizeof(seph_t)*nav->ns))) { - trace(1,"uniqseph malloc error ns=%d\n",nav->ns); - free(nav->seph); nav->seph=NULL; nav->ns=nav->nsmax=0; - return; - } - nav->seph=nav_seph; - nav->nsmax=nav->ns; - - trace(4,"uniqseph: ns=%d\n",nav->ns); + nav->ns = j+1; + + if (!(nav_seph = (seph_t *)realloc(nav->seph, sizeof(seph_t)*nav->ns))) + { + trace(1, "uniqseph malloc error ns=%d\n", nav->ns); + free(nav->seph); nav->seph = NULL; nav->ns = nav->nsmax = 0; + return; + } + nav->seph = nav_seph; + nav->nsmax = nav->ns; + + trace(4, "uniqseph: ns=%d\n", nav->ns); } + + /* unique ephemerides ---------------------------------------------------------- -* unique ephemerides in navigation data and update carrier wave length -* args : nav_t *nav IO navigation data -* return : number of epochs -*-----------------------------------------------------------------------------*/ + * unique ephemerides in navigation data and update carrier wave length + * args : nav_t *nav IO navigation data + * return : number of epochs + *-----------------------------------------------------------------------------*/ void uniqnav(nav_t *nav) { - int i,j; - - trace(3,"uniqnav: neph=%d ngeph=%d nseph=%d\n",nav->n,nav->ng,nav->ns); - + int i, j; + + trace(3, "uniqnav: neph=%d ngeph=%d nseph=%d\n", nav->n, nav->ng, nav->ns); + /* unique ephemeris */ uniqeph (nav); uniqgeph(nav); uniqseph(nav); - + /* update carrier wave length */ - for (i=0;ilam[i][j]=satwavelen(i+1,j,nav); - } + for (i = 0; ilam[i][j] = satwavelen(i+1, j, nav); + } } + + /* compare observation data -------------------------------------------------*/ int cmpobs(const void *p1, const void *p2) { - obsd_t *q1=(obsd_t *)p1,*q2=(obsd_t *)p2; - double tt=timediff(q1->time,q2->time); - if (fabs(tt)>DTTOL) return tt<0?-1:1; - if (q1->rcv!=q2->rcv) return (int)q1->rcv-(int)q2->rcv; + obsd_t *q1 = (obsd_t *)p1, *q2 = (obsd_t *)p2; + double tt = timediff(q1->time, q2->time); + if (fabs(tt)>DTTOL) return tt<0 ? -1 : 1; + if (q1->rcv != q2->rcv) return (int)q1->rcv-(int)q2->rcv; return (int)q1->sat-(int)q2->sat; } + + /* sort and unique observation data -------------------------------------------- -* sort and unique observation data by time, rcv, sat -* args : obs_t *obs IO observation data -* return : number of epochs -*-----------------------------------------------------------------------------*/ + * sort and unique observation data by time, rcv, sat + * args : obs_t *obs IO observation data + * return : number of epochs + *-----------------------------------------------------------------------------*/ int sortobs(obs_t *obs) { - int i,j,n; - - trace(3,"sortobs: nobs=%d\n",obs->n); - - if (obs->n<=0) return 0; - - qsort(obs->data,obs->n,sizeof(obsd_t),cmpobs); - + int i, j, n; + + trace(3, "sortobs: nobs=%d\n", obs->n); + + if (obs->n <= 0) return 0; + + qsort(obs->data, obs->n, sizeof(obsd_t), cmpobs); + /* delete duplicated data */ - for (i=j=0;in;i++) { - if (obs->data[i].sat!=obs->data[j].sat|| - obs->data[i].rcv!=obs->data[j].rcv|| - timediff(obs->data[i].time,obs->data[j].time)!=0.0) { - obs->data[++j]=obs->data[i]; + for (i = j = 0; in; i++) + { + if (obs->data[i].sat != obs->data[j].sat || + obs->data[i].rcv != obs->data[j].rcv || + timediff(obs->data[i].time, obs->data[j].time) != 0.0) + { + obs->data[++j] = obs->data[i]; + } } - } - obs->n=j+1; - - for (i=n=0;in;i=j,n++) { - for (j=i+1;jn;j++) { - if (timediff(obs->data[j].time,obs->data[i].time)>DTTOL) break; + obs->n = j+1; + + for (i = n = 0; in; i = j, n++) + { + for (j = i+1; jn; j++) + { + if (timediff(obs->data[j].time, obs->data[i].time)>DTTOL) break; + } } - } return n; } + + /* screen by time -------------------------------------------------------------- -* screening by time start, time end, and time interval -* args : gtime_t time I time -* gtime_t ts I time start (ts.time==0:no screening by ts) -* gtime_t te I time end (te.time==0:no screening by te) -* double tint I time interval (s) (0.0:no screen by tint) -* return : 1:on condition, 0:not on condition -*-----------------------------------------------------------------------------*/ + * screening by time start, time end, and time interval + * args : gtime_t time I time + * gtime_t ts I time start (ts.time==0:no screening by ts) + * gtime_t te I time end (te.time==0:no screening by te) + * double tint I time interval (s) (0.0:no screen by tint) + * return : 1:on condition, 0:not on condition + *-----------------------------------------------------------------------------*/ int screent(gtime_t time, gtime_t ts, gtime_t te, double tint) { - return (tint<=0.0||fmod(time2gpst(time,NULL)+DTTOL,tint)<=DTTOL*2.0)&& - (ts.time==0||timediff(time,ts)>=-DTTOL)&& - (te.time==0||timediff(time,te)< DTTOL); + return (tint <= 0.0 || fmod(time2gpst(time, NULL)+DTTOL, tint) <= DTTOL*2.0) && + (ts.time == 0 || timediff(time, ts) >= -DTTOL) && + (te.time == 0 || timediff(time, te)< DTTOL); } + + /* read/save navigation data --------------------------------------------------- -* save or load navigation data -* args : char file I file path -* nav_t nav O/I navigation data -* return : status (1:ok,0:no file) -*-----------------------------------------------------------------------------*/ + * save or load navigation data + * args : char file I file path + * nav_t nav O/I navigation data + * return : status (1:ok,0:no file) + *-----------------------------------------------------------------------------*/ int readnav(const char *file, nav_t *nav) { FILE *fp; - eph_t eph0={}; - geph_t geph0={}; - char buff[4096],*p; - long toe_time,tof_time,toc_time,ttr_time; - int i,sat,prn; - - trace(3,"loadnav: file=%s\n",file); - - if (!(fp=fopen(file,"r"))) return 0; - - while (fgets(buff,sizeof(buff),fp)) { - if (!strncmp(buff,"IONUTC",6)) { - for (i=0;i<8;i++) nav->ion_gps[i]=0.0; - for (i=0;i<4;i++) nav->utc_gps[i]=0.0; - nav->leaps=0; - sscanf(buff,"IONUTC,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d", - &nav->ion_gps[0],&nav->ion_gps[1],&nav->ion_gps[2],&nav->ion_gps[3], - &nav->ion_gps[4],&nav->ion_gps[5],&nav->ion_gps[6],&nav->ion_gps[7], - &nav->utc_gps[0],&nav->utc_gps[1],&nav->utc_gps[2],&nav->utc_gps[3], - &nav->leaps); - continue; - } - if ((p=strchr(buff,','))) *p='\0'; else continue; - if (!(sat=satid2no(buff))) continue; - if (satsys(sat,&prn)==SYS_GLO) { - nav->geph[prn-1]=geph0; - nav->geph[prn-1].sat=sat; - toe_time=tof_time=0; - sscanf(p+1,"%d,%d,%d,%d,%d,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," + eph_t eph0 = {}; + geph_t geph0 = {}; + char buff[4096], *p; + long toe_time, tof_time, toc_time, ttr_time; + int i, sat, prn; + + trace(3, "loadnav: file=%s\n", file); + + if (!(fp = fopen(file, "r"))) return 0; + + while (fgets(buff, sizeof(buff), fp)) + { + if (!strncmp(buff, "IONUTC", 6)) + { + for (i = 0; i<8; i++) nav->ion_gps[i] = 0.0; + for (i = 0; i<4; i++) nav->utc_gps[i] = 0.0; + nav->leaps = 0; + sscanf(buff, "IONUTC,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d", + &nav->ion_gps[0], &nav->ion_gps[1], &nav->ion_gps[2], &nav->ion_gps[3], + &nav->ion_gps[4], &nav->ion_gps[5], &nav->ion_gps[6], &nav->ion_gps[7], + &nav->utc_gps[0], &nav->utc_gps[1], &nav->utc_gps[2], &nav->utc_gps[3], + &nav->leaps); + continue; + } + if ((p = strchr(buff, ','))) *p = '\0'; else continue; + if (!(sat = satid2no(buff))) continue; + if (satsys(sat, &prn) == SYS_GLO) + { + nav->geph[prn-1] = geph0; + nav->geph[prn-1].sat = sat; + toe_time = tof_time = 0; + sscanf(p+1, "%d,%d,%d,%d,%d,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," "%lf,%lf,%lf,%lf", - &nav->geph[prn-1].iode,&nav->geph[prn-1].frq,&nav->geph[prn-1].svh, - &nav->geph[prn-1].sva,&nav->geph[prn-1].age, - &toe_time,&tof_time, - &nav->geph[prn-1].pos[0],&nav->geph[prn-1].pos[1],&nav->geph[prn-1].pos[2], - &nav->geph[prn-1].vel[0],&nav->geph[prn-1].vel[1],&nav->geph[prn-1].vel[2], - &nav->geph[prn-1].acc[0],&nav->geph[prn-1].acc[1],&nav->geph[prn-1].acc[2], - &nav->geph[prn-1].taun ,&nav->geph[prn-1].gamn ,&nav->geph[prn-1].dtaun); - nav->geph[prn-1].toe.time=toe_time; - nav->geph[prn-1].tof.time=tof_time; - } - else { - nav->eph[sat-1]=eph0; - nav->eph[sat-1].sat=sat; - toe_time=toc_time=ttr_time=0; - sscanf(p+1,"%d,%d,%d,%d,%ld,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," + &nav->geph[prn-1].iode, &nav->geph[prn-1].frq, &nav->geph[prn-1].svh, + &nav->geph[prn-1].sva, &nav->geph[prn-1].age, + &toe_time, &tof_time, + &nav->geph[prn-1].pos[0], &nav->geph[prn-1].pos[1], &nav->geph[prn-1].pos[2], + &nav->geph[prn-1].vel[0], &nav->geph[prn-1].vel[1], &nav->geph[prn-1].vel[2], + &nav->geph[prn-1].acc[0], &nav->geph[prn-1].acc[1], &nav->geph[prn-1].acc[2], + &nav->geph[prn-1].taun , &nav->geph[prn-1].gamn , &nav->geph[prn-1].dtaun); + nav->geph[prn-1].toe.time = toe_time; + nav->geph[prn-1].tof.time = tof_time; + } + else + { + nav->eph[sat-1] = eph0; + nav->eph[sat-1].sat = sat; + toe_time = toc_time = ttr_time = 0; + sscanf(p+1, "%d,%d,%d,%d,%ld,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," "%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d,%d", - &nav->eph[sat-1].iode,&nav->eph[sat-1].iodc,&nav->eph[sat-1].sva , - &nav->eph[sat-1].svh , - &toe_time,&toc_time,&ttr_time, - &nav->eph[sat-1].A ,&nav->eph[sat-1].e ,&nav->eph[sat-1].i0 , - &nav->eph[sat-1].OMG0,&nav->eph[sat-1].omg ,&nav->eph[sat-1].M0 , - &nav->eph[sat-1].deln,&nav->eph[sat-1].OMGd,&nav->eph[sat-1].idot, - &nav->eph[sat-1].crc ,&nav->eph[sat-1].crs ,&nav->eph[sat-1].cuc , - &nav->eph[sat-1].cus ,&nav->eph[sat-1].cic ,&nav->eph[sat-1].cis , - &nav->eph[sat-1].toes,&nav->eph[sat-1].fit ,&nav->eph[sat-1].f0 , - &nav->eph[sat-1].f1 ,&nav->eph[sat-1].f2 ,&nav->eph[sat-1].tgd[0], - &nav->eph[sat-1].code, &nav->eph[sat-1].flag); - nav->eph[sat-1].toe.time=toe_time; - nav->eph[sat-1].toc.time=toc_time; - nav->eph[sat-1].ttr.time=ttr_time; + &nav->eph[sat-1].iode, &nav->eph[sat-1].iodc, &nav->eph[sat-1].sva , + &nav->eph[sat-1].svh , + &toe_time, &toc_time, &ttr_time, + &nav->eph[sat-1].A , &nav->eph[sat-1].e , &nav->eph[sat-1].i0 , + &nav->eph[sat-1].OMG0, &nav->eph[sat-1].omg , &nav->eph[sat-1].M0 , + &nav->eph[sat-1].deln, &nav->eph[sat-1].OMGd, &nav->eph[sat-1].idot, + &nav->eph[sat-1].crc , &nav->eph[sat-1].crs , &nav->eph[sat-1].cuc , + &nav->eph[sat-1].cus , &nav->eph[sat-1].cic , &nav->eph[sat-1].cis , + &nav->eph[sat-1].toes, &nav->eph[sat-1].fit , &nav->eph[sat-1].f0 , + &nav->eph[sat-1].f1 , &nav->eph[sat-1].f2 , &nav->eph[sat-1].tgd[0], + &nav->eph[sat-1].code, &nav->eph[sat-1].flag); + nav->eph[sat-1].toe.time = toe_time; + nav->eph[sat-1].toc.time = toc_time; + nav->eph[sat-1].ttr.time = ttr_time; + } } - } fclose(fp); return 1; } -int savenav(const char *file, const nav_t *nav) + + +int savenav(const char *file, const nav_t *nav) { FILE *fp; int i; char id[32]; - - trace(3,"savenav: file=%s\n",file); - - if (!(fp=fopen(file,"w"))) return 0; - - for (i=0;ieph[i].ttr.time==0) continue; - satno2id(nav->eph[i].sat,id); - fprintf(fp,"%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," - "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," - "%.14E,%.14E,%.14E,%.14E,%.14E,%d,%d\n", - id,nav->eph[i].iode,nav->eph[i].iodc,nav->eph[i].sva , - nav->eph[i].svh ,(int)nav->eph[i].toe.time, - (int)nav->eph[i].toc.time,(int)nav->eph[i].ttr.time, - nav->eph[i].A ,nav->eph[i].e ,nav->eph[i].i0 ,nav->eph[i].OMG0, - nav->eph[i].omg ,nav->eph[i].M0 ,nav->eph[i].deln,nav->eph[i].OMGd, - nav->eph[i].idot,nav->eph[i].crc,nav->eph[i].crs ,nav->eph[i].cuc , - nav->eph[i].cus ,nav->eph[i].cic,nav->eph[i].cis ,nav->eph[i].toes, - nav->eph[i].fit ,nav->eph[i].f0 ,nav->eph[i].f1 ,nav->eph[i].f2 , - nav->eph[i].tgd[0],nav->eph[i].code,nav->eph[i].flag); - } - for (i=0;igeph[i].tof.time==0) continue; - satno2id(nav->geph[i].sat,id); - fprintf(fp,"%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," - "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E\n", - id,nav->geph[i].iode,nav->geph[i].frq,nav->geph[i].svh, - nav->geph[i].sva,nav->geph[i].age,(int)nav->geph[i].toe.time, - (int)nav->geph[i].tof.time, - nav->geph[i].pos[0],nav->geph[i].pos[1],nav->geph[i].pos[2], - nav->geph[i].vel[0],nav->geph[i].vel[1],nav->geph[i].vel[2], - nav->geph[i].acc[0],nav->geph[i].acc[1],nav->geph[i].acc[2], - nav->geph[i].taun,nav->geph[i].gamn,nav->geph[i].dtaun); - } + + trace(3, "savenav: file=%s\n", file); + + if (!(fp = fopen(file, "w"))) return 0; + + for (i = 0; ieph[i].ttr.time == 0) continue; + satno2id(nav->eph[i].sat, id); + fprintf(fp, "%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," + "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," + "%.14E,%.14E,%.14E,%.14E,%.14E,%d,%d\n", + id, nav->eph[i].iode, nav->eph[i].iodc, nav->eph[i].sva , + nav->eph[i].svh , (int)nav->eph[i].toe.time, + (int)nav->eph[i].toc.time, (int)nav->eph[i].ttr.time, + nav->eph[i].A , nav->eph[i].e , nav->eph[i].i0 , nav->eph[i].OMG0, + nav->eph[i].omg , nav->eph[i].M0 , nav->eph[i].deln, nav->eph[i].OMGd, + nav->eph[i].idot, nav->eph[i].crc, nav->eph[i].crs , nav->eph[i].cuc , + nav->eph[i].cus , nav->eph[i].cic, nav->eph[i].cis , nav->eph[i].toes, + nav->eph[i].fit , nav->eph[i].f0 , nav->eph[i].f1 , nav->eph[i].f2 , + nav->eph[i].tgd[0], nav->eph[i].code, nav->eph[i].flag); + } + for (i = 0; igeph[i].tof.time == 0) continue; + satno2id(nav->geph[i].sat, id); + fprintf(fp, "%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," + "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E\n", + id, nav->geph[i].iode, nav->geph[i].frq, nav->geph[i].svh, + nav->geph[i].sva, nav->geph[i].age, (int)nav->geph[i].toe.time, + (int)nav->geph[i].tof.time, + nav->geph[i].pos[0], nav->geph[i].pos[1], nav->geph[i].pos[2], + nav->geph[i].vel[0], nav->geph[i].vel[1], nav->geph[i].vel[2], + nav->geph[i].acc[0], nav->geph[i].acc[1], nav->geph[i].acc[2], + nav->geph[i].taun, nav->geph[i].gamn, nav->geph[i].dtaun); + } fprintf(fp,"IONUTC,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," - "%.14E,%.14E,%.14E,%d", - nav->ion_gps[0],nav->ion_gps[1],nav->ion_gps[2],nav->ion_gps[3], - nav->ion_gps[4],nav->ion_gps[5],nav->ion_gps[6],nav->ion_gps[7], - nav->utc_gps[0],nav->utc_gps[1],nav->utc_gps[2],nav->utc_gps[3], + "%.14E,%.14E,%.14E,%d", + nav->ion_gps[0], nav->ion_gps[1], nav->ion_gps[2], nav->ion_gps[3], + nav->ion_gps[4], nav->ion_gps[5], nav->ion_gps[6], nav->ion_gps[7], + nav->utc_gps[0], nav->utc_gps[1], nav->utc_gps[2], nav->utc_gps[3], nav->leaps); - + fclose(fp); return 1; } + + /* free observation data ------------------------------------------------------- -* free memory for observation data -* args : obs_t *obs IO observation data -* return : none -*-----------------------------------------------------------------------------*/ + * free memory for observation data + * args : obs_t *obs IO observation data + * return : none + *-----------------------------------------------------------------------------*/ void freeobs(obs_t *obs) { - free(obs->data); obs->data=NULL; obs->n=obs->nmax=0; + free(obs->data); obs->data = NULL; obs->n = obs->nmax = 0; } + + /* free navigation data --------------------------------------------------------- -* free memory for navigation data -* args : nav_t *nav IO navigation data -* int opt I option (or of followings) -* (0x01: gps/qzs ephmeris, 0x02: glonass ephemeris, -* 0x04: sbas ephemeris, 0x08: precise ephemeris, -* 0x10: precise clock 0x20: almanac, -* 0x40: tec data) -* return : none -*-----------------------------------------------------------------------------*/ + * free memory for navigation data + * args : nav_t *nav IO navigation data + * int opt I option (or of followings) + * (0x01: gps/qzs ephmeris, 0x02: glonass ephemeris, + * 0x04: sbas ephemeris, 0x08: precise ephemeris, + * 0x10: precise clock 0x20: almanac, + * 0x40: tec data) + * return : none + *-----------------------------------------------------------------------------*/ void freenav(nav_t *nav, int opt) { - if (opt&0x01) {free(nav->eph ); nav->eph =NULL; nav->n =nav->nmax =0;} - if (opt&0x02) {free(nav->geph); nav->geph=NULL; nav->ng=nav->ngmax=0;} - if (opt&0x04) {free(nav->seph); nav->seph=NULL; nav->ns=nav->nsmax=0;} - if (opt&0x08) {free(nav->peph); nav->peph=NULL; nav->ne=nav->nemax=0;} - if (opt&0x10) {free(nav->pclk); nav->pclk=NULL; nav->nc=nav->ncmax=0;} - if (opt&0x20) {free(nav->alm ); nav->alm =NULL; nav->na=nav->namax=0;} - if (opt&0x40) {free(nav->tec ); nav->tec =NULL; nav->nt=nav->ntmax=0;} - if (opt&0x80) {free(nav->fcb ); nav->fcb =NULL; nav->nf=nav->nfmax=0;} + if (opt&0x01) {free(nav->eph ); nav->eph = NULL; nav->n = nav->nmax = 0;} + if (opt&0x02) {free(nav->geph); nav->geph = NULL; nav->ng = nav->ngmax = 0;} + if (opt&0x04) {free(nav->seph); nav->seph = NULL; nav->ns = nav->nsmax = 0;} + if (opt&0x08) {free(nav->peph); nav->peph = NULL; nav->ne = nav->nemax = 0;} + if (opt&0x10) {free(nav->pclk); nav->pclk = NULL; nav->nc = nav->ncmax = 0;} + if (opt&0x20) {free(nav->alm ); nav->alm = NULL; nav->na = nav->namax = 0;} + if (opt&0x40) {free(nav->tec ); nav->tec = NULL; nav->nt = nav->ntmax = 0;} + if (opt&0x80) {free(nav->fcb ); nav->fcb = NULL; nav->nf = nav->nfmax = 0;} } + + /* debug trace functions -----------------------------------------------------*/ //#ifdef TRACE // @@ -2731,7 +2932,7 @@ void freenav(nav_t *nav, int opt) //static int level_trace=0; /* level of trace */ //static unsigned int tick_trace=0; /* tick time at traceopen (ms) */ //static gtime_t time_trace={0}; /* time at traceopen */ -//static lock_t lock_trace; /* lock for trace */ +//static pthread_mutex_t lock_trace; /* lock for trace */ // //static void traceswap(void) //{ @@ -2785,7 +2986,7 @@ void freenav(nav_t *nav, int opt) // va_list ap; // // /* print error message to stderr */ -// if (level<=1) { +// if (level <= 1) { // va_start(ap,format); vfprintf(stderr,format,ap); va_end(ap); // } // if (!fp_trace||level>level_trace) return; @@ -2856,7 +3057,7 @@ void freenav(nav_t *nav, int opt) // time2str(nav->geph[i].tof,s2,0); // satno2id(nav->geph[i].sat,id); // fprintf(fp_trace,"(%3d) %-3s : %s %s %2d %2d %8.3f\n",i+1, -// id,s1,s2,nav->geph[i].frq,nav->geph[i].svh,nav->geph[i].taun*1E6); +// id,s1,s2,nav->geph[i].frq,nav->geph[i].svh,nav->geph[i].taun*1e6); // } //} //extern void tracehnav(int level, const nav_t *nav) @@ -2887,9 +3088,9 @@ void freenav(nav_t *nav, int opt) // fprintf(fp_trace,"%-3s %d %-3s %13.3f %13.3f %13.3f %13.3f %6.3f %6.3f %6.3f %6.3f\n", // s,nav->peph[i].index,id, // nav->peph[i].pos[j][0],nav->peph[i].pos[j][1], -// nav->peph[i].pos[j][2],nav->peph[i].pos[j][3]*1E9, +// nav->peph[i].pos[j][2],nav->peph[i].pos[j][3]*1e9, // nav->peph[i].std[j][0],nav->peph[i].std[j][1], -// nav->peph[i].std[j][2],nav->peph[i].std[j][3]*1E9); +// nav->peph[i].std[j][2],nav->peph[i].std[j][3]*1e9); // } // } //} @@ -2906,7 +3107,7 @@ void freenav(nav_t *nav, int opt) // satno2id(j+1,id); // fprintf(fp_trace,"%-3s %d %-3s %13.3f %6.3f\n", // s,nav->pclk[i].index,id, -// nav->pclk[i].clk[j][0]*1E9,nav->pclk[i].std[j][0]*1E9); +// nav->pclk[i].clk[j][0]*1e9,nav->pclk[i].std[j][0]*1e9); // } // } //} @@ -2918,801 +3119,877 @@ void freenav(nav_t *nav, int opt) // fprintf(fp_trace,"\n"); //} //#else -void traceopen(const char *file) {} -void traceclose(void) {} -void tracelevel(int level) {} -void trace (int level, const char *format, ...) { + +//void traceopen(const char *file) {} +//void traceclose(void) {} +//void tracelevel(int level) {} +void trace (int level __attribute__((unused)), const char *format __attribute__((unused)), ...) +{ /* va_list ap;*/ /* print error message to stderr */ /*printf("RTKLIB TRACE[%i]:",level); va_start(ap,format); vfprintf(stdout,format,ap); va_end(ap);*/ } -void tracet (int level, const char *format, ...) {} -void tracemat(int level, const double *A, int n, int m, int p, int q) {} -void traceobs(int level, const obsd_t *obs, int n) {} -void tracenav(int level, const nav_t *nav) {} -void tracegnav(int level, const nav_t *nav) {} -void tracehnav(int level, const nav_t *nav) {} -void tracepeph(int level, const nav_t *nav) {} -void tracepclk(int level, const nav_t *nav) {} -void traceb (int level, const unsigned char *p, int n) {} +//void tracet (int level, const char *format, ...) {} +//void tracemat(int level, const double *A, int n, int m, int p, int q) {} +//void traceobs(int level, const obsd_t *obs, int n) {} +//void tracenav(int level, const nav_t *nav) {} +//void tracegnav(int level, const nav_t *nav) {} +//void tracehnav(int level, const nav_t *nav) {} +//void tracepeph(int level, const nav_t *nav) {} +//void tracepclk(int level, const nav_t *nav) {} +//void traceb (int level, const unsigned char *p, int n) {} //#endif /* TRACE */ + /* execute command ------------------------------------------------------------- -* execute command line by operating system shell -* args : char *cmd I command line -* return : execution status (0:ok,0>:error) -*-----------------------------------------------------------------------------*/ + * execute command line by operating system shell + * args : char *cmd I command line + * return : execution status (0:ok,0>:error) + *-----------------------------------------------------------------------------*/ int execcmd(const char *cmd) { - - trace(3,"execcmd: cmd=%s\n",cmd); - + trace(3, "execcmd: cmd=%s\n", cmd); return system(cmd); } + /* create directory ------------------------------------------------------------ -* create directory if not exist -* args : char *path I file path to be saved -* return : none -* notes : not recursive. only one level -*-----------------------------------------------------------------------------*/ + * create directory if not exist + * args : char *path I file path to be saved + * return : none + * notes : not recursive. only one level + *-----------------------------------------------------------------------------*/ void createdir(const char *path) { - char buff[1024],*p; - - tracet(3,"createdir: path=%s\n",path); - - strcpy(buff,path); - if (!(p=strrchr(buff,FILEPATHSEP))) return; - *p='\0'; - - mkdir(buff,0777); + char buff[1024], *p; + //tracet(3, "createdir: path=%s\n", path); + + strcpy(buff, path); + if (!(p = strrchr(buff, FILEPATHSEP))) return; + *p = '\0'; + + mkdir(buff, 0777); } + + /* replace string ------------------------------------------------------------*/ int repstr(char *str, const char *pat, const char *rep) { - int len=(int)strlen(pat); - char buff[1024],*p,*q,*r; - - for (p=str,r=buff;*p;p=q+len) { - if (!(q=strstr(p,pat))) break; - strncpy(r,p,q-p); - r+=q-p; - r+=sprintf(r,"%s",rep); - } - if (p<=str) return 0; - strcpy(r,p); - strcpy(str,buff); + int len = (int)strlen(pat); + char buff[1024], *p, *q, *r; + + for (p = str, r = buff; *p; p = q+len) + { + if (!(q = strstr(p, pat))) break; + strncpy(r, p, q-p); + r += q-p; + r += sprintf(r, "%s", rep); + } + if (p <= str) return 0; + strcpy(r, p); + strcpy(str, buff); return 1; } + + /* replace keywords in file path ----------------------------------------------- -* replace keywords in file path with date, time, rover and base station id -* args : char *path I file path (see below) -* char *rpath O file path in which keywords replaced (see below) -* gtime_t time I time (gpst) (time.time==0: not replaced) -* char *rov I rover id string ("": not replaced) -* char *base I base station id string ("": not replaced) -* return : status (1:keywords replaced, 0:no valid keyword in the path, -* -1:no valid time) -* notes : the following keywords in path are replaced by date, time and name -* %Y -> yyyy : year (4 digits) (1900-2099) -* %y -> yy : year (2 digits) (00-99) -* %m -> mm : month (01-12) -* %d -> dd : day of month (01-31) -* %h -> hh : hours (00-23) -* %M -> mm : minutes (00-59) -* %S -> ss : seconds (00-59) -* %n -> ddd : day of year (001-366) -* %W -> wwww : gps week (0001-9999) -* %D -> d : day of gps week (0-6) -* %H -> h : hour code (a=0,b=1,c=2,...,x=23) -* %ha-> hh : 3 hours (00,03,06,...,21) -* %hb-> hh : 6 hours (00,06,12,18) -* %hc-> hh : 12 hours (00,12) -* %t -> mm : 15 minutes (00,15,30,45) -* %r -> rrrr : rover id -* %b -> bbbb : base station id -*-----------------------------------------------------------------------------*/ + * replace keywords in file path with date, time, rover and base station id + * args : char *path I file path (see below) + * char *rpath O file path in which keywords replaced (see below) + * gtime_t time I time (gpst) (time.time==0: not replaced) + * char *rov I rover id string ("": not replaced) + * char *base I base station id string ("": not replaced) + * return : status (1:keywords replaced, 0:no valid keyword in the path, + * -1:no valid time) + * notes : the following keywords in path are replaced by date, time and name + * %Y -> yyyy : year (4 digits) (1900-2099) + * %y -> yy : year (2 digits) (00-99) + * %m -> mm : month (01-12) + * %d -> dd : day of month (01-31) + * %h -> hh : hours (00-23) + * %M -> mm : minutes (00-59) + * %S -> ss : seconds (00-59) + * %n -> ddd : day of year (001-366) + * %W -> wwww : gps week (0001-9999) + * %D -> d : day of gps week (0-6) + * %H -> h : hour code (a=0,b=1,c=2,...,x=23) + * %ha-> hh : 3 hours (00,03,06,...,21) + * %hb-> hh : 6 hours (00,06,12,18) + * %hc-> hh : 12 hours (00,12) + * %t -> mm : 15 minutes (00,15,30,45) + * %r -> rrrr : rover id + * %b -> bbbb : base station id + *-----------------------------------------------------------------------------*/ int reppath(const char *path, char *rpath, gtime_t time, const char *rov, - const char *base) + const char *base) { - double ep[6],ep0[6]={2000,1,1,0,0,0}; - int week,dow,doy,stat=0; + double ep[6], ep0[6] = {2000, 1, 1, 0, 0, 0}; + int week, dow, doy, stat = 0; char rep[64]; - - strcpy(rpath,path); - - if (!strstr(rpath,"%")) return 0; - if (*rov ) stat|=repstr(rpath,"%r",rov ); - if (*base) stat|=repstr(rpath,"%b",base); - if (time.time!=0) { - time2epoch(time,ep); - ep0[0]=ep[0]; - dow=(int)floor(time2gpst(time,&week)/86400.0); - doy=(int)floor(timediff(time,epoch2time(ep0))/86400.0)+1; - sprintf(rep,"%02d", ((int)ep[3]/3)*3); stat|=repstr(rpath,"%ha",rep); - sprintf(rep,"%02d", ((int)ep[3]/6)*6); stat|=repstr(rpath,"%hb",rep); - sprintf(rep,"%02d", ((int)ep[3]/12)*12); stat|=repstr(rpath,"%hc",rep); - sprintf(rep,"%04.0f",ep[0]); stat|=repstr(rpath,"%Y",rep); - sprintf(rep,"%02.0f",fmod(ep[0],100.0)); stat|=repstr(rpath,"%y",rep); - sprintf(rep,"%02.0f",ep[1]); stat|=repstr(rpath,"%m",rep); - sprintf(rep,"%02.0f",ep[2]); stat|=repstr(rpath,"%d",rep); - sprintf(rep,"%02.0f",ep[3]); stat|=repstr(rpath,"%h",rep); - sprintf(rep,"%02.0f",ep[4]); stat|=repstr(rpath,"%M",rep); - sprintf(rep,"%02.0f",floor(ep[5])); stat|=repstr(rpath,"%S",rep); - sprintf(rep,"%03d", doy); stat|=repstr(rpath,"%n",rep); - sprintf(rep,"%04d", week); stat|=repstr(rpath,"%W",rep); - sprintf(rep,"%d", dow); stat|=repstr(rpath,"%D",rep); - sprintf(rep,"%c", 'a'+(int)ep[3]); stat|=repstr(rpath,"%H",rep); - sprintf(rep,"%02d", ((int)ep[4]/15)*15); stat|=repstr(rpath,"%t",rep); - } - else if (strstr(rpath,"%ha")||strstr(rpath,"%hb")||strstr(rpath,"%hc")|| - strstr(rpath,"%Y" )||strstr(rpath,"%y" )||strstr(rpath,"%m" )|| - strstr(rpath,"%d" )||strstr(rpath,"%h" )||strstr(rpath,"%M" )|| - strstr(rpath,"%S" )||strstr(rpath,"%n" )||strstr(rpath,"%W" )|| - strstr(rpath,"%D" )||strstr(rpath,"%H" )||strstr(rpath,"%t" )) { - return -1; /* no valid time */ - } + + strcpy(rpath, path); + + if (!strstr(rpath, "%")) return 0; + if (*rov ) stat|=repstr(rpath, "%r", rov ); + if (*base) stat|=repstr(rpath, "%b", base); + if (time.time != 0) + { + time2epoch(time, ep); + ep0[0] = ep[0]; + dow = (int)floor(time2gpst(time, &week)/86400.0); + doy = (int)floor(timediff(time, epoch2time(ep0))/86400.0)+1; + sprintf(rep, "%02d", ((int)ep[3]/3)*3); stat|=repstr(rpath, "%ha", rep); + sprintf(rep, "%02d", ((int)ep[3]/6)*6); stat|=repstr(rpath, "%hb", rep); + sprintf(rep, "%02d", ((int)ep[3]/12)*12); stat|=repstr(rpath, "%hc", rep); + sprintf(rep, "%04.0f", ep[0]); stat|=repstr(rpath, "%Y", rep); + sprintf(rep, "%02.0f", fmod(ep[0], 100.0)); stat|=repstr(rpath, "%y", rep); + sprintf(rep, "%02.0f", ep[1]); stat|=repstr(rpath, "%m", rep); + sprintf(rep, "%02.0f", ep[2]); stat|=repstr(rpath, "%d", rep); + sprintf(rep, "%02.0f", ep[3]); stat|=repstr(rpath, "%h", rep); + sprintf(rep, "%02.0f", ep[4]); stat|=repstr(rpath, "%M", rep); + sprintf(rep, "%02.0f", floor(ep[5])); stat|=repstr(rpath, "%S", rep); + sprintf(rep, "%03d", doy); stat|=repstr(rpath, "%n", rep); + sprintf(rep, "%04d", week); stat|=repstr(rpath, "%W", rep); + sprintf(rep, "%d", dow); stat|=repstr(rpath, "%D", rep); + sprintf(rep, "%c", 'a'+(int)ep[3]); stat|=repstr(rpath, "%H", rep); + sprintf(rep, "%02d", ((int)ep[4]/15)*15); stat|=repstr(rpath, "%t", rep); + } + else if (strstr(rpath, "%ha") || strstr(rpath, "%hb") || strstr(rpath, "%hc") || + strstr(rpath, "%Y" ) || strstr(rpath, "%y" ) || strstr(rpath, "%m" ) || + strstr(rpath, "%d" ) || strstr(rpath, "%h" ) || strstr(rpath, "%M" ) || + strstr(rpath, "%S" ) || strstr(rpath, "%n" ) || strstr(rpath, "%W" ) || + strstr(rpath, "%D" ) || strstr(rpath, "%H" ) || strstr(rpath, "%t" )) + { + return -1; /* no valid time */ + } return stat; } + + /* replace keywords in file path and generate multiple paths ------------------- -* replace keywords in file path with date, time, rover and base station id -* generate multiple keywords-replaced paths -* args : char *path I file path (see below) -* char *rpath[] O file paths in which keywords replaced -* int nmax I max number of output file paths -* gtime_t ts I time start (gpst) -* gtime_t te I time end (gpst) -* char *rov I rover id string ("": not replaced) -* char *base I base station id string ("": not replaced) -* return : number of replaced file paths -* notes : see reppath() for replacements of keywords. -* minimum interval of time replaced is 900s. -*-----------------------------------------------------------------------------*/ + * replace keywords in file path with date, time, rover and base station id + * generate multiple keywords-replaced paths + * args : char *path I file path (see below) + * char *rpath[] O file paths in which keywords replaced + * int nmax I max number of output file paths + * gtime_t ts I time start (gpst) + * gtime_t te I time end (gpst) + * char *rov I rover id string ("": not replaced) + * char *base I base station id string ("": not replaced) + * return : number of replaced file paths + * notes : see reppath() for replacements of keywords. + * minimum interval of time replaced is 900s. + *-----------------------------------------------------------------------------*/ int reppaths(const char *path, char *rpath[], int nmax, gtime_t ts, - gtime_t te, const char *rov, const char *base) + gtime_t te, const char *rov, const char *base) { gtime_t time; - double tow,tint=86400.0; - int i,n=0,week; - - trace(3,"reppaths: path =%s nmax=%d rov=%s base=%s\n",path,nmax,rov,base); - - if (ts.time==0||te.time==0||timediff(ts,te)>0.0) return 0; - - if (strstr(path,"%S")||strstr(path,"%M")||strstr(path,"%t")) tint=900.0; - else if (strstr(path,"%h")||strstr(path,"%H")) tint=3600.0; - - tow=time2gpst(ts,&week); - time=gpst2time(week,floor(tow/tint)*tint); - - while (timediff(time,te)<=0.0&&n0.0) return 0; + + if (strstr(path, "%S") || strstr(path, "%M") || strstr(path, "%t")) tint = 900.0; + else if (strstr(path, "%h") || strstr(path, "%H")) tint = 3600.0; + + tow = time2gpst(ts, &week); + time = gpst2time(week, floor(tow/tint)*tint); + + while (timediff(time, te) <= 0.0 && nng;i++) { - if (nav->geph[i].sat!=sat) continue; - return CLIGHT/(freq_glo[frq]+dfrq_glo[frq]*nav->geph[i].frq); - } + const double freq_glo[] = {FREQ1_GLO, FREQ2_GLO}; + const double dfrq_glo[] = {DFRQ1_GLO, DFRQ2_GLO}; + int i, sys = satsys(sat, NULL); + + if (sys == SYS_GLO) + { + if (0 <= frq && frq <= 1) + { + for (i = 0; ing; i++) + { + if (nav->geph[i].sat != sat) continue; + return SPEED_OF_LIGHT/(freq_glo[frq]+dfrq_glo[frq]*nav->geph[i].frq); + } + } + else if (frq == 2) + { /* L3 */ + return SPEED_OF_LIGHT/FREQ3_GLO; + } } - else if (frq==2) { /* L3 */ - return CLIGHT/FREQ3_GLO; + else if (sys == SYS_BDS) + { + if (frq == 0) return SPEED_OF_LIGHT/FREQ1_BDS; /* B1 */ + else if (frq == 1) return SPEED_OF_LIGHT/FREQ2_BDS; /* B2 */ + else if (frq == 2) return SPEED_OF_LIGHT/FREQ3_BDS; /* B3 */ + } + else + { + if (frq == 0) return SPEED_OF_LIGHT/FREQ1; /* L1/E1 */ + else if (frq == 1) return SPEED_OF_LIGHT/FREQ2; /* L2 */ + else if (frq == 2) return SPEED_OF_LIGHT/FREQ5; /* L5/E5a */ + else if (frq == 3) return SPEED_OF_LIGHT/FREQ6; /* L6/LEX */ + else if (frq == 4) return SPEED_OF_LIGHT/FREQ7; /* E5b */ + else if (frq == 5) return SPEED_OF_LIGHT/FREQ8; /* E5a+b */ + else if (frq == 6) return SPEED_OF_LIGHT/FREQ9; /* S */ } - } - else if (sys==SYS_CMP) { - if (frq==0) return CLIGHT/FREQ1_CMP; /* B1 */ - else if (frq==1) return CLIGHT/FREQ2_CMP; /* B2 */ - else if (frq==2) return CLIGHT/FREQ3_CMP; /* B3 */ - } - else { - if (frq==0) return CLIGHT/FREQ1; /* L1/E1 */ - else if (frq==1) return CLIGHT/FREQ2; /* L2 */ - else if (frq==2) return CLIGHT/FREQ5; /* L5/E5a */ - else if (frq==3) return CLIGHT/FREQ6; /* L6/LEX */ - else if (frq==4) return CLIGHT/FREQ7; /* E5b */ - else if (frq==5) return CLIGHT/FREQ8; /* E5a+b */ - else if (frq==6) return CLIGHT/FREQ9; /* S */ - } return 0.0; } + + /* geometric distance ---------------------------------------------------------- -* compute geometric distance and receiver-to-satellite unit vector -* args : double *rs I satellilte position (ecef at transmission) (m) -* double *rr I receiver position (ecef at reception) (m) -* double *e O line-of-sight vector (ecef) -* return : geometric distance (m) (0>:error/no satellite position) -* notes : distance includes sagnac effect correction -*-----------------------------------------------------------------------------*/ + * compute geometric distance and receiver-to-satellite unit vector + * args : double *rs I satellilte position (ecef at transmission) (m) + * double *rr I receiver position (ecef at reception) (m) + * double *e O line-of-sight vector (ecef) + * return : geometric distance (m) (0>:error/no satellite position) + * notes : distance includes sagnac effect correction + *-----------------------------------------------------------------------------*/ double geodist(const double *rs, const double *rr, double *e) { double r; int i; - - if (norm(rs,3)-RE_WGS84) { - ecef2enu(pos,e,enu); - az=dot(enu,enu,2)<1E-12?0.0:atan2(enu[0],enu[1]); - if (az<0.0) az+=2*PI; - el=asin(enu[2]); - } - if (azel) {azel[0]=az; azel[1]=el;} + double az = 0.0, el = PI/2.0, enu[3]; + + if (pos[2]>-RE_WGS84) + { + ecef2enu(pos, e, enu); + az = dot(enu, enu, 2)<1e-12 ? 0.0 : atan2(enu[0], enu[1]); + if (az<0.0) az += 2*PI; + el = asin(enu[2]); + } + if (azel) {azel[0] = az; azel[1] = el;} return el; } + + /* compute dops ---------------------------------------------------------------- -* compute DOP (dilution of precision) -* args : int ns I number of satellites -* double *azel I satellite azimuth/elevation angle (rad) -* double elmin I elevation cutoff angle (rad) -* double *dop O DOPs {GDOP,PDOP,HDOP,VDOP} -* return : none -* notes : dop[0]-[3] return 0 in case of dop computation error -*-----------------------------------------------------------------------------*/ - - + * compute DOP (dilution of precision) + * args : int ns I number of satellites + * double *azel I satellite azimuth/elevation angle (rad) + * double elmin I elevation cutoff angle (rad) + * double *dop O DOPs {GDOP,PDOP,HDOP,VDOP} + * return : none + * notes : dop[0]-[3] return 0 in case of dop computation error + *-----------------------------------------------------------------------------*/ void dops(int ns, const double *azel, double elmin, double *dop) { - double H[4*MAXSAT],Q[16],cosel,sinel; - int i,n; - - for (i=0;i<4;i++) dop[i]=0.0; - for (i=n=0;i 0.416) phi= 0.416; - else if (phi<-0.416) phi=-0.416; - lam=pos[1]/PI+psi*sin(azel[0])/cos(phi*PI); - + phi = pos[0]/PI+psi*cos(azel[0]); + if (phi> 0.416) phi = 0.416; + else if (phi<-0.416) phi = -0.416; + lam = pos[1]/PI+psi*sin(azel[0])/cos(phi*PI); + /* geomagnetic latitude (semi-circle) */ - phi+=0.064*cos((lam-1.617)*PI); - + phi += 0.064*cos((lam-1.617)*PI); + /* local time (s) */ - tt=43200.0*lam+time2gpst(t,&week); - tt-=floor(tt/86400.0)*86400.0; /* 0<=tt<86400 */ - + tt = 43200.0*lam+time2gpst(t, &week); + tt -= floor(tt/86400.0)*86400.0; /* 0 <= tt<86400 */ + /* slant factor */ - f=1.0+16.0*pow(0.53-azel[1]/PI,3.0); - + f = 1.0+16.0*pow(0.53-azel[1]/PI, 3.0); + /* ionospheric delay */ - amp=ion[0]+phi*(ion[1]+phi*(ion[2]+phi*ion[3])); - per=ion[4]+phi*(ion[5]+phi*(ion[6]+phi*ion[7])); - amp=amp< 0.0? 0.0:amp; - per=per<72000.0?72000.0:per; - x=2.0*PI*(tt-50400.0)/per; - - return CLIGHT*f*(fabs(x)<1.57?5E-9+amp*(1.0+x*x*(-0.5+x*x/24.0)):5E-9); + amp = ion[0]+phi*(ion[1]+phi*(ion[2]+phi*ion[3])); + per = ion[4]+phi*(ion[5]+phi*(ion[6]+phi*ion[7])); + amp = amp< 0.0 ? 0.0 : amp; + per = per<72000.0 ? 72000.0 : per; + x = 2.0*PI*(tt-50400.0)/per; + + return SPEED_OF_LIGHT*f*(fabs(x)<1.57 ? 5E-9+amp*(1.0+x*x*(-0.5+x*x/24.0)) : 5E-9); } + + /* ionosphere mapping function ------------------------------------------------- -* compute ionospheric delay mapping function by single layer model -* args : double *pos I receiver position {lat,lon,h} (rad,m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* return : ionospheric mapping function -*-----------------------------------------------------------------------------*/ + * compute ionospheric delay mapping function by single layer model + * args : double *pos I receiver position {lat,lon,h} (rad,m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * return : ionospheric mapping function + *-----------------------------------------------------------------------------*/ double ionmapf(const double *pos, const double *azel) { - if (pos[2]>=HION) return 1.0; + if (pos[2] >= HION) return 1.0; return 1.0/cos(asin((RE_WGS84+pos[2])/(RE_WGS84+HION)*sin(PI/2.0-azel[1]))); } + + /* ionospheric pierce point position ------------------------------------------- -* compute ionospheric pierce point (ipp) position and slant factor -* args : double *pos I receiver position {lat,lon,h} (rad,m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* double re I earth radius (km) -* double hion I altitude of ionosphere (km) -* double *posp O pierce point position {lat,lon,h} (rad,m) -* return : slant factor -* notes : see ref [2], only valid on the earth surface -* fixing bug on ref [2] A.4.4.10.1 A-22,23 -*-----------------------------------------------------------------------------*/ + * compute ionospheric pierce point (ipp) position and slant factor + * args : double *pos I receiver position {lat,lon,h} (rad,m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * double re I earth radius (km) + * double hion I altitude of ionosphere (km) + * double *posp O pierce point position {lat,lon,h} (rad,m) + * return : slant factor + * notes : see ref [2], only valid on the earth surface + * fixing bug on ref [2] A.4.4.10.1 A-22,23 + *-----------------------------------------------------------------------------*/ double ionppp(const double *pos, const double *azel, double re, - double hion, double *posp) + double hion, double *posp) { - double cosaz,rp,ap,sinap,tanap; - - rp=re/(re+hion)*cos(azel[1]); - ap=PI/2.0-azel[1]-asin(rp); - sinap=sin(ap); - tanap=tan(ap); - cosaz=cos(azel[0]); - posp[0]=asin(sin(pos[0])*cos(ap)+cos(pos[0])*sinap*cosaz); - - if ((pos[0]> 70.0*D2R&& tanap*cosaz>tan(PI/2.0-pos[0]))|| - (pos[0]<-70.0*D2R&&-tanap*cosaz>tan(PI/2.0+pos[0]))) { - posp[1]=pos[1]+PI-asin(sinap*sin(azel[0])/cos(posp[0])); - } - else { - posp[1]=pos[1]+asin(sinap*sin(azel[0])/cos(posp[0])); - } + double cosaz, rp, ap, sinap, tanap; + + rp = re/(re+hion)*cos(azel[1]); + ap = PI/2.0-azel[1]-asin(rp); + sinap = sin(ap); + tanap = tan(ap); + cosaz = cos(azel[0]); + posp[0] = asin(sin(pos[0])*cos(ap)+cos(pos[0])*sinap*cosaz); + + if ((pos[0]> 70.0*D2R && tanap*cosaz>tan(PI/2.0-pos[0])) || + (pos[0]<-70.0*D2R && -tanap*cosaz>tan(PI/2.0+pos[0]))) + { + posp[1] = pos[1]+PI-asin(sinap*sin(azel[0])/cos(posp[0])); + } + else + { + posp[1] = pos[1]+asin(sinap*sin(azel[0])/cos(posp[0])); + } return 1.0/sqrt(1.0-rp*rp); } + + /* troposphere model ----------------------------------------------------------- -* compute tropospheric delay by standard atmosphere and saastamoinen model -* args : gtime_t time I time -* double *pos I receiver position {lat,lon,h} (rad,m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* double humi I relative humidity -* return : tropospheric delay (m) -*-----------------------------------------------------------------------------*/ -double tropmodel(gtime_t time, const double *pos, const double *azel, - double humi) + * compute tropospheric delay by standard atmosphere and saastamoinen model + * args : gtime_t time I time + * double *pos I receiver position {lat,lon,h} (rad,m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * double humi I relative humidity + * return : tropospheric delay (m) + *-----------------------------------------------------------------------------*/ +double tropmodel(gtime_t time __attribute__((unused)), const double *pos, const double *azel, + double humi) { - const double temp0=15.0; /* temparature at sea level */ - double hgt,pres,temp,e,z,trph,trpw; - - if (pos[2]<-100.0||1E44) return coef[4]; return coef[i-1]*(1.0-lat/15.0+i)+coef[i]*(lat/15.0-i); } + + double mapf(double el, double a, double b, double c) { - double sinel=sin(el); + double sinel = sin(el); return (1.0+a/(1.0+b/(1.0+c)))/(sinel+(a/(sinel+b/(sinel+c)))); } + + double nmf(gtime_t time, const double pos[], const double azel[], - double *mapfw) + double *mapfw) { /* ref [5] table 3 */ /* hydro-ave-a,b,c, hydro-amp-a,b,c, wet-a,b,c at latitude 15,30,45,60,75 */ - const double coef[][5]={ - { 1.2769934E-3, 1.2683230E-3, 1.2465397E-3, 1.2196049E-3, 1.2045996E-3}, - { 2.9153695E-3, 2.9152299E-3, 2.9288445E-3, 2.9022565E-3, 2.9024912E-3}, - { 62.610505E-3, 62.837393E-3, 63.721774E-3, 63.824265E-3, 64.258455E-3}, - - { 0.0000000E-0, 1.2709626E-5, 2.6523662E-5, 3.4000452E-5, 4.1202191E-5}, - { 0.0000000E-0, 2.1414979E-5, 3.0160779E-5, 7.2562722E-5, 11.723375E-5}, - { 0.0000000E-0, 9.0128400E-5, 4.3497037E-5, 84.795348E-5, 170.37206E-5}, - - { 5.8021897E-4, 5.6794847E-4, 5.8118019E-4, 5.9727542E-4, 6.1641693E-4}, - { 1.4275268E-3, 1.5138625E-3, 1.4572752E-3, 1.5007428E-3, 1.7599082E-3}, - { 4.3472961E-2, 4.6729510E-2, 4.3908931E-2, 4.4626982E-2, 5.4736038E-2} + const double coef[][5] = { + { 1.2769934E-3, 1.2683230E-3, 1.2465397E-3, 1.2196049E-3, 1.2045996E-3}, + { 2.9153695E-3, 2.9152299E-3, 2.9288445E-3, 2.9022565E-3, 2.9024912E-3}, + { 62.610505E-3, 62.837393E-3, 63.721774E-3, 63.824265E-3, 64.258455E-3}, + + { 0.0000000E-0, 1.2709626E-5, 2.6523662E-5, 3.4000452E-5, 4.1202191e-5}, + { 0.0000000E-0, 2.1414979E-5, 3.0160779E-5, 7.2562722E-5, 11.723375E-5}, + { 0.0000000E-0, 9.0128400E-5, 4.3497037E-5, 84.795348E-5, 170.37206E-5}, + + { 5.8021897E-4, 5.6794847E-4, 5.8118019E-4, 5.9727542E-4, 6.1641693E-4}, + { 1.4275268E-3, 1.5138625E-3, 1.4572752E-3, 1.5007428E-3, 1.7599082E-3}, + { 4.3472961e-2, 4.6729510E-2, 4.3908931e-2, 4.4626982E-2, 5.4736038E-2} }; - const double aht[]={ 2.53E-5, 5.49E-3, 1.14E-3}; /* height correction */ - - double y,cosy,ah[3],aw[3],dm,el=azel[1],lat=pos[0]*R2D,hgt=pos[2]; + const double aht[] = { 2.53E-5, 5.49E-3, 1.14E-3}; /* height correction */ + + double y, cosy, ah[3], aw[3], dm, el = azel[1], lat = pos[0]*R2D, hgt = pos[2]; int i; - - if (el<=0.0) { - if (mapfw) *mapfw=0.0; - return 0.0; - } + + if (el <= 0.0) + { + if (mapfw) *mapfw = 0.0; + return 0.0; + } /* year from doy 28, added half a year for southern latitudes */ - y=(time2doy(time)-28.0)/365.25+(lat<0.0?0.5:0.0); - - cosy=cos(2.0*PI*y); - lat=fabs(lat); - - for (i=0;i<3;i++) { - ah[i]=interpc(coef[i ],lat)-interpc(coef[i+3],lat)*cosy; - aw[i]=interpc(coef[i+6],lat); - } + y = (time2doy(time)-28.0)/365.25+(lat<0.0 ? 0.5 : 0.0); + + cosy = cos(2.0*PI*y); + lat = fabs(lat); + + for (i = 0; i<3; i++) + { + ah[i] = interpc(coef[i ], lat)-interpc(coef[i+3], lat)*cosy; + aw[i] = interpc(coef[i+6], lat); + } /* ellipsoidal height is used instead of height above sea level */ - dm=(1.0/sin(el)-mapf(el,aht[0],aht[1],aht[2]))*hgt/1E3; - - if (mapfw) *mapfw=mapf(el,aw[0],aw[1],aw[2]); - - return mapf(el,ah[0],ah[1],ah[2])+dm; + dm = (1.0/sin(el)-mapf(el, aht[0], aht[1], aht[2]))*hgt/1e3; + + if (mapfw) *mapfw = mapf(el, aw[0], aw[1], aw[2]); + + return mapf(el, ah[0], ah[1], ah[2])+dm; } #endif /* !IERS_MODEL */ + /* troposphere mapping function ------------------------------------------------ -* compute tropospheric mapping function by NMF -* args : gtime_t t I time -* double *pos I receiver position {lat,lon,h} (rad,m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* double *mapfw IO wet mapping function (NULL: not output) -* return : dry mapping function -* note : see ref [5] (NMF) and [9] (GMF) -* original JGR paper of [5] has bugs in eq.(4) and (5). the corrected -* paper is obtained from: -* ftp://web.haystack.edu/pub/aen/nmf/NMF_JGR.pdf -*-----------------------------------------------------------------------------*/ + * compute tropospheric mapping function by NMF + * args : gtime_t t I time + * double *pos I receiver position {lat,lon,h} (rad,m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * double *mapfw IO wet mapping function (NULL: not output) + * return : dry mapping function + * note : see ref [5] (NMF) and [9] (GMF) + * original JGR paper of [5] has bugs in eq.(4) and (5). the corrected + * paper is obtained from: + * ftp://web.haystack.edu/pub/aen/nmf/NMF_JGR.pdf + *-----------------------------------------------------------------------------*/ double tropmapf(gtime_t time, const double pos[], const double azel[], - double *mapfw) + double *mapfw) { #ifdef IERS_MODEL - const double ep[]={2000,1,1,12,0,0}; - double mjd,lat,lon,hgt,zd,gmfh,gmfw; + const double ep[] = {2000, 1, 1, 12, 0, 0}; + double mjd, lat, lon, hgt, zd, gmfh, gmfw; #endif - trace(4,"tropmapf: pos=%10.6f %11.6f %6.1f azel=%5.1f %4.1f\n", - pos[0]*R2D,pos[1]*R2D,pos[2],azel[0]*R2D,azel[1]*R2D); - - if (pos[2]<-1000.0||pos[2]>20000.0) { - if (mapfw) *mapfw=0.0; - return 0.0; - } + trace(4, "tropmapf: pos=%10.6f %11.6f %6.1f azel=%5.1f %4.1f\n", + pos[0]*R2D, pos[1]*R2D, pos[2], azel[0]*R2D, azel[1]*R2D); + + if (pos[2]<-1000.0 || pos[2]>20000.0) + { + if (mapfw) *mapfw = 0.0; + return 0.0; + } #ifdef IERS_MODEL - mjd=51544.5+(timediff(time,epoch2time(ep)))/86400.0; - lat=pos[0]; - lon=pos[1]; - hgt=pos[2]-geoidh(pos); /* height in m (mean sea level) */ - zd =PI/2.0-azel[1]; - + mjd = 51544.5+(timediff(time, epoch2time(ep)))/86400.0; + lat = pos[0]; + lon = pos[1]; + hgt = pos[2]-geoidh(pos); /* height in m (mean sea level) */ + zd = PI/2.0-azel[1]; + /* call GMF */ - gmf_(&mjd,&lat,&lon,&hgt,&zd,&gmfh,&gmfw); - - if (mapfw) *mapfw=gmfw; + gmf_(&mjd, &lat, &lon, &hgt, &zd, &gmfh, &gmfw); + + if (mapfw) *mapfw = gmfw; return gmfh; #else - return nmf(time,pos,azel,mapfw); /* NMF */ + return nmf(time, pos, azel, mapfw); /* NMF */ #endif } + + /* interpolate antenna phase center variation --------------------------------*/ double interpvar(double ang, const double *var) { - double a=ang/5.0; /* ang=0-90 */ - int i=(int)a; - if (i<0) return var[0]; else if (i>=18) return var[18]; + double a = ang/5.0; /* ang=0-90 */ + int i = (int)a; + if (i<0) return var[0]; else if (i >= 18) return var[18]; return var[i]*(1.0-a+i)+var[i+1]*(a-i); } + + /* receiver antenna model ------------------------------------------------------ -* compute antenna offset by antenna phase center parameters -* args : pcv_t *pcv I antenna phase center parameters -* double *azel I azimuth/elevation for receiver {az,el} (rad) -* int opt I option (0:only offset,1:offset+pcv) -* double *dant O range offsets for each frequency (m) -* return : none -* notes : current version does not support azimuth dependent terms -*-----------------------------------------------------------------------------*/ + * compute antenna offset by antenna phase center parameters + * args : pcv_t *pcv I antenna phase center parameters + * double *azel I azimuth/elevation for receiver {az,el} (rad) + * int opt I option (0:only offset,1:offset+pcv) + * double *dant O range offsets for each frequency (m) + * return : none + * notes : current version does not support azimuth dependent terms + *-----------------------------------------------------------------------------*/ void antmodel(const pcv_t *pcv, const double *del, const double *azel, - int opt, double *dant) + int opt, double *dant) { - double e[3],off[3],cosel=cos(azel[1]); - int i,j; - - trace(4,"antmodel: azel=%6.1f %4.1f opt=%d\n",azel[0]*R2D,azel[1]*R2D,opt); - - e[0]=sin(azel[0])*cosel; - e[1]=cos(azel[0])*cosel; - e[2]=sin(azel[1]); - - for (i=0;ioff[i][j]+del[j]; - - dant[i]=-dot(off,e,3)+(opt?interpvar(90.0-azel[1]*R2D,pcv->var[i]):0.0); - } - trace(5,"antmodel: dant=%6.3f %6.3f\n",dant[0],dant[1]); + double e[3], off[3], cosel = cos(azel[1]); + int i, j; + + trace(4, "antmodel: azel=%6.1f %4.1f opt=%d\n", azel[0]*R2D, azel[1]*R2D, opt); + + e[0] = sin(azel[0])*cosel; + e[1] = cos(azel[0])*cosel; + e[2] = sin(azel[1]); + + for (i = 0;ioff[i][j]+del[j]; + + dant[i] = -dot(off, e, 3)+(opt ? interpvar(90.0-azel[1]*R2D, pcv->var[i]) : 0.0); + } + trace(5, "antmodel: dant=%6.3f %6.3f\n", dant[0], dant[1]); } + + /* satellite antenna model ------------------------------------------------------ -* compute satellite antenna phase center parameters -* args : pcv_t *pcv I antenna phase center parameters -* double nadir I nadir angle for satellite (rad) -* double *dant O range offsets for each frequency (m) -* return : none -*-----------------------------------------------------------------------------*/ + * compute satellite antenna phase center parameters + * args : pcv_t *pcv I antenna phase center parameters + * double nadir I nadir angle for satellite (rad) + * double *dant O range offsets for each frequency (m) + * return : none + *-----------------------------------------------------------------------------*/ void antmodel_s(const pcv_t *pcv, double nadir, double *dant) { int i; - - trace(4,"antmodel_s: nadir=%6.1f\n",nadir*R2D); - - for (i=0;ivar[i]); - } - trace(5,"antmodel_s: dant=%6.3f %6.3f\n",dant[0],dant[1]); + + trace(4, "antmodel_s: nadir=%6.1f\n", nadir*R2D); + + for (i = 0; ivar[i]); + } + trace(5, "antmodel_s: dant=%6.3f %6.3f\n", dant[0], dant[1]); } + + /* sun and moon position in eci (ref [4] 5.1.1, 5.2.1) -----------------------*/ void sunmoonpos_eci(gtime_t tut, double *rsun, double *rmoon) { - const double ep2000[]={2000,1,1,12,0,0}; - double t,f[5],eps,Ms,ls,rs,lm,pm,rm,sine,cose,sinp,cosp,sinl,cosl; - - trace(4,"sunmoonpos_eci: tut=%s\n",time_str(tut,3)); - - t=timediff(tut,epoch2time(ep2000))/86400.0/36525.0; - + const double ep2000[] = {2000, 1, 1, 12, 0, 0}; + double t, f[5], eps, Ms, ls, rs, lm, pm, rm, sine, cose, sinp, cosp, sinl, cosl; + + trace(4, "sunmoonpos_eci: tut=%s\n", time_str(tut, 3)); + + t = timediff(tut, epoch2time(ep2000))/86400.0/36525.0; + /* astronomical arguments */ - ast_args(t,f); - + ast_args(t, f); + /* obliquity of the ecliptic */ - eps=23.439291-0.0130042*t; - sine=sin(eps*D2R); cose=cos(eps*D2R); - + eps = 23.439291-0.0130042*t; + sine = sin(eps*D2R); cose = cos(eps*D2R); + /* sun position in eci */ - if (rsun) { - Ms=357.5277233+35999.05034*t; - ls=280.460+36000.770*t+1.914666471*sin(Ms*D2R)+0.019994643*sin(2.0*Ms*D2R); - rs=AU*(1.000140612-0.016708617*cos(Ms*D2R)-0.000139589*cos(2.0*Ms*D2R)); - sinl=sin(ls*D2R); cosl=cos(ls*D2R); - rsun[0]=rs*cosl; - rsun[1]=rs*cose*sinl; - rsun[2]=rs*sine*sinl; - - trace(5,"rsun =%.3f %.3f %.3f\n",rsun[0],rsun[1],rsun[2]); - } + if (rsun) + { + Ms = 357.5277233+35999.05034*t; + ls = 280.460+36000.770*t+1.914666471*sin(Ms*D2R)+0.019994643*sin(2.0*Ms*D2R); + rs = AU*(1.000140612-0.016708617*cos(Ms*D2R)-0.000139589*cos(2.0*Ms*D2R)); + sinl = sin(ls*D2R); cosl = cos(ls*D2R); + rsun[0] = rs*cosl; + rsun[1] = rs*cose*sinl; + rsun[2] = rs*sine*sinl; + + trace(5, "rsun =%.3f %.3f %.3f\n", rsun[0], rsun[1], rsun[2]); + } /* moon position in eci */ - if (rmoon) { - lm=218.32+481267.883*t+6.29*sin(f[0])-1.27*sin(f[0]-2.0*f[3])+ - 0.66*sin(2.0*f[3])+0.21*sin(2.0*f[0])-0.19*sin(f[1])-0.11*sin(2.0*f[2]); - pm=5.13*sin(f[2])+0.28*sin(f[0]+f[2])-0.28*sin(f[2]-f[0])- - 0.17*sin(f[2]-2.0*f[3]); - rm=RE_WGS84/sin((0.9508+0.0518*cos(f[0])+0.0095*cos(f[0]-2.0*f[3])+ - 0.0078*cos(2.0*f[3])+0.0028*cos(2.0*f[0]))*D2R); - sinl=sin(lm*D2R); cosl=cos(lm*D2R); - sinp=sin(pm*D2R); cosp=cos(pm*D2R); - rmoon[0]=rm*cosp*cosl; - rmoon[1]=rm*(cose*cosp*sinl-sine*sinp); - rmoon[2]=rm*(sine*cosp*sinl+cose*sinp); - - trace(5,"rmoon=%.3f %.3f %.3f\n",rmoon[0],rmoon[1],rmoon[2]); - } + if (rmoon) + { + lm = 218.32+481267.883*t+6.29*sin(f[0])-1.27*sin(f[0]-2.0*f[3])+ + 0.66*sin(2.0*f[3])+0.21*sin(2.0*f[0])-0.19*sin(f[1])-0.11*sin(2.0*f[2]); + pm = 5.13*sin(f[2])+0.28*sin(f[0]+f[2])-0.28*sin(f[2]-f[0])- + 0.17*sin(f[2]-2.0*f[3]); + rm = RE_WGS84/sin((0.9508+0.0518*cos(f[0])+0.0095*cos(f[0]-2.0*f[3])+ + 0.0078*cos(2.0*f[3])+0.0028*cos(2.0*f[0]))*D2R); + sinl = sin(lm*D2R); cosl = cos(lm*D2R); + sinp = sin(pm*D2R); cosp = cos(pm*D2R); + rmoon[0] = rm*cosp*cosl; + rmoon[1] = rm*(cose*cosp*sinl-sine*sinp); + rmoon[2] = rm*(sine*cosp*sinl+cose*sinp); + + trace(5, "rmoon=%.3f %.3f %.3f\n", rmoon[0], rmoon[1], rmoon[2]); + } } + + /* sun and moon position ------------------------------------------------------- -* get sun and moon position in ecef -* args : gtime_t tut I time in ut1 -* double *erpv I erp value {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) -* double *rsun IO sun position in ecef (m) (NULL: not output) -* double *rmoon IO moon position in ecef (m) (NULL: not output) -* double *gmst O gmst (rad) -* return : none -*-----------------------------------------------------------------------------*/ + * get sun and moon position in ecef + * args : gtime_t tut I time in ut1 + * double *erpv I erp value {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) + * double *rsun IO sun position in ecef (m) (NULL: not output) + * double *rmoon IO moon position in ecef (m) (NULL: not output) + * double *gmst O gmst (rad) + * return : none + *-----------------------------------------------------------------------------*/ void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun, - double *rmoon, double *gmst) + double *rmoon, double *gmst) { gtime_t tut; - double rs[3],rm[3],U[9],gmst_; - - trace(4,"sunmoonpos: tutc=%s\n",time_str(tutc,3)); - - tut=timeadd(tutc,erpv[2]); /* utc -> ut1 */ - + double rs[3], rm[3], U[9], gmst_; + + trace(4, "sunmoonpos: tutc=%s\n", time_str(tutc, 3)); + + tut = timeadd(tutc, erpv[2]); /* utc -> ut1 */ + /* sun and moon position in eci */ - sunmoonpos_eci(tut,rsun?rs:NULL,rmoon?rm:NULL); - + sunmoonpos_eci(tut, rsun ? rs : NULL, rmoon ? rm : NULL); + /* eci to ecef transformation matrix */ - eci2ecef(tutc,erpv,U,&gmst_); - + eci2ecef(tutc, erpv, U, &gmst_); + /* sun and moon postion in ecef */ - if (rsun ) matmul("NN",3,1,3,1.0,U,rs,0.0,rsun ); - if (rmoon) matmul("NN",3,1,3,1.0,U,rm,0.0,rmoon); - if (gmst ) *gmst=gmst_; + if (rsun ) matmul("NN", 3, 1, 3, 1.0, U, rs, 0.0, rsun ); + if (rmoon) matmul("NN", 3, 1, 3, 1.0, U, rm, 0.0, rmoon); + if (gmst ) *gmst = gmst_; } + + /* carrier smoothing ----------------------------------------------------------- -* carrier smoothing by Hatch filter -* args : obs_t *obs IO raw observation data/smoothed observation data -* int ns I smoothing window size (epochs) -* return : none -*-----------------------------------------------------------------------------*/ + * carrier smoothing by Hatch filter + * args : obs_t *obs IO raw observation data/smoothed observation data + * int ns I smoothing window size (epochs) + * return : none + *-----------------------------------------------------------------------------*/ void csmooth(obs_t *obs, int ns) { - double Ps[2][MAXSAT][NFREQ]={},Lp[2][MAXSAT][NFREQ]={},dcp; - int i,j,s,r,n[2][MAXSAT][NFREQ]={}; + double Ps[2][MAXSAT][NFREQ] = {}, Lp[2][MAXSAT][NFREQ] = {}, dcp; + int i, j, s, r, n[2][MAXSAT][NFREQ] = {}; obsd_t *p; - - trace(3,"csmooth: nobs=%d,ns=%d\n",obs->n,ns); - - for (i=0;in;i++) { - p=&obs->data[i]; s=p->sat; r=p->rcv; - for (j=0;jP[j]==0.0||p->L[j]==0.0) continue; - if (p->LLI[j]) n[r-1][s-1][j]=0; - if (n[r-1][s-1][j]==0) Ps[r-1][s-1][j]=p->P[j]; - else { - dcp=lam_carr[j]*(p->L[j]-Lp[r-1][s-1][j]); - Ps[r-1][s-1][j]=p->P[j]/ns+(Ps[r-1][s-1][j]+dcp)*(ns-1)/ns; - } - if (++n[r-1][s-1][j]P[j]=0.0; else p->P[j]=Ps[r-1][s-1][j]; - Lp[r-1][s-1][j]=p->L[j]; + + trace(3, "csmooth: nobs=%d,ns=%d\n", obs->n, ns); + + for (i = 0; in; i++) + { + p = &obs->data[i]; s = p->sat; r = p->rcv; + for (j = 0; jP[j] == 0.0 || p->L[j] == 0.0) continue; + if (p->LLI[j]) n[r-1][s-1][j] = 0; + if (n[r-1][s-1][j] == 0) Ps[r-1][s-1][j] = p->P[j]; + else + { + dcp = lam_carr[j]*(p->L[j]-Lp[r-1][s-1][j]); + Ps[r-1][s-1][j] = p->P[j]/ns+(Ps[r-1][s-1][j]+dcp)*(ns-1)/ns; + } + if (++n[r-1][s-1][j]P[j] = 0.0; else p->P[j] = Ps[r-1][s-1][j]; + Lp[r-1][s-1][j] = p->L[j]; + } } - } } + + /* uncompress file ------------------------------------------------------------- -* uncompress (uncompress/unzip/uncompact hatanaka-compression/tar) file -* args : char *file I input file -* char *uncfile O uncompressed file -* return : status (-1:error,0:not compressed file,1:uncompress completed) -* note : creates uncompressed file in tempolary directory -* gzip and crx2rnx commands have to be installed in commands path -*-----------------------------------------------------------------------------*/ + * uncompress (uncompress/unzip/uncompact hatanaka-compression/tar) file + * args : char *file I input file + * char *uncfile O uncompressed file + * return : status (-1:error,0:not compressed file,1:uncompress completed) + * note : creates uncompressed file in tempolary directory + * gzip and crx2rnx commands have to be installed in commands path + *-----------------------------------------------------------------------------*/ int rtk_uncompress(const char *file, char *uncfile) { - int stat=0; - char *p,cmd[2048]="",tmpfile[1024]="",buff[1024],*fname,*dir=(char*)""; - - trace(3,"rtk_uncompress: file=%s\n",file); - - strcpy(tmpfile,file); - if (!(p=strrchr(tmpfile,'.'))) return 0; - + int stat = 0; + char *p, cmd[2048] = "", tmpfile[1024] = "", buff[1024], *fname, *dir = (char*)""; + + trace(3, "rtk_uncompress: file=%s\n", file); + + strcpy(tmpfile, file); + if (!(p = strrchr(tmpfile, '.'))) return 0; + /* uncompress by gzip */ - if (!strcmp(p,".z" )||!strcmp(p,".Z" )|| - !strcmp(p,".gz" )||!strcmp(p,".GZ" )|| - !strcmp(p,".zip")||!strcmp(p,".ZIP")) { - - strcpy(uncfile,tmpfile); uncfile[p-tmpfile]='\0'; - sprintf(cmd,"gzip -f -d -c \"%s\" > \"%s\"",tmpfile,uncfile); - - if (execcmd(cmd)) { - remove(uncfile); - return -1; + if (!strcmp(p, ".z" ) || !strcmp(p, ".Z" ) || + !strcmp(p, ".gz" ) || !strcmp(p, ".GZ" ) || + !strcmp(p, ".zip") || !strcmp(p, ".ZIP")) + { + strcpy(uncfile, tmpfile); uncfile[p-tmpfile] = '\0'; + sprintf(cmd, "gzip -f -d -c \"%s\" > \"%s\"", tmpfile, uncfile); + + if (execcmd(cmd)) + { + remove(uncfile); + return -1; + } + strcpy(tmpfile, uncfile); + stat = 1; } - strcpy(tmpfile,uncfile); - stat=1; - } /* extract tar file */ - if ((p=strrchr(tmpfile,'.'))&&!strcmp(p,".tar")) { - - strcpy(uncfile,tmpfile); uncfile[p-tmpfile]='\0'; - strcpy(buff,tmpfile); - fname=buff; - if ((p=strrchr(buff,'/'))) { - *p='\0'; dir=fname; fname=p+1; - } - sprintf(cmd,"tar -C \"%s\" -xf \"%s\"",dir,tmpfile); - if (execcmd(cmd)) { + if ((p = strrchr(tmpfile, '.')) && !strcmp(p, ".tar")) + { + strcpy(uncfile, tmpfile); uncfile[p-tmpfile] = '\0'; + strcpy(buff, tmpfile); + fname = buff; + if ((p = strrchr(buff, '/'))) + { + *p = '\0'; dir = fname; fname = p+1; + } + sprintf(cmd, "tar -C \"%s\" -xf \"%s\"", dir, tmpfile); + if (execcmd(cmd)) + { + if (stat) remove(tmpfile); + return -1; + } if (stat) remove(tmpfile); - return -1; + stat = 1; } - if (stat) remove(tmpfile); - stat=1; - } /* extract hatanaka-compressed file by cnx2rnx */ - else if ((p=strrchr(tmpfile,'.'))&&strlen(p)>3&&(*(p+3)=='d'||*(p+3)=='D')) { - - strcpy(uncfile,tmpfile); - uncfile[p-tmpfile+3]=*(p+3)=='D'?'O':'o'; - sprintf(cmd,"crx2rnx < \"%s\" > \"%s\"",tmpfile,uncfile); - - if (execcmd(cmd)) { - remove(uncfile); + else if ((p = strrchr(tmpfile, '.')) && strlen(p)>3 && (*(p+3) == 'd' || *(p+3) == 'D')) + { + + strcpy(uncfile, tmpfile); + uncfile[p-tmpfile+3] = *(p+3) == 'D' ? 'O' : 'o'; + sprintf(cmd, "crx2rnx < \"%s\" > \"%s\"", tmpfile, uncfile); + + if (execcmd(cmd)) + { + remove(uncfile); + if (stat) remove(tmpfile); + return -1; + } if (stat) remove(tmpfile); - return -1; + stat = 1; } - if (stat) remove(tmpfile); - stat=1; - } - trace(3,"rtk_uncompress: stat=%d\n",stat); + trace(3, "rtk_uncompress: stat=%d\n", stat); return stat; } + /* expand file path ------------------------------------------------------------ -* expand file path with wild-card (*) in file -* args : char *path I file path to expand (captal insensitive) -* char *paths O expanded file paths -* int nmax I max number of expanded file paths -* return : number of expanded file paths -* notes : the order of expanded files is alphabetical order -*-----------------------------------------------------------------------------*/ + * expand file path with wild-card (*) in file + * args : char *path I file path to expand (captal insensitive) + * char *paths O expanded file paths + * int nmax I max number of expanded file paths + * return : number of expanded file paths + * notes : the order of expanded files is alphabetical order + *-----------------------------------------------------------------------------*/ int expath(const char *path, char *paths[], int nmax) { - int i,j,n=0; + int i, j, n = 0; char tmp[1024]; struct dirent *d; DIR *dp; - const char *file=path; - char dir[1024]="",s1[1024],s2[1024],*p,*q,*r; + const char *file = path; + char dir[1024] = "", s1[1024], s2[1024], *p, *q, *r; - trace(3,"expath : path=%s nmax=%d\n",path,nmax); + trace(3, "expath : path=%s nmax=%d\n", path, nmax); //TODO: Fix invalid conversion from ‘const char*’ to ‘char*’ - //if ((p=strrchr(path,'/'))||(p=strrchr(path,'\\'))) { + //if ((p=strrchr(path,'/')) || (p=strrchr(path,'\\'))) { // file=p+1; strncpy(dir,path,p-path+1); dir[p-path+1]='\0'; //} - if (!(dp=opendir(*dir?dir:"."))) return 0; - while ((d=readdir(dp))) { - if (*(d->d_name)=='.') continue; - sprintf(s1,"^%s$",d->d_name); - sprintf(s2,"^%s$",file); - for (p=s1;*p;p++) *p=(char)tolower((int)*p); - for (p=s2;*p;p++) *p=(char)tolower((int)*p); + if (!(dp = opendir(*dir ? dir : "."))) return 0; + while ((d = readdir(dp))) + { + if (*(d->d_name) == '.') continue; + sprintf(s1, "^%s$", d->d_name); + sprintf(s2, "^%s$", file); + for (p = s1; *p;p++) *p = (char)tolower((int)*p); + for (p = s2; *p;p++) *p = (char)tolower((int)*p); - for (p=s1,q=strtok_r(s2,"*",&r);q;q=strtok_r(NULL,"*",&r)) { - if ((p=strstr(p,q))) p+=strlen(q); else break; + for (p = s1, q = strtok_r(s2, "*", &r); q; q = strtok_r(NULL, "*", &r)) + { + if ((p = strstr(p, q))) p += strlen(q); else break; + } + if (p && nd_name); } - if (p&&nd_name); - } closedir(dp); /* sort paths in alphabetical order */ - for (i=0;i0) { - strcpy(tmp,paths[i]); - strcpy(paths[i],paths[j]); - strcpy(paths[j],tmp); - } + for (i = 0; i0) + { + strcpy(tmp, paths[i]); + strcpy(paths[i], paths[j]); + strcpy(paths[j], tmp); + } + } } - } - for (i=0;i strtok_r() in expath() for thread-safe -* add bdsmodear in procopt_default -* 2015/03/19 1.30 fix bug on interpolation of erp values in geterp() -* add leap second insertion before 2015/07/01 00:00 -* add api read_leaps() -* 2015/05/31 1.31 delte api windupcorr() -* 2015/08/08 1.32 add compile option CPUTIME_IN_GPST -* add api add_fatal() -* support usno leapsec.dat for api read_leaps() -* 2016/01/23 1.33 enable septentrio -* 2016/02/05 1.34 support GLONASS for savenav(), loadnav() -* 2016/06/11 1.35 delete trace() in reppath() to avoid deadlock -* 2016/07/01 1.36 support IRNSS -* add leap second before 2017/1/1 00:00:00 -* 2016/07/29 1.37 rename api compress() -> rtk_uncompress() -* rename api crc16() -> rtk_crc16() -* rename api crc24q() -> rtk_crc24q() -* rename api crc32() -> rtk_crc32() -* 2016/08/20 1.38 fix type incompatibility in win64 environment -* change constant _POSIX_C_SOURCE 199309 -> 199506 -* 2016/08/21 1.39 fix bug on week overflow in time2gpst()/gpst2time() -* 2016/09/05 1.40 fix bug on invalid nav data read in readnav() -* 2016/09/17 1.41 suppress warnings -* 2016/09/19 1.42 modify api deg2dms() to consider numerical error -*-----------------------------------------------------------------------------*/ -#ifndef RTKLIB_RTKCMN_H_ -#define RTKLIB_RTKCMN_H_ + * + * options : -DLAPACK use LAPACK/BLAS + * -DMKL use Intel MKL + * -DTRACE enable debug trace + * -DWIN32 use WIN32 API + * -DNOCALLOC no use calloc for zero matrix + * -DIERS_MODEL use GMF instead of NMF + * -DDLL built for shared library + * -DCPUTIME_IN_GPST cputime operated in gpst + * + * references : + * [1] IS-GPS-200D, Navstar GPS Space Segment/Navigation User Interfaces, + * 7 March, 2006 + * [2] RTCA/DO-229C, Minimum operational performanc standards for global + * positioning system/wide area augmentation system airborne equipment, + * RTCA inc, November 28, 2001 + * [3] M.Rothacher, R.Schmid, ANTEX: The Antenna Exchange Format Version 1.4, + * 15 September, 2010 + * [4] A.Gelb ed., Applied Optimal Estimation, The M.I.T Press, 1974 + * [5] A.E.Niell, Global mapping functions for the atmosphere delay at radio + * wavelengths, Jounal of geophysical research, 1996 + * [6] W.Gurtner and L.Estey, RINEX The Receiver Independent Exchange Format + * Version 3.00, November 28, 2007 + * [7] J.Kouba, A Guide to using International GNSS Service (IGS) products, + * May 2009 + * [8] China Satellite Navigation Office, BeiDou navigation satellite system + * signal in space interface control document, open service signal B1I + * (version 1.0), Dec 2012 + * [9] J.Boehm, A.Niell, P.Tregoning and H.Shuh, Global Mapping Function + * (GMF): A new empirical mapping function base on numerical weather + * model data, Geophysical Research Letters, 33, L07304, 2006 + * [10] GLONASS/GPS/Galileo/Compass/SBAS NV08C receiver series BINR interface + * protocol specification ver.1.3, August, 2012 + * + *-----------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_RTKCMN_H_ +#define GNSS_SDR_RTKLIB_RTKCMN_H_ #include "rtklib.h" @@ -307,19 +219,19 @@ int savenav(const char *file, const nav_t *nav); void freeobs(obs_t *obs); void freenav(nav_t *nav, int opt); -void traceopen(const char *file); -void traceclose(void); -void tracelevel(int level); +//void traceopen(const char *file); +//void traceclose(void); +//void tracelevel(int level); void trace (int level, const char *format, ...); -void tracet (int level, const char *format, ...); -void tracemat(int level, const double *A, int n, int m, int p, int q); -void traceobs(int level, const obsd_t *obs, int n); -void tracenav(int level, const nav_t *nav); -void tracegnav(int level, const nav_t *nav); -void tracehnav(int level, const nav_t *nav); -void tracepeph(int level, const nav_t *nav); -void tracepclk(int level, const nav_t *nav); -void traceb (int level, const unsigned char *p, int n); +//void tracet (int level, const char *format, ...); +//void tracemat(int level, const double *A, int n, int m, int p, int q); +//void traceobs(int level, const obsd_t *obs, int n); +//void tracenav(int level, const nav_t *nav); +//void tracegnav(int level, const nav_t *nav); +//void tracehnav(int level, const nav_t *nav); +//void tracepeph(int level, const nav_t *nav); +//void tracepclk(int level, const nav_t *nav); +//void traceb (int level, const unsigned char *p, int n); int execcmd(const char *cmd); void createdir(const char *path); @@ -360,4 +272,4 @@ void csmooth(obs_t *obs, int ns); int rtk_uncompress(const char *file, char *uncfile); int expath(const char *path, char *paths[], int nmax); -#endif /* RTKLIB_RTKCMN_H_ */ +#endif /* GNSS_SDR_RTKLIB_RTKCMN_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_sbas.cc b/src/algorithms/libs/rtklib/rtklib_sbas.cc index 623bd9cf0..efebc30f8 100644 --- a/src/algorithms/libs/rtklib/rtklib_sbas.cc +++ b/src/algorithms/libs/rtklib/rtklib_sbas.cc @@ -48,863 +48,960 @@ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * -* -* option : -DRRCENA enable rrc correction -* -* references : -* [1] RTCA/DO-229C, Minimum operational performanc standards for global -* positioning system/wide area augmentation system airborne equipment, -* RTCA inc, November 28, 2001 -* [2] IS-QZSS v.1.1, Quasi-Zenith Satellite System Navigation Service -* Interface Specification for QZSS, Japan Aerospace Exploration Agency, -* July 31, 2009 -* -* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $ -* history : 2007/10/14 1.0 new -* 2009/01/24 1.1 modify sbspntpos() api -* improve fast/ion correction update -* 2009/04/08 1.2 move function crc24q() to rcvlog.c -* support glonass, galileo and qzss -* 2009/06/08 1.3 modify sbsupdatestat() -* delete sbssatpos() -* 2009/12/12 1.4 support glonass -* 2010/01/22 1.5 support ems (egnos message service) format -* 2010/06/10 1.6 added api: -* sbssatcorr(),sbstropcorr(),sbsioncorr(), -* sbsupdatecorr() -* changed api: -* sbsreadmsgt(),sbsreadmsg() -* deleted api: -* sbspntpos(),sbsupdatestat() -* 2010/08/16 1.7 not reject udre==14 or give==15 correction message -* (2.4.0_p4) -* 2011/01/15 1.8 use api ionppp() -* add prn mask of qzss for qzss L1SAIF -* 2016/07/29 1.9 crc24q() -> rtk_crc24q() -*-----------------------------------------------------------------------------*/ + * + * option : -DRRCENA enable rrc correction + * + * References : + * [1] RTCA/DO-229C, Minimum operational performanc standards for global + * positioning system/wide area augmentation system airborne equipment, + * RTCA inc, November 28, 2001 + * [2] IS-QZSS v.1.1, Quasi-Zenith Satellite System Navigation Service + * Interface Specification for QZSS, Japan Aerospace Exploration Agency, + * July 31, 2009 + * + *-----------------------------------------------------------------------------*/ #include "rtklib_sbas.h" /* extract field from line ---------------------------------------------------*/ char *getfield(char *p, int pos) { - for (pos--;pos>0;pos--,p++) if (!(p=strchr(p,','))) return NULL; + for (pos--; pos > 0 ; pos--, p++) if (!(p = strchr(p, ','))) return NULL; return p; } + + /* variance of fast correction (udre=UDRE+1) ---------------------------------*/ double varfcorr(int udre) { - const double var[14]={ - 0.052,0.0924,0.1444,0.283,0.4678,0.8315,1.2992,1.8709,2.5465,3.326, - 5.1968,20.7870,230.9661,2078.695 + const double var[14] = { + 0.052, 0.0924, 0.1444, 0.283, 0.4678, 0.8315, 1.2992, 1.8709, 2.5465, 3.326, + 5.1968, 20.7870, 230.9661, 2078.695 }; - return 0msg,13+i,1)) { - if (i<= 37) sat=satno(SYS_GPS,i); /* 0- 37: gps */ - else if (i<= 61) sat=satno(SYS_GLO,i-37); /* 38- 61: glonass */ - else if (i<=119) sat=0; /* 62-119: future gnss */ - else if (i<=138) sat=satno(SYS_SBS,i); /* 120-138: geo/waas */ - else if (i<=182) sat=0; /* 139-182: reserved */ - else if (i<=192) sat=satno(SYS_SBS,i+10); /* 183-192: qzss ref [2] */ - else if (i<=202) sat=satno(SYS_QZS,i); /* 193-202: qzss ref [2] */ - else sat=0; /* 203- : reserved */ - sbssat->sat[n++].sat=sat; + int i, n, sat; + + trace(4, "decode_sbstype1:\n"); + + for (i = 1, n = 0;i<=210 && nmsg, 13+i, 1)) + { + if (i<= 37) sat = satno(SYS_GPS, i); /* 0- 37: gps */ + else if (i<= 61) sat = satno(SYS_GLO, i-37); /* 38- 61: glonass */ + else if (i<=119) sat = 0; /* 62-119: future gnss */ + else if (i<=138) sat = satno(SYS_SBS, i); /* 120-138: geo/waas */ + else if (i<=182) sat = 0; /* 139-182: reserved */ + else if (i<=192) sat = satno(SYS_SBS, i+10); /* 183-192: qzss ref [2] */ + else if (i<=202) sat = satno(SYS_QZS, i); /* 193-202: qzss ref [2] */ + else sat = 0; /* 203- : reserved */ + sbssat->sat[n++].sat = sat; + } } - } - sbssat->iodp=getbitu(msg->msg,224,2); - sbssat->nsat=n; - - trace(5,"decode_sbstype1: nprn=%d iodp=%d\n",n,sbssat->iodp); + sbssat->iodp = getbitu(msg->msg, 224, 2); + sbssat->nsat = n; + + trace(5, "decode_sbstype1: nprn=%d iodp=%d\n", n, sbssat->iodp); return 1; } + + /* decode type 2-5,0: fast corrections ---------------------------------------*/ int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat) { - int i,j,iodf,type,udre; - double prc,dt; + int i, j, iodf, type, udre; + double prc, dt; gtime_t t0; - - trace(4,"decode_sbstype2:\n"); - - if (sbssat->iodp!=(int)getbitu(msg->msg,16,2)) return 0; - - type=getbitu(msg->msg, 8,6); - iodf=getbitu(msg->msg,14,2); - - for (i=0;i<13;i++) { - if ((j=13*((type==0?2:type)-2)+i)>=sbssat->nsat) break; - udre=getbitu(msg->msg,174+4*i,4); - t0 =sbssat->sat[j].fcorr.t0; - prc=sbssat->sat[j].fcorr.prc; - sbssat->sat[j].fcorr.t0=gpst2time(msg->week,msg->tow); - sbssat->sat[j].fcorr.prc=getbits(msg->msg,18+i*12,12)*0.125f; - sbssat->sat[j].fcorr.udre=udre+1; - dt=timediff(sbssat->sat[j].fcorr.t0,t0); - if (t0.time==0||dt<=0.0||18.0sat[j].fcorr.ai==0) { - sbssat->sat[j].fcorr.rrc=0.0; - sbssat->sat[j].fcorr.dt=0.0; + + trace(4, "decode_sbstype2:\n"); + + if (sbssat->iodp!=(int)getbitu(msg->msg, 16, 2)) return 0; + + type = getbitu(msg->msg, 8, 6); + iodf = getbitu(msg->msg, 14, 2); + + for (i = 0; i < 13; i++) + { + if ((j = 13 * ((type == 0 ? 2 : type) - 2) + i) >= sbssat->nsat) break; + udre = getbitu(msg->msg, 174+4*i, 4); + t0 = sbssat->sat[j].fcorr.t0; + prc = sbssat->sat[j].fcorr.prc; + sbssat->sat[j].fcorr.t0 = gpst2time(msg->week, msg->tow); + sbssat->sat[j].fcorr.prc = getbits(msg->msg, 18 + i * 12, 12) * 0.125f; + sbssat->sat[j].fcorr.udre = udre + 1; + dt = timediff(sbssat->sat[j].fcorr.t0, t0); + if (t0.time == 0||dt <= 0.0 || 18.0 < dt || sbssat->sat[j].fcorr.ai == 0) + { + sbssat->sat[j].fcorr.rrc = 0.0; + sbssat->sat[j].fcorr.dt = 0.0; + } + else + { + sbssat->sat[j].fcorr.rrc = (sbssat->sat[j].fcorr.prc-prc) / dt; + sbssat->sat[j].fcorr.dt = dt; + } + sbssat->sat[j].fcorr.iodf = iodf; } - else { - sbssat->sat[j].fcorr.rrc=(sbssat->sat[j].fcorr.prc-prc)/dt; - sbssat->sat[j].fcorr.dt=dt; - } - sbssat->sat[j].fcorr.iodf=iodf; - } - trace(5,"decode_sbstype2: type=%d iodf=%d\n",type,iodf); + trace(5, "decode_sbstype2: type=%d iodf=%d\n", type, iodf); return 1; } + + /* decode type 6: integrity info ---------------------------------------------*/ int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat) { - int i,iodf[4],udre; - - trace(4,"decode_sbstype6:\n"); - - for (i=0;i<4;i++) { - iodf[i]=getbitu(msg->msg,14+i*2,2); - } - for (i=0;insat&&isat[i].fcorr.iodf!=iodf[i/13]) continue; - udre=getbitu(msg->msg,22+i*4,4); - sbssat->sat[i].fcorr.udre=udre+1; - } - trace(5,"decode_sbstype6: iodf=%d %d %d %d\n",iodf[0],iodf[1],iodf[2],iodf[3]); + int i, iodf[4], udre; + + trace(4, "decode_sbstype6:\n"); + + for (i = 0;i<4;i++) + { + iodf[i] = getbitu(msg->msg, 14+i*2, 2); + } + for (i = 0;insat && isat[i].fcorr.iodf != iodf[i/13]) continue; + udre = getbitu(msg->msg, 22+i*4, 4); + sbssat->sat[i].fcorr.udre = udre+1; + } + trace(5, "decode_sbstype6: iodf=%d %d %d %d\n", iodf[0], iodf[1], iodf[2], iodf[3]); return 1; } + + /* decode type 7: fast correction degradation factor -------------------------*/ int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat) { int i; - - trace(4,"decode_sbstype7\n"); - - if (sbssat->iodp!=(int)getbitu(msg->msg,18,2)) return 0; - - sbssat->tlat=getbitu(msg->msg,14,4); - - for (i=0;insat&&isat[i].fcorr.ai=getbitu(msg->msg,22+i*4,4); - } + + trace(4, "decode_sbstype7\n"); + + if (sbssat->iodp!=(int)getbitu(msg->msg, 18, 2)) return 0; + + sbssat->tlat = getbitu(msg->msg, 14, 4); + + for (i = 0;insat && isat[i].fcorr.ai = getbitu(msg->msg, 22+i*4, 4); + } return 1; } + + /* decode type 9: geo navigation message -------------------------------------*/ int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav) { - seph_t seph={}; - int i,sat,t; - - trace(4,"decode_sbstype9:\n"); - - if (!(sat=satno(SYS_SBS,msg->prn))) { - trace(2,"invalid prn in sbas type 9: prn=%3d\n",msg->prn); - return 0; - } - t=(int)getbitu(msg->msg,22,13)*16-(int)msg->tow%86400; + seph_t seph = {}; + int i, sat, t; + + trace(4, "decode_sbstype9:\n"); + + if (!(sat = satno(SYS_SBS, msg->prn))) + { + trace(2, "invalid prn in sbas type 9: prn=%3d\n", msg->prn); + return 0; + } + t = (int)getbitu(msg->msg, 22, 13)*16-(int)msg->tow%86400; if (t<=-43200) t+=86400; else if (t> 43200) t-=86400; - seph.sat=sat; - seph.t0 =gpst2time(msg->week,msg->tow+t); - seph.tof=gpst2time(msg->week,msg->tow); - seph.sva=getbitu(msg->msg,35,4); - seph.svh=seph.sva==15?1:0; /* unhealthy if ura==15 */ - - seph.pos[0]=getbits(msg->msg, 39,30)*0.08; - seph.pos[1]=getbits(msg->msg, 69,30)*0.08; - seph.pos[2]=getbits(msg->msg, 99,25)*0.4; - seph.vel[0]=getbits(msg->msg,124,17)*0.000625; - seph.vel[1]=getbits(msg->msg,141,17)*0.000625; - seph.vel[2]=getbits(msg->msg,158,18)*0.004; - seph.acc[0]=getbits(msg->msg,176,10)*0.0000125; - seph.acc[1]=getbits(msg->msg,186,10)*0.0000125; - seph.acc[2]=getbits(msg->msg,196,10)*0.0000625; - - seph.af0=getbits(msg->msg,206,12)*P2_31; - seph.af1=getbits(msg->msg,218, 8)*P2_39/2.0; - - i=msg->prn-MINPRNSBS; - if (!nav->seph||fabs(timediff(nav->seph[i].t0,seph.t0))<1E-3) { /* not change */ - return 0; - } - nav->seph[NSATSBS+i]=nav->seph[i]; /* previous */ - nav->seph[i]=seph; /* current */ - - trace(5,"decode_sbstype9: prn=%d\n",msg->prn); + seph.sat = sat; + seph.t0 = gpst2time(msg->week, msg->tow+t); + seph.tof = gpst2time(msg->week, msg->tow); + seph.sva = getbitu(msg->msg, 35, 4); + seph.svh = seph.sva == 15 ? 1 : 0; /* unhealthy if ura==15 */ + + seph.pos[0] = getbits(msg->msg, 39, 30)*0.08; + seph.pos[1] = getbits(msg->msg, 69, 30)*0.08; + seph.pos[2] = getbits(msg->msg, 99, 25)*0.4; + seph.vel[0] = getbits(msg->msg, 124, 17)*0.000625; + seph.vel[1] = getbits(msg->msg, 141, 17)*0.000625; + seph.vel[2] = getbits(msg->msg, 158, 18)*0.004; + seph.acc[0] = getbits(msg->msg, 176, 10)*0.0000125; + seph.acc[1] = getbits(msg->msg, 186, 10)*0.0000125; + seph.acc[2] = getbits(msg->msg, 196, 10)*0.0000625; + + seph.af0 = getbits(msg->msg, 206, 12)*TWO_N31; + seph.af1 = getbits(msg->msg, 218, 8)*TWO_N39/2.0; + + i = msg->prn-MINPRNSBS; + if (!nav->seph||fabs(timediff(nav->seph[i].t0, seph.t0))<1e-3) + { /* not change */ + return 0; + } + nav->seph[NSATSBS+i] = nav->seph[i]; /* previous */ + nav->seph[i] = seph; /* current */ + + trace(5, "decode_sbstype9: prn=%d\n", msg->prn); return 1; } + + /* decode type 18: ionospheric grid point masks ------------------------------*/ int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion) { const sbsigpband_t *p; - int i,j,n,m,band=getbitu(msg->msg,18,4); - - trace(4,"decode_sbstype18:\n"); - - if (0<=band&&band<= 8) {p=igpband1[band ]; m=8;} - else if (9<=band&&band<=10) {p=igpband2[band-9]; m=5;} + int i, j, n, m, band = getbitu(msg->msg, 18, 4); + + trace(4, "decode_sbstype18:\n"); + + if (0<=band && band<= 8) {p = igpband1[band ]; m = 8;} + else if (9<=band && band<=10) {p = igpband2[band-9]; m = 5;} else return 0; - - sbsion[band].iodi=(short)getbitu(msg->msg,22,2); - - for (i=1,n=0;i<=201;i++) { - if (!getbitu(msg->msg,23+i,1)) continue; - for (j=0;jmsg, 22, 2); + + for (i = 1, n = 0; i<= 201;i++) + { + if (!getbitu(msg->msg , 23 + i, 1)) continue; + for (j = 0; j < m; j++) + { + if (i < p[j].bits || p[j].bite < i) continue; + sbsion[band].igp[n].lat = band <= 8 ? p[j].y[i - p[j].bits] : p[j].x; + sbsion[band].igp[n++].lon = band <= 8 ? p[j].x : p[j].y[i - p[j].bits]; + break; + } } - } - sbsion[band].nigp=n; - - trace(5,"decode_sbstype18: band=%d nigp=%d\n",band,n); + sbsion[band].nigp = n; + + trace(5, "decode_sbstype18: band=%d nigp=%d\n", band, n); return 1; } + + /* decode half long term correction (vel code=0) -----------------------------*/ int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat) { - int i,n=getbitu(msg->msg,p,6); - - trace(4,"decode_longcorr0:\n"); - - if (n==0||n>MAXSAT) return 0; - - sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8); - - for (i=0;i<3;i++) { - sbssat->sat[n-1].lcorr.dpos[i]=getbits(msg->msg,p+14+9*i,9)*0.125; - sbssat->sat[n-1].lcorr.dvel[i]=0.0; - } - sbssat->sat[n-1].lcorr.daf0=getbits(msg->msg,p+41,10)*P2_31; - sbssat->sat[n-1].lcorr.daf1=0.0; - sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow); - - trace(5,"decode_longcorr0:sat=%2d\n",sbssat->sat[n-1].sat); + int i, n = getbitu(msg->msg, p, 6); + + trace(4, "decode_longcorr0:\n"); + + if (n == 0||n>MAXSAT) return 0; + + sbssat->sat[n-1].lcorr.iode = getbitu(msg->msg, p+6, 8); + + for (i = 0;i<3;i++) + { + sbssat->sat[n-1].lcorr.dpos[i] = getbits(msg->msg, p+14+9*i, 9)*0.125; + sbssat->sat[n-1].lcorr.dvel[i] = 0.0; + } + sbssat->sat[n-1].lcorr.daf0 = getbits(msg->msg, p + 41, 10) * TWO_N31; + sbssat->sat[n-1].lcorr.daf1 = 0.0; + sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow); + + trace(5, "decode_longcorr0:sat=%2d\n", sbssat->sat[n-1].sat); return 1; } + + /* decode half long term correction (vel code=1) -----------------------------*/ int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat) { - int i,n=getbitu(msg->msg,p,6),t; - - trace(4,"decode_longcorr1:\n"); - - if (n==0||n>MAXSAT) return 0; - - sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8); - - for (i=0;i<3;i++) { - sbssat->sat[n-1].lcorr.dpos[i]=getbits(msg->msg,p+14+i*11,11)*0.125; - sbssat->sat[n-1].lcorr.dvel[i]=getbits(msg->msg,p+58+i* 8, 8)*P2_11; - } - sbssat->sat[n-1].lcorr.daf0=getbits(msg->msg,p+47,11)*P2_31; - sbssat->sat[n-1].lcorr.daf1=getbits(msg->msg,p+82, 8)*P2_39; - t=(int)getbitu(msg->msg,p+90,13)*16-(int)msg->tow%86400; + int i, n = getbitu(msg->msg, p, 6), t; + + trace(4, "decode_longcorr1:\n"); + + if (n == 0||n>MAXSAT) return 0; + + sbssat->sat[n-1].lcorr.iode = getbitu(msg->msg, p+6, 8); + + for (i = 0;i<3;i++) + { + sbssat->sat[n-1].lcorr.dpos[i] = getbits(msg->msg, p + 14 + i * 11, 11) * 0.125; + sbssat->sat[n-1].lcorr.dvel[i] = getbits(msg->msg, p + 58 + i * 8, 8) * TWO_N11; + } + sbssat->sat[n-1].lcorr.daf0 = getbits(msg->msg, p+47, 11) * TWO_N31; + sbssat->sat[n-1].lcorr.daf1 = getbits(msg->msg, p+82, 8) * TWO_N39; + t = (int)getbitu(msg->msg, p+90, 13)*16-(int)msg->tow%86400; if (t<=-43200) t+=86400; else if (t> 43200) t-=86400; - sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow+t); - - trace(5,"decode_longcorr1: sat=%2d\n",sbssat->sat[n-1].sat); + sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow+t); + + trace(5, "decode_longcorr1: sat=%2d\n", sbssat->sat[n-1].sat); return 1; } + + /* decode half long term correction ------------------------------------------*/ int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat) { - trace(4,"decode_longcorrh:\n"); - - if (getbitu(msg->msg,p,1)==0) { /* vel code=0 */ - if (sbssat->iodp==(int)getbitu(msg->msg,p+103,2)) { - return decode_longcorr0(msg,p+ 1,sbssat)&& - decode_longcorr0(msg,p+52,sbssat); + trace(4, "decode_longcorrh:\n"); + + if (getbitu(msg->msg, p, 1) == 0) { /* vel code=0 */ + if (sbssat->iodp == (int)getbitu(msg->msg, p+103, 2)) + { + return decode_longcorr0(msg, p+ 1, sbssat) && + decode_longcorr0(msg, p+52, sbssat); + } + } + else if (sbssat->iodp == (int)getbitu(msg->msg, p+104, 2)) + { + return decode_longcorr1(msg, p+1, sbssat); } - } - else if (sbssat->iodp==(int)getbitu(msg->msg,p+104,2)) { - return decode_longcorr1(msg,p+1,sbssat); - } return 0; } + + /* decode type 24: mixed fast/long term correction ---------------------------*/ int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat) { - int i,j,iodf,blk,udre; - - trace(4,"decode_sbstype24:\n"); - - if (sbssat->iodp!=(int)getbitu(msg->msg,110,2)) return 0; /* check IODP */ - - blk =getbitu(msg->msg,112,2); - iodf=getbitu(msg->msg,114,2); - - for (i=0;i<6;i++) { - if ((j=13*blk+i)>=sbssat->nsat) break; - udre=getbitu(msg->msg,86+4*i,4); - - sbssat->sat[j].fcorr.t0 =gpst2time(msg->week,msg->tow); - sbssat->sat[j].fcorr.prc =getbits(msg->msg,14+i*12,12)*0.125f; - sbssat->sat[j].fcorr.udre=udre+1; - sbssat->sat[j].fcorr.iodf=iodf; - } - return decode_longcorrh(msg,120,sbssat); + int i, j, iodf, blk, udre; + + trace(4, "decode_sbstype24:\n"); + + if (sbssat->iodp!=(int)getbitu(msg->msg, 110, 2)) return 0; /* check IODP */ + + blk = getbitu(msg->msg, 112, 2); + iodf = getbitu(msg->msg, 114, 2); + + for (i = 0;i<6;i++) + { + if ((j = 13*blk+i)>=sbssat->nsat) break; + udre = getbitu(msg->msg, 86+4*i, 4); + + sbssat->sat[j].fcorr.t0 = gpst2time(msg->week, msg->tow); + sbssat->sat[j].fcorr.prc = getbits(msg->msg, 14+i*12, 12)*0.125f; + sbssat->sat[j].fcorr.udre = udre+1; + sbssat->sat[j].fcorr.iodf = iodf; + } + return decode_longcorrh(msg, 120, sbssat); } + + /* decode type 25: long term satellite error correction ----------------------*/ int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat) { - trace(4,"decode_sbstype25:\n"); - - return decode_longcorrh(msg,14,sbssat)&&decode_longcorrh(msg,120,sbssat); + trace(4, "decode_sbstype25:\n"); + + return decode_longcorrh(msg, 14, sbssat) && decode_longcorrh(msg, 120, sbssat); } + + /* decode type 26: ionospheric deley corrections -----------------------------*/ int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion) { - int i,j,block,delay,give,band=getbitu(msg->msg,14,4); - - trace(4,"decode_sbstype26:\n"); - - if (band>MAXBAND||sbsion[band].iodi!=(int)getbitu(msg->msg,217,2)) return 0; - - block=getbitu(msg->msg,18,4); - - for (i=0;i<15;i++) { - if ((j=block*15+i)>=sbsion[band].nigp) continue; - give=getbitu(msg->msg,22+i*13+9,4); - - delay=getbitu(msg->msg,22+i*13,9); - sbsion[band].igp[j].t0=gpst2time(msg->week,msg->tow); - sbsion[band].igp[j].delay=delay==0x1FF?0.0f:delay*0.125f; - sbsion[band].igp[j].give=give+1; - - if (sbsion[band].igp[j].give>=16) { - sbsion[band].igp[j].give=0; + int i, j, block, delay, give, band = getbitu(msg->msg, 14, 4); + + trace(4, "decode_sbstype26:\n"); + + if (band>MAXBAND || sbsion[band].iodi != (int)getbitu(msg->msg, 217, 2)) return 0; + + block = getbitu(msg->msg, 18, 4); + + for (i = 0;i<15;i++) + { + if ((j = block*15+i) >= sbsion[band].nigp) continue; + give = getbitu(msg->msg, 22+i*13+9, 4); + + delay = getbitu(msg->msg, 22+i*13, 9); + sbsion[band].igp[j].t0 = gpst2time(msg->week, msg->tow); + sbsion[band].igp[j].delay = delay == 0x1FF ? 0.0f : delay * 0.125f; + sbsion[band].igp[j].give = give+1; + + if (sbsion[band].igp[j].give>=16) + { + sbsion[band].igp[j].give = 0; + } } - } - trace(5,"decode_sbstype26: band=%d block=%d\n",band,block); + trace(5, "decode_sbstype26: band=%d block=%d\n", band, block); return 1; } + + /* update sbas corrections ----------------------------------------------------- -* update sbas correction parameters in navigation data with a sbas message -* args : sbsmg_t *msg I sbas message -* nav_t *nav IO navigation data -* return : message type (-1: error or not supported type) -* notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t) -* seph[prn-MINPRNSBS+1] : sat prn current epehmeris -* seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris -*-----------------------------------------------------------------------------*/ + * update sbas correction parameters in navigation data with a sbas message + * args : sbsmg_t *msg I sbas message + * nav_t *nav IO navigation data + * return : message type (-1: error or not supported type) + * notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t) + * seph[prn-MINPRNSBS+1] : sat prn current epehmeris + * seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris + *-----------------------------------------------------------------------------*/ int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav) { - int type=getbitu(msg->msg,8,6),stat=-1; - - trace(3,"sbsupdatecorr: type=%d\n",type); - - if (msg->week==0) return -1; - - switch (type) { - case 0: stat=decode_sbstype2 (msg,&nav->sbssat); break; - case 1: stat=decode_sbstype1 (msg,&nav->sbssat); break; - case 2: - case 3: - case 4: - case 5: stat=decode_sbstype2 (msg,&nav->sbssat); break; - case 6: stat=decode_sbstype6 (msg,&nav->sbssat); break; - case 7: stat=decode_sbstype7 (msg,&nav->sbssat); break; - case 9: stat=decode_sbstype9 (msg,nav); break; - case 18: stat=decode_sbstype18(msg,nav ->sbsion); break; - case 24: stat=decode_sbstype24(msg,&nav->sbssat); break; - case 25: stat=decode_sbstype25(msg,&nav->sbssat); break; - case 26: stat=decode_sbstype26(msg,nav ->sbsion); break; - case 63: break; /* null message */ - - /*default: trace(2,"unsupported sbas message: type=%d\n",type); break;*/ + int type = getbitu(msg->msg, 8, 6), stat = -1; + + trace(3, "sbsupdatecorr: type=%d\n", type); + + if (msg->week == 0) return -1; + + switch (type) + { + case 0: stat = decode_sbstype2 (msg, &nav->sbssat); break; + case 1: stat = decode_sbstype1 (msg, &nav->sbssat); break; + case 2: + case 3: + case 4: + case 5: stat = decode_sbstype2 (msg, &nav->sbssat); break; + case 6: stat = decode_sbstype6 (msg, &nav->sbssat); break; + case 7: stat = decode_sbstype7 (msg, &nav->sbssat); break; + case 9: stat = decode_sbstype9 (msg, nav); break; + case 18: stat = decode_sbstype18(msg, nav ->sbsion); break; + case 24: stat = decode_sbstype24(msg, &nav->sbssat); break; + case 25: stat = decode_sbstype25(msg, &nav->sbssat); break; + case 26: stat = decode_sbstype26(msg, nav ->sbsion); break; + case 63: break; /* null message */ + + /*default: trace(2, "unsupported sbas message: type=%d\n", type); break;*/ } - return stat?type:-1; + return stat ? type : -1; } + + /* read sbas log file --------------------------------------------------------*/ void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te, - sbs_t *sbs) + sbs_t *sbs) { sbsmsg_t *sbs_msgs; - int i,week,prn,ch,msg; + int i, week, prn, ch, msg; unsigned int b; - double tow,ep[6]={}; - char buff[256],*p; + double tow, ep[6] = {}; + char buff[256], *p; gtime_t time; FILE *fp; - - trace(3,"readmsgs: file=%s sel=%d\n",file,sel); - - if (!(fp=fopen(file,"r"))) { - trace(2,"sbas message file open error: %s\n",file); - return; - } - while (fgets(buff,sizeof(buff),fp)) { - if (sscanf(buff,"%d %lf %d",&week,&tow,&prn)==3&&(p=strstr(buff,": "))) { - p+=2; /* rtklib form */ + + trace(3, "readmsgs: file=%s sel=%d\n", file, sel); + + if (!(fp = fopen(file, "r"))) + { + trace(2, "sbas message file open error: %s\n", file); + return; } - else if (sscanf(buff,"%d %lf %lf %lf %lf %lf %lf %d", - &prn,ep,ep+1,ep+2,ep+3,ep+4,ep+5,&msg)==8) { - /* ems (EGNOS Message Service) form */ - ep[0]+=ep[0]<70.0?2000.0:1900.0; - tow=time2gpst(epoch2time(ep),&week); - p=buff+(msg>=10?25:24); + while (fgets(buff, sizeof(buff), fp)) + { + if (sscanf(buff, "%d %lf %d", &week, &tow, &prn) == 3 && (p = strstr(buff, ": "))) + { + p+=2; /* rtklib form */ + } + else if (sscanf(buff, "%d %lf %lf %lf %lf %lf %lf %d", + &prn, ep, ep+1, ep+2, ep+3, ep+4, ep+5, &msg) == 8) + { + /* ems (EGNOS Message Service) form */ + ep[0] += ep[0] < 70.0 ? 2000.0 : 1900.0; + tow = time2gpst(epoch2time(ep), &week); + p = buff + (msg >= 10 ? 25 : 24); + } + else if (!strncmp(buff,"#RAWWAASFRAMEA",14)) + { /* NovAtel OEM4/V */ + if (!(p = getfield(buff,6))) continue; + if (sscanf(p,"%d,%lf", &week, &tow) < 2) continue; + if (!(p = strchr(++p, ';'))) continue; + if (sscanf(++p, "%d,%d", &ch, &prn) < 2) continue; + if (!(p = getfield(p, 4))) continue; + } + else if (!strncmp(buff,"$FRMA",5)) + { /* NovAtel OEM3 */ + if (!(p = getfield(buff,2))) continue; + if (sscanf(p, "%d,%lf,%d", &week, &tow, &prn) < 3) continue; + if (!(p = getfield(p, 6))) continue; + if (week < WEEKOFFSET) week += WEEKOFFSET; + } + else continue; + + if (sel != 0 && sel != prn) continue; + + time = gpst2time(week, tow); + + if (!screent(time, ts,te,0.0)) continue; + + if (sbs->n>=sbs->nmax) + { + sbs->nmax = sbs->nmax == 0 ? 1024 : sbs->nmax * 2; + if (!(sbs_msgs = (sbsmsg_t *)realloc(sbs->msgs, sbs->nmax * sizeof(sbsmsg_t)))) + { + trace(1, "readsbsmsg malloc error: nmax=%d\n", sbs->nmax); + free(sbs->msgs); sbs->msgs = NULL; sbs->n = sbs->nmax = 0; + return; + } + sbs->msgs = sbs_msgs; + } + sbs->msgs[sbs->n].week = week; + sbs->msgs[sbs->n].tow = (int)(tow + 0.5); + sbs->msgs[sbs->n].prn = prn; + for (i = 0; i < 29; i++) sbs->msgs[sbs->n].msg[i] = 0; + for (i = 0; *(p-1) && *p && i < 29; p += 2, i++) + { + if (sscanf(p, "%2X", &b) == 1) sbs->msgs[sbs->n].msg[i] = (unsigned char)b; + } + sbs->msgs[sbs->n++].msg[28] &= 0xC0; } - else if (!strncmp(buff,"#RAWWAASFRAMEA",14)) { /* NovAtel OEM4/V */ - if (!(p=getfield(buff,6))) continue; - if (sscanf(p,"%d,%lf",&week,&tow)<2) continue; - if (!(p=strchr(++p,';'))) continue; - if (sscanf(++p,"%d,%d",&ch,&prn)<2) continue; - if (!(p=getfield(p,4))) continue; - } - else if (!strncmp(buff,"$FRMA",5)) { /* NovAtel OEM3 */ - if (!(p=getfield(buff,2))) continue; - if (sscanf(p,"%d,%lf,%d",&week,&tow,&prn)<3) continue; - if (!(p=getfield(p,6))) continue; - if (weekn>=sbs->nmax) { - sbs->nmax=sbs->nmax==0?1024:sbs->nmax*2; - if (!(sbs_msgs=(sbsmsg_t *)realloc(sbs->msgs,sbs->nmax*sizeof(sbsmsg_t)))) { - trace(1,"readsbsmsg malloc error: nmax=%d\n",sbs->nmax); - free(sbs->msgs); sbs->msgs=NULL; sbs->n=sbs->nmax=0; - return; - } - sbs->msgs=sbs_msgs; - } - sbs->msgs[sbs->n].week=week; - sbs->msgs[sbs->n].tow=(int)(tow+0.5); - sbs->msgs[sbs->n].prn=prn; - for (i=0;i<29;i++) sbs->msgs[sbs->n].msg[i]=0; - for (i=0;*(p-1)&&*p&&i<29;p+=2,i++) { - if (sscanf(p,"%2X",&b)==1) sbs->msgs[sbs->n].msg[i]=(unsigned char)b; - } - sbs->msgs[sbs->n++].msg[28]&=0xC0; - } fclose(fp); } + + /* compare sbas messages -----------------------------------------------------*/ int cmpmsgs(const void *p1, const void *p2) { - sbsmsg_t *q1=(sbsmsg_t *)p1,*q2=(sbsmsg_t *)p2; - return q1->week!=q2->week?q1->week-q2->week: - (q1->towtow?-1:(q1->tow>q2->tow?1:q1->prn-q2->prn)); + sbsmsg_t *q1 = (sbsmsg_t *)p1,*q2 = (sbsmsg_t *)p2; + return q1->week != q2->week ? q1->week-q2->week : + (q1->tow < q2->tow ? -1 : (q1->tow > q2->tow ? 1 : q1->prn - q2->prn)); } + + /* read sbas message file ------------------------------------------------------ -* read sbas message file -* args : char *file I sbas message file (wind-card * is expanded) -* int sel I sbas satellite prn number selection (0:all) -* (gtime_t ts I start time) -* (gtime_t te I end time ) -* sbs_t *sbs IO sbas messages -* return : number of sbas messages -* notes : sbas message are appended and sorted. before calling the funciton, -* sbs->n, sbs->nmax and sbs->msgs must be set properly. (initially -* sbs->n=sbs->nmax=0, sbs->msgs=NULL) -* only the following file extentions after wild card expanded are valid -* to read. others are skipped -* .sbs, .SBS, .ems, .EMS -*-----------------------------------------------------------------------------*/ + * read sbas message file + * args : char *file I sbas message file (wind-card * is expanded) + * int sel I sbas satellite prn number selection (0:all) + * (gtime_t ts I start time) + * (gtime_t te I end time ) + * sbs_t *sbs IO sbas messages + * return : number of sbas messages + * notes : sbas message are appended and sorted. before calling the funciton, + * sbs->n, sbs->nmax and sbs->msgs must be set properly. (initially + * sbs->n=sbs->nmax=0, sbs->msgs=NULL) + * only the following file extentions after wild card expanded are valid + * to read. others are skipped + * .sbs, .SBS, .ems, .EMS + *-----------------------------------------------------------------------------*/ int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te, - sbs_t *sbs) + sbs_t *sbs) { - char *efiles[MAXEXFILE]={},*ext; + char *efiles[MAXEXFILE] = {},*ext; int i,n; - + trace(3,"sbsreadmsgt: file=%s sel=%d\n",file,sel); - - for (i=0;i=0;i--) free(efiles[i]); - return 0; + + for (i = 0; i < MAXEXFILE ; i++) + { + if (!(efiles[i] = (char *)malloc(1024))) + { + for (i--; i >= 0; i--) free(efiles[i]); + return 0; + } } - } /* expand wild card in file path */ - n=expath(file,efiles,MAXEXFILE); - - for (i=0;in>0) { - qsort(sbs->msgs,sbs->n,sizeof(sbsmsg_t),cmpmsgs); - } + if (sbs->n >0) + { + qsort(sbs->msgs, sbs->n, sizeof(sbsmsg_t), cmpmsgs); + } return sbs->n; } + + int sbsreadmsg(const char *file, int sel, sbs_t *sbs) { - gtime_t ts={},te={}; - - trace(3,"sbsreadmsg: file=%s sel=%d\n",file,sel); - - return sbsreadmsgt(file,sel,ts,te,sbs); + gtime_t ts = {}, te = {}; + + trace(3, "sbsreadmsg: file=%s sel=%d\n", file, sel); + + return sbsreadmsgt(file, sel, ts, te, sbs); } + + /* output sbas messages -------------------------------------------------------- -* output sbas message record to output file in rtklib sbas log format -* args : FILE *fp I output file pointer -* sbsmsg_t *sbsmsg I sbas messages -* return : none -*-----------------------------------------------------------------------------*/ + * output sbas message record to output file in rtklib sbas log format + * args : FILE *fp I output file pointer + * sbsmsg_t *sbsmsg I sbas messages + * return : none + *-----------------------------------------------------------------------------*/ void sbsoutmsg(FILE *fp, sbsmsg_t *sbsmsg) { - int i,type=sbsmsg->msg[1]>>2; - + int i, type = sbsmsg->msg[1] >> 2; + trace(4,"sbsoutmsg:\n"); - - fprintf(fp,"%4d %6d %3d %2d : ",sbsmsg->week,sbsmsg->tow,sbsmsg->prn,type); - for (i=0;i<29;i++) fprintf(fp,"%02X",sbsmsg->msg[i]); + + fprintf(fp, "%4d %6d %3d %2d : ", sbsmsg->week, sbsmsg->tow, sbsmsg->prn, type); + for (i = 0; i < 29; i++) fprintf(fp, "%02X", sbsmsg->msg[i]); fprintf(fp,"\n"); } + + /* search igps ---------------------------------------------------------------*/ -void searchigp(gtime_t time, const double *pos, const sbsion_t *ion, - const sbsigp_t **igp, double *x, double *y) +void searchigp(gtime_t time __attribute__((unused)), const double *pos, const sbsion_t *ion, + const sbsigp_t **igp, double *x, double *y) { int i,latp[2],lonp[4]; - double lat=pos[0]*R2D,lon=pos[1]*R2D; + double lat = pos[0] * R2D,lon = pos[1] * R2D; const sbsigp_t *p; - - trace(4,"searchigp: pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D); - - if (lon>=180.0) lon-=360.0; - if (-55.0<=lat&&lat<55.0) { - latp[0]=(int)floor(lat/5.0)*5; - latp[1]=latp[0]+5; - lonp[0]=lonp[1]=(int)floor(lon/5.0)*5; - lonp[2]=lonp[3]=lonp[0]+5; - *x=(lon-lonp[0])/5.0; - *y=(lat-latp[0])/5.0; - } - else { - latp[0]=(int)floor((lat-5.0)/10.0)*10+5; - latp[1]=latp[0]+10; - lonp[0]=lonp[1]=(int)floor(lon/10.0)*10; - lonp[2]=lonp[3]=lonp[0]+10; - *x=(lon-lonp[0])/10.0; - *y=(lat-latp[0])/10.0; - if (75.0<=lat&&lat<85.0) { - lonp[1]=(int)floor(lon/90.0)*90; - lonp[3]=lonp[1]+90; + + trace(4,"searchigp: pos=%.3f %.3f\n",pos[0] * R2D, pos[1] * R2D); + + if (lon >= 180.0) lon -= 360.0; + if (-55.0 <= lat && lat < 55.0) + { + latp[0] = (int)floor(lat / 5.0) * 5; + latp[1] = latp[0] + 5; + lonp[0] = lonp[1] = (int)floor(lon / 5.0) * 5; + lonp[2] = lonp[3] = lonp[0] + 5; + *x = (lon - lonp[0]) / 5.0; + *y = (lat - latp[0]) / 5.0; } - else if (-85.0<=lat&&lat<-75.0) { - lonp[0]=(int)floor((lon-50.0)/90.0)*90+40; - lonp[2]=lonp[0]+90; + else + { + latp[0] = (int)floor((lat-5.0) / 10.0) * 10 + 5; + latp[1] = latp[0] + 10; + lonp[0] = lonp[1] = (int)floor(lon / 10.0) * 10; + lonp[2] = lonp[3] = lonp[0] + 10; + *x = (lon - lonp[0]) / 10.0; + *y = (lat - latp[0]) / 10.0; + if (75.0 <= lat && lat < 85.0) + { + lonp[1] = (int)floor(lon / 90.0) * 90; + lonp[3] = lonp[1] + 90; + } + else if (-85.0 <= lat && lat < -75.0) + { + lonp[0] = (int)floor((lon - 50.0) / 90.0) * 90 + 40; + lonp[2] = lonp[0] + 90; + } + else if (lat >= 85.0) + { + for (i = 0; i < 4; i++) lonp[i] = (int)floor(lon / 90.0) * 90; + } + else if (lat < -85.0) + { + for (i = 0; i < 4; i++) lonp[i] = (int)floor((lon - 50.0) / 90.0) * 90 + 40; + } } - else if (lat>=85.0) { - for (i=0;i<4;i++) lonp[i]=(int)floor(lon/90.0)*90; + for (i = 0; i < 4; i++) if (lonp[i] == 180) lonp[i] = -180; + for (i = 0; i <= MAXBAND; i++) + { + for (p = ion[i].igp; p < ion[i].igp + ion[i].nigp; p++) + { + if (p->t0.time == 0) continue; + if (p->lat == latp[0] && p->lon == lonp[0] && p->give > 0) igp[0] = p; + else if (p->lat == latp[1] && p->lon == lonp[1] && p->give > 0) igp[1] = p; + else if (p->lat == latp[0] && p->lon == lonp[2] && p->give > 0) igp[2] = p; + else if (p->lat == latp[1] && p->lon == lonp[3] && p->give > 0) igp[3] = p; + if (igp[0] && igp[1] && igp[2] && igp[3]) return; + } } - else if (lat<-85.0) { - for (i=0;i<4;i++) lonp[i]=(int)floor((lon-50.0)/90.0)*90+40; - } - } - for (i=0;i<4;i++) if (lonp[i]==180) lonp[i]=-180; - for (i=0;i<=MAXBAND;i++) { - for (p=ion[i].igp;pt0.time==0) continue; - if (p->lat==latp[0]&&p->lon==lonp[0]&&p->give>0) igp[0]=p; - else if (p->lat==latp[1]&&p->lon==lonp[1]&&p->give>0) igp[1]=p; - else if (p->lat==latp[0]&&p->lon==lonp[2]&&p->give>0) igp[2]=p; - else if (p->lat==latp[1]&&p->lon==lonp[3]&&p->give>0) igp[3]=p; - if (igp[0]&&igp[1]&&igp[2]&&igp[3]) return; - } - } } + + /* sbas ionospheric delay correction ------------------------------------------- -* compute sbas ionosphric delay correction -* args : gtime_t time I time -* nav_t *nav I navigation data -* double *pos I receiver position {lat,lon,height} (rad/m) -* double *azel I satellite azimuth/elavation angle (rad) -* double *delay O slant ionospheric delay (L1) (m) -* double *var O variance of ionospheric delay (m^2) -* return : status (1:ok, 0:no correction) -* notes : before calling the function, sbas ionosphere correction parameters -* in navigation data (nav->sbsion) must be set by callig -* sbsupdatecorr() -*-----------------------------------------------------------------------------*/ + * compute sbas ionosphric delay correction + * args : gtime_t time I time + * nav_t *nav I navigation data + * double *pos I receiver position {lat,lon,height} (rad/m) + * double *azel I satellite azimuth/elavation angle (rad) + * double *delay O slant ionospheric delay (L1) (m) + * double *var O variance of ionospheric delay (m^2) + * return : status (1:ok, 0:no correction) + * notes : before calling the function, sbas ionosphere correction parameters + * in navigation data (nav->sbsion) must be set by callig + * sbsupdatecorr() + *-----------------------------------------------------------------------------*/ int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos, - const double *azel, double *delay, double *var) + const double *azel, double *delay, double *var) { - const double re=6378.1363,hion=350.0; - int i,err=0; - double fp,posp[2],x=0.0,y=0.0,t,w[4]={}; - const sbsigp_t *igp[4]={}; /* {ws,wn,es,en} */ - + const double re = 6378.1363,hion = 350.0; + int i,err = 0; + double fp,posp[2],x = 0.0,y = 0.0,t,w[4] = {}; + const sbsigp_t *igp[4] = {}; /* {ws,wn,es,en} */ + trace(4,"sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D, - azel[0]*R2D,azel[1]*R2D); - - *delay=*var=0.0; + azel[0]*R2D,azel[1]*R2D); + + *delay = *var = 0.0; if (pos[2]<-100.0||azel[1]<=0) return 1; - + /* ipp (ionospheric pierce point) position */ - fp=ionppp(pos,azel,re,hion,posp); - + fp = ionppp(pos,azel,re,hion,posp); + /* search igps around ipp */ searchigp(time,posp,nav->sbsion,igp,&x,&y); - + /* weight of igps */ - if (igp[0]&&igp[1]&&igp[2]&&igp[3]) { - w[0]=(1.0-x)*(1.0-y); w[1]=(1.0-x)*y; w[2]=x*(1.0-y); w[3]=x*y; - } - else if (igp[0]&&igp[1]&&igp[2]) { - w[1]=y; w[2]=x; - if ((w[0]=1.0-w[1]-w[2])<0.0) err=1; - } - else if (igp[0]&&igp[2]&&igp[3]) { - w[0]=1.0-x; w[3]=y; - if ((w[2]=1.0-w[0]-w[3])<0.0) err=1; - } - else if (igp[0]&&igp[1]&&igp[3]) { - w[0]=1.0-y; w[3]=x; - if ((w[1]=1.0-w[0]-w[3])<0.0) err=1; - } - else if (igp[1]&&igp[2]&&igp[3]) { - w[1]=1.0-x; w[2]=1.0-y; - if ((w[3]=1.0-w[1]-w[2])<0.0) err=1; - } - else err=1; - - if (err) { - trace(2,"no sbas iono correction: lat=%3.0f lon=%4.0f\n",posp[0]*R2D, - posp[1]*R2D); - return 0; - } - for (i=0;i<4;i++) { - if (!igp[i]) continue; - t=timediff(time,igp[i]->t0); - *delay+=w[i]*igp[i]->delay; - *var+=w[i]*varicorr(igp[i]->give)*9E-8*fabs(t); - } - *delay*=fp; *var*=fp*fp; - + if (igp[0] && igp[1] && igp[2] && igp[3]) + { + w[0] = (1.0 - x) * (1.0 - y); + w[1] = (1.0 - x) * y; + w[2] = x * (1.0 - y); + w[3] = x * y; + } + else if (igp[0] && igp[1] && igp[2]) + { + w[1] = y; w[2] = x; + if ((w[0] = 1.0- w[1] - w[2]) < 0.0) err = 1; + } + else if (igp[0] && igp[2] && igp[3]) + { + w[0] = 1.0 - x; w[3] = y; + if ((w[2] = 1.0 - w[0] - w[3]) < 0.0) err = 1; + } + else if (igp[0] && igp[1] && igp[3]) + { + w[0] = 1.0 - y; w[3] = x; + if ((w[1] = 1.0 - w[0] - w[3]) < 0.0) err = 1; + } + else if (igp[1] && igp[2] && igp[3]) + { + w[1] = 1.0-x; w[2] = 1.0-y; + if ((w[3] = 1.0 - w[1] - w[2]) < 0.0) err = 1; + } + else err = 1; + + if (err) + { + trace(2, "no sbas iono correction: lat=%3.0f lon=%4.0f\n", posp[0] * R2D, + posp[1] * R2D); + return 0; + } + for (i = 0; i < 4; i++) + { + if (!igp[i]) continue; + t = timediff(time,igp[i]->t0); + *delay += w[i] * igp[i]->delay; + *var += w[i] * varicorr(igp[i]->give) * 9e-8 * fabs(t); + } + *delay *= fp; *var *= fp * fp; + trace(5,"sbsioncorr: dion=%7.2f sig=%7.2f\n",*delay,sqrt(*var)); return 1; } + + /* get meterological parameters ----------------------------------------------*/ void getmet(double lat, double *met) { - static const double metprm[][10]={ /* lat=15,30,45,60,75 */ - {1013.25,299.65,26.31,6.30E-3,2.77, 0.00, 0.00,0.00,0.00E-3,0.00}, - {1017.25,294.15,21.79,6.05E-3,3.15, -3.75, 7.00,8.85,0.25E-3,0.33}, - {1015.75,283.15,11.66,5.58E-3,2.57, -2.25,11.00,7.24,0.32E-3,0.46}, - {1011.75,272.15, 6.78,5.39E-3,1.81, -1.75,15.00,5.36,0.81E-3,0.74}, - {1013.00,263.65, 4.11,4.53E-3,1.55, -0.50,14.50,3.39,0.62E-3,0.30} + static const double metprm[][10] = { /* lat=15,30,45,60,75 */ + {1013.25,299.65,26.31,6.30E-3,2.77, 0.00, 0.00,0.00,0.00E-3,0.00}, + {1017.25,294.15,21.79,6.05E-3,3.15, -3.75, 7.00,8.85,0.25E-3,0.33}, + {1015.75,283.15,11.66,5.58E-3,2.57, -2.25,11.00,7.24,0.32E-3,0.46}, + {1011.75,272.15, 6.78,5.39E-3,1.81, -1.75,15.00,5.36,0.81E-3,0.74}, + {1013.00,263.65, 4.11,4.53E-3,1.55, -0.50,14.50,3.39,0.62E-3,0.30} }; int i,j; double a; - lat=fabs(lat); - if (lat<=15.0) for (i=0;i<10;i++) met[i]=metprm[0][i]; - else if (lat>=75.0) for (i=0;i<10;i++) met[i]=metprm[4][i]; - else { - j=(int)(lat/15.0); a=(lat-j*15.0)/15.0; - for (i=0;i<10;i++) met[i]=(1.0-a)*metprm[j-1][i]+a*metprm[j][i]; - } + lat = fabs(lat); + if (lat <= 15.0) for (i = 0;i<10;i++) met[i] = metprm[0][i]; + else if (lat >= 75.0) for (i = 0;i<10;i++) met[i] = metprm[4][i]; + else + { + j = (int)(lat / 15.0); a = (lat - j * 15.0) / 15.0; + for (i = 0; i < 10; i++) met[i] = (1.0 - a) * metprm[j-1][i] + a * metprm[j][i]; + } } + + /* tropospheric delay correction ----------------------------------------------- -* compute sbas tropospheric delay correction (mops model) -* args : gtime_t time I time -* double *pos I receiver position {lat,lon,height} (rad/m) -* double *azel I satellite azimuth/elavation (rad) -* double *var O variance of troposphric error (m^2) -* return : slant tropospheric delay (m) -*-----------------------------------------------------------------------------*/ + * compute sbas tropospheric delay correction (mops model) + * args : gtime_t time I time + * double *pos I receiver position {lat,lon,height} (rad/m) + * double *azel I satellite azimuth/elavation (rad) + * double *var O variance of troposphric error (m^2) + * return : slant tropospheric delay (m) + *-----------------------------------------------------------------------------*/ double sbstropcorr(gtime_t time, const double *pos, const double *azel, - double *var) + double *var) { - const double k1=77.604,k2=382000.0,rd=287.054,gm=9.784,g=9.80665; - static double pos_[3]={},zh=0.0,zw=0.0; + const double k1 = 77.604,k2 = 382000.0,rd = 287.054,gm = 9.784,g = 9.80665; + static double pos_[3] = {}, zh = 0.0, zw = 0.0; int i; - double c,met[10],sinel=sin(azel[1]),h=pos[2],m; - - trace(4,"sbstropcorr: pos=%.3f %.3f azel=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D, - azel[0]*R2D,azel[1]*R2D); - - if (pos[2]<-100.0||10000.01E-7||fabs(pos[1]-pos_[1])>1E-7|| - fabs(pos[2]-pos_[2])>1.0) { - getmet(pos[0]*R2D,met); - c=cos(2.0*PI*(time2doy(time)-(pos[0]>=0.0?28.0:211.0))/365.25); - for (i=0;i<5;i++) met[i]-=met[i+5]*c; - zh=1E-6*k1*rd*met[0]/gm; - zw=1E-6*k2*rd/(gm*(met[4]+1.0)-met[3]*rd)*met[2]/met[1]; - zh*=pow(1.0-met[3]*h/met[1],g/(rd*met[3])); - zw*=pow(1.0-met[3]*h/met[1],(met[4]+1.0)*g/(rd*met[3])-1.0); - for (i=0;i<3;i++) pos_[i]=pos[i]; - } - m=1.001/sqrt(0.002001+sinel*sinel); - *var=0.12*0.12*m*m; + double c, met[10], sinel = sin(azel[1]), h = pos[2], m; + + trace(4, "sbstropcorr: pos=%.3f %.3f azel=%.3f %.3f\n", pos[0] * R2D, pos[1] * R2D, + azel[0] * R2D, azel[1] * R2D); + + if (pos[2] < -100.0 || 10000.0 < pos[2] || azel[1] <= 0) + { + *var = 0.0; + return 0.0; + } + if (zh == 0.0 || fabs(pos[0] - pos_[0]) > 1e-7 || fabs(pos[1] - pos_[1]) > 1e-7 || + fabs(pos[2] - pos_[2]) > 1.0) + { + getmet(pos[0] * R2D,met); + c = cos(2.0 * PI * (time2doy(time) - (pos[0] >= 0.0 ? 28.0 : 211.0)) / 365.25); + for (i = 0; i < 5;i++) met[i] -= met[i+5] * c; + zh = 1e-6 * k1 * rd * met[0] / gm; + zw = 1e-6 * k2 * rd / (gm * (met[4] + 1.0) - met[3] * rd) * met[2] / met[1]; + zh *= pow(1.0 - met[3] * h / met[1], g / (rd * met[3])); + zw *= pow(1.0 - met[3] * h / met[1], (met[4] + 1.0) * g / (rd * met[3]) - 1.0); + for (i = 0; i < 3; i++) pos_[i] = pos[i]; + } + m = 1.001/sqrt(0.002001+sinel*sinel); + *var = 0.12*0.12*m*m; return (zh+zw)*m; } + + /* long term correction ------------------------------------------------------*/ int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat, - double *drs, double *ddts) + double *drs, double *ddts) { const sbssatp_t *p; double t; int i; - + trace(3,"sbslongcorr: sat=%2d\n",sat); - - for (p=sbssat->sat;psat+sbssat->nsat;p++) { - if (p->sat!=sat||p->lcorr.t0.time==0) continue; - t=timediff(time,p->lcorr.t0); - if (fabs(t)>MAXSBSAGEL) { - trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n", - time_str(time,0),sat,t); - return 0; + + for (p = sbssat->sat;psat+sbssat->nsat;p++) + { + if (p->sat != sat || p->lcorr.t0.time == 0) continue; + t = timediff(time, p->lcorr.t0); + if (fabs(t) > MAXSBSAGEL) + { + trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n", + time_str(time,0), sat, t); + return 0; + } + for (i = 0; i < 3;i++) drs[i] = p->lcorr.dpos[i] + p->lcorr.dvel[i] * t; + *ddts = p->lcorr.daf0 + p->lcorr.daf1 * t; + + trace(5,"sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n", + sat, drs[0], drs[1], drs[2], *ddts * SPEED_OF_LIGHT); + + return 1; } - for (i=0;i<3;i++) drs[i]=p->lcorr.dpos[i]+p->lcorr.dvel[i]*t; - *ddts=p->lcorr.daf0+p->lcorr.daf1*t; - - trace(5,"sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n", - sat,drs[0],drs[1],drs[2],*ddts*CLIGHT); - - return 1; - } /* if sbas satellite without correction, no correction applied */ - if (satsys(sat,NULL)==SYS_SBS) return 1; - + if (satsys(sat,NULL) == SYS_SBS) return 1; + trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat); return 0; } + + /* fast correction -----------------------------------------------------------*/ int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat, - double *prc, double *var) + double *prc, double *var) { const sbssatp_t *p; double t; - + trace(3,"sbsfastcorr: sat=%2d\n",sat); - - for (p=sbssat->sat;psat+sbssat->nsat;p++) { - if (p->sat!=sat) continue; - if (p->fcorr.t0.time==0) break; - t=timediff(time,p->fcorr.t0)+sbssat->tlat; - - /* expire age of correction or UDRE==14 (not monitored) */ - if (fabs(t)>MAXSBSAGEF||p->fcorr.udre>=15) continue; - *prc=p->fcorr.prc; + + for (p = sbssat->sat;psat+sbssat->nsat;p++) + { + if (p->sat!=sat) continue; + if (p->fcorr.t0.time == 0) break; + t = timediff(time, p->fcorr.t0) + sbssat->tlat; + + /* expire age of correction or UDRE==14 (not monitored) */ + if (fabs(t) > MAXSBSAGEF || p->fcorr.udre >= 15) continue; + *prc = p->fcorr.prc; #ifdef RRCENA - if (p->fcorr.ai>0&&fabs(t)<=8.0*p->fcorr.dt) { - *prc+=p->fcorr.rrc*t; - } + if (p->fcorr.ai > 0 && fabs(t) <= 8.0 * p->fcorr.dt) + { + *prc += p->fcorr.rrc * t; + } #endif - *var=varfcorr(p->fcorr.udre)+degfcorr(p->fcorr.ai)*t*t/2.0; - - trace(5,"sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f\n",sat, - *prc,sqrt(*var),t); - return 1; - } - trace(2,"no sbas fast correction: %s sat=%2d\n",time_str(time,0),sat); + *var = varfcorr(p->fcorr.udre) + degfcorr(p->fcorr.ai) * t * t / 2.0; + + trace(5,"sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f\n",sat, + *prc, sqrt(*var), t); + return 1; + } + trace(2,"no sbas fast correction: %s sat=%2d\n", time_str(time, 0), sat); return 0; } + + /* sbas satellite ephemeris and clock correction ------------------------------- -* correct satellite position and clock bias with sbas satellite corrections -* args : gtime_t time I reception time -* int sat I satellite -* nav_t *nav I navigation data -* double *rs IO sat position and corrected {x,y,z} (ecef) (m) -* double *dts IO sat clock bias and corrected (s) -* double *var O sat position and clock variance (m^2) -* return : status (1:ok,0:no correction) -* notes : before calling the function, sbas satellite correction parameters -* in navigation data (nav->sbssat) must be set by callig -* sbsupdatecorr(). -* satellite clock correction include long-term correction and fast -* correction. -* sbas clock correction is usually based on L1C/A code. TGD or DCB has -* to be considered for other codes -*-----------------------------------------------------------------------------*/ + * correct satellite position and clock bias with sbas satellite corrections + * args : gtime_t time I reception time + * int sat I satellite + * nav_t *nav I navigation data + * double *rs IO sat position and corrected {x,y,z} (ecef) (m) + * double *dts IO sat clock bias and corrected (s) + * double *var O sat position and clock variance (m^2) + * return : status (1:ok,0:no correction) + * notes : before calling the function, sbas satellite correction parameters + * in navigation data (nav->sbssat) must be set by callig + * sbsupdatecorr(). + * satellite clock correction include long-term correction and fast + * correction. + * sbas clock correction is usually based on L1C/A code. TGD or DCB has + * to be considered for other codes + *-----------------------------------------------------------------------------*/ int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs, - double *dts, double *var) + double *dts, double *var) { - double drs[3]={},dclk=0.0,prc=0.0; + double drs[3] = {}, dclk = 0.0, prc = 0.0; int i; - + trace(3,"sbssatcorr : sat=%2d\n",sat); - + /* sbas long term corrections */ - if (!sbslongcorr(time,sat,&nav->sbssat,drs,&dclk)) { - return 0; - } + if (!sbslongcorr(time, sat, &nav->sbssat, drs, &dclk)) + { + return 0; + } /* sbas fast corrections */ - if (!sbsfastcorr(time,sat,&nav->sbssat,&prc,var)) { - return 0; - } - for (i=0;i<3;i++) rs[i]+=drs[i]; - - dts[0]+=dclk+prc/CLIGHT; - + if (!sbsfastcorr(time, sat, &nav->sbssat, &prc, var)) + { + return 0; + } + for (i = 0; i < 3; i++) rs[i] += drs[i]; + + dts[0] += dclk + prc / SPEED_OF_LIGHT; + trace(5,"sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f\n", - sat,drs[0],drs[1],drs[2],dclk,prc/CLIGHT,*var); - + sat, drs[0], drs[1], drs[2], dclk, prc / SPEED_OF_LIGHT, *var); + return 1; } + + /* decode sbas message --------------------------------------------------------- -* decode sbas message frame words and check crc -* args : gtime_t time I reception time -* int prn I sbas satellite prn number -* unsigned int *word I message frame words (24bit x 10) -* sbsmsg_t *sbsmsg O sbas message -* return : status (1:ok,0:crc error) -*-----------------------------------------------------------------------------*/ + * decode sbas message frame words and check crc + * args : gtime_t time I reception time + * int prn I sbas satellite prn number + * unsigned int *word I message frame words (24bit x 10) + * sbsmsg_t *sbsmsg O sbas message + * return : status (1:ok,0:crc error) + *-----------------------------------------------------------------------------*/ int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words, - sbsmsg_t *sbsmsg) + sbsmsg_t *sbsmsg) { int i,j; unsigned char f[29]; double tow; - + trace(5,"sbsdecodemsg: prn=%d\n",prn); - - if (time.time==0) return 0; - tow=time2gpst(time,&sbsmsg->week); - sbsmsg->tow=(int)(tow+DTTOL); - sbsmsg->prn=prn; - for (i=0;i<7;i++) for (j=0;j<4;j++) { - sbsmsg->msg[i*4+j]=(unsigned char)(words[i]>>((3-j)*8)); - } - sbsmsg->msg[28]=(unsigned char)(words[7]>>18)&0xC0; - for (i=28;i>0;i--) f[i]=(sbsmsg->msg[i]>>6)+(sbsmsg->msg[i-1]<<2); - f[0]=sbsmsg->msg[0]>>6; - - return rtk_crc24q(f,29)==(words[7]&0xFFFFFF); /* check crc */ + + if (time.time == 0) return 0; + tow = time2gpst(time,&sbsmsg->week); + sbsmsg->tow = (int)(tow + DTTOL); + sbsmsg->prn = prn; + for (i = 0; i < 7; i++) for (j = 0; j < 4; j++) + { + sbsmsg->msg[i*4+j] = (unsigned char)(words[i] >> ((3-j)*8)); + } + sbsmsg->msg[28] = (unsigned char)(words[7] >> 18 ) & 0xC0; + for (i = 28; i > 0; i--) f[i] = (sbsmsg->msg[i] >> 6) + (sbsmsg->msg[i-1] << 2); + f[0] = sbsmsg->msg[0] >> 6; + + return rtk_crc24q(f, 29) == (words[7] & 0xFFFFFF); /* check crc */ } diff --git a/src/algorithms/libs/rtklib/rtklib_sbas.h b/src/algorithms/libs/rtklib/rtklib_sbas.h index f3ec8ee51..9f55bfc04 100644 --- a/src/algorithms/libs/rtklib/rtklib_sbas.h +++ b/src/algorithms/libs/rtklib/rtklib_sbas.h @@ -48,43 +48,21 @@ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * -* -* option : -DRRCENA enable rrc correction -* -* references : -* [1] RTCA/DO-229C, Minimum operational performanc standards for global -* positioning system/wide area augmentation system airborne equipment, -* RTCA inc, November 28, 2001 -* [2] IS-QZSS v.1.1, Quasi-Zenith Satellite System Navigation Service -* Interface Specification for QZSS, Japan Aerospace Exploration Agency, -* July 31, 2009 -* -* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $ -* history : 2007/10/14 1.0 new -* 2009/01/24 1.1 modify sbspntpos() api -* improve fast/ion correction update -* 2009/04/08 1.2 move function crc24q() to rcvlog.c -* support glonass, galileo and qzss -* 2009/06/08 1.3 modify sbsupdatestat() -* delete sbssatpos() -* 2009/12/12 1.4 support glonass -* 2010/01/22 1.5 support ems (egnos message service) format -* 2010/06/10 1.6 added api: -* sbssatcorr(),sbstropcorr(),sbsioncorr(), -* sbsupdatecorr() -* changed api: -* sbsreadmsgt(),sbsreadmsg() -* deleted api: -* sbspntpos(),sbsupdatestat() -* 2010/08/16 1.7 not reject udre==14 or give==15 correction message -* (2.4.0_p4) -* 2011/01/15 1.8 use api ionppp() -* add prn mask of qzss for qzss L1SAIF -* 2016/07/29 1.9 crc24q() -> rtk_crc24q() -*-----------------------------------------------------------------------------*/ + * + * option : -DRRCENA enable rrc correction + * + * References : + * [1] RTCA/DO-229C, Minimum operational performanc standards for global + * positioning system/wide area augmentation system airborne equipment, + * RTCA inc, November 28, 2001 + * [2] IS-QZSS v.1.1, Quasi-Zenith Satellite System Navigation Service + * Interface Specification for QZSS, Japan Aerospace Exploration Agency, + * July 31, 2009 + * + *-----------------------------------------------------------------------------*/ -#ifndef RTKLIB_SBAS_H_ -#define RTKLIB_SBAS_H_ +#ifndef GNSS_SDR_RTKLIB_SBAS_H_ +#define GNSS_SDR_RTKLIB_SBAS_H_ #include "rtklib.h" #include "rtklib_rtkcmn.h" @@ -94,51 +72,51 @@ /* sbas igp definition -------------------------------------------------------*/ static const short -x1[]={-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, 20, +x1[] = {-75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, - 5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 65, 75, 85}, -x2[]={-55,-50,-45,-40,-35,-30,-25,-20,-15,-10, -5, 0, 5, 10, 15, 20, 25, 30, +x2[] = {-55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55}, -x3[]={-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, 20, +x3[] = {-75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, - 5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 65, 75}, -x4[]={-85,-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, +x4[] = {-85, -75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, - 5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 65, 75}, -x5[]={-180,-175,-170,-165,-160,-155,-150,-145,-140,-135,-130,-125,-120,-115, - -110,-105,-100,- 95,- 90,- 85,- 80,- 75,- 70,- 65,- 60,- 55,- 50,- 45, - - 40,- 35,- 30,- 25,- 20,- 15,- 10,- 5, 0, 5, 10, 15, 20, 25, +x5[] = {-180, -175, -170, -165, -160, -155, -150, -145, -140, -135, -130, -125, -120, -115, + -110, -105, -100, - 95, - 90, - 85, - 80, - 75, - 70, - 65, - 60, - 55, - 50, - 45, + - 40, - 35, - 30, - 25, - 20, - 15, - 10, - 5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95, 100, 105, 110, 115, 120, 125, 130, 135, 140, 145, 150, 155, 160, 165, 170, 175}, -x6[]={-180,-170,-160,-150,-140,-130,-120,-110,-100,- 90,- 80,- 70,- 60,- 50, - - 40,- 30,- 20,- 10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, +x6[] = {-180, -170, -160, -150, -140, -130, -120, -110, -100, - 90, - 80, - 70, - 60, - 50, + - 40, - 30, - 20, - 10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170}, -x7[]={-180,-150,-120,- 90,- 60,- 30, 0, 30, 60, 90, 120, 150}, -x8[]={-170,-140,-110,- 80,- 50,- 20, 10, 40, 70, 100, 130, 160}; +x7[] = {-180, -150, -120, - 90, - 60, - 30, 0, 30, 60, 90, 120, 150}, +x8[] = {-170, -140, -110, - 80, - 50, - 20, 10, 40, 70, 100, 130, 160}; -const sbsigpband_t igpband1[9][8]={ /* band 0-8 */ - {{-180,x1, 1, 28},{-175,x2, 29, 51},{-170,x3, 52, 78},{-165,x2, 79,101}, - {-160,x3,102,128},{-155,x2,129,151},{-150,x3,152,178},{-145,x2,179,201}}, - {{-140,x4, 1, 28},{-135,x2, 29, 51},{-130,x3, 52, 78},{-125,x2, 79,101}, - {-120,x3,102,128},{-115,x2,129,151},{-110,x3,152,178},{-105,x2,179,201}}, - {{-100,x3, 1, 27},{- 95,x2, 28, 50},{- 90,x1, 51, 78},{- 85,x2, 79,101}, - {- 80,x3,102,128},{- 75,x2,129,151},{- 70,x3,152,178},{- 65,x2,179,201}}, - {{- 60,x3, 1, 27},{- 55,x2, 28, 50},{- 50,x4, 51, 78},{- 45,x2, 79,101}, - {- 40,x3,102,128},{- 35,x2,129,151},{- 30,x3,152,178},{- 25,x2,179,201}}, - {{- 20,x3, 1, 27},{- 15,x2, 28, 50},{- 10,x3, 51, 77},{- 5,x2, 78,100}, - { 0,x1,101,128},{ 5,x2,129,151},{ 10,x3,152,178},{ 15,x2,179,201}}, - {{ 20,x3, 1, 27},{ 25,x2, 28, 50},{ 30,x3, 51, 77},{ 35,x2, 78,100}, - { 40,x4,101,128},{ 45,x2,129,151},{ 50,x3,152,178},{ 55,x2,179,201}}, - {{ 60,x3, 1, 27},{ 65,x2, 28, 50},{ 70,x3, 51, 77},{ 75,x2, 78,100}, - { 80,x3,101,127},{ 85,x2,128,150},{ 90,x1,151,178},{ 95,x2,179,201}}, - {{ 100,x3, 1, 27},{ 105,x2, 28, 50},{ 110,x3, 51, 77},{ 115,x2, 78,100}, - { 120,x3,101,127},{ 125,x2,128,150},{ 130,x4,151,178},{ 135,x2,179,201}}, - {{ 140,x3, 1, 27},{ 145,x2, 28, 50},{ 150,x3, 51, 77},{ 155,x2, 78,100}, - { 160,x3,101,127},{ 165,x2,128,150},{ 170,x3,151,177},{ 175,x2,178,200}} +const sbsigpband_t igpband1[9][8] = { /* band 0-8 */ + {{-180, x1, 1, 28}, {-175, x2, 29, 51}, {-170, x3, 52, 78}, {-165, x2, 79, 101}, + {-160, x3, 102, 128}, {-155, x2, 129, 151}, {-150, x3, 152, 178}, {-145, x2, 179, 201}}, + {{-140, x4, 1, 28}, {-135, x2, 29, 51}, {-130, x3, 52, 78}, {-125, x2, 79, 101}, + {-120, x3, 102, 128}, {-115, x2, 129, 151}, {-110, x3, 152, 178}, {-105, x2, 179, 201}}, + {{-100, x3, 1, 27}, {- 95,x2, 28, 50},{- 90, x1, 51, 78}, {- 85, x2, 79, 101}, + {- 80, x3, 102, 128}, {- 75, x2, 129, 151}, {- 70, x3, 152, 178}, {- 65, x2,179, 201}}, + {{- 60, x3, 1, 27}, {- 55, x2, 28, 50}, {- 50,x4, 51, 78}, {- 45, x2, 79, 101}, + {- 40, x3, 102, 128}, {- 35, x2, 129, 151}, {- 30, x3, 152, 178}, {- 25, x2, 179, 201}}, + {{- 20, x3, 1, 27}, {- 15, x2, 28, 50}, {- 10, x3, 51, 77}, {- 5, x2, 78, 100}, + { 0, x1, 101, 128}, { 5, x2, 129, 151}, { 10, x3, 152, 178}, { 15, x2, 179, 201}}, + {{ 20, x3, 1, 27}, { 25, x2, 28, 50}, { 30, x3, 51, 77}, { 35, x2, 78, 100}, + { 40, x4, 101, 128}, { 45, x2, 129, 151},{ 50, x3, 152, 178}, { 55, x2, 179, 201}}, + {{ 60, x3, 1, 27}, { 65, x2, 28, 50}, { 70, x3, 51, 77}, { 75, x2, 78, 100}, + { 80, x3, 101, 127}, { 85, x2, 128, 150}, { 90,x1, 151, 178}, { 95, x2, 179, 201}}, + {{ 100, x3, 1, 27}, { 105, x2, 28, 50}, { 110, x3, 51, 77}, { 115, x2, 78 ,100}, + { 120, x3, 101, 127}, { 125, x2, 128, 150}, { 130, x4, 151, 178}, { 135, x2, 179, 201}}, + {{ 140, x3, 1, 27}, { 145, x2, 28, 50}, { 150, x3, 51, 77}, { 155, x2, 78, 100}, + { 160, x3, 101, 127}, { 165, x2,128, 150}, { 170, x3,151,177}, { 175, x2, 178, 200}} }; -const sbsigpband_t igpband2[2][5]={ /* band 9-10 */ - {{ 60,x5, 1, 72},{ 65,x6, 73,108},{ 70,x6,109,144},{ 75,x6,145,180}, - { 85,x7,181,192}}, - {{- 60,x5, 1, 72},{- 65,x6, 73,108},{- 70,x6,109,144},{- 75,x6,145,180}, - {- 85,x8,181,192}} +const sbsigpband_t igpband2[2][5] = { /* band 9-10 */ + {{ 60, x5, 1, 72}, { 65, x6, 73, 108}, { 70, x6, 109, 144}, { 75, x6, 145, 180}, + { 85, x7, 181, 192}}, + {{- 60, x5, 1, 72}, {- 65, x6, 73, 108}, {- 70, x6, 109, 144}, {- 75 ,x6, 145, 180}, + {- 85, x8, 181, 192}} }; @@ -146,6 +124,7 @@ char *getfield(char *p, int pos); double varfcorr(int udre); double varicorr(int give); double degfcorr(int ai); + int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat); int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat); int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat); @@ -158,6 +137,7 @@ int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat); int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat); int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat); int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion); + int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav); void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,sbs_t *sbs); int cmpmsgs(const void *p1, const void *p2); @@ -184,4 +164,4 @@ int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words, sbsmsg_t *sbsmsg); -#endif /* RTKLIB_SBAS_H_ */ +#endif /* GNSS_SDR_RTKLIB_SBAS_H_ */ diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt index 5e12f5fcb..d6e69819b 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt @@ -76,6 +76,14 @@ if(MSVC) add_compile_options(/wo4838) endif(MSVC) +# allow 'large' files in 32 bit builds +if(UNIX) + add_definitions( -D_LARGEFILE_SOURCE + -D_FILE_OFFSET_BITS=64 + -D_LARGE_FILES + ) +endif(UNIX) + ######################################################################## # Dependencies setup ######################################################################## diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/README.md b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/README.md index d50eb2e02..6c3d217ee 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/README.md +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/README.md @@ -1,26 +1,56 @@ # Welcome to VOLK_GNSSSDR, the Vector-Optimized Library of Kernels for GNSS-SDR -VOLK_GNSSSDR is a sub-project of GNSS-SDR. This library provides a set of extra kernels that can be used stand-alone or in combination with VOLK's. Please see http://libvolk.org for documentation, source code, and contact information about the original VOLK library. +VOLK_GNSSSDR is a sub-project of GNSS-SDR. This library provides a set +of extra kernels that can be used stand-alone or in combination with +VOLK's. Please see http://libvolk.org for documentation, source code, +and contact information about the original VOLK library. -The boilerplate of this code was initially generated with ```volk_modtool```, an application provided by VOLK that creates the skeleton than can then be filled with custom kernels. Some modifications were added to accomodate the specificities of Global Navigation Satellite Systems (GNSS) signal processing. Those changes are clearly indicated in the source code, and do not break compatibility. +The boilerplate of this code was initially generated with +```volk_modtool```, an application provided by VOLK that creates the +skeleton than can then be filled with custom kernels. Some modifications +were added to accomodate the specificities of Global Navigation +Satellite Systems (GNSS) signal processing. Those changes are clearly +indicated in the source code, and do not break compatibility. -This library contains kernels of hand-written SIMD code for different mathematical operations, mainly with 8-bit and 16-bit real and complex data types, offering a platform/architecture agnostic version that will run in all machines, plus other versions for different SIMD instruction sets. Then, the application ```volk_gnsssdr_profile``` runs some iterations of all versions that your machine can execute and annotates which is the fastest, which will then be selected at runtime when executing GNSS-SDR. In this way, we can address at the same time portability (by creating executables that will run in nearly all processor architectures) and efficiency (by providing custom implementations specially designed to take advantage of the specific processor that is running the code). +This library contains kernels of hand-written SIMD code for different +mathematical operations, mainly with 8-bit and 16-bit real and complex +data types, offering a platform/architecture agnostic version that will +run in all machines, plus other versions for different SIMD instruction +sets. Then, the application ```volk_gnsssdr_profile``` runs some +iterations of all versions that your machine can execute and annotates +which is the fastest, which will then be selected at runtime when +executing GNSS-SDR. In this way, we can address at the same time +[portability](http://gnss-sdr.org/design-forces/portability/) (by +creating executables that will run in nearly all processor +architectures) and +[efficiency](http://gnss-sdr.org/design-forces/efficiency/) (by +providing custom implementations specially designed to take advantage of +the specific processor that is running the code). -These kernels have some specific features (e.g. saturation arithmetics) that are aimed to GNSS signal processing, but could make them not suitable for its general use in other applications. Check out the documentation generated by Doxygen and the *generic* (that is, plain C) implementation to see what each kernel is actually doing. +These kernels have some specific features (e.g. saturation arithmetics) +that are aimed to GNSS signal processing, but could make them not +suitable for its general use in other applications. Check out the +documentation generated by Doxygen and the *generic* (that is, plain C) +implementation to see what each kernel is actually doing. ## How to use VOLK_GNSSSDR: -This library is automatically built and installed along with GNSS-SDR if it is not found by CMake on your system at configure time. +This library is automatically built and installed along with GNSS-SDR if +it is not found by CMake on your system at configure time. -However, you can install and use VOLK_GNSSSDR kernels as you use VOLK's, independently from GNSS-SDR. +However, you can install and use VOLK_GNSSSDR kernels as you use VOLK's, +independently from GNSS-SDR. -First, make sure that the required dependencies are installed in you machine: +First, make sure that the required dependencies are installed in your +machine: ~~~~~~ -$ sudo apt-get install git cmake python-mako python-six libboost-dev libbbost-filesystem +$ sudo apt-get install cmake python-mako python-six libboost-dev \ + libboost-filesystem-dev libboost-system-dev libboost-program-options-dev ~~~~~~ -In order to build and install the library, go to the base folder of the source code and do: +In order to build and install the library, go to the base folder of the +source code and do: ~~~~~~ $ mkdir build @@ -32,15 +62,20 @@ $ sudo make install That's it! -Before its first use, please execute ```volk_gnsssdr_profile``` to let your system know which is the fastest available implementation. This only has to be done once: +Before its first use, please execute ```volk_gnsssdr_profile``` to let +your system know which is the fastest available implementation. This +only has to be done once: ~~~~~~ $ volk_gnsssdr_profile ~~~~~~ -From now on, GNSS-SDR (and any other program of your own that makes use of VOLK_GNSSSDR) will benefit from the acceleration provided by SIMD instructions available in your processor. +From now on, GNSS-SDR (and any other program of your own that makes use +of VOLK_GNSSSDR) will benefit from the acceleration provided by SIMD +instructions available in your processor. -The execution of ```volk_gnsssdr_profile``` can be set automatically after building, leaving your system ready to use: +The execution of ```volk_gnsssdr_profile``` can be set automatically +after building, leaving your system ready to use: ~~~~~~ $ cmake -DENABLE_PROFILING=ON ../ @@ -49,22 +84,35 @@ $ sudo make install ~~~~~~ -This figure shows the role of some VOLK_GNSSSDR kernels in the context of a GNSS baseband processor: +This figure shows the role of some VOLK_GNSSSDR kernels in the context +of a GNSS baseband processor: -![Example of VOLK_GNSSSDR usage.](./docs/images/VOLK_GNSSSDR_Usage_Example.png) +![Example of VOLK_GNSSSDR +usage.](./docs/images/VOLK_GNSSSDR_Usage_Example.png) -If you use VOLK_GNSSSDR in your research and/or software, please cite the following paper: +If you use VOLK_GNSSSDR in your research and/or software, please cite +the following paper: - * C. Fernández-Prades, J. Arribas, P. Closas, [*Accelerating GNSS Software Receivers*](https://zenodo.org/record/266493), in Proc. of the ION GNSS+ 2016 Conference, pp. 44-61, Portland, Oregon, Sept. 12-16, 2016. + * C. Fernández-Prades, J. Arribas, P. Closas, [*Accelerating +GNSS Software Receivers*](https://zenodo.org/record/266493), in Proc. of +the ION GNSS+ 2016 Conference, pp. 44-61, Portland, Oregon, Sept. 12-16, 2016. -Citations are useful for the continued development and maintenance of the library. +Citations are useful for the continued development and maintenance of +the library. ___ -VOLK_GNSSSDR was originally created by Andres Cecilia Luque in the framework of the [Summer Of Code In Space (SOCIS 2014)](http://sophia.estec.esa.int/socis2014/?q=about "SOCIS 2014 webpage") program organized by the European Space Agency, and then evolved and maintained by Carles Fernández-Prades and Javier Arribas. This software is released under the GNU General Public License version 3, see the file COPYING. +VOLK_GNSSSDR was originally created by Andres Cecilia Luque in the +framework of the [Summer Of Code In Space (SOCIS +2014)](http://sophia.estec.esa.int/socis2014/?q=about "SOCIS 2014 +webpage") program organized by the European Space Agency, and then +evolved and maintained by Carles Fernández-Prades and Javier +Arribas. This software is released under the GNU General Public License +version 3, see the file COPYING. -This project is managed by [Centre Tecnologic de Telecomunicacions de Catalunya](http://www.cttc.es "CTTC webpage"). +This project is managed by [Centre Tecnologic de Telecomunicacions de +Catalunya](http://www.cttc.es "CTTC webpage"). diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_dot_prod_16ic.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_dot_prod_16ic.h index fa19ee50e..7f6219468 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_dot_prod_16ic.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_dot_prod_16ic.h @@ -101,8 +101,8 @@ static inline void volk_gnsssdr_16ic_x2_dot_prod_16ic_a_sse2(lv_16sc_t* out, con realcacc = _mm_setzero_si128(); imagcacc = _mm_setzero_si128(); - mask_imag = _mm_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - mask_real = _mm_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + mask_imag = _mm_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + mask_real = _mm_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); for(number = 0; number < sse_iters; number++) { @@ -179,8 +179,8 @@ static inline void volk_gnsssdr_16ic_x2_dot_prod_16ic_u_sse2(lv_16sc_t* out, con realcacc = _mm_setzero_si128(); imagcacc = _mm_setzero_si128(); - mask_imag = _mm_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - mask_real = _mm_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + mask_imag = _mm_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + mask_real = _mm_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); for(number = 0; number < sse_iters; number++) { @@ -258,8 +258,8 @@ static inline void volk_gnsssdr_16ic_x2_dot_prod_16ic_u_axv2(lv_16sc_t* out, con realcacc = _mm256_setzero_si256(); imagcacc = _mm256_setzero_si256(); - mask_imag = _mm256_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - mask_real = _mm256_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + mask_imag = _mm256_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + mask_real = _mm256_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); for(number = 0; number < avx_iters; number++) { @@ -335,8 +335,8 @@ static inline void volk_gnsssdr_16ic_x2_dot_prod_16ic_a_axv2(lv_16sc_t* out, con realcacc = _mm256_setzero_si256(); imagcacc = _mm256_setzero_si256(); - mask_imag = _mm256_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - mask_real = _mm256_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + mask_imag = _mm256_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + mask_real = _mm256_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); for(number = 0; number < avx_iters; number++) { diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_dot_prod_16ic_xn.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_dot_prod_16ic_xn.h index 4f53594a8..c1beceead 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_dot_prod_16ic_xn.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_dot_prod_16ic_xn.h @@ -138,8 +138,8 @@ static inline void volk_gnsssdr_16ic_x2_dot_prod_16ic_xn_a_sse2(lv_16sc_t* resul __m128i a, b, c, c_sr, mask_imag, mask_real, real, imag; - mask_imag = _mm_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - mask_real = _mm_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + mask_imag = _mm_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + mask_real = _mm_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); for(index = 0; index < sse_iters; index++) { @@ -232,8 +232,8 @@ static inline void volk_gnsssdr_16ic_x2_dot_prod_16ic_xn_u_sse2(lv_16sc_t* resul __m128i a, b, c, c_sr, mask_imag, mask_real, real, imag; - mask_imag = _mm_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - mask_real = _mm_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + mask_imag = _mm_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + mask_real = _mm_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); for(index = 0; index < sse_iters; index++) { @@ -326,8 +326,8 @@ static inline void volk_gnsssdr_16ic_x2_dot_prod_16ic_xn_a_avx2(lv_16sc_t* resul __m256i a, b, c, c_sr, mask_imag, mask_real, real, imag; - mask_imag = _mm256_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - mask_real = _mm256_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + mask_imag = _mm256_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + mask_real = _mm256_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); for(index = 0; index < sse_iters; index++) { @@ -420,8 +420,8 @@ static inline void volk_gnsssdr_16ic_x2_dot_prod_16ic_xn_u_avx2(lv_16sc_t* resul __m256i a, b, c, c_sr, mask_imag, mask_real, real, imag; - mask_imag = _mm256_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - mask_real = _mm256_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + mask_imag = _mm256_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + mask_real = _mm256_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); for(index = 0; index < sse_iters; index++) { diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_multiply_16ic.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_multiply_16ic.h index 9dbc9bd31..2f1036953 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_multiply_16ic.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_multiply_16ic.h @@ -85,8 +85,8 @@ static inline void volk_gnsssdr_16ic_x2_multiply_16ic_a_sse2(lv_16sc_t* out, con unsigned int number; __m128i a, b, c, c_sr, mask_imag, mask_real, real, imag, imag1, imag2, b_sl, a_sl, result; - mask_imag = _mm_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - mask_real = _mm_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + mask_imag = _mm_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + mask_real = _mm_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); const lv_16sc_t* _in_a = in_a; const lv_16sc_t* _in_b = in_b; @@ -139,8 +139,8 @@ static inline void volk_gnsssdr_16ic_x2_multiply_16ic_u_sse2(lv_16sc_t* out, con unsigned int number; __m128i a, b, c, c_sr, mask_imag, mask_real, real, imag, imag1,imag2, b_sl, a_sl, result; - mask_imag = _mm_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - mask_real = _mm_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + mask_imag = _mm_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + mask_real = _mm_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); const lv_16sc_t* _in_a = in_a; const lv_16sc_t* _in_b = in_b; @@ -198,8 +198,8 @@ static inline void volk_gnsssdr_16ic_x2_multiply_16ic_u_avx2(lv_16sc_t* out, con __m256i a, b, c, c_sr, real, imag, imag1, imag2, b_sl, a_sl, result; - const __m256i mask_imag = _mm256_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - const __m256i mask_real = _mm256_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + const __m256i mask_imag = _mm256_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + const __m256i mask_real = _mm256_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); for(;number < avx2_points; number++) { @@ -252,8 +252,8 @@ static inline void volk_gnsssdr_16ic_x2_multiply_16ic_a_avx2(lv_16sc_t* out, con __m256i a, b, c, c_sr, real, imag, imag1, imag2, b_sl, a_sl, result; - const __m256i mask_imag = _mm256_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - const __m256i mask_real = _mm256_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + const __m256i mask_imag = _mm256_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + const __m256i mask_real = _mm256_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); for(;number < avx2_points; number++) { diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_rotator_dot_prod_16ic_xn.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_rotator_dot_prod_16ic_xn.h index f6e3a05c5..dc9289af6 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_rotator_dot_prod_16ic_xn.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_16ic_x2_rotator_dot_prod_16ic_xn.h @@ -205,8 +205,8 @@ static inline void volk_gnsssdr_16ic_x2_rotator_dot_prod_16ic_xn_a_sse3(lv_16sc_ __m128i a, b, c, c_sr, mask_imag, mask_real, real, imag, imag1, imag2, b_sl, a_sl; - mask_imag = _mm_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - mask_real = _mm_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + mask_imag = _mm_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + mask_real = _mm_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); // phase rotation registers __m128 pa, pb, two_phase_acc_reg, two_phase_inc_reg; @@ -383,8 +383,8 @@ static inline void volk_gnsssdr_16ic_x2_rotator_dot_prod_16ic_xn_a_sse3_reload(l __m128i a, b, c, c_sr, mask_imag, mask_real, real, imag, imag1, imag2, b_sl, a_sl; - mask_imag = _mm_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - mask_real = _mm_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + mask_imag = _mm_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + mask_real = _mm_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); // phase rotation registers __m128 pa, pb, two_phase_acc_reg, two_phase_inc_reg; @@ -629,8 +629,8 @@ static inline void volk_gnsssdr_16ic_x2_rotator_dot_prod_16ic_xn_u_sse3(lv_16sc_ __m128i a, b, c, c_sr, mask_imag, mask_real, real, imag, imag1, imag2, b_sl, a_sl; - mask_imag = _mm_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - mask_real = _mm_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + mask_imag = _mm_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + mask_real = _mm_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); // phase rotation registers __m128 pa, pb, two_phase_acc_reg, two_phase_inc_reg; @@ -794,8 +794,8 @@ static inline void volk_gnsssdr_16ic_x2_rotator_dot_prod_16ic_xn_a_avx2(lv_16sc_ imagcacc[n_vec] = _mm256_setzero_si256(); } - const __m256i mask_imag = _mm256_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - const __m256i mask_real = _mm256_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + const __m256i mask_imag = _mm256_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + const __m256i mask_real = _mm256_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); __m128 a, b, two_phase_acc_reg, two_phase_inc_reg; __m128i c1, c2, result1, result2; @@ -1002,8 +1002,8 @@ static inline void volk_gnsssdr_16ic_x2_rotator_dot_prod_16ic_xn_a_avx2_reload(l imagcacc[n_vec] = _mm256_setzero_si256(); } - const __m256i mask_imag = _mm256_set_epi8(255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0); - const __m256i mask_real = _mm256_set_epi8(0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255); + const __m256i mask_imag = _mm256_set_epi8(0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0); + const __m256i mask_real = _mm256_set_epi8(0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF, 0, 0, 0xFF, 0xFF); __m128 a, b, two_phase_acc_reg, two_phase_inc_reg; __m128i c1, c2, result1, result2; diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_conjugate_8ic.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_conjugate_8ic.h index 0ccbd7aba..7e89fe5d1 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_conjugate_8ic.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_conjugate_8ic.h @@ -71,7 +71,7 @@ static inline void volk_gnsssdr_8ic_conjugate_8ic_u_avx(lv_8sc_t* cVector, const __m256 tmp; __m128i tmp128lo, tmp128hi; - __m256 conjugator1 = _mm256_castsi256_ps(_mm256_setr_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255)); + __m256 conjugator1 = _mm256_castsi256_ps(_mm256_setr_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF)); __m128i conjugator2 = _mm_setr_epi8(0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1); for (i = 0; i < sse_iters; ++i) @@ -140,7 +140,7 @@ static inline void volk_gnsssdr_8ic_conjugate_8ic_u_sse3(lv_8sc_t* cVector, cons const lv_8sc_t* a = aVector; __m128i tmp; - __m128i conjugator1 = _mm_setr_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255); + __m128i conjugator1 = _mm_setr_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF); __m128i conjugator2 = _mm_setr_epi8(0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1); for (i = 0; i < sse_iters; ++i) @@ -190,7 +190,7 @@ static inline void volk_gnsssdr_8ic_conjugate_8ic_a_avx(lv_8sc_t* cVector, const __m256 tmp; __m128i tmp128lo, tmp128hi; - __m256 conjugator1 = _mm256_castsi256_ps(_mm256_setr_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255)); + __m256 conjugator1 = _mm256_castsi256_ps(_mm256_setr_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF)); __m128i conjugator2 = _mm_setr_epi8(0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1); for (i = 0; i < sse_iters; ++i) @@ -257,7 +257,7 @@ static inline void volk_gnsssdr_8ic_conjugate_8ic_a_sse3(lv_8sc_t* cVector, cons const lv_8sc_t* a = aVector; __m128i tmp; - __m128i conjugator1 = _mm_setr_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255); + __m128i conjugator1 = _mm_setr_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF); __m128i conjugator2 = _mm_setr_epi8(0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1); for (i = 0; i < sse_iters; ++i) diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_s8ic_multiply_8ic.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_s8ic_multiply_8ic.h index eae5363c2..21b1abb1b 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_s8ic_multiply_8ic.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_s8ic_multiply_8ic.h @@ -73,7 +73,7 @@ static inline void volk_gnsssdr_8ic_s8ic_multiply_8ic_u_sse3(lv_8sc_t* cVector, lv_8sc_t* c = cVector; const lv_8sc_t* a = aVector; - mult1 = _mm_set_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255); + mult1 = _mm_set_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF); y = _mm_set1_epi16(*(short*)&scalar); imagy = _mm_srli_si128(y, 1); @@ -166,7 +166,7 @@ static inline void volk_gnsssdr_8ic_s8ic_multiply_8ic_a_sse3(lv_8sc_t* cVector, lv_8sc_t* c = cVector; const lv_8sc_t* a = aVector; - mult1 = _mm_set_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255); + mult1 = _mm_set_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF); y = _mm_set1_epi16(*(short*)&scalar); imagy = _mm_srli_si128(y, 1); diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_x2_dot_prod_8ic.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_x2_dot_prod_8ic.h index 7e381a9c4..e9633d682 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_x2_dot_prod_8ic.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_x2_dot_prod_8ic.h @@ -127,7 +127,7 @@ static inline void volk_gnsssdr_8ic_x2_dot_prod_8ic_u_sse2(lv_8sc_t* result, con { __m128i x, y, mult1, realx, imagx, realy, imagy, realx_mult_realy, imagx_mult_imagy, realx_mult_imagy, imagx_mult_realy, realc, imagc, totalc, realcacc, imagcacc; - mult1 = _mm_set_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255); + mult1 = _mm_set_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF); realcacc = _mm_setzero_si128(); imagcacc = _mm_setzero_si128(); @@ -204,7 +204,7 @@ static inline void volk_gnsssdr_8ic_x2_dot_prod_8ic_u_sse4_1(lv_8sc_t* result, c { __m128i x, y, mult1, realx, imagx, realy, imagy, realx_mult_realy, imagx_mult_imagy, realx_mult_imagy, imagx_mult_realy, realc, imagc, totalc, realcacc, imagcacc; - mult1 = _mm_set_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255); + mult1 = _mm_set_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF); realcacc = _mm_setzero_si128(); imagcacc = _mm_setzero_si128(); @@ -279,7 +279,7 @@ static inline void volk_gnsssdr_8ic_x2_dot_prod_8ic_a_sse2(lv_8sc_t* result, con { __m128i x, y, mult1, realx, imagx, realy, imagy, realx_mult_realy, imagx_mult_imagy, realx_mult_imagy, imagx_mult_realy, realc, imagc, totalc, realcacc, imagcacc; - mult1 = _mm_set_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255); + mult1 = _mm_set_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF); realcacc = _mm_setzero_si128(); imagcacc = _mm_setzero_si128(); @@ -355,7 +355,7 @@ static inline void volk_gnsssdr_8ic_x2_dot_prod_8ic_a_sse4_1(lv_8sc_t* result, c { __m128i x, y, mult1, realx, imagx, realy, imagy, realx_mult_realy, imagx_mult_imagy, realx_mult_imagy, imagx_mult_realy, realc, imagc, totalc, realcacc, imagcacc; - mult1 = _mm_set_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255); + mult1 = _mm_set_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF); realcacc = _mm_setzero_si128(); imagcacc = _mm_setzero_si128(); diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_x2_multiply_8ic.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_x2_multiply_8ic.h index 185ffa39f..1b3fd5532 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_x2_multiply_8ic.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8ic_x2_multiply_8ic.h @@ -73,7 +73,7 @@ static inline void volk_gnsssdr_8ic_x2_multiply_8ic_u_sse2(lv_8sc_t* cVector, co const lv_8sc_t* a = aVector; const lv_8sc_t* b = bVector; - mult1 = _mm_set_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255); + mult1 = _mm_set_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF); for(number = 0; number < sse_iters; number++) { @@ -131,7 +131,7 @@ static inline void volk_gnsssdr_8ic_x2_multiply_8ic_u_sse4_1(lv_8sc_t* cVector, const lv_8sc_t* b = bVector; _mm_setzero_si128(); - mult1 = _mm_set_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255); + mult1 = _mm_set_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF); for(number = 0; number < sse_iters; number++) { @@ -202,7 +202,7 @@ static inline void volk_gnsssdr_8ic_x2_multiply_8ic_a_sse2(lv_8sc_t* cVector, co const lv_8sc_t* a = aVector; const lv_8sc_t* b = bVector; - mult1 = _mm_set_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255); + mult1 = _mm_set_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF); for(number = 0; number < sse_iters; number++) { @@ -260,7 +260,7 @@ static inline void volk_gnsssdr_8ic_x2_multiply_8ic_a_sse4_1(lv_8sc_t* cVector, const lv_8sc_t* b = bVector; _mm_setzero_si128(); - mult1 = _mm_set_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255); + mult1 = _mm_set_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF); for(number = 0; number < sse_iters; number++) { diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8u_x2_multiply_8u.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8u_x2_multiply_8u.h index 156f83a48..df7a0cd7d 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8u_x2_multiply_8u.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_8u_x2_multiply_8u.h @@ -78,7 +78,7 @@ static inline void volk_gnsssdr_8u_x2_multiply_8u_u_sse3(unsigned char* cChar, c x = _mm_lddqu_si128((__m128i*)a); y = _mm_lddqu_si128((__m128i*)b); - mult1 = _mm_set_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255); + mult1 = _mm_set_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF); x1 = _mm_srli_si128(x, 1); x1 = _mm_and_si128(x1, mult1); x2 = _mm_and_si128(x, mult1); @@ -144,7 +144,7 @@ static inline void volk_gnsssdr_8u_x2_multiply_8u_a_sse3(unsigned char* cChar, c x = _mm_load_si128((__m128i*)a); y = _mm_load_si128((__m128i*)b); - mult1 = _mm_set_epi8(0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255, 0, 255); + mult1 = _mm_set_epi8(0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF, 0, 0xFF); x1 = _mm_srli_si128(x, 1); x1 = _mm_and_si128(x1, mult1); x2 = _mm_and_si128(x, mult1); diff --git a/src/algorithms/signal_source/adapters/CMakeLists.txt b/src/algorithms/signal_source/adapters/CMakeLists.txt index ad00956da..388b2ed59 100644 --- a/src/algorithms/signal_source/adapters/CMakeLists.txt +++ b/src/algorithms/signal_source/adapters/CMakeLists.txt @@ -16,6 +16,9 @@ # along with GNSS-SDR. If not, see . # +file(GLOB SIGNAL_SOURCE_ADAPTER_HEADERS "*.h") +list(SORT SIGNAL_SOURCE_ADAPTER_HEADERS) + # Optional drivers if(ENABLE_GN3S) @@ -72,26 +75,29 @@ if(ENABLE_OSMOSDR) ################################################################################ # OsmoSDR - http://sdr.osmocom.org/trac/ ################################################################################ - find_package(GrOsmoSDR REQUIRED) if(NOT GROSMOSDR_FOUND) - message(FATAL_ERROR "gr-osmosdr required to build gnss-sdr with the optional OSMOSDR driver") + if(ENABLE_PACKAGING) + list(REMOVE_ITEM SIGNAL_SOURCE_ADAPTER_HEADERS ${CMAKE_SOURCE_DIR}/src/algorithms/signal_source/adapters/osmosdr_signal_source.h) + endif(ENABLE_PACKAGING) + else(NOT GROSMOSDR_FOUND) + # set OSMO include dirs + set(OSMO_DRIVER_INCLUDE_DIRS + ${OPT_DRIVER_INCLUDE_DIRS} + ${GROSMOSDR_INCLUDE_DIR}/osmosdr + ) + set(OPT_DRIVER_SOURCES ${OPT_DRIVER_SOURCES} osmosdr_signal_source.cc) + set(OPT_LIBRARIES ${OPT_LIBRARIES} ${GROSMOSDR_LIBRARIES}) + set(OPT_DRIVER_INCLUDE_DIRS ${OPT_DRIVER_INCLUDE_DIRS} ${OSMO_DRIVER_INCLUDE_DIRS}) endif(NOT GROSMOSDR_FOUND) - - # set OSMO include dirs - set(OSMO_DRIVER_INCLUDE_DIRS - ${OPT_DRIVER_INCLUDE_DIRS} - ${GROSMOSDR_INCLUDE_DIR}/osmosdr - ) - set(OPT_DRIVER_SOURCES ${OPT_DRIVER_SOURCES} osmosdr_signal_source.cc) - set(OPT_LIBRARIES ${OPT_LIBRARIES} ${GROSMOSDR_LIBRARIES}) - set(OPT_DRIVER_INCLUDE_DIRS ${OPT_DRIVER_INCLUDE_DIRS} ${OSMO_DRIVER_INCLUDE_DIRS}) endif(ENABLE_OSMOSDR) -if(ENABLE_UHD) +if(ENABLE_UHD AND GNURADIO_UHD_LIBRARIES_gnuradio-uhd) set(OPT_DRIVER_SOURCES ${OPT_DRIVER_SOURCES} uhd_signal_source.cc) set(OPT_LIBRARIES ${OPT_LIBRARIES} ${UHD_LIBRARIES} ${GNURADIO_UHD_LIBRARIES}) set(OPT_DRIVER_INCLUDE_DIRS ${OPT_DRIVER_INCLUDE_DIRS} ${UHD_INCLUDE_DIRS}) -endif(ENABLE_UHD) +else(ENABLE_UHD AND GNURADIO_UHD_LIBRARIES_gnuradio-uhd) + list(REMOVE_ITEM SIGNAL_SOURCE_ADAPTER_HEADERS ${CMAKE_SOURCE_DIR}/src/algorithms/signal_source/adapters/uhd_signal_source.h) +endif(ENABLE_UHD AND GNURADIO_UHD_LIBRARIES_gnuradio-uhd) set(SIGNAL_SOURCE_ADAPTER_SOURCES file_signal_source.cc @@ -129,8 +135,6 @@ endif(ARCH_64BITS) add_definitions(-DGNSSSDR_INSTALL_DIR="${CMAKE_INSTALL_PREFIX}") -file(GLOB SIGNAL_SOURCE_ADAPTER_HEADERS "*.h") -list(SORT SIGNAL_SOURCE_ADAPTER_HEADERS) add_library(signal_source_adapters ${SIGNAL_SOURCE_ADAPTER_SOURCES} ${SIGNAL_SOURCE_ADAPTER_HEADERS}) source_group(Headers FILES ${SIGNAL_SOURCE_ADAPTER_HEADERS}) target_link_libraries(signal_source_adapters signal_source_gr_blocks @@ -140,4 +144,3 @@ target_link_libraries(signal_source_adapters signal_source_gr_blocks ${OPT_LIBRARIES} gnss_sp_libs ) - diff --git a/src/core/receiver/CMakeLists.txt b/src/core/receiver/CMakeLists.txt index 99ec3b69d..dc1f24f1b 100644 --- a/src/core/receiver/CMakeLists.txt +++ b/src/core/receiver/CMakeLists.txt @@ -80,7 +80,7 @@ include_directories( ${OPT_RECEIVER_INCLUDE_DIRS} ${VOLK_GNSSSDR_INCLUDE_DIRS} ) - + if(Boost_VERSION LESS 105000) add_definitions(-DOLD_BOOST=1) endif(Boost_VERSION LESS 105000) @@ -102,12 +102,14 @@ if(ENABLE_FLEXIBAND) endif(ENABLE_FLEXIBAND) if(ENABLE_OSMOSDR) - add_definitions(-DOSMOSDR_DRIVER=1) + if(GROSMOSDR_FOUND) + add_definitions(-DOSMOSDR_DRIVER=1) + endif(GROSMOSDR_FOUND) endif(ENABLE_OSMOSDR) -if(ENABLE_UHD) +if(ENABLE_UHD AND GNURADIO_UHD_LIBRARIES_gnuradio-uhd) add_definitions(-DUHD_DRIVER=1) -endif(ENABLE_UHD) +endif(ENABLE_UHD AND GNURADIO_UHD_LIBRARIES_gnuradio-uhd) #Enable OpenCL if found in the system if(OPENCL_FOUND) diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index ec6b79fe2..6c50153a8 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -35,19 +35,20 @@ #include #include // std::pair #include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" // Physical constants -const double GPS_C_m_s = 299792458.0; //!< The speed of light, [m/s] +const double GPS_C_m_s = SPEED_OF_LIGHT; //!< The speed of light, [m/s] const double GPS_C_m_ms = 299792.4580; //!< The speed of light, [m/ms] const double GPS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E const double GPS_TWO_PI = 6.283185307179586;//!< 2Pi as defined in IS-GPS-200E -const double OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Earth rotation rate, [rad/s] +const double OMEGA_EARTH_DOT = DEFAULT_OMEGA_EARTH_DOT; //!< Earth rotation rate, [rad/s] const double GM = 3.986005e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2] const double F = -4.442807633e-10; //!< Constant, [s/(m)^(1/2)] // carrier and code frequencies -const double GPS_L1_FREQ_HZ = 1.57542e9; //!< L1 [Hz] +const double GPS_L1_FREQ_HZ = FREQ1; //!< L1 [Hz] const double GPS_L1_CA_CODE_RATE_HZ = 1.023e6; //!< GPS L1 C/A code rate [chips/s] const double GPS_L1_CA_CODE_LENGTH_CHIPS = 1023.0; //!< GPS L1 C/A code length [chips] const double GPS_L1_CA_CODE_PERIOD = 0.001; //!< GPS L1 C/A code period [seconds] diff --git a/src/core/system_parameters/GPS_L2C.h b/src/core/system_parameters/GPS_L2C.h index c6c703952..689cc524f 100644 --- a/src/core/system_parameters/GPS_L2C.h +++ b/src/core/system_parameters/GPS_L2C.h @@ -36,6 +36,7 @@ #include #include // std::pair #include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" // Physical constants const double GPS_L2_C_m_s = 299792458.0; //!< The speed of light, [m/s] @@ -48,7 +49,7 @@ const double GPS_L2_F = -4.442807633e-10; //!< Constant, [s/(m)^(1 // carrier and code frequencies -const double GPS_L2_FREQ_HZ = 1.2276e9; //!< L2 [Hz] +const double GPS_L2_FREQ_HZ = FREQ2; //!< L2 [Hz] const double GPS_L2_M_CODE_RATE_HZ = 0.5115e6; //!< GPS L2 M code rate [chips/s] const int GPS_L2_M_CODE_LENGTH_CHIPS = 10230; //!< GPS L2 M code length [chips] diff --git a/src/core/system_parameters/Galileo_E1.h b/src/core/system_parameters/Galileo_E1.h index 258a9e954..2790cd11c 100644 --- a/src/core/system_parameters/Galileo_E1.h +++ b/src/core/system_parameters/Galileo_E1.h @@ -37,6 +37,7 @@ #include #include // std::pair #include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" // Physical constants const double GALILEO_PI = 3.1415926535898; //!< Pi as defined in GALILEO ICD @@ -48,7 +49,7 @@ const double GALILEO_C_m_ms = 299792.4580; //!< The speed of light, [m/ms] const double GALILEO_F = -4.442807309e-10; //!< Constant, [s/(m)^(1/2)] // carrier and code frequencies -const double Galileo_E1_FREQ_HZ = 1.57542e9; //!< Galileo E1 carrier frequency [Hz] +const double Galileo_E1_FREQ_HZ = FREQ1; //!< Galileo E1 carrier frequency [Hz] const double Galileo_E1_CODE_CHIP_RATE_HZ = 1.023e6; //!< Galileo E1 code rate [chips/s] const double Galileo_E1_CODE_PERIOD = 0.004; //!< Galileo E1 code period [s] const double Galileo_E1_SUB_CARRIER_A_RATE_HZ = 1.023e6; //!< Galileo E1 sub-carrier 'a' rate [Hz] diff --git a/src/core/system_parameters/Galileo_E5a.h b/src/core/system_parameters/Galileo_E5a.h index 70ec19a1e..cbd16c752 100644 --- a/src/core/system_parameters/Galileo_E5a.h +++ b/src/core/system_parameters/Galileo_E5a.h @@ -35,11 +35,11 @@ #include #include // std::pair #include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" -// Physical constants already defined in E1 // Carrier and code frequencies -const double Galileo_E5a_FREQ_HZ = 1.176450e9; //!< Galileo E5a carrier frequency [Hz] +const double Galileo_E5a_FREQ_HZ = FREQ5; //!< Galileo E5a carrier frequency [Hz] const double Galileo_E5a_CODE_CHIP_RATE_HZ = 1.023e7; //!< Galileo E5a code rate [chips/s] const double Galileo_E5a_I_TIERED_CODE_PERIOD = 0.020; //!< Galileo E5a-I tiered code period [s] const double Galileo_E5a_Q_TIERED_CODE_PERIOD = 0.100; //!< Galileo E5a-Q tiered code period [s] diff --git a/src/core/system_parameters/MATH_CONSTANTS.h b/src/core/system_parameters/MATH_CONSTANTS.h index 1ed316ed5..9be1a41cf 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -40,6 +40,9 @@ PI_TWO_PX ==> Pi*2^X ONE_PI_TWO_PX = (1/Pi)*2^X */ + +const double PI = 3.1415926535897932; //!< pi + const double TWO_P4 = (16); //!< 2^4 const double TWO_P11 = (2048); //!< 2^11 const double TWO_P12 = (4096); //!< 2^12 @@ -53,6 +56,7 @@ const double TWO_P57 = (1.441151880758559e+017); //!< 2^57 const double TWO_N2 = (0.25); //!< 2^-2 const double TWO_N5 = (0.03125); //!< 2^-5 +const double TWO_N6 = (0.015625); //!< 2^-6 const double TWO_N8 = (0.00390625); //!< 2^-8 const double TWO_N9 = (0.001953125); //!< 2^-9 const double TWO_N10 = (0.0009765625); //!< 2^-10 @@ -60,9 +64,11 @@ const double TWO_N11 = (4.882812500000000e-004); //!< 2^-11 const double TWO_N14 = (0.00006103515625); //!< 2^-14 const double TWO_N15 = (0.00003051757813); //!< 2^-15 const double TWO_N16 = (0.0000152587890625); //!< 2^-16 +const double TWO_N17 = (7.629394531250000e-006); //!< 2^-17 const double TWO_N19 = (1.907348632812500e-006); //!< 2^-19 const double TWO_N20 = (9.536743164062500e-007); //!< 2^-20 const double TWO_N21 = (4.768371582031250e-007); //!< 2^-21 +const double TWO_N23 = (1.192092895507810e-007); //!< 2^-23 const double TWO_N24 = (5.960464477539063e-008); //!< 2^-24 const double TWO_N25 = (2.980232238769531e-008); //!< 2^-25 const double TWO_N27 = (7.450580596923828e-009); //!< 2^-27 @@ -73,8 +79,9 @@ const double TWO_N32 = (2.328306436538696e-010); //!< 2^-32 const double TWO_N33 = (1.164153218269348e-010); //!< 2^-33 const double TWO_N34 = (5.82076609134674e-011); //!< 2^-34 const double TWO_N35 = (2.91038304567337e-011); //!< 2^-35 - const double TWO_N38 = (3.637978807091713e-012); //!< 2^-38 +const double TWO_N39 = (1.818989403545856e-012); //!< 2^-39 +const double TWO_N40 = (9.094947017729280e-013); //!< 2^-40 const double TWO_N43 = (1.136868377216160e-013); //!< 2^-43 const double TWO_N44 = (5.684341886080802e-14); //!< 2^-44 const double TWO_N46 = (1.4210854715202e-014); //!< 2^-46 @@ -95,4 +102,13 @@ const double PI_TWO_N31 = (1.462918079267160e-009); //!< Pi*2^-31 const double PI_TWO_N38 = (1.142904749427469e-011); //!< Pi*2^-38 const double PI_TWO_N23 = (3.745070282923929e-007); //!< Pi*2^-23 +const double D2R = (PI/180.0); //!< deg to rad +const double R2D = (180.0/PI); //!< rad to deg +const double SC2RAD = 3.1415926535898; //!< semi-circle to radian (IS-GPS) +const double AS2R = (D2R / 3600.0); //!< arc sec to radian + +const double DEFAULT_OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Default Earth rotation rate, [rad/s] +const double SPEED_OF_LIGHT = 299792458.0; //!< [m/s] +const double AU = 149597870691.0; //!< 1 Astronomical Unit AU (m) distance from Earth to the Sun. + #endif /* GNSS_SDR_MATH_CONSTANTS_H_ */ diff --git a/src/core/system_parameters/gnss_frequencies.h b/src/core/system_parameters/gnss_frequencies.h new file mode 100644 index 000000000..bd85aa414 --- /dev/null +++ b/src/core/system_parameters/gnss_frequencies.h @@ -0,0 +1,53 @@ +/*! + * \file gnss_frequencies.h + * \brief GNSS Frequencies + * \author Carles Fernandez, 2017. cfernandez(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GNSS_FREQUENCIES_H_ +#define GNSS_SDR_GNSS_FREQUENCIES_H_ + +const double FREQ1 = 1.57542e9; //!< L1/E1 frequency (Hz) +const double FREQ2 = 1.22760e9; //!< L2 frequency (Hz) +const double FREQ5 = 1.17645e9; //!< L5/E5a frequency (Hz) +const double FREQ6 = 1.27875e9; //!< E6/LEX frequency (Hz) +const double FREQ7 = 1.20714e9; //!< E5b frequency (Hz) +const double FREQ8 = 1.191795e9; //!< E5a+b frequency (Hz) +const double FREQ9 = 2.492028e9; //!< S frequency (Hz) +const double FREQ1_GLO = 1.60200e9; //!< GLONASS G1 base frequency (Hz) +const double DFRQ1_GLO = 0.56250e6; //!< GLONASS G1 bias frequency (Hz/n) +const double FREQ2_GLO = 1.24600e9; //!< GLONASS G2 base frequency (Hz) +const double DFRQ2_GLO = 0.43750e6; //!< GLONASS G2 bias frequency (Hz/n) +const double FREQ3_GLO = 1.202025e9; //!< GLONASS G3 frequency (Hz) +const double FREQ1_BDS = 1.561098e9; //!< BeiDou B1 frequency (Hz) +const double FREQ2_BDS = 1.20714e9; //!< BeiDou B2 frequency (Hz) +const double FREQ3_BDS = 1.26852e9; //!< BeiDou B3 frequency (Hz) + +#endif + diff --git a/src/core/system_parameters/gnss_obs_codes.h b/src/core/system_parameters/gnss_obs_codes.h new file mode 100644 index 000000000..1c17cafc9 --- /dev/null +++ b/src/core/system_parameters/gnss_obs_codes.h @@ -0,0 +1,97 @@ +/*! + * \file gnss_obs_codes.h + * \brief GNSS Observable codes + * \author Carles Fernandez, 2017. cfernandez(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GNSS_OBS_CODES_H_ +#define GNSS_SDR_GNSS_OBS_CODES_H_ + + + +const unsigned int CODE_NONE = 0; //!< obs code: none or unknown +const unsigned int CODE_L1C = 1; //!< obs code: L1C/A,G1C/A,E1C (GPS,GLO,GAL,QZS,SBS) +const unsigned int CODE_L1P = 2; //!< obs code: L1P,G1P (GPS,GLO) +const unsigned int CODE_L1W = 3; //!< obs code: L1 Z-track (GPS) +const unsigned int CODE_L1Y = 4; //!< obs code: L1Y (GPS) +const unsigned int CODE_L1M = 5; //!< obs code: L1M (GPS) +const unsigned int CODE_L1N = 6; //!< obs code: L1codeless (GPS) +const unsigned int CODE_L1S = 7; //!< obs code: L1C(D) (GPS,QZS) +const unsigned int CODE_L1L = 8; //!< obs code: L1C(P) (GPS,QZS) +const unsigned int CODE_L1E = 9; //!< (not used) +const unsigned int CODE_L1A = 10; //!< obs code: E1A (GAL) +const unsigned int CODE_L1B = 11; //!< obs code: E1B (GAL) +const unsigned int CODE_L1X = 12; //!< obs code: E1B+C,L1C(D+P) (GAL,QZS) +const unsigned int CODE_L1Z = 13; //!< obs code: E1A+B+C,L1SAIF (GAL,QZS) +const unsigned int CODE_L2C = 14; //!< obs code: L2C/A,G1C/A (GPS,GLO) +const unsigned int CODE_L2D = 15; //!< obs code: L2 L1C/A-(P2-P1) (GPS) +const unsigned int CODE_L2S = 16; //!< obs code: L2C(M) (GPS,QZS) +const unsigned int CODE_L2L = 17; //!< obs code: L2C(L) (GPS,QZS) +const unsigned int CODE_L2X = 18; //!< obs code: L2C(M+L),B1I+Q (GPS,QZS,BDS) +const unsigned int CODE_L2P = 19; //!< obs code: L2P,G2P (GPS,GLO) +const unsigned int CODE_L2W = 20; //!< obs code: L2 Z-track (GPS) +const unsigned int CODE_L2Y = 21; //!< obs code: L2Y (GPS) +const unsigned int CODE_L2M = 22; //!< obs code: L2M (GPS) +const unsigned int CODE_L2N = 23; //!< obs code: L2codeless (GPS) +const unsigned int CODE_L5I = 24; //!< obs code: L5/E5aI (GPS,GAL,QZS,SBS) +const unsigned int CODE_L5Q = 25; //!< obs code: L5/E5aQ (GPS,GAL,QZS,SBS) +const unsigned int CODE_L5X = 26; //!< obs code: L5/E5aI+Q/L5B+C (GPS,GAL,QZS,IRN,SBS) +const unsigned int CODE_L7I = 27; //!< obs code: E5bI,B2I (GAL,BDS) +const unsigned int CODE_L7Q = 28; //!< obs code: E5bQ,B2Q (GAL,BDS) +const unsigned int CODE_L7X = 29; //!< obs code: E5bI+Q,B2I+Q (GAL,BDS) +const unsigned int CODE_L6A = 30; //!< obs code: E6A (GAL) +const unsigned int CODE_L6B = 31; //!< obs code: E6B (GAL) +const unsigned int CODE_L6C = 32; //!< obs code: E6C (GAL) +const unsigned int CODE_L6X = 33; //!< obs code: E6B+C,LEXS+L,B3I+Q (GAL,QZS,BDS) +const unsigned int CODE_L6Z = 34; //!< obs code: E6A+B+C (GAL) +const unsigned int CODE_L6S = 35; //!< obs code: LEXS (QZS) +const unsigned int CODE_L6L = 36; //!< obs code: LEXL (QZS) +const unsigned int CODE_L8I = 37; //!< obs code: E5(a+b)I (GAL) +const unsigned int CODE_L8Q = 38; //!< obs code: E5(a+b)Q (GAL) +const unsigned int CODE_L8X = 39; //!< obs code: E5(a+b)I+Q (GAL) +const unsigned int CODE_L2I = 40; //!< obs code: B1I (BDS) +const unsigned int CODE_L2Q = 41; //!< obs code: B1Q (BDS) +const unsigned int CODE_L6I = 42; //!< obs code: B3I (BDS) +const unsigned int CODE_L6Q = 43; //!< obs code: B3Q (BDS) +const unsigned int CODE_L3I = 44; //!< obs code: G3I (GLO) +const unsigned int CODE_L3Q = 45; //!< obs code: G3Q (GLO) +const unsigned int CODE_L3X = 46; //!< obs code: G3I+Q (GLO) +const unsigned int CODE_L1I = 47; //!< obs code: B1I (BDS) +const unsigned int CODE_L1Q = 48; //!< obs code: B1Q (BDS) +const unsigned int CODE_L5A = 49; //!< obs code: L5A SPS (IRN) +const unsigned int CODE_L5B = 50; //!< obs code: L5B RS(D) (IRN) +const unsigned int CODE_L5C = 51; //!< obs code: L5C RS(P) (IRN) +const unsigned int CODE_L9A = 52; //!< obs code: SA SPS (IRN) +const unsigned int CODE_L9B = 53; //!< obs code: SB RS(D) (IRN) +const unsigned int CODE_L9C = 54; //!< obs code: SC RS(P) (IRN) +const unsigned int CODE_L9X = 55; //!< obs code: SB+C (IRN) +const unsigned int MAXCODE = 55; //!< max number of obs code + + +#endif diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index f8ef0b376..6a8e42a1b 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -28,10 +28,10 @@ if(ENABLE_GPERFTOOLS) endif(GPERFTOOLS_FOUND) endif(ENABLE_GPERFTOOLS) -if(ENABLE_UHD) +if(ENABLE_UHD AND GNURADIO_UHD_LIBRARIES_gnuradio-uhd) set(GNSS_SDR_OPTIONAL_LIBS ${GNSS_SDR_OPTIONAL_LIBS} ${UHD_LIBRARIES} ${GNURADIO_UHD_LIBRARIES}) set(GNSS_SDR_OPTIONAL_HEADERS ${GNSS_SDR_OPTIONAL_HEADERS} ${UHD_INCLUDE_DIRS}) -endif(ENABLE_UHD) +endif(ENABLE_UHD AND GNURADIO_UHD_LIBRARIES_gnuradio-uhd) if(OPENSSL_FOUND) add_definitions( -DUSE_OPENSSL_FALLBACK=1 ) @@ -127,16 +127,22 @@ find_program(GZIP ) if(NOT GZIP_NOTFOUND) execute_process(COMMAND gzip -9 -c ${CMAKE_SOURCE_DIR}/docs/manpage/gnss-sdr-manpage - WORKING_DIRECTORY ${CMAKE_BINARY_DIR} OUTPUT_FILE "${CMAKE_BINARY_DIR}/gnss-sdr.1.gz") + WORKING_DIRECTORY ${CMAKE_BINARY_DIR} OUTPUT_FILE "${CMAKE_BINARY_DIR}/gnss-sdr.1.gz") - install(FILES ${CMAKE_BINARY_DIR}/gnss-sdr.1.gz DESTINATION share/man/man1) - if(NOT VOLK_GNSSSDR_FOUND) + install(FILES ${CMAKE_BINARY_DIR}/gnss-sdr.1.gz DESTINATION share/man/man1) + + execute_process(COMMAND gzip -9 -c ${CMAKE_SOURCE_DIR}/docs/changelog + WORKING_DIRECTORY ${CMAKE_BINARY_DIR} OUTPUT_FILE "${CMAKE_BINARY_DIR}/changelog.gz") + + install(FILES ${CMAKE_BINARY_DIR}/changelog.gz DESTINATION share/doc/gnss-sdr) + + if(NOT VOLK_GNSSSDR_FOUND) execute_process(COMMAND gzip -9 -c ${CMAKE_SOURCE_DIR}/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/cmake/Packaging/volk_gnsssdr_profile-manpage WORKING_DIRECTORY ${CMAKE_BINARY_DIR} OUTPUT_FILE "${CMAKE_BINARY_DIR}/volk_gnsssdr_profile.1.gz") execute_process(COMMAND gzip -9 -c ${CMAKE_SOURCE_DIR}/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/cmake/Packaging/volk_gnsssdr-config-info-manpage WORKING_DIRECTORY ${CMAKE_BINARY_DIR} OUTPUT_FILE "${CMAKE_BINARY_DIR}/volk_gnsssdr-config-info.1.gz") - + install(FILES ${CMAKE_BINARY_DIR}/volk_gnsssdr_profile.1.gz DESTINATION share/man/man1) install(FILES ${CMAKE_BINARY_DIR}/volk_gnsssdr-config-info.1.gz DESTINATION share/man/man1) - endif(NOT VOLK_GNSSSDR_FOUND) + endif(NOT VOLK_GNSSSDR_FOUND) endif(NOT GZIP_NOTFOUND) diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 42138bec6..cb1533246 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -392,6 +392,7 @@ if(ENABLE_SYSTEM_TESTING) gnss_rx gnss_system_parameters ) + if(ENABLE_INSTALL_TESTS) if(EXISTS ${CMAKE_SOURCE_DIR}/install/ttff) file(REMOVE ${CMAKE_SOURCE_DIR}/install/ttff) @@ -404,6 +405,39 @@ if(ENABLE_SYSTEM_TESTING) endif(ENABLE_INSTALL_TESTS) if(ENABLE_SYSTEM_TESTING_EXTRA) + add_executable(position_test + ${CMAKE_CURRENT_SOURCE_DIR}/system-tests/position_test.cc ) + if(NOT ${GTEST_DIR_LOCAL}) + add_dependencies(position_test gtest-${gtest_RELEASE}) + else(NOT ${GTEST_DIR_LOCAL}) + add_dependencies(position_test gtest) + endif(NOT ${GTEST_DIR_LOCAL}) + target_link_libraries(position_test + ${Boost_LIBRARIES} + ${GFlags_LIBS} + ${GLOG_LIBRARIES} + ${GTEST_LIBRARIES} + ${GNURADIO_RUNTIME_LIBRARIES} + ${GNURADIO_BLOCKS_LIBRARIES} + ${GNURADIO_FILTER_LIBRARIES} + ${GNURADIO_ANALOG_LIBRARIES} + ${VOLK_GNSSSDR_LIBRARIES} + gnss_sp_libs + gnss_rx + gnss_system_parameters + ) + + if(ENABLE_INSTALL_TESTS) + if(EXISTS ${CMAKE_SOURCE_DIR}/install/position_test) + file(REMOVE ${CMAKE_SOURCE_DIR}/install/position_test) + endif(EXISTS ${CMAKE_SOURCE_DIR}/install/position_test) + install(TARGETS position_test RUNTIME DESTINATION bin COMPONENT "position_test") + else(ENABLE_INSTALL_TESTS) + add_custom_command(TARGET position_test POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy $ + ${CMAKE_SOURCE_DIR}/install/$ ) + endif(ENABLE_INSTALL_TESTS) + if(GPSTK_FOUND OR OWN_GPSTK) add_executable(obs_gps_l1_system_test ${CMAKE_CURRENT_SOURCE_DIR}/system-tests/obs_gps_l1_system_test.cc) if(NOT ${GTEST_DIR_LOCAL}) @@ -418,6 +452,7 @@ if(ENABLE_SYSTEM_TESTING) gnss_sp_libs gnss_rx ${gpstk_libs}) + if(ENABLE_INSTALL_TESTS) if(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test) file(REMOVE ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test) @@ -430,6 +465,7 @@ if(ENABLE_SYSTEM_TESTING) endif(ENABLE_INSTALL_TESTS) endif(GPSTK_FOUND OR OWN_GPSTK) endif(ENABLE_SYSTEM_TESTING_EXTRA) + endif(ENABLE_SYSTEM_TESTING) @@ -439,115 +475,98 @@ endif(ENABLE_SYSTEM_TESTING) set(CMAKE_CTEST_COMMAND ctest -V) add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND}) -add_executable(control_thread_test - ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc - ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/control-plane/control_message_factory_test.cc - ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/control-plane/control_thread_test.cc -) if(NOT ${ENABLE_PACKAGING}) + add_executable(control_thread_test ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/control-plane/control_message_factory_test.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/control-plane/control_thread_test.cc + ) + target_link_libraries(control_thread_test ${Boost_LIBRARIES} + ${GFlags_LIBS} + ${GLOG_LIBRARIES} + ${GTEST_LIBRARIES} + gnss_sp_libs + gnss_system_parameters + gnss_rx + ${VOLK_GNSSSDR_LIBRARIES} + ) + add_test(control_thread_test control_thread_test) + if(NOT ${GTEST_DIR_LOCAL}) + add_dependencies(control_thread_test gtest-${gtest_RELEASE}) + else(NOT ${GTEST_DIR_LOCAL}) + add_dependencies(control_thread_test gtest) + endif(NOT ${GTEST_DIR_LOCAL}) + set_property(TEST control_thread_test PROPERTY TIMEOUT 30) set_property(TARGET control_thread_test PROPERTY EXCLUDE_FROM_ALL TRUE) -endif(NOT ${ENABLE_PACKAGING}) - -target_link_libraries(control_thread_test ${Boost_LIBRARIES} + + ######################################################### + + add_executable(flowgraph_test ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/control-plane/gnss_flowgraph_test.cc ) + set_property(TARGET flowgraph_test PROPERTY EXCLUDE_FROM_ALL TRUE) + target_link_libraries(flowgraph_test ${Boost_LIBRARIES} ${GFlags_LIBS} ${GLOG_LIBRARIES} ${GTEST_LIBRARIES} gnss_sp_libs - gnss_system_parameters gnss_rx - ${VOLK_GNSSSDR_LIBRARIES} - ) - -add_test(control_thread_test control_thread_test) -if(NOT ${GTEST_DIR_LOCAL}) - add_dependencies(control_thread_test gtest-${gtest_RELEASE}) -else(NOT ${GTEST_DIR_LOCAL}) - add_dependencies(control_thread_test gtest) -endif(NOT ${GTEST_DIR_LOCAL}) -set_property(TEST control_thread_test PROPERTY TIMEOUT 30) - -add_executable(flowgraph_test - ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc - ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/control-plane/gnss_flowgraph_test.cc -) -if(NOT ${ENABLE_PACKAGING}) + gnss_system_parameters + ${VOLK_GNSSSDR_LIBRARIES} ) + add_test(flowgraph_test flowgraph_test) + if(NOT ${GTEST_DIR_LOCAL}) + add_dependencies(flowgraph_test gtest-${gtest_RELEASE}) + else(NOT ${GTEST_DIR_LOCAL}) + add_dependencies(flowgraph_test gtest) + endif(NOT ${GTEST_DIR_LOCAL}) + set_property(TEST flowgraph_test PROPERTY TIMEOUT 30) set_property(TARGET flowgraph_test PROPERTY EXCLUDE_FROM_ALL TRUE) -endif(NOT ${ENABLE_PACKAGING}) - -target_link_libraries(flowgraph_test ${Boost_LIBRARIES} - ${GFlags_LIBS} - ${GLOG_LIBRARIES} - ${GTEST_LIBRARIES} - gnss_sp_libs - gnss_rx - gnss_system_parameters - ${VOLK_GNSSSDR_LIBRARIES} - ) - -add_test(flowgraph_test flowgraph_test) -if(NOT ${GTEST_DIR_LOCAL}) - add_dependencies(flowgraph_test gtest-${gtest_RELEASE}) -else(NOT ${GTEST_DIR_LOCAL}) - add_dependencies(flowgraph_test gtest) -endif(NOT ${GTEST_DIR_LOCAL}) -set_property(TEST flowgraph_test PROPERTY TIMEOUT 30) - -add_executable(gnss_block_test - ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc - ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/sources/file_signal_source_test.cc - ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/filter/fir_filter_test.cc - ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/adapter/pass_through_test.cc - ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/control-plane/gnss_block_factory_test.cc -) -if(NOT ${ENABLE_PACKAGING}) + + ######################################################### + + add_executable(gnss_block_test ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/sources/file_signal_source_test.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/filter/fir_filter_test.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/adapter/pass_through_test.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/control-plane/gnss_block_factory_test.cc + ) set_property(TARGET gnss_block_test PROPERTY EXCLUDE_FROM_ALL TRUE) + target_link_libraries(gnss_block_test ${Boost_LIBRARIES} + ${GFlags_LIBS} + ${GLOG_LIBRARIES} + ${GTEST_LIBRARIES} + ${GNURADIO_RUNTIME_LIBRARIES} + ${GNURADIO_BLOCKS_LIBRARIES} + ${GNURADIO_FILTER_LIBRARIES} + ${GNURADIO_ANALOG_LIBRARIES} + gnss_sp_libs + gnss_rx + gnss_system_parameters + ${VOLK_GNSSSDR_LIBRARIES} + ) + add_test(gnss_block_test gnss_block_test) + if(NOT ${GTEST_DIR_LOCAL}) + add_dependencies(gnss_block_test gtest-${gtest_RELEASE}) + else(NOT ${GTEST_DIR_LOCAL}) + add_dependencies(gnss_block_test gtest) + endif(NOT ${GTEST_DIR_LOCAL}) endif(NOT ${ENABLE_PACKAGING}) -target_link_libraries(gnss_block_test ${Boost_LIBRARIES} - ${GFlags_LIBS} - ${GLOG_LIBRARIES} - ${GTEST_LIBRARIES} - ${GNURADIO_RUNTIME_LIBRARIES} - ${GNURADIO_BLOCKS_LIBRARIES} - ${GNURADIO_FILTER_LIBRARIES} - ${GNURADIO_ANALOG_LIBRARIES} - gnss_sp_libs - gnss_rx - gnss_system_parameters - # signal_generator_blocks - ${VOLK_GNSSSDR_LIBRARIES} - ) - -add_test(gnss_block_test gnss_block_test) -if(NOT ${GTEST_DIR_LOCAL}) - add_dependencies(gnss_block_test gtest-${gtest_RELEASE}) -else(NOT ${GTEST_DIR_LOCAL}) - add_dependencies(gnss_block_test gtest) -endif(NOT ${GTEST_DIR_LOCAL}) - -add_executable(gnuradio_block_test - ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc - ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/sources/unpack_2bit_samples_test.cc -) -if(NOT ${ENABLE_PACKAGING}) - set_property(TARGET gnuradio_block_test PROPERTY EXCLUDE_FROM_ALL TRUE) -endif(NOT ${ENABLE_PACKAGING}) +add_executable(gnuradio_block_test ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/sources/unpack_2bit_samples_test.cc + ) target_link_libraries(gnuradio_block_test ${Boost_LIBRARIES} - ${GFlags_LIBS} - ${GLOG_LIBRARIES} - ${GTEST_LIBRARIES} - ${GNURADIO_RUNTIME_LIBRARIES} - ${GNURADIO_BLOCKS_LIBRARIES} - ${GNURADIO_FILTER_LIBRARIES} - ${GNURADIO_ANALOG_LIBRARIES} - gnss_sp_libs - gnss_rx - gnss_system_parameters - # signal_generator_blocks - ${VOLK_GNSSSDR_LIBRARIES} - ) - + ${GFlags_LIBS} + ${GLOG_LIBRARIES} + ${GTEST_LIBRARIES} + ${GNURADIO_RUNTIME_LIBRARIES} + ${GNURADIO_BLOCKS_LIBRARIES} + ${GNURADIO_FILTER_LIBRARIES} + ${GNURADIO_ANALOG_LIBRARIES} + gnss_sp_libs + gnss_rx + gnss_system_parameters + ${VOLK_GNSSSDR_LIBRARIES} + ) add_test(gnuradio_block_test gnuradio_block_test) if(NOT ${GTEST_DIR_LOCAL}) add_dependencies(gnuradio_block_test gtest-${gtest_RELEASE}) @@ -576,16 +595,10 @@ endif(NOT ${GTEST_DIR_LOCAL}) # ) # add_test(acq_test acq_test) -add_executable(trk_test - ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc - ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc - ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc - ) - -if(NOT ${ENABLE_PACKAGING}) - set_property(TARGET trk_test PROPERTY EXCLUDE_FROM_ALL TRUE) -endif(NOT ${ENABLE_PACKAGING}) - +add_executable(trk_test ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc + ) target_link_libraries(trk_test ${Boost_LIBRARIES} ${GFlags_LIBS} ${GLOG_LIBRARIES} @@ -599,7 +612,7 @@ target_link_libraries(trk_test ${Boost_LIBRARIES} gnss_system_parameters signal_generator_blocks ${VOLK_GNSSSDR_LIBRARIES} - ) + ) add_test(trk_test trk_test) if(NOT ${GTEST_DIR_LOCAL}) add_dependencies(trk_test gtest-${gtest_RELEASE}) @@ -607,5 +620,10 @@ else(NOT ${GTEST_DIR_LOCAL}) add_dependencies(trk_test gtest) endif(NOT ${GTEST_DIR_LOCAL}) -add_dependencies(check control_thread_test flowgraph_test gnss_block_test - gnuradio_block_test trk_test) + +if(${ENABLE_PACKAGING}) + add_dependencies(check gnuradio_block_test trk_test) +else(${ENABLE_PACKAGING}) + add_dependencies(check control_thread_test flowgraph_test gnss_block_test + gnuradio_block_test trk_test) +endif(${ENABLE_PACKAGING}) \ No newline at end of file diff --git a/src/tests/single_test_main.cc b/src/tests/single_test_main.cc index 12b44d943..f9f91573b 100644 --- a/src/tests/single_test_main.cc +++ b/src/tests/single_test_main.cc @@ -48,10 +48,10 @@ #include "gps_ref_location.h" #include "gps_ref_time.h" #include "galileo_navigation_message.h" -#include "sbas_ionospheric_correction.h" -#include "sbas_telemetry_data.h" -#include "sbas_ephemeris.h" -#include "sbas_satellite_correction.h" +//#include "sbas_ionospheric_correction.h" +//#include "sbas_telemetry_data.h" +//#include "sbas_ephemeris.h" +//#include "sbas_satellite_correction.h" concurrent_queue global_gps_acq_assist_queue; diff --git a/src/tests/system-tests/obs_gps_l1_system_test.cc b/src/tests/system-tests/obs_gps_l1_system_test.cc index 2e8dc3db6..2cb098286 100644 --- a/src/tests/system-tests/obs_gps_l1_system_test.cc +++ b/src/tests/system-tests/obs_gps_l1_system_test.cc @@ -33,10 +33,12 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include @@ -57,6 +59,7 @@ concurrent_queue global_gps_acq_assist_queue; concurrent_map global_gps_acq_assist_map; + class Obs_Gps_L1_System_Test: public ::testing::Test { public: @@ -71,7 +74,7 @@ public: std::string filename_rinex_obs = FLAGS_filename_rinex_obs; std::string filename_raw_data = FLAGS_filename_raw_data; - + std::string generated_rinex_obs; int configure_generator(); int generate_signal(); int configure_receiver(); @@ -82,7 +85,6 @@ public: double compute_stdev(const std::vector & vec); std::shared_ptr config; - std::string generated_rinex_obs; }; @@ -341,6 +343,7 @@ int Obs_Gps_L1_System_Test::run_receiver() std::cout << "STD exception: " << ex.what(); } // Get the name of the RINEX obs file generated by the receiver + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); FILE *fp; std::string argum2 = std::string("/bin/ls *O | grep GSDR | tail -1"); char buffer[1035]; @@ -350,15 +353,12 @@ int Obs_Gps_L1_System_Test::run_receiver() std::cout << "Failed to run command: " << argum2 << std::endl; return -1; } - char * without_trailing = (char*)"0"; while (fgets(buffer, sizeof(buffer), fp) != NULL) { std::string aux = std::string(buffer); - without_trailing = strtok(&aux[0], "\n"); + Obs_Gps_L1_System_Test::generated_rinex_obs = aux.erase(aux.length() - 1, 1); } - generated_rinex_obs = std::string(without_trailing); pclose(fp); - return 0; } @@ -436,7 +436,7 @@ void Obs_Gps_L1_System_Test::check_results() try { - std::string arg2_gen = std::string("./") + generated_rinex_obs; + std::string arg2_gen = std::string("./") + Obs_Gps_L1_System_Test::generated_rinex_obs; gpstk::Rinex3ObsStream r_meas(arg2_gen); r_meas.exceptions(std::ios::failbit); gpstk::Rinex3ObsData r_meas_data; @@ -685,9 +685,9 @@ TEST_F(Obs_Gps_L1_System_Test, Observables_system_test) // Run the receiver EXPECT_EQ( run_receiver(), 0) << "Problem executing the software-defined signal generator"; - std::cout << "Validating RINEX obs file obtained by GNSS-SDR: " << generated_rinex_obs << " ..." << std::endl; - is_gen_rinex_obs_valid = check_valid_rinex_obs( "./" + generated_rinex_obs); - EXPECT_EQ(true, is_gen_rinex_obs_valid) << "The RINEX observation file " << generated_rinex_obs << ", generated by GNSS-SDR, is not well formed."; + std::cout << "Validating RINEX obs file obtained by GNSS-SDR: " << Obs_Gps_L1_System_Test::generated_rinex_obs << " ..." << std::endl; + is_gen_rinex_obs_valid = check_valid_rinex_obs( "./" + Obs_Gps_L1_System_Test::generated_rinex_obs); + EXPECT_EQ(true, is_gen_rinex_obs_valid) << "The RINEX observation file " << Obs_Gps_L1_System_Test::generated_rinex_obs << ", generated by GNSS-SDR, is not well formed."; std::cout << "The file is valid." << std::endl; // Check results diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc new file mode 100644 index 000000000..a5ef80eac --- /dev/null +++ b/src/tests/system-tests/position_test.cc @@ -0,0 +1,557 @@ +/*! + * \file position_test.cc + * \brief This class implements a test for the validation of computed position. + * \author Carles Fernandez-Prades, 2016. cfernandez(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include +#include +#include +#include +#include +#include +#include "concurrent_map.h" +#include "concurrent_queue.h" +#include "control_thread.h" +#include "in_memory_configuration.h" +#include "signal_generator_flags.h" + + +// For GPS NAVIGATION (L1) +concurrent_queue global_gps_acq_assist_queue; +concurrent_map global_gps_acq_assist_map; + +class Position_Gps_L1_System_Test: public ::testing::Test +{ +public: + std::string generator_binary; + std::string p1; + std::string p2; + std::string p3; + std::string p4; + std::string p5; + + const double baseband_sampling_freq = 2.6e6; + + std::string filename_rinex_obs = FLAGS_filename_rinex_obs; + std::string filename_raw_data = FLAGS_filename_raw_data; + + int configure_generator(); + int generate_signal(); + int configure_receiver(); + int run_receiver(); + void check_results(); + + double compute_stdev_precision(const std::vector & vec); + double compute_stdev_accuracy(const std::vector & vec, double ref); + + + void geodetic2Enu(const double latitude, const double longitude, const double altitude, + double* east, double* north, double* up); + + std::shared_ptr config; + std::string generated_kml_file; + +private: + void geodetic2Ecef(const double latitude, const double longitude, const double altitude, + double* x, double* y, double* z); + +}; + + + +void Position_Gps_L1_System_Test::geodetic2Ecef(const double latitude, const double longitude, const double altitude, + double* x, double* y, double* z) +{ + const double a = 6378137.0; // WGS84 + const double b = 6356752.314245; // WGS84 + + double aux_x, aux_y, aux_z; + + // Convert to ECEF (See https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates ) + double N = std::pow(a, 2.0) / sqrt( std::pow(a, 2.0) * std::pow(cos(latitude), 2.0) + std::pow(b, 2.0) * std::pow(sin(latitude), 2.0)); + + aux_x = (N + altitude) * cos(latitude) * cos(longitude); + aux_y = (N + altitude) * cos(latitude) * sin(longitude); + aux_z = ((std::pow(b, 2.0) / std::pow(a, 2.0)) * N + altitude) * sin(latitude); + + *x = aux_x; + *y = aux_y; + *z = aux_z; +} + + +void Position_Gps_L1_System_Test::geodetic2Enu(const double latitude, const double longitude, const double altitude, + double* east, double* north, double* up) +{ + // Reference : https://github.com/ethz-asl/geodetic_utils/blob/master/include/geodetic_utils/geodetic_conv.hpp + double x, y, z; + const double d2r = 3.1415926535898 / 180.0; + + geodetic2Ecef(latitude * d2r, longitude * d2r, altitude, &x, &y, &z); + + double aux_north, aux_east, aux_down; + + std::istringstream iss2(FLAGS_static_position); + std::string str_aux; + std::getline(iss2, str_aux, ','); + double ref_long = std::stod(str_aux); + std::getline(iss2, str_aux, ','); + double ref_lat = std::stod(str_aux); + std::getline(iss2, str_aux, '\n'); + double ref_h = std::stod(str_aux); + double ref_x, ref_y, ref_z; + + geodetic2Ecef(ref_lat * d2r, ref_long * d2r, ref_h, &ref_x, &ref_y, &ref_z); + + double aux_x = x - ref_x; + double aux_y = y - ref_y; + double aux_z = z - ref_z; + + // ECEF to NED matrix + double phiP = atan2(ref_z, sqrt(std::pow(ref_x, 2.0) + std::pow(ref_y, 2.0))); + const double sLat = sin(phiP); + const double sLon = sin(ref_long * d2r); + const double cLat = cos(phiP); + const double cLon = cos(ref_long * d2r); + + aux_north = -aux_x * sLat * cLon - aux_y * sLon + aux_z * cLat * cLon; + aux_east = -aux_x * sLat * sLon + aux_y * cLon + aux_z * cLat * sLon; + aux_down = aux_x * cLat + aux_z * sLat; + + *east = aux_east; + *north = aux_north; + *up = -aux_down; +} + + +double Position_Gps_L1_System_Test::compute_stdev_precision(const std::vector & vec) +{ + double sum__ = std::accumulate(vec.begin(), vec.end(), 0.0); + double mean__ = sum__ / vec.size(); + double accum__ = 0.0; + std::for_each (std::begin(vec), std::end(vec), [&](const double d) { + accum__ += (d - mean__) * (d - mean__); + }); + double stdev__ = std::sqrt(accum__ / (vec.size() - 1)); + return stdev__; +} + + +double Position_Gps_L1_System_Test::compute_stdev_accuracy(const std::vector & vec, double ref) +{ + double mean__ = ref; + double accum__ = 0.0; + std::for_each (std::begin(vec), std::end(vec), [&](const double d) { + accum__ += (d - mean__) * (d - mean__); + }); + double stdev__ = std::sqrt(accum__ / (vec.size() - 1)); + return stdev__; +} + + +int Position_Gps_L1_System_Test::configure_generator() +{ + // Configure signal generator + generator_binary = FLAGS_generator_binary; + + p1 = std::string("-rinex_nav_file=") + FLAGS_rinex_nav_file; + if(FLAGS_dynamic_position.empty()) + { + p2 = std::string("-static_position=") + FLAGS_static_position + std::string(",") + std::to_string(std::min(FLAGS_duration * 10, 3000)); + if(FLAGS_duration > 300) std::cout << "WARNING: Duration has been set to its maximum value of 300 s" << std::endl; + } + else + { + p2 = std::string("-obs_pos_file=") + std::string(FLAGS_dynamic_position); + } + p3 = std::string("-rinex_obs_file=") + FLAGS_filename_rinex_obs; // RINEX 2.10 observation file output + p4 = std::string("-sig_out_file=") + FLAGS_filename_raw_data; // Baseband signal output file. Will be stored in int8_t IQ multiplexed samples + p5 = std::string("-sampling_freq=") + std::to_string(baseband_sampling_freq); //Baseband sampling frequency [MSps] + return 0; +} + + +int Position_Gps_L1_System_Test::generate_signal() +{ + pid_t wait_result; + int child_status; + + char *const parmList[] = { &generator_binary[0], &generator_binary[0], &p1[0], &p2[0], &p3[0], &p4[0], &p5[0], NULL }; + + int pid; + if ((pid = fork()) == -1) + perror("fork error"); + else if (pid == 0) + { + execv(&generator_binary[0], parmList); + std::cout << "Return not expected. Must be an execv error." << std::endl; + std::terminate(); + } + + wait_result = waitpid(pid, &child_status, 0); + if (wait_result == -1) perror("waitpid error"); + return 0; +} + + +int Position_Gps_L1_System_Test::configure_receiver() +{ + config = std::make_shared(); + + const int sampling_rate_internal = baseband_sampling_freq; + + const int number_of_taps = 11; + const int number_of_bands = 2; + const float band1_begin = 0.0; + const float band1_end = 0.48; + const float band2_begin = 0.52; + const float band2_end = 1.0; + const float ampl1_begin = 1.0; + const float ampl1_end = 1.0; + const float ampl2_begin = 0.0; + const float ampl2_end = 0.0; + const float band1_error = 1.0; + const float band2_error = 1.0; + const int grid_density = 16; + const int decimation_factor = 1; + + const float zero = 0.0; + const int number_of_channels = 8; + const int in_acquisition = 1; + + const float threshold = 0.01; + const float doppler_max = 8000.0; + const float doppler_step = 500.0; + const int max_dwells = 1; + const int tong_init_val = 2; + const int tong_max_val = 10; + const int tong_max_dwells = 30; + const int coherent_integration_time_ms = 1; + + const float pll_bw_hz = 30.0; + const float dll_bw_hz = 4.0; + const float early_late_space_chips = 0.5; + const float pll_bw_narrow_hz = 20.0; + const float dll_bw_narrow_hz = 2.0; + const int extend_correlation_ms = 1; + + const int display_rate_ms = 500; + const int output_rate_ms = 1000; + const int averaging_depth = 1; + + config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(sampling_rate_internal)); + + // Set the assistance system parameters + config->set_property("GNSS-SDR.SUPL_read_gps_assistance_xml", "false"); + config->set_property("GNSS-SDR.SUPL_gps_enabled", "false"); + config->set_property("GNSS-SDR.SUPL_gps_ephemeris_server", "supl.google.com"); + config->set_property("GNSS-SDR.SUPL_gps_ephemeris_port", std::to_string(7275)); + config->set_property("GNSS-SDR.SUPL_gps_acquisition_server", "supl.google.com"); + config->set_property("GNSS-SDR.SUPL_gps_acquisition_port", std::to_string(7275)); + config->set_property("GNSS-SDR.SUPL_MCC", std::to_string(244)); + config->set_property("GNSS-SDR.SUPL_MNS", std::to_string(5)); + config->set_property("GNSS-SDR.SUPL_LAC", "0x59e2"); + config->set_property("GNSS-SDR.SUPL_CI", "0x31b0"); + + // Set the Signal Source + config->set_property("SignalSource.implementation", "File_Signal_Source"); + config->set_property("SignalSource.filename", "./" + filename_raw_data); + config->set_property("SignalSource.sampling_frequency", std::to_string(sampling_rate_internal)); + config->set_property("SignalSource.item_type", "ibyte"); + config->set_property("SignalSource.samples", std::to_string(zero)); + + // Set the Signal Conditioner + config->set_property("SignalConditioner.implementation", "Signal_Conditioner"); + config->set_property("DataTypeAdapter.implementation", "Ibyte_To_Complex"); + config->set_property("InputFilter.implementation", "Fir_Filter"); + config->set_property("InputFilter.dump", "false"); + config->set_property("InputFilter.input_item_type", "gr_complex"); + config->set_property("InputFilter.output_item_type", "gr_complex"); + config->set_property("InputFilter.taps_item_type", "float"); + config->set_property("InputFilter.number_of_taps", std::to_string(number_of_taps)); + config->set_property("InputFilter.number_of_bands", std::to_string(number_of_bands)); + config->set_property("InputFilter.band1_begin", std::to_string(band1_begin)); + config->set_property("InputFilter.band1_end", std::to_string(band1_end)); + config->set_property("InputFilter.band2_begin", std::to_string(band2_begin)); + config->set_property("InputFilter.band2_end", std::to_string(band2_end)); + config->set_property("InputFilter.ampl1_begin", std::to_string(ampl1_begin)); + config->set_property("InputFilter.ampl1_end", std::to_string(ampl1_end)); + config->set_property("InputFilter.ampl2_begin", std::to_string(ampl2_begin)); + config->set_property("InputFilter.ampl2_end", std::to_string(ampl2_end)); + config->set_property("InputFilter.band1_error", std::to_string(band1_error)); + config->set_property("InputFilter.band2_error", std::to_string(band2_error)); + config->set_property("InputFilter.filter_type", "bandpass"); + config->set_property("InputFilter.grid_density", std::to_string(grid_density)); + config->set_property("InputFilter.sampling_frequency", std::to_string(sampling_rate_internal)); + config->set_property("InputFilter.IF", std::to_string(zero)); + config->set_property("Resampler.implementation", "Pass_Through"); + config->set_property("Resampler.dump", "false"); + config->set_property("Resampler.item_type", "gr_complex"); + config->set_property("Resampler.sample_freq_in", std::to_string(sampling_rate_internal)); + config->set_property("Resampler.sample_freq_out", std::to_string(sampling_rate_internal)); + + // Set the number of Channels + config->set_property("Channels_1C.count", std::to_string(number_of_channels)); + config->set_property("Channels.in_acquisition", std::to_string(in_acquisition)); + config->set_property("Channel.signal", "1C"); + + // Set Acquisition + config->set_property("Acquisition_1C.implementation", "GPS_L1_CA_PCPS_Tong_Acquisition"); + config->set_property("Acquisition_1C.item_type", "gr_complex"); + config->set_property("Acquisition_1C.if", std::to_string(zero)); + config->set_property("Acquisition_1C.coherent_integration_time_ms", std::to_string(coherent_integration_time_ms)); + config->set_property("Acquisition_1C.threshold", std::to_string(threshold)); + config->set_property("Acquisition_1C.doppler_max", std::to_string(doppler_max)); + config->set_property("Acquisition_1C.doppler_step", std::to_string(doppler_step)); + config->set_property("Acquisition_1C.bit_transition_flag", "false"); + config->set_property("Acquisition_1C.max_dwells", std::to_string(max_dwells)); + config->set_property("Acquisition_1C.tong_init_val", std::to_string(tong_init_val)); + config->set_property("Acquisition_1C.tong_max_val", std::to_string(tong_max_val)); + config->set_property("Acquisition_1C.tong_max_dwells", std::to_string(tong_max_dwells)); + + // Set Tracking + config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_Tracking"); + //config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_C_Aid_Tracking"); + config->set_property("Tracking_1C.item_type", "gr_complex"); + config->set_property("Tracking_1C.if", std::to_string(zero)); + config->set_property("Tracking_1C.dump", "false"); + config->set_property("Tracking_1C.dump_filename", "./tracking_ch_"); + config->set_property("Tracking_1C.pll_bw_hz", std::to_string(pll_bw_hz)); + config->set_property("Tracking_1C.dll_bw_hz", std::to_string(dll_bw_hz)); + config->set_property("Tracking_1C.early_late_space_chips", std::to_string(early_late_space_chips)); + + config->set_property("Tracking_1C.pll_bw_narrow_hz", std::to_string(pll_bw_narrow_hz)); + config->set_property("Tracking_1C.dll_bw_narrow_hz", std::to_string(dll_bw_narrow_hz)); + config->set_property("Tracking_1C.extend_correlation_ms", std::to_string(extend_correlation_ms)); + + // Set Telemetry + config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder"); + config->set_property("TelemetryDecoder_1C.dump", "false"); + config->set_property("TelemetryDecoder_1C.decimation_factor", std::to_string(decimation_factor)); + + // Set Observables + config->set_property("Observables.implementation", "Hybrid_Observables"); + config->set_property("Observables.dump", "false"); + config->set_property("Observables.dump_filename", "./observables.dat"); + config->set_property("Observables.averaging_depth", std::to_string(100)); + + // Set PVT + config->set_property("PVT.implementation", "RTKLIB_PVT"); + //config->set_property("PVT.implementation", "Hybrid_PVT"); + config->set_property("PVT.averaging_depth", std::to_string(averaging_depth)); + config->set_property("PVT.flag_averaging", "false"); + config->set_property("PVT.output_rate_ms", std::to_string(output_rate_ms)); + config->set_property("PVT.display_rate_ms", std::to_string(display_rate_ms)); + config->set_property("PVT.dump_filename", "./PVT"); + config->set_property("PVT.nmea_dump_filename", "./gnss_sdr_pvt.nmea"); + config->set_property("PVT.flag_nmea_tty_port", "false"); + config->set_property("PVT.nmea_dump_devname", "/dev/pts/4"); + config->set_property("PVT.flag_rtcm_server", "false"); + config->set_property("PVT.flag_rtcm_tty_port", "false"); + config->set_property("PVT.rtcm_dump_devname", "/dev/pts/1"); + config->set_property("PVT.dump", "false"); + config->set_property("PVT.rinex_version", std::to_string(2)); + + return 0; +} + + +int Position_Gps_L1_System_Test::run_receiver() +{ + std::shared_ptr control_thread; + control_thread = std::make_shared(config); + // start receiver + try + { + control_thread->run(); + } + catch( boost::exception & e ) + { + std::cout << "Boost exception: " << boost::diagnostic_information(e); + } + catch(std::exception const& ex) + { + std::cout << "STD exception: " << ex.what(); + } + + // Get the name of the KML file generated by the receiver + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + FILE *fp; + std::string argum2 = std::string("/bin/ls *kml | tail -1"); + char buffer[1035]; + fp = popen(&argum2[0], "r"); + if (fp == NULL) + { + std::cout << "Failed to run command: " << argum2 << std::endl; + return -1; + } + while (fgets(buffer, sizeof(buffer), fp) != NULL) + { + std::string aux = std::string(buffer); + EXPECT_EQ(aux.empty(), false); + Position_Gps_L1_System_Test::generated_kml_file = aux.erase(aux.length() - 1, 1); + } + pclose(fp); + EXPECT_EQ(Position_Gps_L1_System_Test::generated_kml_file.empty(), false); + return 0; +} + + +void Position_Gps_L1_System_Test::check_results() +{ + std::fstream myfile(Position_Gps_L1_System_Test::generated_kml_file, std::ios_base::in); + std::string line; + + std::vector pos_e; + std::vector pos_n; + std::vector pos_u; + + // Skip header + std::getline(myfile, line); + bool is_header = true; + while(is_header) + { + std::getline(myfile, line); + std::size_t found = line.find(""); + if (found != std::string::npos) is_header = false; + } + bool is_data = true; + + //read data + while(is_data) + { + std::getline(myfile, line); + std::size_t found = line.find(""); + if (found != std::string::npos) is_data = false; + else + { + std::string str2; + std::istringstream iss(line); + double value; + double lat, longitude, h; + for (int i = 0; i < 3; i++) + { + std::getline(iss, str2, ','); + value = std::stod(str2); + if(i == 0) lat = value; + if(i == 1) longitude = value; + if(i == 2) h = value; + } + + double north, east, up; + geodetic2Enu(lat, longitude, h, &east, &north, &up); + //std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl; + pos_e.push_back(east); + pos_n.push_back(north); + pos_u.push_back(up); + } + } + myfile.close(); + + double sigma_E_2_precision = std::pow(compute_stdev_precision(pos_e), 2.0); + double sigma_N_2_precision = std::pow(compute_stdev_precision(pos_n), 2.0); + double sigma_U_2_precision = std::pow(compute_stdev_precision(pos_u), 2.0); + + double sigma_E_2_accuracy = std::pow(compute_stdev_accuracy(pos_e, 0.0), 2.0); + double sigma_N_2_accuracy = std::pow(compute_stdev_accuracy(pos_n, 0.0), 2.0); + double sigma_U_2_accuracy = std::pow(compute_stdev_accuracy(pos_u, 0.0), 2.0); + + std::cout << "---- ACCURACY ----" << std::endl; + std::cout << "2DRMS = " << 2 * sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl; + std::cout << "DRMS = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl; + std::cout << "CEP = " << 0.62 * compute_stdev_accuracy(pos_n, 0.0) + 0.56 * compute_stdev_accuracy(pos_e, 0.0) << " [m]" << std::endl; + std::cout << "99% SAS = " << 1.122 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; + std::cout << "90% SAS = " << 0.833 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; + std::cout << "MRSE = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; + std::cout << "SEP = " << 0.51 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; + std::cout << std::endl; + + std::cout << "---- PRECISION ----" << std::endl; + std::cout << "2DRMS = " << 2 * sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl; + std::cout << "DRMS = " << sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl; + std::cout << "CEP = " << 0.62 * compute_stdev_precision(pos_n) + 0.56 * compute_stdev_precision(pos_e) << " [m]" << std::endl; + std::cout << "99% SAS = " << 1.122 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; + std::cout << "90% SAS = " << 0.833 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; + std::cout << "MRSE = " << sqrt(sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; + std::cout << "SEP = " << 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; + + + // Sanity Check + double precision_SEP = 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision); + ASSERT_LT(precision_SEP, 20.0); +} + + +TEST_F(Position_Gps_L1_System_Test, Position_system_test) +{ + // Configure the signal generator + configure_generator(); + + // Generate signal raw signal samples and observations RINEX file + if(!FLAGS_disable_generator) + { + generate_signal(); + } + + // Configure receiver + configure_receiver(); + + // Run the receiver + EXPECT_EQ( run_receiver(), 0) << "Problem executing the software-defined signal generator"; + + // Check results + check_results(); +} + + +int main(int argc, char **argv) +{ + std::cout << "Running Position precision test..." << std::endl; + int res = 0; + try + { + testing::InitGoogleTest(&argc, argv); + } + catch(...) {} // catch the "testing::internal::::ClassUniqueToAlwaysTrue" from gtest + + google::ParseCommandLineFlags(&argc, &argv, true); + google::InitGoogleLogging(argv[0]); + + // Run the Tests + try + { + res = RUN_ALL_TESTS(); + } + catch(...) + { + LOG(WARNING) << "Unexpected catch"; + } + google::ShutDownCommandLineFlags(); + return res; +}