mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-15 12:40:35 +00:00
Code Issues Releases Wiki Activity

Merging fpga with next

Browse Source
This commit is contained in:
Javier Arribas 2018-03-05 11:03:30 +01:00
commit 7c406bb6eb
982 changed files with 48752 additions and 45403 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
.clang-format Normal file
View File

@ -0,0 +1,97 @@
---
Language: Cpp
# BasedOnStyle: Google
# More info: http://clang.llvm.org/docs/ClangFormatStyleOptions.html
AccessModifierOffset: -4
AlignAfterOpenBracket: DontAlign
AlignConsecutiveAssignments: false
AlignConsecutiveDeclarations: false
AlignEscapedNewlinesLeft: true
AlignOperands: true
AlignTrailingComments: true
AllowAllParametersOfDeclarationOnNextLine: true
AllowShortBlocksOnASingleLine: false
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: All
AllowShortIfStatementsOnASingleLine: true
AllowShortLoopsOnASingleLine: true
AlwaysBreakAfterDefinitionReturnType: None
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: false
BinPackArguments: true
BinPackParameters: true
BraceWrapping:
AfterClass: false
AfterControlStatement: false
AfterEnum: false
AfterFunction: false
AfterNamespace: false
AfterObjCDeclaration: false
AfterStruct: false
AfterUnion: false
BeforeCatch: false
BeforeElse: false
IndentBraces: false
BreakBeforeBinaryOperators: None
BreakBeforeBraces: GNU
BreakBeforeTernaryOperators: true
BreakConstructorInitializersBeforeComma: false
BreakAfterJavaFieldAnnotations: false
BreakStringLiterals: true
ColumnLimit: 0
CommentPragmas: '^ IWYU pragma:'
ConstructorInitializerAllOnOneLineOrOnePerLine: true
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 4
Cpp11BracedListStyle: true
DerivePointerAlignment: true
DisableFormat: false
ExperimentalAutoDetectBinPacking: false
ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ]
IncludeCategories:
- Regex: '^<.*\.h>'
Priority: 1
- Regex: '^<.*'
Priority: 2
- Regex: '.*'
Priority: 3
IncludeIsMainRegex: '([-_](test|unittest))?$'
IndentCaseLabels: false
IndentWidth: 4
IndentWrappedFunctionNames: false
JavaScriptQuotes: Leave
JavaScriptWrapImports: true
KeepEmptyLinesAtTheStartOfBlocks: false
MacroBlockBegin: ''
MacroBlockEnd: ''
MaxEmptyLinesToKeep: 2
NamespaceIndentation: None
ObjCBlockIndentWidth: 2
ObjCSpaceAfterProperty: false
ObjCSpaceBeforeProtocolList: false
PenaltyBreakBeforeFirstCallParameter: 1
PenaltyBreakComment: 300
PenaltyBreakFirstLessLess: 120
PenaltyBreakString: 1000
PenaltyExcessCharacter: 1000000
PenaltyReturnTypeOnItsOwnLine: 200
PointerAlignment: Left
ReflowComments: true
SortIncludes: false
SpaceAfterCStyleCast: false
SpaceAfterTemplateKeyword: true
SpaceBeforeAssignmentOperators: true
SpaceBeforeParens: ControlStatements
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 2
SpacesInAngles: false
SpacesInContainerLiterals: true
SpacesInCStyleCastParentheses: false
SpacesInParentheses: false
SpacesInSquareBrackets: false
Standard: Auto
TabWidth: 8
UseTab: Never
...

2
CONTRIBUTING.md
View File

@ -128,6 +128,8 @@ $ git pull --rebase upstream next
### How to submit a pull request ### How to submit a pull request
Before submitting you code, please be sure to [apply clang-format](http://gnss-sdr.org/coding-style/#use-tools-for-automated-code-formatting).
When the contribution is ready, you can [submit a pull When the contribution is ready, you can [submit a pull
request](https://github.com/gnss-sdr/gnss-sdr/compare/). Head to your request](https://github.com/gnss-sdr/gnss-sdr/compare/). Head to your
GitHub repository, switch to your `my_feature` branch, and click the GitHub repository, switch to your `my_feature` branch, and click the

8
README.md
View File

@ -84,7 +84,7 @@ $ sudo yum install make automake gcc gcc-c++ kernel-devel cmake git boost-devel
boost-date-time boost-system boost-filesystem boost-thread boost-chrono \ boost-date-time boost-system boost-filesystem boost-thread boost-chrono \
boost-serialization log4cpp-devel gnuradio-devel gr-osmosdr-devel \ boost-serialization log4cpp-devel gnuradio-devel gr-osmosdr-devel \
blas-devel lapack-devel matio-devel armadillo-devel gflags-devel \ blas-devel lapack-devel matio-devel armadillo-devel gflags-devel \
glog-devel gnutls-devel openssl-devel python-mako python-six glog-devel openssl-devel python-mako python-six
~~~~~~ ~~~~~~
Once you have installed these packages, you can jump directly to [download the source code and build GNSS-SDR](#download-and-build-linux). Once you have installed these packages, you can jump directly to [download the source code and build GNSS-SDR](#download-and-build-linux).
@ -101,7 +101,7 @@ $ sudo yum install make automake gcc gcc-c++ kernel-devel libtool \
hdf5-devel cmake git boost-devel boost-date-time boost-system \ hdf5-devel cmake git boost-devel boost-date-time boost-system \
boost-filesystem boost-thread boost-chrono boost-serialization \ boost-filesystem boost-thread boost-chrono boost-serialization \
log4cpp-devel gnuradio-devel gr-osmosdr-devel blas-devel lapack-devel \ log4cpp-devel gnuradio-devel gr-osmosdr-devel blas-devel lapack-devel \
armadillo-devel gnutls-devel openssl-devel python-mako python-six armadillo-devel openssl-devel python-mako python-six
~~~~~~ ~~~~~~
Once you have installed these packages, you can jump directly to [download the source code and build GNSS-SDR](#download-and-build-linux). Once you have installed these packages, you can jump directly to [download the source code and build GNSS-SDR](#download-and-build-linux).
@ -244,11 +244,11 @@ changing `/home/username/googletest-release-1.8.0/googletest` by the actual dire
#### Install the [GnuTLS library](http://www.gnutls.org/ "GnuTLS's Homepage"): #### Install the [GnuTLS](http://www.gnutls.org/ "GnuTLS's Homepage") or [OpenSSL](https://www.openssl.org/ "OpenSSL's Homepage") libraries:
~~~~~~ ~~~~~~
$ sudo apt-get install libgnutls-openssl-dev # For Debian/Ubuntu/LinuxMint $ sudo apt-get install libgnutls-openssl-dev # For Debian/Ubuntu/LinuxMint
$ sudo yum install libgnutls-openssl-devel # For Fedora/CentOS/RHEL $ sudo yum install openssl-devel # For Fedora/CentOS/RHEL
~~~~~~ ~~~~~~
In case the GnuTLS library with openssl extensions package is not available in your GNU/Linux distribution, GNSS-SDR can also work well with OpenSSL. In case the GnuTLS library with openssl extensions package is not available in your GNU/Linux distribution, GNSS-SDR can also work well with OpenSSL.

5
docs/PULL_REQUEST_TEMPLATE.md
View File

@ -5,7 +5,8 @@ Before submitting your pull request, please make sure the following is done:
2. If you are a first-time contributor, after your pull request you will be asked to sign an Individual Contributor License Agreement ([CLA](https://en.wikipedia.org/wiki/Contributor_License_Agreement)) before your code gets accepted into `master`. This license is for your protection as a Contributor as well as for the protection of [CTTC](http://www.cttc.es/); it does not change your rights to use your own contributions for any other purpose. Except for the license granted therein to CTTC and recipients of software distributed by CTTC, you reserve all right, title, and interest in and to your contributions. The information you provide in that CLA will be maintained in accordance with [CTTC's privacy policy](http://www.cttc.es/privacy/). 2. If you are a first-time contributor, after your pull request you will be asked to sign an Individual Contributor License Agreement ([CLA](https://en.wikipedia.org/wiki/Contributor_License_Agreement)) before your code gets accepted into `master`. This license is for your protection as a Contributor as well as for the protection of [CTTC](http://www.cttc.es/); it does not change your rights to use your own contributions for any other purpose. Except for the license granted therein to CTTC and recipients of software distributed by CTTC, you reserve all right, title, and interest in and to your contributions. The information you provide in that CLA will be maintained in accordance with [CTTC's privacy policy](http://www.cttc.es/privacy/).
3. You have read the [Contributing Guidelines](https://github.com/gnss-sdr/gnss-sdr/blob/master/CONTRIBUTING.md). 3. You have read the [Contributing Guidelines](https://github.com/gnss-sdr/gnss-sdr/blob/master/CONTRIBUTING.md).
4. You have read the [coding style guide](http://gnss-sdr.org/coding-style/). 4. You have read the [coding style guide](http://gnss-sdr.org/coding-style/).
5. You have forked the [gnss-sdr upstream repository](https://github.com/gnss-sdr/gnss-sdr) and have created your branch from `next` (or any other currently living branch in the upstream repository). 5. Specifically, you have read [about clang-format](http://gnss-sdr.org/coding-style/#use-tools-for-automated-code-formatting) and you have applied it.
6. Please include a description of your changes here. 6. You have forked the [gnss-sdr upstream repository](https://github.com/gnss-sdr/gnss-sdr) and have created your branch from `next` (or any other currently living branch in the upstream repository).
7. Please include a description of your changes here.
**Please feel free to delete this line and the above text once you have read it and in case you want to go on with your pull request.** **Please feel free to delete this line and the above text once you have read it and in case you want to go on with your pull request.**

282
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@ -42,12 +42,11 @@
using google::LogMessage; using google::LogMessage;
RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
std::string role, std::string role,
unsigned int in_streams, unsigned int in_streams,
unsigned int out_streams) : unsigned int out_streams) : role_(role),
role_(role), in_streams_(in_streams),
in_streams_(in_streams), out_streams_(out_streams)
out_streams_(out_streams)
{ {
// dump parameters // dump parameters
std::string default_dump_filename = "./pvt.dat"; std::string default_dump_filename = "./pvt.dat";
@ -71,27 +70,27 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
// RINEX version // RINEX version
int rinex_version = configuration->property(role + ".rinex_version", 3); int rinex_version = configuration->property(role + ".rinex_version", 3);
if ( FLAGS_RINEX_version.compare("3.01") == 0 ) if (FLAGS_RINEX_version.compare("3.01") == 0)
{ {
rinex_version = 3; rinex_version = 3;
} }
else if ( FLAGS_RINEX_version.compare("3.02") == 0 ) else if (FLAGS_RINEX_version.compare("3.02") == 0)
{ {
rinex_version = 3; rinex_version = 3;
} }
else if ( FLAGS_RINEX_version.compare("3") == 0 ) else if (FLAGS_RINEX_version.compare("3") == 0)
{ {
rinex_version = 3; rinex_version = 3;
} }
else if ( FLAGS_RINEX_version.compare("2.11") == 0 ) else if (FLAGS_RINEX_version.compare("2.11") == 0)
{ {
rinex_version = 2; rinex_version = 2;
} }
else if ( FLAGS_RINEX_version.compare("2.10") == 0 ) else if (FLAGS_RINEX_version.compare("2.10") == 0)
{ {
rinex_version = 2; rinex_version = 2;
} }
else if ( FLAGS_RINEX_version.compare("2") == 0 ) else if (FLAGS_RINEX_version.compare("2") == 0)
{ {
rinex_version = 2; rinex_version = 2;
} }
@ -110,19 +109,19 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
int rtcm_MT1077_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1077_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); int rtcm_MT1077_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1077_rate_ms", rtcm_MSM_rate_ms), output_rate_ms);
int rtcm_MT1087_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1087_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); int rtcm_MT1087_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1087_rate_ms", rtcm_MSM_rate_ms), output_rate_ms);
int rtcm_MT1097_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1097_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); int rtcm_MT1097_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1097_rate_ms", rtcm_MSM_rate_ms), output_rate_ms);
std::map<int,int> rtcm_msg_rate_ms; std::map<int, int> rtcm_msg_rate_ms;
rtcm_msg_rate_ms[1019] = rtcm_MT1019_rate_ms; rtcm_msg_rate_ms[1019] = rtcm_MT1019_rate_ms;
rtcm_msg_rate_ms[1020] = rtcm_MT1020_rate_ms; rtcm_msg_rate_ms[1020] = rtcm_MT1020_rate_ms;
rtcm_msg_rate_ms[1045] = rtcm_MT1045_rate_ms; rtcm_msg_rate_ms[1045] = rtcm_MT1045_rate_ms;
for (int k = 1071; k < 1078; k++) // All GPS MSM for (int k = 1071; k < 1078; k++) // All GPS MSM
{ {
rtcm_msg_rate_ms[k] = rtcm_MT1077_rate_ms; rtcm_msg_rate_ms[k] = rtcm_MT1077_rate_ms;
} }
for (int k = 1081; k < 1088; k++) // All GLONASS MSM for (int k = 1081; k < 1088; k++) // All GLONASS MSM
{ {
rtcm_msg_rate_ms[k] = rtcm_MT1087_rate_ms; rtcm_msg_rate_ms[k] = rtcm_MT1087_rate_ms;
} }
for (int k = 1091; k < 1098; k++) // All Galileo MSM for (int k = 1091; k < 1098; k++) // All Galileo MSM
{ {
rtcm_msg_rate_ms[k] = rtcm_MT1097_rate_ms; rtcm_msg_rate_ms[k] = rtcm_MT1097_rate_ms;
} }
@ -184,47 +183,47 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
// *******************WARNING!!!!!!!*********** // *******************WARNING!!!!!!!***********
// GPS L5 only configurable for single frequency, single system at the moment!!!!!! // GPS L5 only configurable for single frequency, single system at the moment!!!!!!
if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 1; if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 1;
if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 2; if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 2;
if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 3; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 3;
if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 4; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 4;
if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 5; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 5;
if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0)) type_of_receiver = 6; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0)) type_of_receiver = 6;
if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 7; if ((gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 7;
//if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 8; //if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 8;
if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 9; if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 9;
if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 10; if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 10;
if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0)) type_of_receiver = 11; if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0)) type_of_receiver = 11;
if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 12; if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 12;
//if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 13; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 13;
if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 14; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 14;
if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0)) type_of_receiver = 15; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0)) type_of_receiver = 15;
//if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 16; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 16;
if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 17; if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 17;
if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0)) type_of_receiver = 18; if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0)) type_of_receiver = 18;
//if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 19; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 19;
//if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 20; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 20;
if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 21; if ((gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0)) type_of_receiver = 21;
//if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count = 0)) type_of_receiver = 22; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count = 0)) type_of_receiver = 22;
if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count != 0)) type_of_receiver = 23; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count != 0)) type_of_receiver = 23;
//if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2R_count != 0)) type_of_receiver = 24; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2R_count != 0)) type_of_receiver = 24;
//if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count != 0) && (glo_1G_count != 0)) type_of_receiver = 25; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count != 0) && (glo_1G_count != 0)) type_of_receiver = 25;
if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count != 0)) type_of_receiver = 26; if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count != 0)) type_of_receiver = 26;
if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count != 0)) type_of_receiver = 27; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count != 0)) type_of_receiver = 27;
if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count != 0)) type_of_receiver = 28; if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count != 0)) type_of_receiver = 28;
//RTKLIB PVT solver options //RTKLIB PVT solver options
// Settings 1 // Settings 1
int positioning_mode = -1; int positioning_mode = -1;
std::string default_pos_mode("Single"); std::string default_pos_mode("Single");
std::string positioning_mode_str = configuration->property(role + ".positioning_mode", default_pos_mode); /* (PMODE_XXX) see src/algorithms/libs/rtklib/rtklib.h */ std::string positioning_mode_str = configuration->property(role + ".positioning_mode", default_pos_mode); /* (PMODE_XXX) see src/algorithms/libs/rtklib/rtklib.h */
if(positioning_mode_str.compare("Single") == 0) positioning_mode = PMODE_SINGLE; if (positioning_mode_str.compare("Single") == 0) positioning_mode = PMODE_SINGLE;
if(positioning_mode_str.compare("Static") == 0) positioning_mode = PMODE_STATIC; if (positioning_mode_str.compare("Static") == 0) positioning_mode = PMODE_STATIC;
if(positioning_mode_str.compare("Kinematic") == 0) positioning_mode = PMODE_KINEMA; if (positioning_mode_str.compare("Kinematic") == 0) positioning_mode = PMODE_KINEMA;
if(positioning_mode_str.compare("PPP_Static") == 0) positioning_mode = PMODE_PPP_STATIC; if (positioning_mode_str.compare("PPP_Static") == 0) positioning_mode = PMODE_PPP_STATIC;
if(positioning_mode_str.compare("PPP_Kinematic") == 0) positioning_mode = PMODE_PPP_KINEMA; if (positioning_mode_str.compare("PPP_Kinematic") == 0) positioning_mode = PMODE_PPP_KINEMA;
if( positioning_mode == -1 ) if (positioning_mode == -1)
{ {
//warn user and set the default //warn user and set the default
std::cout << "WARNING: Bad specification of positioning mode." << std::endl; std::cout << "WARNING: Bad specification of positioning mode." << std::endl;
@ -237,19 +236,19 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
int num_bands = 0; int num_bands = 0;
if ((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) num_bands = 1; if ((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) num_bands = 1;
if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) && (gps_2S_count > 0) ) num_bands = 2; if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) && (gps_2S_count > 0)) num_bands = 2;
if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) && ((gal_E5a_count > 0) || (gal_E5b_count > 0) || (gps_L5_count > 0))) num_bands = 2; if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) && ((gal_E5a_count > 0) || (gal_E5b_count > 0) || (gps_L5_count > 0))) num_bands = 2;
if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) && (gps_2S_count > 0) && ((gal_E5a_count > 0) || (gal_E5b_count > 0) || (gps_L5_count > 0))) num_bands = 3; if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) && (gps_2S_count > 0) && ((gal_E5a_count > 0) || (gal_E5b_count > 0) || (gps_L5_count > 0))) num_bands = 3;
int number_of_frequencies = configuration->property(role + ".num_bands", num_bands); /* (1:L1, 2:L1+L2, 3:L1+L2+L5) */ int number_of_frequencies = configuration->property(role + ".num_bands", num_bands); /* (1:L1, 2:L1+L2, 3:L1+L2+L5) */
if( (number_of_frequencies < 1) || (number_of_frequencies > 3) ) if ((number_of_frequencies < 1) || (number_of_frequencies > 3))
{ {
//warn user and set the default //warn user and set the default
number_of_frequencies = num_bands; number_of_frequencies = num_bands;
} }
double elevation_mask = configuration->property(role + ".elevation_mask", 15.0); double elevation_mask = configuration->property(role + ".elevation_mask", 15.0);
if( (elevation_mask < 0.0) || (elevation_mask > 90.0) ) if ((elevation_mask < 0.0) || (elevation_mask > 90.0))
{ {
//warn user and set the default //warn user and set the default
LOG(WARNING) << "Erroneous Elevation Mask. Setting to default value of 15.0 degrees"; LOG(WARNING) << "Erroneous Elevation Mask. Setting to default value of 15.0 degrees";
@ -257,7 +256,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
} }
int dynamics_model = configuration->property(role + ".dynamics_model", 0); /* dynamics model (0:none, 1:velocity, 2:accel) */ int dynamics_model = configuration->property(role + ".dynamics_model", 0); /* dynamics model (0:none, 1:velocity, 2:accel) */
if( (dynamics_model < 0) || (dynamics_model > 2) ) if ((dynamics_model < 0) || (dynamics_model > 2))
{ {
//warn user and set the default //warn user and set the default
LOG(WARNING) << "Erroneous Dynamics Model configuration. Setting to default value of (0:none)"; LOG(WARNING) << "Erroneous Dynamics Model configuration. Setting to default value of (0:none)";
@ -267,13 +266,13 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
std::string default_iono_model("OFF"); std::string default_iono_model("OFF");
std::string iono_model_str = configuration->property(role + ".iono_model", default_iono_model); /* (IONOOPT_XXX) see src/algorithms/libs/rtklib/rtklib.h */ std::string iono_model_str = configuration->property(role + ".iono_model", default_iono_model); /* (IONOOPT_XXX) see src/algorithms/libs/rtklib/rtklib.h */
int iono_model = -1; int iono_model = -1;
if(iono_model_str.compare("OFF") == 0) iono_model = IONOOPT_OFF; if (iono_model_str.compare("OFF") == 0) iono_model = IONOOPT_OFF;
if(iono_model_str.compare("Broadcast") == 0) iono_model = IONOOPT_BRDC; if (iono_model_str.compare("Broadcast") == 0) iono_model = IONOOPT_BRDC;
if(iono_model_str.compare("SBAS") == 0) iono_model = IONOOPT_SBAS; if (iono_model_str.compare("SBAS") == 0) iono_model = IONOOPT_SBAS;
if(iono_model_str.compare("Iono-Free-LC") == 0) iono_model = IONOOPT_IFLC; if (iono_model_str.compare("Iono-Free-LC") == 0) iono_model = IONOOPT_IFLC;
if(iono_model_str.compare("Estimate_STEC") == 0) iono_model = IONOOPT_EST; if (iono_model_str.compare("Estimate_STEC") == 0) iono_model = IONOOPT_EST;
if(iono_model_str.compare("IONEX") == 0) iono_model = IONOOPT_TEC; if (iono_model_str.compare("IONEX") == 0) iono_model = IONOOPT_TEC;
if( iono_model == -1 ) if (iono_model == -1)
{ {
//warn user and set the default //warn user and set the default
std::cout << "WARNING: Bad specification of ionospheric model." << std::endl; std::cout << "WARNING: Bad specification of ionospheric model." << std::endl;
@ -286,12 +285,12 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
std::string default_trop_model("OFF"); std::string default_trop_model("OFF");
int trop_model = -1; int trop_model = -1;
std::string trop_model_str = configuration->property(role + ".trop_model", default_trop_model); /* (TROPOPT_XXX) see src/algorithms/libs/rtklib/rtklib.h */ std::string trop_model_str = configuration->property(role + ".trop_model", default_trop_model); /* (TROPOPT_XXX) see src/algorithms/libs/rtklib/rtklib.h */
if(trop_model_str.compare("OFF") == 0) trop_model = TROPOPT_OFF; if (trop_model_str.compare("OFF") == 0) trop_model = TROPOPT_OFF;
if(trop_model_str.compare("Saastamoinen") == 0) trop_model = TROPOPT_SAAS; if (trop_model_str.compare("Saastamoinen") == 0) trop_model = TROPOPT_SAAS;
if(trop_model_str.compare("SBAS") == 0) trop_model = TROPOPT_SBAS; if (trop_model_str.compare("SBAS") == 0) trop_model = TROPOPT_SBAS;
if(trop_model_str.compare("Estimate_ZTD") == 0) trop_model = TROPOPT_EST; if (trop_model_str.compare("Estimate_ZTD") == 0) trop_model = TROPOPT_EST;
if(trop_model_str.compare("Estimate_ZTD_Grad") == 0) trop_model = TROPOPT_ESTG; if (trop_model_str.compare("Estimate_ZTD_Grad") == 0) trop_model = TROPOPT_ESTG;
if( trop_model == -1 ) if (trop_model == -1)
{ {
//warn user and set the default //warn user and set the default
std::cout << "WARNING: Bad specification of tropospheric model." << std::endl; std::cout << "WARNING: Bad specification of tropospheric model." << std::endl;
@ -323,8 +322,8 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
if ((gps_1C_count > 0) || (gps_2S_count > 0) || (gps_L5_count > 0)) nsys += SYS_GPS; if ((gps_1C_count > 0) || (gps_2S_count > 0) || (gps_L5_count > 0)) nsys += SYS_GPS;
if ((gal_1B_count > 0) || (gal_E5a_count > 0) || (gal_E5b_count > 0)) nsys += SYS_GAL; if ((gal_1B_count > 0) || (gal_E5a_count > 0) || (gal_E5b_count > 0)) nsys += SYS_GAL;
if ((glo_1G_count > 0)) nsys += SYS_GLO; if ((glo_1G_count > 0)) nsys += SYS_GLO;
int navigation_system = configuration->property(role + ".navigation_system", nsys); /* (SYS_XXX) see src/algorithms/libs/rtklib/rtklib.h */ int navigation_system = configuration->property(role + ".navigation_system", nsys); /* (SYS_XXX) see src/algorithms/libs/rtklib/rtklib.h */
if( (navigation_system < 1) || (navigation_system > 255) ) /* GPS: 1 SBAS: 2 GPS+SBAS: 3 Galileo: 8 Galileo+GPS: 9 GPS+SBAS+Galileo: 11 All: 255 */ if ((navigation_system < 1) || (navigation_system > 255)) /* GPS: 1 SBAS: 2 GPS+SBAS: 3 Galileo: 8 Galileo+GPS: 9 GPS+SBAS+Galileo: 11 All: 255 */
{ {
//warn user and set the default //warn user and set the default
LOG(WARNING) << "Erroneous Navigation System. Setting to default value of (0:none)"; LOG(WARNING) << "Erroneous Navigation System. Setting to default value of (0:none)";
@ -335,12 +334,12 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
std::string default_gps_ar("Continuous"); std::string default_gps_ar("Continuous");
std::string integer_ambiguity_resolution_gps_str = configuration->property(role + ".AR_GPS", default_gps_ar); /* Integer Ambiguity Resolution mode for GPS (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */ std::string integer_ambiguity_resolution_gps_str = configuration->property(role + ".AR_GPS", default_gps_ar); /* Integer Ambiguity Resolution mode for GPS (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */
int integer_ambiguity_resolution_gps = -1; int integer_ambiguity_resolution_gps = -1;
if(integer_ambiguity_resolution_gps_str.compare("OFF") == 0) integer_ambiguity_resolution_gps = ARMODE_OFF; if (integer_ambiguity_resolution_gps_str.compare("OFF") == 0) integer_ambiguity_resolution_gps = ARMODE_OFF;
if(integer_ambiguity_resolution_gps_str.compare("Continuous") == 0) integer_ambiguity_resolution_gps = ARMODE_CONT; if (integer_ambiguity_resolution_gps_str.compare("Continuous") == 0) integer_ambiguity_resolution_gps = ARMODE_CONT;
if(integer_ambiguity_resolution_gps_str.compare("Instantaneous") == 0) integer_ambiguity_resolution_gps = ARMODE_INST; if (integer_ambiguity_resolution_gps_str.compare("Instantaneous") == 0) integer_ambiguity_resolution_gps = ARMODE_INST;
if(integer_ambiguity_resolution_gps_str.compare("Fix-and-Hold") == 0) integer_ambiguity_resolution_gps = ARMODE_FIXHOLD; if (integer_ambiguity_resolution_gps_str.compare("Fix-and-Hold") == 0) integer_ambiguity_resolution_gps = ARMODE_FIXHOLD;
if(integer_ambiguity_resolution_gps_str.compare("PPP-AR") == 0) integer_ambiguity_resolution_gps = ARMODE_PPPAR; if (integer_ambiguity_resolution_gps_str.compare("PPP-AR") == 0) integer_ambiguity_resolution_gps = ARMODE_PPPAR;
if( integer_ambiguity_resolution_gps == -1 ) if (integer_ambiguity_resolution_gps == -1)
{ {
//warn user and set the default //warn user and set the default
std::cout << "WARNING: Bad specification of GPS ambiguity resolution method." << std::endl; std::cout << "WARNING: Bad specification of GPS ambiguity resolution method." << std::endl;
@ -351,7 +350,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
} }
int integer_ambiguity_resolution_glo = configuration->property(role + ".AR_GLO", 1); /* Integer Ambiguity Resolution mode for GLONASS (0:off,1:on,2:auto cal,3:ext cal) */ int integer_ambiguity_resolution_glo = configuration->property(role + ".AR_GLO", 1); /* Integer Ambiguity Resolution mode for GLONASS (0:off,1:on,2:auto cal,3:ext cal) */
if( (integer_ambiguity_resolution_glo < 0) || (integer_ambiguity_resolution_glo > 3) ) if ((integer_ambiguity_resolution_glo < 0) || (integer_ambiguity_resolution_glo > 3))
{ {
//warn user and set the default //warn user and set the default
LOG(WARNING) << "Erroneous Integer Ambiguity Resolution for GLONASS . Setting to default value of (1:on)"; LOG(WARNING) << "Erroneous Integer Ambiguity Resolution for GLONASS . Setting to default value of (1:on)";
@ -359,7 +358,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
} }
int integer_ambiguity_resolution_bds = configuration->property(role + ".AR_DBS", 1); /* Integer Ambiguity Resolution mode for BEIDOU (0:off,1:on) */ int integer_ambiguity_resolution_bds = configuration->property(role + ".AR_DBS", 1); /* Integer Ambiguity Resolution mode for BEIDOU (0:off,1:on) */
if( (integer_ambiguity_resolution_bds < 0) || (integer_ambiguity_resolution_bds > 1) ) if ((integer_ambiguity_resolution_bds < 0) || (integer_ambiguity_resolution_bds > 1))
{ {
//warn user and set the default //warn user and set the default
LOG(WARNING) << "Erroneous Integer Ambiguity Resolution for BEIDOU . Setting to default value of (1:on)"; LOG(WARNING) << "Erroneous Integer Ambiguity Resolution for BEIDOU . Setting to default value of (1:on)";
@ -369,13 +368,13 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
double min_ratio_to_fix_ambiguity = configuration->property(role + ".min_ratio_to_fix_ambiguity", 3.0); /* Set the integer ambiguity validation threshold for ratio‐test, double min_ratio_to_fix_ambiguity = configuration->property(role + ".min_ratio_to_fix_ambiguity", 3.0); /* Set the integer ambiguity validation threshold for ratio‐test,
which uses the ratio of squared residuals of the best integer vector to the second‐best vector. */ which uses the ratio of squared residuals of the best integer vector to the second‐best vector. */
int min_lock_to_fix_ambiguity = configuration->property(role + ".min_lock_to_fix_ambiguity", 0); /* Set the minimum lock count to fix integer ambiguity. int min_lock_to_fix_ambiguity = configuration->property(role + ".min_lock_to_fix_ambiguity", 0); /* Set the minimum lock count to fix integer ambiguity.
If the lock count is less than the value, the ambiguity is excluded from the fixed integer vector. */ If the lock count is less than the value, the ambiguity is excluded from the fixed integer vector. */
double min_elevation_to_fix_ambiguity = configuration->property(role + ".min_elevation_to_fix_ambiguity", 0.0); /* Set the minimum elevation (deg) to fix integer ambiguity. double min_elevation_to_fix_ambiguity = configuration->property(role + ".min_elevation_to_fix_ambiguity", 0.0); /* Set the minimum elevation (deg) to fix integer ambiguity.
If the elevation of the satellite is less than the value, the ambiguity is excluded from the fixed integer vector. */ If the elevation of the satellite is less than the value, the ambiguity is excluded from the fixed integer vector. */
int outage_reset_ambiguity = configuration->property(role + ".outage_reset_ambiguity", 5); /* Set the outage count to reset ambiguity. If the data outage count is over the value, the estimated ambiguity is reset to the initial value. */ int outage_reset_ambiguity = configuration->property(role + ".outage_reset_ambiguity", 5); /* Set the outage count to reset ambiguity. If the data outage count is over the value, the estimated ambiguity is reset to the initial value. */
double slip_threshold = configuration->property(role + ".slip_threshold", 0.05); /* set the cycle‐slip threshold (m) of geometry‐free LC carrier‐phase difference between epochs */ double slip_threshold = configuration->property(role + ".slip_threshold", 0.05); /* set the cycle‐slip threshold (m) of geometry‐free LC carrier‐phase difference between epochs */
@ -404,7 +403,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
double sigma_acch = configuration->property(role + ".sigma_acch", 1e-1); /* Set the process noise standard deviation of the receiver acceleration as double sigma_acch = configuration->property(role + ".sigma_acch", 1e-1); /* Set the process noise standard deviation of the receiver acceleration as
the horizontal component. (m/s2/sqrt(s)). If Receiver Dynamics is set to OFF, they are not used. */ the horizontal component. (m/s2/sqrt(s)). If Receiver Dynamics is set to OFF, they are not used. */
double sigma_accv = configuration->property(role + ".sigma_accv", 1e-2); /* Set the process noise standard deviation of the receiver acceleration as double sigma_accv = configuration->property(role + ".sigma_accv", 1e-2); /* Set the process noise standard deviation of the receiver acceleration as
the vertical component. (m/s2/sqrt(s)). If Receiver Dynamics is set to OFF, they are not used. */ the vertical component. (m/s2/sqrt(s)). If Receiver Dynamics is set to OFF, they are not used. */
double sigma_pos = configuration->property(role + ".sigma_pos", 0.0); double sigma_pos = configuration->property(role + ".sigma_pos", 0.0);
@ -415,70 +414,71 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
double carrier_phase_error_factor_a = configuration->property(role + ".carrier_phase_error_factor_a", 0.003); double carrier_phase_error_factor_a = configuration->property(role + ".carrier_phase_error_factor_a", 0.003);
double carrier_phase_error_factor_b = configuration->property(role + ".carrier_phase_error_factor_b", 0.003); double carrier_phase_error_factor_b = configuration->property(role + ".carrier_phase_error_factor_b", 0.003);
snrmask_t snrmask = { {}, {{},{}} }; snrmask_t snrmask = {{}, {{}, {}}};
prcopt_t rtklib_configuration_options = {positioning_mode, /* positioning mode (PMODE_XXX) see src/algorithms/libs/rtklib/rtklib.h */ prcopt_t rtklib_configuration_options = {
0, /* solution type (0:forward,1:backward,2:combined) */ positioning_mode, /* positioning mode (PMODE_XXX) see src/algorithms/libs/rtklib/rtklib.h */
number_of_frequencies, /* number of frequencies (1:L1, 2:L1+L2, 3:L1+L2+L5)*/ 0, /* solution type (0:forward,1:backward,2:combined) */
navigation_system, /* navigation system */ number_of_frequencies, /* number of frequencies (1:L1, 2:L1+L2, 3:L1+L2+L5)*/
elevation_mask * D2R, /* elevation mask angle (degrees) */ navigation_system, /* navigation system */
snrmask, /* snrmask_t snrmask SNR mask */ elevation_mask * D2R, /* elevation mask angle (degrees) */
0, /* satellite ephemeris/clock (EPHOPT_XXX) */ snrmask, /* snrmask_t snrmask SNR mask */
integer_ambiguity_resolution_gps, /* AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */ 0, /* satellite ephemeris/clock (EPHOPT_XXX) */
integer_ambiguity_resolution_glo, /* GLONASS AR mode (0:off,1:on,2:auto cal,3:ext cal) */ integer_ambiguity_resolution_gps, /* AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */
integer_ambiguity_resolution_bds, /* BeiDou AR mode (0:off,1:on) */ integer_ambiguity_resolution_glo, /* GLONASS AR mode (0:off,1:on,2:auto cal,3:ext cal) */
outage_reset_ambiguity, /* obs outage count to reset bias */ integer_ambiguity_resolution_bds, /* BeiDou AR mode (0:off,1:on) */
min_lock_to_fix_ambiguity, /* min lock count to fix ambiguity */ outage_reset_ambiguity, /* obs outage count to reset bias */
10, /* min fix count to hold ambiguity */ min_lock_to_fix_ambiguity, /* min lock count to fix ambiguity */
1, /* max iteration to resolve ambiguity */ 10, /* min fix count to hold ambiguity */
iono_model, /* ionosphere option (IONOOPT_XXX) */ 1, /* max iteration to resolve ambiguity */
trop_model, /* troposphere option (TROPOPT_XXX) */ iono_model, /* ionosphere option (IONOOPT_XXX) */
dynamics_model, /* dynamics model (0:none, 1:velocity, 2:accel) */ trop_model, /* troposphere option (TROPOPT_XXX) */
earth_tide, /* earth tide correction (0:off,1:solid,2:solid+otl+pole) */ dynamics_model, /* dynamics model (0:none, 1:velocity, 2:accel) */
number_filter_iter, /* number of filter iteration */ earth_tide, /* earth tide correction (0:off,1:solid,2:solid+otl+pole) */
0, /* code smoothing window size (0:none) */ number_filter_iter, /* number of filter iteration */
0, /* interpolate reference obs (for post mission) */ 0, /* code smoothing window size (0:none) */
0, /* sbssat_t sbssat SBAS correction options */ 0, /* interpolate reference obs (for post mission) */
0, /* sbsion_t sbsion[MAXBAND+1] SBAS satellite selection (0:all) */ 0, /* sbssat_t sbssat SBAS correction options */
0, /* rover position for fixed mode */ 0, /* sbsion_t sbsion[MAXBAND+1] SBAS satellite selection (0:all) */
0, /* base position for relative mode */ 0, /* rover position for fixed mode */
/* 0:pos in prcopt, 1:average of single pos, */ 0, /* base position for relative mode */
/* 2:read from file, 3:rinex header, 4:rtcm pos */ /* 0:pos in prcopt, 1:average of single pos, */
{code_phase_error_ratio_l1,code_phase_error_ratio_l2,code_phase_error_ratio_l5}, /* eratio[NFREQ] code/phase error ratio */ /* 2:read from file, 3:rinex header, 4:rtcm pos */
{100.0,carrier_phase_error_factor_a,carrier_phase_error_factor_b,0.0,1.0}, /* err[5]: measurement error factor [0]:reserved, [1-3]:error factor a/b/c of phase (m) , [4]:doppler frequency (hz) */ {code_phase_error_ratio_l1, code_phase_error_ratio_l2, code_phase_error_ratio_l5}, /* eratio[NFREQ] code/phase error ratio */
{bias_0,iono_0,trop_0}, /* std[3]: initial-state std [0]bias,[1]iono [2]trop*/ {100.0, carrier_phase_error_factor_a, carrier_phase_error_factor_b, 0.0, 1.0}, /* err[5]: measurement error factor [0]:reserved, [1-3]:error factor a/b/c of phase (m) , [4]:doppler frequency (hz) */
{sigma_bias,sigma_iono,sigma_trop,sigma_acch,sigma_accv,sigma_pos}, /* prn[6] process-noise std */ {bias_0, iono_0, trop_0}, /* std[3]: initial-state std [0]bias,[1]iono [2]trop*/
5e-12, /* sclkstab: satellite clock stability (sec/sec) */ {sigma_bias, sigma_iono, sigma_trop, sigma_acch, sigma_accv, sigma_pos}, /* prn[6] process-noise std */
{min_ratio_to_fix_ambiguity,0.9999,0.25,0.1,0.05,0.0,0.0,0.0}, /* thresar[8]: AR validation threshold */ 5e-12, /* sclkstab: satellite clock stability (sec/sec) */
min_elevation_to_fix_ambiguity, /* elevation mask of AR for rising satellite (deg) */ {min_ratio_to_fix_ambiguity, 0.9999, 0.25, 0.1, 0.05, 0.0, 0.0, 0.0}, /* thresar[8]: AR validation threshold */
0.0, /* elevation mask to hold ambiguity (deg) */ min_elevation_to_fix_ambiguity, /* elevation mask of AR for rising satellite (deg) */
slip_threshold, /* slip threshold of geometry-free phase (m) */ 0.0, /* elevation mask to hold ambiguity (deg) */
30.0, /* max difference of time (sec) */ slip_threshold, /* slip threshold of geometry-free phase (m) */
threshold_reject_innovation, /* reject threshold of innovation (m) */ 30.0, /* max difference of time (sec) */
threshold_reject_gdop, /* reject threshold of gdop */ threshold_reject_innovation, /* reject threshold of innovation (m) */
{}, /* double baseline[2] baseline length constraint {const,sigma} (m) */ threshold_reject_gdop, /* reject threshold of gdop */
{}, /* double ru[3] rover position for fixed mode {x,y,z} (ecef) (m) */ {}, /* double baseline[2] baseline length constraint {const,sigma} (m) */
{}, /* double rb[3] base position for relative mode {x,y,z} (ecef) (m) */ {}, /* double ru[3] rover position for fixed mode {x,y,z} (ecef) (m) */
{"",""}, /* char anttype[2][MAXANT] antenna types {rover,base} */ {}, /* double rb[3] base position for relative mode {x,y,z} (ecef) (m) */
{{},{}}, /* double antdel[2][3] antenna delta {{rov_e,rov_n,rov_u},{ref_e,ref_n,ref_u}} */ {"", ""}, /* char anttype[2][MAXANT] antenna types {rover,base} */
{}, /* pcv_t pcvr[2] receiver antenna parameters {rov,base} */ {{}, {}}, /* double antdel[2][3] antenna delta {{rov_e,rov_n,rov_u},{ref_e,ref_n,ref_u}} */
{}, /* unsigned char exsats[MAXSAT] excluded satellites (1:excluded, 2:included) */ {}, /* pcv_t pcvr[2] receiver antenna parameters {rov,base} */
0, /* max averaging epoches */ {}, /* unsigned char exsats[MAXSAT] excluded satellites (1:excluded, 2:included) */
0, /* initialize by restart */ 0, /* max averaging epoches */
1, /* output single by dgps/float/fix/ppp outage */ 0, /* initialize by restart */
{"",""}, /* char rnxopt[2][256] rinex options {rover,base} */ 1, /* output single by dgps/float/fix/ppp outage */
{sat_PCV,rec_PCV,phwindup,reject_GPS_IIA,raim_fde}, /* posopt[6] positioning options [0]: satellite and receiver antenna PCV model; [1]: interpolate antenna parameters; [2]: apply phase wind-up correction for PPP modes; [3]: exclude measurements of GPS Block IIA satellites satellite [4]: RAIM FDE (fault detection and exclusion) [5]: handle day-boundary clock jump */ {"", ""}, /* char rnxopt[2][256] rinex options {rover,base} */
0, /* solution sync mode (0:off,1:on) */ {sat_PCV, rec_PCV, phwindup, reject_GPS_IIA, raim_fde}, /* posopt[6] positioning options [0]: satellite and receiver antenna PCV model; [1]: interpolate antenna parameters; [2]: apply phase wind-up correction for PPP modes; [3]: exclude measurements of GPS Block IIA satellites satellite [4]: RAIM FDE (fault detection and exclusion) [5]: handle day-boundary clock jump */
{{},{}}, /* odisp[2][6*11] ocean tide loading parameters {rov,base} */ 0, /* solution sync mode (0:off,1:on) */
{ {}, {{},{}}, {{},{}}, {}, {} }, /* exterr_t exterr extended receiver error model */ {{}, {}}, /* odisp[2][6*11] ocean tide loading parameters {rov,base} */
0, /* disable L2-AR */ {{}, {{}, {}}, {{}, {}}, {}, {}}, /* exterr_t exterr extended receiver error model */
{} /* char pppopt[256] ppp option "-GAP_RESION=" default gap to reset iono parameters (ep) */ 0, /* disable L2-AR */
{} /* char pppopt[256] ppp option "-GAP_RESION=" default gap to reset iono parameters (ep) */
}; };
rtkinit(&rtk, &rtklib_configuration_options); rtkinit(&rtk, &rtklib_configuration_options);
// make PVT object // make PVT object
pvt_ = rtklib_make_pvt_cc(in_streams_, dump_, dump_filename_, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, rinex_version, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_msg_rate_ms, rtcm_dump_devname, type_of_receiver, rtk); pvt_ = rtklib_make_pvt_cc(in_streams_, dump_, dump_filename_, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, rinex_version, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_msg_rate_ms, rtcm_dump_devname, type_of_receiver, rtk);
DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")";
} }
@ -486,7 +486,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
bool RtklibPvt::save_assistance_to_XML() bool RtklibPvt::save_assistance_to_XML()
{ {
LOG(INFO) << "SUPL: Try to save GPS ephemeris to XML file " << eph_xml_filename_; LOG(INFO) << "SUPL: Try to save GPS ephemeris to XML file " << eph_xml_filename_;
std::map<int,Gps_Ephemeris> eph_map = pvt_->get_GPS_L1_ephemeris_map(); std::map<int, Gps_Ephemeris> eph_map = pvt_->get_GPS_L1_ephemeris_map();
if (eph_map.size() > 0) if (eph_map.size() > 0)
{ {
@ -498,12 +498,12 @@ bool RtklibPvt::save_assistance_to_XML()
ofs.close(); ofs.close();
LOG(INFO) << "Saved GPS L1 Ephemeris map data"; LOG(INFO) << "Saved GPS L1 Ephemeris map data";
} }
catch (const std::exception & e) catch (const std::exception& e)
{ {
LOG(WARNING) << e.what(); LOG(WARNING) << e.what();
return false; return false;
} }
return true; // return variable (true == succeeded) return true; // return variable (true == succeeded)
} }
else else
{ {
@ -522,7 +522,9 @@ RtklibPvt::~RtklibPvt()
void RtklibPvt::connect(gr::top_block_sptr top_block) void RtklibPvt::connect(gr::top_block_sptr top_block)
{ {
if(top_block) { /* top_block is not null */}; if (top_block)
{ /* top_block is not null */
};
// Nothing to connect internally // Nothing to connect internally
DLOG(INFO) << "nothing to connect internally"; DLOG(INFO) << "nothing to connect internally";
} }
@ -530,7 +532,9 @@ void RtklibPvt::connect(gr::top_block_sptr top_block)
void RtklibPvt::disconnect(gr::top_block_sptr top_block) void RtklibPvt::disconnect(gr::top_block_sptr top_block)
{ {
if(top_block) { /* top_block is not null */}; if (top_block)
{ /* top_block is not null */
};
// Nothing to disconnect // Nothing to disconnect
} }
@ -543,5 +547,5 @@ gr::basic_block_sptr RtklibPvt::get_left_block()
gr::basic_block_sptr RtklibPvt::get_right_block() gr::basic_block_sptr RtklibPvt::get_right_block()
{ {
return pvt_; // this is a sink, nothing downstream return pvt_; // this is a sink, nothing downstream
} }

7
src/algorithms/PVT/adapters/rtklib_pvt.h
View File

@ -29,7 +29,6 @@
*/ */
#ifndef GNSS_SDR_RTKLIB_PVT_H_ #ifndef GNSS_SDR_RTKLIB_PVT_H_
#define GNSS_SDR_RTKLIB_PVT_H_ #define GNSS_SDR_RTKLIB_PVT_H_
@ -47,9 +46,9 @@ class RtklibPvt : public PvtInterface
{ {
public: public:
RtklibPvt(ConfigurationInterface* configuration, RtklibPvt(ConfigurationInterface* configuration,
std::string role, std::string role,
unsigned int in_streams, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~RtklibPvt(); virtual ~RtklibPvt();

691
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc
View File

File diff suppressed because it is too large Load Diff

125
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h
View File

@ -53,22 +53,22 @@ class rtklib_pvt_cc;
typedef boost::shared_ptr<rtklib_pvt_cc> rtklib_pvt_cc_sptr; typedef boost::shared_ptr<rtklib_pvt_cc> rtklib_pvt_cc_sptr;
rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int n_channels, rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int n_channels,
bool dump, bool dump,
std::string dump_filename, std::string dump_filename,
int output_rate_ms, int output_rate_ms,
int display_rate_ms, int display_rate_ms,
bool flag_nmea_tty_port, bool flag_nmea_tty_port,
std::string nmea_dump_filename, std::string nmea_dump_filename,
std::string nmea_dump_devname, std::string nmea_dump_devname,
int rinex_version, int rinex_version,
bool flag_rtcm_server, bool flag_rtcm_server,
bool flag_rtcm_tty_port, bool flag_rtcm_tty_port,
unsigned short rtcm_tcp_port, unsigned short rtcm_tcp_port,
unsigned short rtcm_station_id, unsigned short rtcm_station_id,
std::map<int,int> rtcm_msg_rate_ms, std::map<int, int> rtcm_msg_rate_ms,
std::string rtcm_dump_devname, std::string rtcm_dump_devname,
const unsigned int type_of_receiver, const unsigned int type_of_receiver,
rtk_t & rtk); rtk_t& rtk);
/*! /*!
* \brief This class implements a block that computes the PVT solution with Galileo E1 signals * \brief This class implements a block that computes the PVT solution with Galileo E1 signals
@ -77,22 +77,22 @@ class rtklib_pvt_cc : public gr::sync_block
{ {
private: private:
friend rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int nchannels, friend rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int nchannels,
bool dump, bool dump,
std::string dump_filename, std::string dump_filename,
int output_rate_ms, int output_rate_ms,
int display_rate_ms, int display_rate_ms,
bool flag_nmea_tty_port, bool flag_nmea_tty_port,
std::string nmea_dump_filename, std::string nmea_dump_filename,
std::string nmea_dump_devname, std::string nmea_dump_devname,
int rinex_version, int rinex_version,
bool flag_rtcm_server, bool flag_rtcm_server,
bool flag_rtcm_tty_port, bool flag_rtcm_tty_port,
unsigned short rtcm_tcp_port, unsigned short rtcm_tcp_port,
unsigned short rtcm_station_id, unsigned short rtcm_station_id,
std::map<int,int> rtcm_msg_rate_ms, std::map<int, int> rtcm_msg_rate_ms,
std::string rtcm_dump_devname, std::string rtcm_dump_devname,
const unsigned int type_of_receiver, const unsigned int type_of_receiver,
rtk_t & rtk); rtk_t& rtk);
void msg_handler_telemetry(pmt::pmt_t msg); void msg_handler_telemetry(pmt::pmt_t msg);
@ -101,15 +101,15 @@ private:
bool b_rinex_header_updated; bool b_rinex_header_updated;
double d_rinex_version; double d_rinex_version;
bool b_rtcm_writing_started; bool b_rtcm_writing_started;
int d_rtcm_MT1045_rate_ms; //!< Galileo Broadcast Ephemeris int d_rtcm_MT1045_rate_ms; //!< Galileo Broadcast Ephemeris
int d_rtcm_MT1019_rate_ms; //!< GPS Broadcast Ephemeris (orbits) int d_rtcm_MT1019_rate_ms; //!< GPS Broadcast Ephemeris (orbits)
int d_rtcm_MT1020_rate_ms; //!< GLONASS Broadcast Ephemeris (orbits) int d_rtcm_MT1020_rate_ms; //!< GLONASS Broadcast Ephemeris (orbits)
int d_rtcm_MT1077_rate_ms; //!< The type 7 Multiple Signal Message format for the USA’s GPS system, popular int d_rtcm_MT1077_rate_ms; //!< The type 7 Multiple Signal Message format for the USA’s GPS system, popular
int d_rtcm_MT1087_rate_ms; //!< GLONASS MSM7. The type 7 Multiple Signal Message format for the Russian GLONASS system int d_rtcm_MT1087_rate_ms; //!< GLONASS MSM7. The type 7 Multiple Signal Message format for the Russian GLONASS system
int d_rtcm_MT1097_rate_ms; //!< Galileo MSM7. The type 7 Multiple Signal Message format for Europe’s Galileo system int d_rtcm_MT1097_rate_ms; //!< Galileo MSM7. The type 7 Multiple Signal Message format for Europe’s Galileo system
int d_rtcm_MSM_rate_ms; int d_rtcm_MSM_rate_ms;
int d_last_status_print_seg; //for status printer int d_last_status_print_seg; //for status printer
unsigned int d_nchannels; unsigned int d_nchannels;
std::string d_dump_filename; std::string d_dump_filename;
@ -136,16 +136,17 @@ private:
double last_RINEX_nav_output_time; double last_RINEX_nav_output_time;
std::shared_ptr<rtklib_solver> d_ls_pvt; std::shared_ptr<rtklib_solver> d_ls_pvt;
std::map<int,Gnss_Synchro> gnss_observables_map; std::map<int, Gnss_Synchro> gnss_observables_map;
bool observables_pairCompare_min(const std::pair<int,Gnss_Synchro>& a, const std::pair<int,Gnss_Synchro>& b); bool observables_pairCompare_min(const std::pair<int, Gnss_Synchro>& a, const std::pair<int, Gnss_Synchro>& b);
unsigned int type_of_rx; unsigned int type_of_rx;
bool first_fix; bool first_fix;
key_t sysv_msg_key; key_t sysv_msg_key;
int sysv_msqid; int sysv_msqid;
typedef struct { typedef struct
long mtype;//required by sys v message {
long mtype; //required by sys v message
double ttff; double ttff;
} ttff_msgbuf; } ttff_msgbuf;
bool send_sys_v_ttff_msg(ttff_msgbuf ttff); bool send_sys_v_ttff_msg(ttff_msgbuf ttff);
@ -153,33 +154,33 @@ private:
public: public:
rtklib_pvt_cc(unsigned int nchannels, rtklib_pvt_cc(unsigned int nchannels,
bool dump, std::string dump_filename, bool dump, std::string dump_filename,
int output_rate_ms, int output_rate_ms,
int display_rate_ms, int display_rate_ms,
bool flag_nmea_tty_port, bool flag_nmea_tty_port,
std::string nmea_dump_filename, std::string nmea_dump_filename,
std::string nmea_dump_devname, std::string nmea_dump_devname,
int rinex_version, int rinex_version,
bool flag_rtcm_server, bool flag_rtcm_server,
bool flag_rtcm_tty_port, bool flag_rtcm_tty_port,
unsigned short rtcm_tcp_port, unsigned short rtcm_tcp_port,
unsigned short rtcm_station_id, unsigned short rtcm_station_id,
std::map<int,int> rtcm_msg_rate_ms, std::map<int, int> rtcm_msg_rate_ms,
std::string rtcm_dump_devname, std::string rtcm_dump_devname,
const unsigned int type_of_receiver, const unsigned int type_of_receiver,
rtk_t & rtk); rtk_t& rtk);
/*! /*!
* \brief Get latest set of GPS L1 ephemeris from PVT block * \brief Get latest set of GPS L1 ephemeris from PVT block
* *
* It is used to save the assistance data at the receiver shutdown * It is used to save the assistance data at the receiver shutdown
*/ */
std::map<int,Gps_Ephemeris> get_GPS_L1_ephemeris_map(); std::map<int, Gps_Ephemeris> get_GPS_L1_ephemeris_map();
~rtklib_pvt_cc(); //!< Default destructor ~rtklib_pvt_cc(); //!< Default destructor
int work (int noutput_items, gr_vector_const_void_star &input_items, int work(int noutput_items, gr_vector_const_void_star& input_items,
gr_vector_void_star &output_items); //!< PVT Signal Processing gr_vector_void_star& output_items); //!< PVT Signal Processing
}; };
#endif #endif

18
src/algorithms/PVT/libs/geojson_printer.cc
View File

@ -43,7 +43,7 @@ GeoJSON_Printer::GeoJSON_Printer()
} }
GeoJSON_Printer::~GeoJSON_Printer () GeoJSON_Printer::~GeoJSON_Printer()
{ {
GeoJSON_Printer::close_file(); GeoJSON_Printer::close_file();
} }
@ -60,37 +60,37 @@ bool GeoJSON_Printer::set_headers(std::string filename, bool time_tag_name)
const int year = timeinfo.tm_year - 100; const int year = timeinfo.tm_year - 100;
strm0 << year; strm0 << year;
const int month = timeinfo.tm_mon + 1; const int month = timeinfo.tm_mon + 1;
if(month < 10) if (month < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << month; strm0 << month;
const int day = timeinfo.tm_mday; const int day = timeinfo.tm_mday;
if(day < 10) if (day < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << day << "_"; strm0 << day << "_";
const int hour = timeinfo.tm_hour; const int hour = timeinfo.tm_hour;
if(hour < 10) if (hour < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << hour; strm0 << hour;
const int min = timeinfo.tm_min; const int min = timeinfo.tm_min;
if(min < 10) if (min < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << min; strm0 << min;
const int sec = timeinfo.tm_sec; const int sec = timeinfo.tm_sec;
if(sec < 10) if (sec < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << sec; strm0 << sec;
filename_ = filename + "_" + strm0.str() + ".geojson"; filename_ = filename + "_" + strm0.str() + ".geojson";
} }
else else
{ {
@ -184,7 +184,7 @@ bool GeoJSON_Printer::close_file()
// if nothing is written, erase the file // if nothing is written, erase the file
if (first_pos == true) if (first_pos == true)
{ {
if(remove(filename_.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; if (remove(filename_.c_str()) != 0) LOG(INFO) << "Error deleting temporary file";
} }
return true; return true;
@ -194,5 +194,3 @@ bool GeoJSON_Printer::close_file()
return false; return false;
} }
} }

1
src/algorithms/PVT/libs/geojson_printer.h
View File

@ -50,6 +50,7 @@ private:
std::ofstream geojson_file; std::ofstream geojson_file;
bool first_pos; bool first_pos;
std::string filename_; std::string filename_;
public: public:
GeoJSON_Printer(); GeoJSON_Printer();
~GeoJSON_Printer(); ~GeoJSON_Printer();

385
src/algorithms/PVT/libs/hybrid_ls_pvt.cc
View File

@ -49,21 +49,21 @@ hybrid_ls_pvt::hybrid_ls_pvt(int nchannels, std::string dump_filename, bool flag
this->set_averaging_flag(false); this->set_averaging_flag(false);
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true) if (d_flag_dump_enabled == true)
{
if (d_dump_file.is_open() == false)
{ {
try if (d_dump_file.is_open() == false)
{ {
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); try
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); {
LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str(); d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
} d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
catch (const std::ifstream::failure &e) LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
{ }
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what(); catch (const std::ifstream::failure& e)
} {
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
}
}
} }
}
} }
@ -72,27 +72,27 @@ hybrid_ls_pvt::~hybrid_ls_pvt()
if (d_dump_file.is_open() == true) if (d_dump_file.is_open() == true)
{ {
try try
{ {
d_dump_file.close(); d_dump_file.close();
} }
catch(const std::exception & ex) catch (const std::exception& ex)
{ {
LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what();
} }
} }
} }
bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, double hybrid_current_time, bool flag_averaging) bool hybrid_ls_pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, double hybrid_current_time, bool flag_averaging)
{ {
std::map<int,Gnss_Synchro>::iterator gnss_observables_iter; std::map<int, Gnss_Synchro>::iterator gnss_observables_iter;
std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter; std::map<int, Galileo_Ephemeris>::iterator galileo_ephemeris_iter;
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter; std::map<int, Gps_Ephemeris>::iterator gps_ephemeris_iter;
std::map<int,Gps_CNAV_Ephemeris>::iterator gps_cnav_ephemeris_iter; std::map<int, Gps_CNAV_Ephemeris>::iterator gps_cnav_ephemeris_iter;
arma::vec W; // channels weight vector arma::vec W; // channels weight vector
arma::vec obs; // pseudoranges observation vector arma::vec obs; // pseudoranges observation vector
arma::mat satpos; // satellite positions matrix arma::mat satpos; // satellite positions matrix
int Galileo_week_number = 0; int Galileo_week_number = 0;
int GPS_week = 0; int GPS_week = 0;
@ -109,188 +109,188 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// ******************************************************************************** // ********************************************************************************
// ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) **** // ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) ****
// ******************************************************************************** // ********************************************************************************
int valid_obs = 0; //valid observations counter int valid_obs = 0; //valid observations counter
for(gnss_observables_iter = gnss_observables_map.begin(); for (gnss_observables_iter = gnss_observables_map.begin();
gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter != gnss_observables_map.end();
gnss_observables_iter++) gnss_observables_iter++)
{ {
switch(gnss_observables_iter->second.System) switch (gnss_observables_iter->second.System)
{
case 'E':
{ {
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key case 'E':
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); {
if (galileo_ephemeris_iter != galileo_ephemeris_map.end()) // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
{ galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
/*! if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
{
/*!
* \todo Place here the satellite CN0 (power level, or weight factor) * \todo Place here the satellite CN0 (power level, or weight factor)
*/ */
W.resize(valid_obs + 1, 1); W.resize(valid_obs + 1, 1);
W(valid_obs) = 1; W(valid_obs) = 1;
// COMMON RX TIME PVT ALGORITHM // COMMON RX TIME PVT ALGORITHM
double Rx_time = hybrid_current_time; double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GALILEO_C_m_s; double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GALILEO_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV // 2- compute the clock drift using the clock model (broadcast) for this SV
SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time); SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time);
// 3- compute the current ECEF position for this SV using corrected TX time // 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s; TX_time_corrected_s = Tx_time - SV_clock_bias_s;
galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
//store satellite positions in a matrix //store satellite positions in a matrix
satpos.resize(3, valid_obs + 1); satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = galileo_ephemeris_iter->second.d_satpos_X; satpos(0, valid_obs) = galileo_ephemeris_iter->second.d_satpos_X;
satpos(1, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Y; satpos(1, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Y;
satpos(2, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Z; satpos(2, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Z;
// 4- fill the observations vector with the corrected observables // 4- fill the observations vector with the corrected observables
obs.resize(valid_obs + 1, 1); obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s - this->get_time_offset_s() * GALILEO_C_m_s; obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s - this->get_time_offset_s() * GALILEO_C_m_s;
this->set_visible_satellites_ID(valid_obs, galileo_ephemeris_iter->second.i_satellite_PRN); this->set_visible_satellites_ID(valid_obs, galileo_ephemeris_iter->second.i_satellite_PRN);
this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz); this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz);
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time); GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
// SV ECEF DEBUG OUTPUT // SV ECEF DEBUG OUTPUT
DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
<< " X=" << galileo_ephemeris_iter->second.d_satpos_X << " X=" << galileo_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y << " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z << " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]"; << " [m] PR_obs=" << obs(valid_obs) << " [m]";
valid_obs++; valid_obs++;
} }
else // the ephemeris are not available for this SV else // the ephemeris are not available for this SV
{ {
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
} }
break; break;
} }
case 'G': case 'G':
{ {
// 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key // 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key
std::string sig_(gnss_observables_iter->second.Signal); std::string sig_(gnss_observables_iter->second.Signal);
if(sig_.compare("1C") == 0) if (sig_.compare("1C") == 0)
{ {
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_ephemeris_iter != gps_ephemeris_map.end()) if (gps_ephemeris_iter != gps_ephemeris_map.end())
{ {
/*! /*!
* \todo Place here the satellite CN0 (power level, or weight factor) * \todo Place here the satellite CN0 (power level, or weight factor)
*/ */
W.resize(valid_obs + 1, 1); W.resize(valid_obs + 1, 1);
W(valid_obs) = 1; W(valid_obs) = 1;
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files) // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time // first estimate of transmit time
double Rx_time = hybrid_current_time; double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s; double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect // 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect
SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD; SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD;
// 3- compute the current ECEF position for this SV using corrected TX time and obtain clock bias including relativistic effect // 3- compute the current ECEF position for this SV using corrected TX time and obtain clock bias including relativistic effect
TX_time_corrected_s = Tx_time - SV_clock_bias_s; TX_time_corrected_s = Tx_time - SV_clock_bias_s;
double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
//store satellite positions in a matrix //store satellite positions in a matrix
satpos.resize(3, valid_obs + 1); satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X; satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X;
satpos(1, valid_obs) = gps_ephemeris_iter->second.d_satpos_Y; satpos(1, valid_obs) = gps_ephemeris_iter->second.d_satpos_Y;
satpos(2, valid_obs) = gps_ephemeris_iter->second.d_satpos_Z; satpos(2, valid_obs) = gps_ephemeris_iter->second.d_satpos_Z;
// 4- fill the observations vector with the corrected pseudoranges // 4- fill the observations vector with the corrected pseudoranges
// compute code bias: TGD for single frequency // compute code bias: TGD for single frequency
// See IS-GPS-200E section 20.3.3.3.3.2 // See IS-GPS-200E section 20.3.3.3.3.2
double sqrt_Gamma=GPS_L1_FREQ_HZ/GPS_L2_FREQ_HZ; double sqrt_Gamma = GPS_L1_FREQ_HZ / GPS_L2_FREQ_HZ;
double Gamma=sqrt_Gamma*sqrt_Gamma; double Gamma = sqrt_Gamma * sqrt_Gamma;
double P1_P2=(1.0-Gamma)*(gps_ephemeris_iter->second.d_TGD* GPS_C_m_s); double P1_P2 = (1.0 - Gamma) * (gps_ephemeris_iter->second.d_TGD * GPS_C_m_s);
double Code_bias_m= P1_P2/(1.0-Gamma); double Code_bias_m = P1_P2 / (1.0 - Gamma);
obs.resize(valid_obs + 1, 1); obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s-Code_bias_m-this->get_time_offset_s() * GPS_C_m_s; obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s - Code_bias_m - this->get_time_offset_s() * GPS_C_m_s;
this->set_visible_satellites_ID(valid_obs, gps_ephemeris_iter->second.i_satellite_PRN); this->set_visible_satellites_ID(valid_obs, gps_ephemeris_iter->second.i_satellite_PRN);
this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz); this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz);
// SV ECEF DEBUG OUTPUT // SV ECEF DEBUG OUTPUT
LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
<< " TX Time corrected="<<TX_time_corrected_s << " X=" << gps_ephemeris_iter->second.d_satpos_X << " TX Time corrected=" << TX_time_corrected_s << " X=" << gps_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y << " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z << " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]"; << " [m] PR_obs=" << obs(valid_obs) << " [m]";
valid_obs++; valid_obs++;
// compute the UTC time for this SV (just to print the associated UTC timestamp) // compute the UTC time for this SV (just to print the associated UTC timestamp)
GPS_week = gps_ephemeris_iter->second.i_GPS_week; GPS_week = gps_ephemeris_iter->second.i_GPS_week;
} }
else // the ephemeris are not available for this SV else // the ephemeris are not available for this SV
{ {
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first; DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first;
} }
} }
if(sig_.compare("2S") == 0) if (sig_.compare("2S") == 0)
{ {
gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN); gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end()) if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end())
{ {
/*! /*!
* \todo Place here the satellite CN0 (power level, or weight factor) * \todo Place here the satellite CN0 (power level, or weight factor)
*/ */
W.resize(valid_obs + 1, 1); W.resize(valid_obs + 1, 1);
W(valid_obs) = 1; W(valid_obs) = 1;
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files) // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time // first estimate of transmit time
double Rx_time = hybrid_current_time; double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s; double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV // 2- compute the clock drift using the clock model (broadcast) for this SV
SV_clock_bias_s = gps_cnav_ephemeris_iter->second.sv_clock_drift(Tx_time); SV_clock_bias_s = gps_cnav_ephemeris_iter->second.sv_clock_drift(Tx_time);
// 3- compute the current ECEF position for this SV using corrected TX time // 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s; TX_time_corrected_s = Tx_time - SV_clock_bias_s;
//std::cout<<"TX time["<<gps_cnav_ephemeris_iter->second.i_satellite_PRN<<"]="<<TX_time_corrected_s<<std::endl; //std::cout<<"TX time["<<gps_cnav_ephemeris_iter->second.i_satellite_PRN<<"]="<<TX_time_corrected_s<<std::endl;
double dtr = gps_cnav_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); double dtr = gps_cnav_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
//store satellite positions in a matrix //store satellite positions in a matrix
satpos.resize(3, valid_obs + 1); satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_X; satpos(0, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_X;
satpos(1, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_Y; satpos(1, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_Y;
satpos(2, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_Z; satpos(2, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_Z;
// 4- fill the observations vector with the corrected observables // 4- fill the observations vector with the corrected observables
obs.resize(valid_obs + 1, 1); obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr*GPS_C_m_s + SV_clock_bias_s * GPS_C_m_s; obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s + SV_clock_bias_s * GPS_C_m_s;
this->set_visible_satellites_ID(valid_obs, gps_cnav_ephemeris_iter->second.i_satellite_PRN); this->set_visible_satellites_ID(valid_obs, gps_cnav_ephemeris_iter->second.i_satellite_PRN);
this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz); this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz);
GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week; GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week;
GPS_week=GPS_week%1024; //Necessary due to the increase of WN bits in CNAV message (10 in GPS NAV and 13 in CNAV) GPS_week = GPS_week % 1024; //Necessary due to the increase of WN bits in CNAV message (10 in GPS NAV and 13 in CNAV)
// SV ECEF DEBUG OUTPUT // SV ECEF DEBUG OUTPUT
LOG(INFO) << "(new)ECEF GPS L2M satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN LOG(INFO) << "(new)ECEF GPS L2M satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN
<< " TX Time corrected="<<TX_time_corrected_s << " TX Time corrected=" << TX_time_corrected_s
<< " X=" << gps_cnav_ephemeris_iter->second.d_satpos_X << " X=" << gps_cnav_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << gps_cnav_ephemeris_iter->second.d_satpos_Y << " [m] Y=" << gps_cnav_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << gps_cnav_ephemeris_iter->second.d_satpos_Z << " [m] Z=" << gps_cnav_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]"; << " [m] PR_obs=" << obs(valid_obs) << " [m]";
valid_obs++; valid_obs++;
} }
else // the ephemeris are not available for this SV else // the ephemeris are not available for this SV
{ {
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
} }
} }
break;
}
default:
DLOG(INFO) << "Hybrid observables: Unknown GNSS";
break; break;
} }
default :
DLOG(INFO) << "Hybrid observables: Unknown GNSS";
break;
}
} }
// ******************************************************************************** // ********************************************************************************
@ -300,35 +300,35 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
LOG(INFO) << "HYBRID PVT: valid observations=" << valid_obs; LOG(INFO) << "HYBRID PVT: valid observations=" << valid_obs;
if(valid_obs >= 4) if (valid_obs >= 4)
{ {
arma::vec rx_position_and_time; arma::vec rx_position_and_time;
DLOG(INFO) << "satpos=" << satpos; DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs=" << obs; DLOG(INFO) << "obs=" << obs;
DLOG(INFO) << "W=" << W; DLOG(INFO) << "W=" << W;
try try
{ {
// check if this is the initial position computation // check if this is the initial position computation
if (this->get_time_offset_s() == 0) if (this->get_time_offset_s() == 0)
{ {
// execute Bancroft's algorithm to estimate initial receiver position and time // execute Bancroft's algorithm to estimate initial receiver position and time
DLOG(INFO) << " Executing Bancroft algorithm..."; DLOG(INFO) << " Executing Bancroft algorithm...";
rx_position_and_time = bancroftPos(satpos.t(), obs); rx_position_and_time = bancroftPos(satpos.t(), obs);
this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration
this->set_time_offset_s(rx_position_and_time(3) / GPS_C_m_s); // save time for the next iteration [meters]->[seconds] this->set_time_offset_s(rx_position_and_time(3) / GPS_C_m_s); // save time for the next iteration [meters]->[seconds]
} }
// Execute WLS using previous position as the initialization point // Execute WLS using previous position as the initialization point
rx_position_and_time = leastSquarePos(satpos, obs, W); rx_position_and_time = leastSquarePos(satpos, obs, W);
this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration
this->set_time_offset_s(this->get_time_offset_s() + rx_position_and_time(3) / GPS_C_m_s); // accumulate the rx time error for the next iteration [meters]->[seconds] this->set_time_offset_s(this->get_time_offset_s() + rx_position_and_time(3) / GPS_C_m_s); // accumulate the rx time error for the next iteration [meters]->[seconds]
DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
DLOG(INFO) << "Accumulated rx clock error=" << this->get_time_offset_s() << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]"; DLOG(INFO) << "Accumulated rx clock error=" << this->get_time_offset_s() << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]";
// Compute GST and Gregorian time // Compute GST and Gregorian time
if( GST != 0.0) if (GST != 0.0)
{ {
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number); utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
} }
@ -347,17 +347,18 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
DLOG(INFO) << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time) DLOG(INFO) << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude() << " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude()
<< " [deg], Height= " << this->get_height() << " [m]" << " RX time offset= " << this->get_time_offset_s() << " [s]"; << " [deg], Height= " << this->get_height() << " [m]"
<< " RX time offset= " << this->get_time_offset_s() << " [s]";
// ###### Compute DOPs ######## // ###### Compute DOPs ########
hybrid_ls_pvt::compute_DOP(); hybrid_ls_pvt::compute_DOP();
// ######## LOG FILE ######### // ######## LOG FILE #########
if(d_flag_dump_enabled == true) if (d_flag_dump_enabled == true)
{ {
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file
try try
{ {
double tmp_double; double tmp_double;
// PVT GPS time // PVT GPS time
tmp_double = hybrid_current_time; tmp_double = hybrid_current_time;
@ -383,22 +384,22 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// GEO user position Height [m] // GEO user position Height [m]
tmp_double = this->get_height(); tmp_double = this->get_height();
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
} }
catch (const std::ifstream::failure& e) catch (const std::ifstream::failure& e)
{ {
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what(); LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
} }
} }
// MOVING AVERAGE PVT // MOVING AVERAGE PVT
this->perform_pos_averaging(); this->perform_pos_averaging();
} }
catch(const std::exception & e) catch (const std::exception& e)
{ {
this->set_time_offset_s(0.0); //reset rx time estimation this->set_time_offset_s(0.0); //reset rx time estimation
LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what(); LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what();
this->set_valid_position(false); this->set_valid_position(false);
} }
} }
else else
{ {

13
src/algorithms/PVT/libs/hybrid_ls_pvt.h
View File

@ -52,17 +52,18 @@ private:
bool d_flag_dump_enabled; bool d_flag_dump_enabled;
std::string d_dump_filename; std::string d_dump_filename;
std::ofstream d_dump_file; std::ofstream d_dump_file;
int d_nchannels; // Number of available channels for positioning int d_nchannels; // Number of available channels for positioning
double d_galileo_current_time; double d_galileo_current_time;
public: public:
hybrid_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file); hybrid_ls_pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file);
~hybrid_ls_pvt(); ~hybrid_ls_pvt();
bool get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, double Rx_time, bool flag_averaging); bool get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, double Rx_time, bool flag_averaging);
std::map<int,Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris std::map<int, Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris
std::map<int,Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new GPS_Ephemeris std::map<int, Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new GPS_Ephemeris
std::map<int,Gps_CNAV_Ephemeris> gps_cnav_ephemeris_map; std::map<int, Gps_CNAV_Ephemeris> gps_cnav_ephemeris_map;
Galileo_Utc_Model galileo_utc_model; Galileo_Utc_Model galileo_utc_model;
Galileo_Iono galileo_iono; Galileo_Iono galileo_iono;

73
src/algorithms/PVT/libs/kml_printer.cc
View File

@ -36,7 +36,7 @@
using google::LogMessage; using google::LogMessage;
bool Kml_Printer::set_headers(std::string filename, bool time_tag_name) bool Kml_Printer::set_headers(std::string filename, bool time_tag_name)
{ {
boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time(); boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time();
tm timeinfo = boost::posix_time::to_tm(pt); tm timeinfo = boost::posix_time::to_tm(pt);
@ -47,37 +47,37 @@ bool Kml_Printer::set_headers(std::string filename, bool time_tag_name)
const int year = timeinfo.tm_year - 100; const int year = timeinfo.tm_year - 100;
strm0 << year; strm0 << year;
const int month = timeinfo.tm_mon + 1; const int month = timeinfo.tm_mon + 1;
if(month < 10) if (month < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << month; strm0 << month;
const int day = timeinfo.tm_mday; const int day = timeinfo.tm_mday;
if(day < 10) if (day < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << day << "_"; strm0 << day << "_";
const int hour = timeinfo.tm_hour; const int hour = timeinfo.tm_hour;
if(hour < 10) if (hour < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << hour; strm0 << hour;
const int min = timeinfo.tm_min; const int min = timeinfo.tm_min;
if(min < 10) if (min < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << min; strm0 << min;
const int sec = timeinfo.tm_sec; const int sec = timeinfo.tm_sec;
if(sec < 10) if (sec < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << sec; strm0 << sec;
kml_filename = filename + "_" + strm0.str() + ".kml"; kml_filename = filename + "_" + strm0.str() + ".kml";
} }
else else
{ {
@ -92,29 +92,29 @@ bool Kml_Printer::set_headers(std::string filename, bool time_tag_name)
kml_file.setf(kml_file.fixed, kml_file.floatfield); kml_file.setf(kml_file.fixed, kml_file.floatfield);
kml_file << std::setprecision(14); kml_file << std::setprecision(14);
kml_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl kml_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl
<< "<kml xmlns=\"http://www.opengis.net/kml/2.2\">" << std::endl << "<kml xmlns=\"http://www.opengis.net/kml/2.2\">" << std::endl
<< " <Document>" << std::endl << " <Document>" << std::endl
<< " <name>GNSS Track</name>" << std::endl << " <name>GNSS Track</name>" << std::endl
<< " <description>GNSS-SDR Receiver position log file created at " << pt << " <description>GNSS-SDR Receiver position log file created at " << pt
<< " </description>" << std::endl << " </description>" << std::endl
<< "<Style id=\"yellowLineGreenPoly\">" << std::endl << "<Style id=\"yellowLineGreenPoly\">" << std::endl
<< " <LineStyle>" << std::endl << " <LineStyle>" << std::endl
<< " <color>7f00ffff</color>" << std::endl << " <color>7f00ffff</color>" << std::endl
<< " <width>1</width>" << std::endl << " <width>1</width>" << std::endl
<< " </LineStyle>" << std::endl << " </LineStyle>" << std::endl
<< "<PolyStyle>" << std::endl << "<PolyStyle>" << std::endl
<< " <color>7f00ff00</color>" << std::endl << " <color>7f00ff00</color>" << std::endl
<< "</PolyStyle>" << std::endl << "</PolyStyle>" << std::endl
<< "</Style>" << std::endl << "</Style>" << std::endl
<< "<Placemark>" << std::endl << "<Placemark>" << std::endl
<< "<name>GNSS-SDR PVT</name>" << std::endl << "<name>GNSS-SDR PVT</name>" << std::endl
<< "<description>GNSS-SDR position log</description>" << std::endl << "<description>GNSS-SDR position log</description>" << std::endl
<< "<styleUrl>#yellowLineGreenPoly</styleUrl>" << std::endl << "<styleUrl>#yellowLineGreenPoly</styleUrl>" << std::endl
<< "<LineString>" << std::endl << "<LineString>" << std::endl
<< "<extrude>0</extrude>" << std::endl << "<extrude>0</extrude>" << std::endl
<< "<tessellate>1</tessellate>" << std::endl << "<tessellate>1</tessellate>" << std::endl
<< "<altitudeMode>absolute</altitudeMode>" << std::endl << "<altitudeMode>absolute</altitudeMode>" << std::endl
<< "<coordinates>" << std::endl; << "<coordinates>" << std::endl;
return true; return true;
} }
else else
@ -124,7 +124,6 @@ bool Kml_Printer::set_headers(std::string filename, bool time_tag_name)
} }
bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position, bool print_average_values) bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position, bool print_average_values)
{ {
double latitude; double latitude;
@ -164,7 +163,6 @@ bool Kml_Printer::close_file()
{ {
if (kml_file.is_open()) if (kml_file.is_open())
{ {
kml_file << "</coordinates>" << std::endl kml_file << "</coordinates>" << std::endl
<< "</LineString>" << std::endl << "</LineString>" << std::endl
<< "</Placemark>" << std::endl << "</Placemark>" << std::endl
@ -180,20 +178,17 @@ bool Kml_Printer::close_file()
} }
Kml_Printer::Kml_Printer()
Kml_Printer::Kml_Printer ()
{ {
positions_printed = false; positions_printed = false;
} }
Kml_Printer::~Kml_Printer()
Kml_Printer::~Kml_Printer ()
{ {
close_file(); close_file();
if(!positions_printed) if (!positions_printed)
{ {
if(remove(kml_filename.c_str()) != 0) LOG(INFO) << "Error deleting temporary KML file"; if (remove(kml_filename.c_str()) != 0) LOG(INFO) << "Error deleting temporary KML file";
} }
} }

1
src/algorithms/PVT/libs/kml_printer.h
View File

@ -50,6 +50,7 @@ private:
std::ofstream kml_file; std::ofstream kml_file;
bool positions_printed; bool positions_printed;
std::string kml_filename; std::string kml_filename;
public: public:
Kml_Printer(); Kml_Printer();
~Kml_Printer(); ~Kml_Printer();

89
src/algorithms/PVT/libs/ls_pvt.cc
View File

@ -41,7 +41,6 @@ using google::LogMessage;
Ls_Pvt::Ls_Pvt() : Pvt_Solution() Ls_Pvt::Ls_Pvt() : Pvt_Solution()
{ {
} }
arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs) arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs)
@ -70,7 +69,7 @@ arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs)
// 6995655.459 -23537808.269 -9927906.485 24222112.972 ]; // 6995655.459 -23537808.269 -9927906.485 24222112.972 ];
// Solution: 596902.683 -4847843.316 4088216.740 // Solution: 596902.683 -4847843.316 4088216.740
arma::vec pos = arma::zeros(4,1); arma::vec pos = arma::zeros(4, 1);
arma::mat B_pass = arma::zeros(obs.size(), 4); arma::mat B_pass = arma::zeros(obs.size(), 4);
B_pass.submat(0, 0, obs.size() - 1, 2) = satpos; B_pass.submat(0, 0, obs.size() - 1, 2) = satpos;
B_pass.col(3) = obs; B_pass.col(3) = obs;
@ -81,27 +80,27 @@ arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs)
for (int iter = 0; iter < 2; iter++) for (int iter = 0; iter < 2; iter++)
{ {
B = B_pass; B = B_pass;
int m = arma::size(B,0); int m = arma::size(B, 0);
for (int i = 0; i < m; i++) for (int i = 0; i < m; i++)
{ {
int x = B(i,0); int x = B(i, 0);
int y = B(i,1); int y = B(i, 1);
if (iter == 0) if (iter == 0)
{ {
traveltime = 0.072; traveltime = 0.072;
} }
else else
{ {
int z = B(i,2); int z = B(i, 2);
double rho = (x - pos(0)) * (x - pos(0)) + (y - pos(1)) * (y - pos(1)) + (z - pos(2)) * (z - pos(2)); double rho = (x - pos(0)) * (x - pos(0)) + (y - pos(1)) * (y - pos(1)) + (z - pos(2)) * (z - pos(2));
traveltime = sqrt(rho) / GPS_C_m_s; traveltime = sqrt(rho) / GPS_C_m_s;
} }
double angle = traveltime * 7.292115147e-5; double angle = traveltime * 7.292115147e-5;
double cosa = cos(angle); double cosa = cos(angle);
double sina = sin(angle); double sina = sin(angle);
B(i,0) = cosa * x + sina * y; B(i, 0) = cosa * x + sina * y;
B(i,1) = -sina * x + cosa * y; B(i, 1) = -sina * x + cosa * y;
}// % i-loop } // % i-loop
if (m > 3) if (m > 3)
{ {
@ -111,8 +110,8 @@ arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs)
{ {
BBB = arma::inv(B); BBB = arma::inv(B);
} }
arma::vec e = arma::ones(m,1); arma::vec e = arma::ones(m, 1);
arma::vec alpha = arma::zeros(m,1); arma::vec alpha = arma::zeros(m, 1);
for (int i = 0; i < m; i++) for (int i = 0; i < m; i++)
{ {
alpha(i) = lorentz(B.row(i).t(), B.row(i).t()) / 2.0; alpha(i) = lorentz(B.row(i).t(), B.row(i).t()) / 2.0;
@ -124,24 +123,24 @@ arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs)
double c = lorentz(BBBalpha, BBBalpha); double c = lorentz(BBBalpha, BBBalpha);
double root = sqrt(b * b - a * c); double root = sqrt(b * b - a * c);
arma::vec r = {(-b - root) / a, (-b + root) / a}; arma::vec r = {(-b - root) / a, (-b + root) / a};
arma::mat possible_pos = arma::zeros(4,2); arma::mat possible_pos = arma::zeros(4, 2);
for (int i = 0; i < 2; i++) for (int i = 0; i < 2; i++)
{ {
possible_pos.col(i) = r(i) * BBBe + BBBalpha; possible_pos.col(i) = r(i) * BBBe + BBBalpha;
possible_pos(3,i) = -possible_pos(3,i); possible_pos(3, i) = -possible_pos(3, i);
} }
arma::vec abs_omc = arma::zeros(2,1); arma::vec abs_omc = arma::zeros(2, 1);
for (int j = 0; j < m; j++) for (int j = 0; j < m; j++)
{ {
for (int i = 0; i < 2; i++) for (int i = 0; i < 2; i++)
{ {
double c_dt = possible_pos(3,i); double c_dt = possible_pos(3, i);
double calc = arma::norm(satpos.row(i).t() - possible_pos.col(i).rows(0,2)) + c_dt; double calc = arma::norm(satpos.row(i).t() - possible_pos.col(i).rows(0, 2)) + c_dt;
double omc = obs(j) - calc; double omc = obs(j) - calc;
abs_omc(i) = std::abs(omc); abs_omc(i) = std::abs(omc);
} }
} // % j-loop } // % j-loop
// discrimination between roots // discrimination between roots
if (abs_omc(0) > abs_omc(1)) if (abs_omc(0) > abs_omc(1))
@ -152,7 +151,7 @@ arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs)
{ {
pos = possible_pos.col(0); pos = possible_pos.col(0);
} }
} // % iter loop } // % iter loop
return pos; return pos;
} }
@ -167,11 +166,11 @@ double Ls_Pvt::lorentz(const arma::vec& x, const arma::vec& y)
// M = diag([1 1 1 -1]); // M = diag([1 1 1 -1]);
// p = x'*M*y; // p = x'*M*y;
return(x(0) * y(0) + x(1) * y(1) + x(2) * y(2) - x(3) * y(3)); return (x(0) * y(0) + x(1) * y(1) + x(2) * y(2) - x(3) * y(3));
} }
arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::vec & w_vec) arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs, const arma::vec& w_vec)
{ {
/* Computes the Least Squares Solution. /* Computes the Least Squares Solution.
* Inputs: * Inputs:
@ -185,14 +184,14 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
*/ */
//=== Initialization ======================================================= //=== Initialization =======================================================
int nmbOfIterations = 10; // TODO: include in config int nmbOfIterations = 10; // TODO: include in config
int nmbOfSatellites; int nmbOfSatellites;
nmbOfSatellites = satpos.n_cols; // Armadillo nmbOfSatellites = satpos.n_cols; // Armadillo
arma::mat w = arma::zeros(nmbOfSatellites, nmbOfSatellites); arma::mat w = arma::zeros(nmbOfSatellites, nmbOfSatellites);
w.diag() = w_vec; //diagonal weight matrix w.diag() = w_vec; //diagonal weight matrix
arma::vec rx_pos = this->get_rx_pos(); arma::vec rx_pos = this->get_rx_pos();
arma::vec pos = {rx_pos(0), rx_pos(1), rx_pos(2), 0}; // time error in METERS (time x speed) arma::vec pos = {rx_pos(0), rx_pos(1), rx_pos(2), 0}; // time error in METERS (time x speed)
arma::mat A; arma::mat A;
arma::mat omc; arma::mat omc;
A = arma::zeros(nmbOfSatellites, 4); A = arma::zeros(nmbOfSatellites, 4);
@ -215,31 +214,33 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
if (iter == 0) if (iter == 0)
{ {
//--- Initialize variables at the first iteration -------------- //--- Initialize variables at the first iteration --------------
Rot_X = X.col(i); //Armadillo Rot_X = X.col(i); //Armadillo
trop = 0.0; trop = 0.0;
} }
else else
{ {
//--- Update equations ----------------------------------------- //--- Update equations -----------------------------------------
rho2 = (X(0, i) - pos(0)) * rho2 = (X(0, i) - pos(0)) *
(X(0, i) - pos(0)) + (X(1, i) - pos(1)) * (X(0, i) - pos(0)) +
(X(1, i) - pos(1)) + (X(2, i) - pos(2)) * (X(1, i) - pos(1)) *
(X(2, i) - pos(2)); (X(1, i) - pos(1)) +
(X(2, i) - pos(2)) *
(X(2, i) - pos(2));
traveltime = sqrt(rho2) / GPS_C_m_s; traveltime = sqrt(rho2) / GPS_C_m_s;
//--- Correct satellite position (do to earth rotation) -------- //--- Correct satellite position (do to earth rotation) --------
Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo
//--- Find DOA and range of satellites //--- Find DOA and range of satellites
double * azim = 0; double* azim = 0;
double * elev = 0; double* elev = 0;
double * dist = 0; double* dist = 0;
Ls_Pvt::topocent(azim, elev, dist, pos.subvec(0,2), Rot_X - pos.subvec(0, 2)); Ls_Pvt::topocent(azim, elev, dist, pos.subvec(0, 2), Rot_X - pos.subvec(0, 2));
this->set_visible_satellites_Az(i, *azim); this->set_visible_satellites_Az(i, *azim);
this->set_visible_satellites_El(i, *elev); this->set_visible_satellites_El(i, *elev);
this->set_visible_satellites_Distance(i, *dist); this->set_visible_satellites_Distance(i, *dist);
if(traveltime < 0.1 && nmbOfSatellites > 3) if (traveltime < 0.1 && nmbOfSatellites > 3)
{ {
//--- Find receiver's height //--- Find receiver's height
Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2)); Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
@ -253,29 +254,29 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
{ {
//--- Find delay due to troposphere (in meters) //--- Find delay due to troposphere (in meters)
Ls_Pvt::tropo(&trop, sin(this->get_visible_satellites_El(i) * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); Ls_Pvt::tropo(&trop, sin(this->get_visible_satellites_El(i) * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
if(trop > 5.0 ) trop = 0.0; //check for erratic values if (trop > 5.0) trop = 0.0; //check for erratic values
} }
} }
} }
//--- Apply the corrections ---------------------------------------- //--- Apply the corrections ----------------------------------------
omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo
//--- Construct the A matrix --------------------------------------- //--- Construct the A matrix ---------------------------------------
//Armadillo //Armadillo
A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i); A(i, 0) = (-(Rot_X(0) - pos(0))) / obs(i);
A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i); A(i, 1) = (-(Rot_X(1) - pos(1))) / obs(i);
A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i); A(i, 2) = (-(Rot_X(2) - pos(2))) / obs(i);
A(i,3) = 1.0; A(i, 3) = 1.0;
} }
//--- Find position update --------------------------------------------- //--- Find position update ---------------------------------------------
x = arma::solve(w*A, w*omc); // Armadillo x = arma::solve(w * A, w * omc); // Armadillo
//--- Apply position update -------------------------------------------- //--- Apply position update --------------------------------------------
pos = pos + x; pos = pos + x;
if (arma::norm(x,2) < 1e-4) if (arma::norm(x, 2) < 1e-4)
{ {
break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm) break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
} }
} }
@ -290,5 +291,3 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
} }
return pos; return pos;
} }

8
src/algorithms/PVT/libs/ls_pvt.h
View File

@ -45,20 +45,20 @@ private:
/*! /*!
* \brief Computes the Lorentz inner product between two vectors * \brief Computes the Lorentz inner product between two vectors
*/ */
double lorentz(const arma::vec & x,const arma::vec & y); double lorentz(const arma::vec& x, const arma::vec& y);
public: public:
Ls_Pvt(); Ls_Pvt();
/*! /*!
* \brief Computes the initial position solution based on the Bancroft algorithm * \brief Computes the initial position solution based on the Bancroft algorithm
*/ */
arma::vec bancroftPos(const arma::mat & satpos, const arma::vec & obs); arma::vec bancroftPos(const arma::mat& satpos, const arma::vec& obs);
/*! /*!
* \brief Computes the Weighted Least Squares position solution * \brief Computes the Weighted Least Squares position solution
*/ */
arma::vec leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::vec & w_vec); arma::vec leastSquarePos(const arma::mat& satpos, const arma::vec& obs, const arma::vec& w_vec);
}; };
#endif #endif

70
src/algorithms/PVT/libs/nmea_printer.cc
View File

@ -79,7 +79,7 @@ Nmea_Printer::~Nmea_Printer()
} }
int Nmea_Printer::init_serial (std::string serial_device) int Nmea_Printer::init_serial(std::string serial_device)
{ {
/*! /*!
* Opens the serial device and sets the default baud rate for a NMEA transmission (9600,8,N,1) * Opens the serial device and sets the default baud rate for a NMEA transmission (9600,8,N,1)
@ -93,12 +93,12 @@ int Nmea_Printer::init_serial (std::string serial_device)
long PARITY; long PARITY;
fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
if (fd == -1) return fd; //failed to open TTY port if (fd == -1) return fd; //failed to open TTY port
if(fcntl(fd, F_SETFL, 0) == -1) LOG(INFO) << "Error enabling direct I/O"; // clear all flags on descriptor, enable direct I/O if (fcntl(fd, F_SETFL, 0) == -1) LOG(INFO) << "Error enabling direct I/O"; // clear all flags on descriptor, enable direct I/O
tcgetattr(fd, &options); // read serial port options tcgetattr(fd, &options); // read serial port options
BAUD = B9600; BAUD = B9600;
//BAUD = B38400; //BAUD = B38400;
DATABITS = CS8; DATABITS = CS8;
STOPBITS = 0; STOPBITS = 0;
@ -116,7 +116,7 @@ int Nmea_Printer::init_serial (std::string serial_device)
} }
void Nmea_Printer::close_serial () void Nmea_Printer::close_serial()
{ {
if (nmea_dev_descriptor != -1) if (nmea_dev_descriptor != -1)
{ {
@ -149,7 +149,7 @@ bool Nmea_Printer::Print_Nmea_Line(const std::shared_ptr<Pvt_Solution>& pvt_data
// write to log file // write to log file
try try
{ {
//GPRMC //GPRMC
nmea_file_descriptor << GPRMC; nmea_file_descriptor << GPRMC;
//GPGGA (Global Positioning System Fixed Data) //GPGGA (Global Positioning System Fixed Data)
@ -158,31 +158,32 @@ bool Nmea_Printer::Print_Nmea_Line(const std::shared_ptr<Pvt_Solution>& pvt_data
nmea_file_descriptor << GPGSA; nmea_file_descriptor << GPGSA;
//GPGSV //GPGSV
nmea_file_descriptor << GPGSV; nmea_file_descriptor << GPGSV;
} }
catch(const std::exception & ex) catch (const std::exception& ex)
{ {
DLOG(INFO) << "NMEA printer can not write on output file" << nmea_filename.c_str();; DLOG(INFO) << "NMEA printer can not write on output file" << nmea_filename.c_str();
} ;
}
//write to serial device //write to serial device
if (nmea_dev_descriptor!=-1) if (nmea_dev_descriptor != -1)
{ {
if(write(nmea_dev_descriptor, GPRMC.c_str(), GPRMC.length()) == -1) if (write(nmea_dev_descriptor, GPRMC.c_str(), GPRMC.length()) == -1)
{ {
DLOG(INFO) << "NMEA printer cannot write on serial device" << nmea_devname.c_str(); DLOG(INFO) << "NMEA printer cannot write on serial device" << nmea_devname.c_str();
return false; return false;
} }
if(write(nmea_dev_descriptor, GPGGA.c_str(), GPGGA.length()) == -1) if (write(nmea_dev_descriptor, GPGGA.c_str(), GPGGA.length()) == -1)
{ {
DLOG(INFO) << "NMEA printer cannot write on serial device" << nmea_devname.c_str(); DLOG(INFO) << "NMEA printer cannot write on serial device" << nmea_devname.c_str();
return false; return false;
} }
if(write(nmea_dev_descriptor, GPGSA.c_str(), GPGSA.length()) == -1) if (write(nmea_dev_descriptor, GPGSA.c_str(), GPGSA.length()) == -1)
{ {
DLOG(INFO) << "NMEA printer cannot write on serial device" << nmea_devname.c_str(); DLOG(INFO) << "NMEA printer cannot write on serial device" << nmea_devname.c_str();
return false; return false;
} }
if(write(nmea_dev_descriptor, GPGSV.c_str(), GPGSV.length()) == -1) if (write(nmea_dev_descriptor, GPGSV.c_str(), GPGSV.length()) == -1)
{ {
DLOG(INFO) << "NMEA printer cannot write on serial device" << nmea_devname.c_str(); DLOG(INFO) << "NMEA printer cannot write on serial device" << nmea_devname.c_str();
return false; return false;
@ -211,7 +212,7 @@ std::string Nmea_Printer::latitude_to_hm(double lat)
if (lat < 0.0) if (lat < 0.0)
{ {
north = false; north = false;
lat = -lat ; lat = -lat;
} }
else else
{ {
@ -220,7 +221,7 @@ std::string Nmea_Printer::latitude_to_hm(double lat)
int deg = static_cast<int>(lat); int deg = static_cast<int>(lat);
double mins = lat - static_cast<double>(deg); double mins = lat - static_cast<double>(deg);
mins *= 60.0 ; mins *= 60.0;
std::ostringstream out_string; std::ostringstream out_string;
out_string.setf(std::ios::fixed, std::ios::floatfield); out_string.setf(std::ios::fixed, std::ios::floatfield);
out_string.fill('0'); out_string.fill('0');
@ -249,7 +250,7 @@ std::string Nmea_Printer::longitude_to_hm(double longitude)
if (longitude < 0.0) if (longitude < 0.0)
{ {
east = false; east = false;
longitude = -longitude ; longitude = -longitude;
} }
else else
{ {
@ -257,7 +258,7 @@ std::string Nmea_Printer::longitude_to_hm(double longitude)
} }
int deg = static_cast<int>(longitude); int deg = static_cast<int>(longitude);
double mins = longitude - static_cast<double>(deg); double mins = longitude - static_cast<double>(deg);
mins *= 60.0 ; mins *= 60.0;
std::ostringstream out_string; std::ostringstream out_string;
out_string.setf(std::ios::fixed, std::ios::floatfield); out_string.setf(std::ios::fixed, std::ios::floatfield);
out_string.width(3); out_string.width(3);
@ -294,30 +295,30 @@ std::string Nmea_Printer::get_UTC_NMEA_time(boost::posix_time::ptime d_position_
utc_hours = td.hours(); utc_hours = td.hours();
utc_mins = td.minutes(); utc_mins = td.minutes();
utc_seconds = td.seconds(); utc_seconds = td.seconds();
utc_milliseconds = td.total_milliseconds() - td.total_seconds()*1000; utc_milliseconds = td.total_milliseconds() - td.total_seconds() * 1000;
if (utc_hours < 10) sentence_str << "0"; // two digits for hours if (utc_hours < 10) sentence_str << "0"; // two digits for hours
sentence_str << utc_hours; sentence_str << utc_hours;
if (utc_mins < 10) sentence_str << "0"; // two digits for minutes if (utc_mins < 10) sentence_str << "0"; // two digits for minutes
sentence_str << utc_mins; sentence_str << utc_mins;
if (utc_seconds < 10) sentence_str << "0"; // two digits for seconds if (utc_seconds < 10) sentence_str << "0"; // two digits for seconds
sentence_str << utc_seconds; sentence_str << utc_seconds;
if (utc_milliseconds < 10) if (utc_milliseconds < 10)
{ {
sentence_str << ".00"; // three digits for ms sentence_str << ".00"; // three digits for ms
sentence_str << utc_milliseconds; sentence_str << utc_milliseconds;
} }
else if (utc_milliseconds < 100) else if (utc_milliseconds < 100)
{ {
sentence_str << ".0"; // three digits for ms sentence_str << ".0"; // three digits for ms
sentence_str << utc_milliseconds; sentence_str << utc_milliseconds;
} }
else else
{ {
sentence_str << "."; // three digits for ms sentence_str << "."; // three digits for ms
sentence_str << utc_milliseconds; sentence_str << utc_milliseconds;
} }
return sentence_str.str(); return sentence_str.str();
@ -450,7 +451,7 @@ std::string Nmea_Printer::get_GPGSA()
// 1 fix not available // 1 fix not available
// 2 fix 2D // 2 fix 2D
// 3 fix 3D // 3 fix 3D
if (valid_fix==true) if (valid_fix == true)
{ {
sentence_str << ",3"; sentence_str << ",3";
} }
@ -460,7 +461,7 @@ std::string Nmea_Printer::get_GPGSA()
}; };
// Used satellites // Used satellites
for (int i=0; i<12; i++) for (int i = 0; i < 12; i++)
{ {
sentence_str << ","; sentence_str << ",";
if (i < n_sats_used) if (i < n_sats_used)
@ -479,7 +480,7 @@ std::string Nmea_Printer::get_GPGSA()
sentence_str.fill('0'); sentence_str.fill('0');
sentence_str << pdop; sentence_str << pdop;
//HDOP //HDOP
sentence_str<<","; sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield); sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.width(2); sentence_str.width(2);
sentence_str.precision(1); sentence_str.precision(1);
@ -528,7 +529,7 @@ std::string Nmea_Printer::get_GPGSV()
// generate the frames // generate the frames
int current_satellite = 0; int current_satellite = 0;
for (int i=1; i<(n_frames+1); i++) for (int i = 1; i < (n_frames + 1); i++)
{ {
frame_str.str(""); frame_str.str("");
frame_str << sentence_header; frame_str << sentence_header;
@ -547,7 +548,7 @@ std::string Nmea_Printer::get_GPGSV()
frame_str << std::dec << n_sats_used; frame_str << std::dec << n_sats_used;
//satellites info //satellites info
for (int j=0; j<4; j++) for (int j = 0; j < 4; j++)
{ {
// write satellite info // write satellite info
frame_str << ","; frame_str << ",";
@ -601,7 +602,7 @@ std::string Nmea_Printer::get_GPGGA()
{ {
//boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time(); //boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time();
bool valid_fix = d_PVT_data->is_valid_position(); bool valid_fix = d_PVT_data->is_valid_position();
int n_channels = d_PVT_data->get_num_valid_observations();//d_nchannels int n_channels = d_PVT_data->get_num_valid_observations(); //d_nchannels
double hdop = d_PVT_data->get_HDOP(); double hdop = d_PVT_data->get_HDOP();
double MSL_altitude; double MSL_altitude;
@ -708,4 +709,3 @@ std::string Nmea_Printer::get_GPGGA()
return sentence_str.str(); return sentence_str.str();
//$GPGGA,104427.591,5920.7009,N,01803.2938,E,1,05,3.3,78.2,M,23.2,M,0.0,0000*4A //$GPGGA,104427.591,5920.7009,N,01803.2938,E,1,05,3.3,78.2,M,23.2,M,0.0,0000*4A
} }

16
src/algorithms/PVT/libs/nmea_printer.h
View File

@ -66,17 +66,17 @@ public:
~Nmea_Printer(); ~Nmea_Printer();
private: private:
std::string nmea_filename; // String with the NMEA log filename std::string nmea_filename; // String with the NMEA log filename
std::ofstream nmea_file_descriptor; // Output file stream for NMEA log file std::ofstream nmea_file_descriptor; // Output file stream for NMEA log file
std::string nmea_devname; std::string nmea_devname;
int nmea_dev_descriptor; // NMEA serial device descriptor (i.e. COM port) int nmea_dev_descriptor; // NMEA serial device descriptor (i.e. COM port)
std::shared_ptr<Pvt_Solution> d_PVT_data; std::shared_ptr<Pvt_Solution> d_PVT_data;
int init_serial(std::string serial_device); //serial port control int init_serial(std::string serial_device); //serial port control
void close_serial(); void close_serial();
std::string get_GPGGA(); // fix data std::string get_GPGGA(); // fix data
std::string get_GPGSV(); // satellite data std::string get_GPGSV(); // satellite data
std::string get_GPGSA(); // overall satellite reception data std::string get_GPGSA(); // overall satellite reception data
std::string get_GPRMC(); // minimum recommended data std::string get_GPRMC(); // minimum recommended data
std::string get_UTC_NMEA_time(boost::posix_time::ptime d_position_UTC_time); std::string get_UTC_NMEA_time(boost::posix_time::ptime d_position_UTC_time);
std::string longitude_to_hm(double longitude); std::string longitude_to_hm(double longitude);
std::string latitude_to_hm(double lat); std::string latitude_to_hm(double lat);

206
src/algorithms/PVT/libs/pvt_solution.cc
View File

@ -55,11 +55,11 @@ Pvt_Solution::Pvt_Solution()
b_valid_position = false; b_valid_position = false;
d_averaging_depth = 0; d_averaging_depth = 0;
d_valid_observations = 0; d_valid_observations = 0;
d_rx_pos = arma::zeros(3,1); d_rx_pos = arma::zeros(3, 1);
d_rx_dt_s = 0.0; d_rx_dt_s = 0.0;
} }
arma::vec Pvt_Solution::rotateSatellite(double const traveltime, const arma::vec & X_sat) arma::vec Pvt_Solution::rotateSatellite(double const traveltime, const arma::vec &X_sat)
{ {
/* /*
* Returns rotated satellite ECEF coordinates due to Earth * Returns rotated satellite ECEF coordinates due to Earth
@ -78,7 +78,7 @@ arma::vec Pvt_Solution::rotateSatellite(double const traveltime, const arma::vec
omegatau = OMEGA_EARTH_DOT * traveltime; omegatau = OMEGA_EARTH_DOT * traveltime;
//--- Build a rotation matrix ---------------------------------------------- //--- Build a rotation matrix ----------------------------------------------
arma::mat R3 = arma::zeros(3,3); arma::mat R3 = arma::zeros(3, 3);
R3(0, 0) = cos(omegatau); R3(0, 0) = cos(omegatau);
R3(0, 1) = sin(omegatau); R3(0, 1) = sin(omegatau);
R3(0, 2) = 0.0; R3(0, 2) = 0.0;
@ -112,7 +112,7 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection)
const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0}; const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0};
const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563}; const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563};
double lambda = atan2(Y, X); double lambda = atan2(Y, X);
double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection])); double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection]));
double c = a[elipsoid_selection] * sqrt(1.0 + ex2); double c = a[elipsoid_selection] * sqrt(1.0 + ex2);
double phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection]))); double phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection])));
@ -125,7 +125,7 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection)
{ {
oldh = h; oldh = h;
N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi))); N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi)));
phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h) )))); phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h)))));
h = sqrt(X * X + Y * Y) / cos(phi) - N; h = sqrt(X * X + Y * Y) / cos(phi) - N;
iterations = iterations + 1; iterations = iterations + 1;
if (iterations > 100) if (iterations > 100)
@ -182,7 +182,7 @@ int Pvt_Solution::togeod(double *dphi, double *dlambda, double *h, double a, dou
} }
// first guess // first guess
double P = sqrt(X * X + Y * Y); // P is distance from spin axis double P = sqrt(X * X + Y * Y); // P is distance from spin axis
//direct calculation of longitude //direct calculation of longitude
if (P > 1.0E-20) if (P > 1.0E-20)
@ -200,12 +200,12 @@ int Pvt_Solution::togeod(double *dphi, double *dlambda, double *h, double a, dou
*dlambda = *dlambda + 360.0; *dlambda = *dlambda + 360.0;
} }
double r = sqrt(P * P + Z * Z); // r is distance from origin (0,0,0) double r = sqrt(P * P + Z * Z); // r is distance from origin (0,0,0)
double sinphi; double sinphi;
if (r > 1.0E-20) if (r > 1.0E-20)
{ {
sinphi = Z/r; sinphi = Z / r;
} }
else else
{ {
@ -221,7 +221,7 @@ int Pvt_Solution::togeod(double *dphi, double *dlambda, double *h, double a, dou
return 1; return 1;
} }
*h = r - a * (1 - sinphi * sinphi/finv); *h = r - a * (1 - sinphi * sinphi / finv);
// iterate // iterate
double cosphi; double cosphi;
@ -244,7 +244,7 @@ int Pvt_Solution::togeod(double *dphi, double *dlambda, double *h, double a, dou
// update height and latitude // update height and latitude
*h = *h + (sinphi * dZ + cosphi * dP); *h = *h + (sinphi * dZ + cosphi * dP);
*dphi = *dphi + (cosphi * dZ - sinphi * dP)/(N_phi + (*h)); *dphi = *dphi + (cosphi * dZ - sinphi * dP) / (N_phi + (*h));
// test for convergence // test for convergence
if ((dP * dP + dZ * dZ) < tolsq) if ((dP * dP + dZ * dZ) < tolsq)
@ -285,61 +285,66 @@ int Pvt_Solution::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb
Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre
*/ */
const double a_e = 6378.137; // semi-major axis of earth ellipsoid const double a_e = 6378.137; // semi-major axis of earth ellipsoid
const double b0 = 7.839257e-5; const double b0 = 7.839257e-5;
const double tlapse = -6.5; const double tlapse = -6.5;
const double em = -978.77 / (2.8704e6 * tlapse * 1.0e-5); const double em = -978.77 / (2.8704e6 * tlapse * 1.0e-5);
double tkhum = t_kel + tlapse * (hhum_km - htkel_km); double tkhum = t_kel + tlapse * (hhum_km - htkel_km);
double atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15); double atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15);
double e0 = 0.0611 * hum * pow(10, atkel); double e0 = 0.0611 * hum * pow(10, atkel);
double tksea = t_kel - tlapse * htkel_km; double tksea = t_kel - tlapse * htkel_km;
double tkelh = tksea + tlapse * hhum_km; double tkelh = tksea + tlapse * hhum_km;
double e0sea = e0 * pow((tksea / tkelh), (4 * em)); double e0sea = e0 * pow((tksea / tkelh), (4 * em));
double tkelp = tksea + tlapse * hp_km; double tkelp = tksea + tlapse * hp_km;
double psea = p_mb * pow((tksea / tkelp), em); double psea = p_mb * pow((tksea / tkelp), em);
if(sinel < 0) { sinel = 0.0; } if (sinel < 0)
{
sinel = 0.0;
}
double tropo_delay = 0.0; double tropo_delay = 0.0;
bool done = false; bool done = false;
double refsea = 77.624e-6 / tksea; double refsea = 77.624e-6 / tksea;
double htop = 1.1385e-5 / refsea; double htop = 1.1385e-5 / refsea;
refsea = refsea * psea; refsea = refsea * psea;
double ref = refsea * pow(((htop - hsta_km) / htop), 4); double ref = refsea * pow(((htop - hsta_km) / htop), 4);
double a; double a;
double b; double b;
double rtop; double rtop;
while(1) while (1)
{ {
rtop = pow((a_e + htop), 2) - pow((a_e + hsta_km), 2) * (1 - pow(sinel, 2)); rtop = pow((a_e + htop), 2) - pow((a_e + hsta_km), 2) * (1 - pow(sinel, 2));
// check to see if geometry is crazy // check to see if geometry is crazy
if(rtop < 0) { rtop = 0; } if (rtop < 0)
{
rtop = 0;
}
rtop = sqrt(rtop) - (a_e + hsta_km) * sinel; rtop = sqrt(rtop) - (a_e + hsta_km) * sinel;
a = -sinel / (htop - hsta_km); a = -sinel / (htop - hsta_km);
b = -b0 * (1 - pow(sinel,2)) / (htop - hsta_km); b = -b0 * (1 - pow(sinel, 2)) / (htop - hsta_km);
arma::vec rn = arma::vec(8); arma::vec rn = arma::vec(8);
rn.zeros(); rn.zeros();
for(int i = 0; i<8; i++) for (int i = 0; i < 8; i++)
{ {
rn(i) = pow(rtop, (i+1+1)); rn(i) = pow(rtop, (i + 1 + 1));
} }
arma::rowvec alpha = {2 * a, 2 * pow(a, 2) + 4 * b /3, a * (pow(a, 2) + 3 * b), arma::rowvec alpha = {2 * a, 2 * pow(a, 2) + 4 * b / 3, a * (pow(a, 2) + 3 * b),
pow(a, 4)/5 + 2.4 * pow(a, 2) * b + 1.2 * pow(b, 2), 2 * a * b * (pow(a, 2) + 3 * b)/3, pow(a, 4) / 5 + 2.4 * pow(a, 2) * b + 1.2 * pow(b, 2), 2 * a * b * (pow(a, 2) + 3 * b) / 3,
pow(b, 2) * (6 * pow(a, 2) + 4 * b) * 1.428571e-1, 0, 0}; pow(b, 2) * (6 * pow(a, 2) + 4 * b) * 1.428571e-1, 0, 0};
if(pow(b, 2) > 1.0e-35) if (pow(b, 2) > 1.0e-35)
{ {
alpha(6) = a * pow(b, 3) /2; alpha(6) = a * pow(b, 3) / 2;
alpha(7) = pow(b, 4) / 9; alpha(7) = pow(b, 4) / 9;
} }
@ -348,22 +353,22 @@ int Pvt_Solution::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb
dr = dr + aux_(0, 0); dr = dr + aux_(0, 0);
tropo_delay = tropo_delay + dr * ref * 1000; tropo_delay = tropo_delay + dr * ref * 1000;
if(done == true) if (done == true)
{ {
*ddr_m = tropo_delay; *ddr_m = tropo_delay;
break; break;
} }
done = true; done = true;
refsea = (371900.0e-6 / tksea - 12.92e-6) / tksea; refsea = (371900.0e-6 / tksea - 12.92e-6) / tksea;
htop = 1.1385e-5 * (1255 / tksea + 0.05) / refsea; htop = 1.1385e-5 * (1255 / tksea + 0.05) / refsea;
ref = refsea * e0sea * pow(((htop - hsta_km) / htop), 4); ref = refsea * e0sea * pow(((htop - hsta_km) / htop), 4);
} }
return 0; return 0;
} }
int Pvt_Solution::topocent(double *Az, double *El, double *D, const arma::vec & x, const arma::vec & dx) int Pvt_Solution::topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx)
{ {
/* Transformation of vector dx into topocentric coordinate /* Transformation of vector dx into topocentric coordinate
system with origin at x system with origin at x
@ -383,8 +388,8 @@ int Pvt_Solution::topocent(double *Az, double *El, double *D, const arma::vec &
double phi; double phi;
double h; double h;
double dtr = GPS_PI / 180.0; double dtr = GPS_PI / 180.0;
double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84 double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84
double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84 double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84
// Transform x into geodetic coordinates // Transform x into geodetic coordinates
Pvt_Solution::togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2)); Pvt_Solution::togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
@ -394,19 +399,19 @@ int Pvt_Solution::topocent(double *Az, double *El, double *D, const arma::vec &
double cb = cos(phi * dtr); double cb = cos(phi * dtr);
double sb = sin(phi * dtr); double sb = sin(phi * dtr);
arma::mat F = arma::zeros(3,3); arma::mat F = arma::zeros(3, 3);
F(0,0) = -sl; F(0, 0) = -sl;
F(0,1) = -sb * cl; F(0, 1) = -sb * cl;
F(0,2) = cb * cl; F(0, 2) = cb * cl;
F(1,0) = cl; F(1, 0) = cl;
F(1,1) = -sb * sl; F(1, 1) = -sb * sl;
F(1,2) = cb * sl; F(1, 2) = cb * sl;
F(2,0) = 0; F(2, 0) = 0;
F(2,1) = cb; F(2, 1) = cb;
F(2,2) = sb; F(2, 2) = sb;
arma::vec local_vector; arma::vec local_vector;
@ -440,47 +445,46 @@ int Pvt_Solution::topocent(double *Az, double *El, double *D, const arma::vec &
} }
int Pvt_Solution::compute_DOP() int Pvt_Solution::compute_DOP()
{ {
// ###### Compute DOPs ######## // ###### Compute DOPs ########
// 1- Rotation matrix from ECEF coordinates to ENU coordinates // 1- Rotation matrix from ECEF coordinates to ENU coordinates
// ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates // ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
arma::mat F = arma::zeros(3,3); arma::mat F = arma::zeros(3, 3);
F(0,0) = -sin(GPS_TWO_PI * (d_longitude_d/360.0)); F(0, 0) = -sin(GPS_TWO_PI * (d_longitude_d / 360.0));
F(0,1) = -sin(GPS_TWO_PI * (d_latitude_d/360.0)) * cos(GPS_TWO_PI * (d_longitude_d/360.0)); F(0, 1) = -sin(GPS_TWO_PI * (d_latitude_d / 360.0)) * cos(GPS_TWO_PI * (d_longitude_d / 360.0));
F(0,2) = cos(GPS_TWO_PI * (d_latitude_d/360.0)) * cos(GPS_TWO_PI * (d_longitude_d/360.0)); F(0, 2) = cos(GPS_TWO_PI * (d_latitude_d / 360.0)) * cos(GPS_TWO_PI * (d_longitude_d / 360.0));
F(1,0) = cos((GPS_TWO_PI * d_longitude_d)/360.0); F(1, 0) = cos((GPS_TWO_PI * d_longitude_d) / 360.0);
F(1,1) = -sin((GPS_TWO_PI * d_latitude_d)/360.0) * sin((GPS_TWO_PI * d_longitude_d)/360.0); F(1, 1) = -sin((GPS_TWO_PI * d_latitude_d) / 360.0) * sin((GPS_TWO_PI * d_longitude_d) / 360.0);
F(1,2) = cos((GPS_TWO_PI * d_latitude_d/360.0)) * sin((GPS_TWO_PI * d_longitude_d)/360.0); F(1, 2) = cos((GPS_TWO_PI * d_latitude_d / 360.0)) * sin((GPS_TWO_PI * d_longitude_d) / 360.0);
F(2,0) = 0; F(2, 0) = 0;
F(2,1) = cos((GPS_TWO_PI * d_latitude_d)/360.0); F(2, 1) = cos((GPS_TWO_PI * d_latitude_d) / 360.0);
F(2,2) = sin((GPS_TWO_PI * d_latitude_d/360.0)); F(2, 2) = sin((GPS_TWO_PI * d_latitude_d / 360.0));
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS) // 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2); arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2);
arma::mat DOP_ENU = arma::zeros(3, 3); arma::mat DOP_ENU = arma::zeros(3, 3);
try try
{ {
DOP_ENU = arma::htrans(F) * Q_ECEF * F; DOP_ENU = arma::htrans(F) * Q_ECEF * F;
d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP
d_PDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1) + DOP_ENU(2, 2));// PDOP d_PDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1) + DOP_ENU(2, 2)); // PDOP
d_HDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1)); // HDOP d_HDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1)); // HDOP
d_VDOP = sqrt(DOP_ENU(2, 2)); // VDOP d_VDOP = sqrt(DOP_ENU(2, 2)); // VDOP
d_TDOP = sqrt(d_Q(3, 3)); // TDOP d_TDOP = sqrt(d_Q(3, 3)); // TDOP
} }
catch(const std::exception & ex) catch (const std::exception &ex)
{ {
d_GDOP = -1; // Geometric DOP d_GDOP = -1; // Geometric DOP
d_PDOP = -1; // PDOP d_PDOP = -1; // PDOP
d_HDOP = -1; // HDOP d_HDOP = -1; // HDOP
d_VDOP = -1; // VDOP d_VDOP = -1; // VDOP
d_TDOP = -1; // TDOP d_TDOP = -1; // TDOP
} }
return 0; return 0;
} }
@ -521,7 +525,7 @@ void Pvt_Solution::perform_pos_averaging()
{ {
d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i); d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i);
d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i); d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i); d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i);
} }
d_avg_latitude_d = d_avg_latitude_d / static_cast<double>(d_averaging_depth); d_avg_latitude_d = d_avg_latitude_d / static_cast<double>(d_averaging_depth);
d_avg_longitude_d = d_avg_longitude_d / static_cast<double>(d_averaging_depth); d_avg_longitude_d = d_avg_longitude_d / static_cast<double>(d_averaging_depth);
@ -614,7 +618,7 @@ void Pvt_Solution::set_valid_position(bool is_valid)
} }
void Pvt_Solution::set_rx_pos(const arma::vec & pos) void Pvt_Solution::set_rx_pos(const arma::vec &pos)
{ {
d_rx_pos = pos; d_rx_pos = pos;
d_latitude_d = d_rx_pos(0); d_latitude_d = d_rx_pos(0);
@ -635,7 +639,7 @@ boost::posix_time::ptime Pvt_Solution::get_position_UTC_time() const
} }
void Pvt_Solution::set_position_UTC_time(const boost::posix_time::ptime & pt) void Pvt_Solution::set_position_UTC_time(const boost::posix_time::ptime &pt)
{ {
d_position_UTC_time = pt; d_position_UTC_time = pt;
} }
@ -655,14 +659,14 @@ void Pvt_Solution::set_num_valid_observations(int num)
bool Pvt_Solution::set_visible_satellites_ID(size_t index, unsigned int prn) bool Pvt_Solution::set_visible_satellites_ID(size_t index, unsigned int prn)
{ {
if(index >= PVT_MAX_CHANNELS) if (index >= PVT_MAX_CHANNELS)
{ {
LOG(WARNING) << "Setting sat ID to channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; LOG(WARNING) << "Setting sat ID to channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false; return false;
} }
else else
{ {
if(prn >= PVT_MAX_PRN) if (prn >= PVT_MAX_PRN)
{ {
LOG(WARNING) << "Setting to channel " << index << " a PRN of " << prn << " (the maximum is " << PVT_MAX_PRN << ")"; LOG(WARNING) << "Setting to channel " << index << " a PRN of " << prn << " (the maximum is " << PVT_MAX_PRN << ")";
return false; return false;
@ -678,7 +682,7 @@ bool Pvt_Solution::set_visible_satellites_ID(size_t index, unsigned int prn)
unsigned int Pvt_Solution::get_visible_satellites_ID(size_t index) const unsigned int Pvt_Solution::get_visible_satellites_ID(size_t index) const
{ {
if(index >= PVT_MAX_CHANNELS) if (index >= PVT_MAX_CHANNELS)
{ {
LOG(WARNING) << "Getting sat ID for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; LOG(WARNING) << "Getting sat ID for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0; return 0;
@ -692,21 +696,21 @@ unsigned int Pvt_Solution::get_visible_satellites_ID(size_t index) const
bool Pvt_Solution::set_visible_satellites_El(size_t index, double el) bool Pvt_Solution::set_visible_satellites_El(size_t index, double el)
{ {
if(index >= PVT_MAX_CHANNELS) if (index >= PVT_MAX_CHANNELS)
{ {
LOG(WARNING) << "Setting sat elevation for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; LOG(WARNING) << "Setting sat elevation for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false; return false;
} }
else else
{ {
if(el > 90.0) if (el > 90.0)
{ {
LOG(WARNING) << "Setting a sat elevation > 90 [degrees]. Saturating to 90"; LOG(WARNING) << "Setting a sat elevation > 90 [degrees]. Saturating to 90";
d_visible_satellites_El[index] = 90.0; d_visible_satellites_El[index] = 90.0;
} }
else else
{ {
if(el < -90.0) if (el < -90.0)
{ {
LOG(WARNING) << "Setting a sat elevation < -90 [degrees]. Saturating to -90"; LOG(WARNING) << "Setting a sat elevation < -90 [degrees]. Saturating to -90";
d_visible_satellites_El[index] = -90.0; d_visible_satellites_El[index] = -90.0;
@ -723,7 +727,7 @@ bool Pvt_Solution::set_visible_satellites_El(size_t index, double el)
double Pvt_Solution::get_visible_satellites_El(size_t index) const double Pvt_Solution::get_visible_satellites_El(size_t index) const
{ {
if(index >= PVT_MAX_CHANNELS) if (index >= PVT_MAX_CHANNELS)
{ {
LOG(WARNING) << "Getting sat elevation for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; LOG(WARNING) << "Getting sat elevation for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0.0; return 0.0;
@ -737,7 +741,7 @@ double Pvt_Solution::get_visible_satellites_El(size_t index) const
bool Pvt_Solution::set_visible_satellites_Az(size_t index, double az) bool Pvt_Solution::set_visible_satellites_Az(size_t index, double az)
{ {
if(index >= PVT_MAX_CHANNELS) if (index >= PVT_MAX_CHANNELS)
{ {
LOG(WARNING) << "Getting sat azimuth for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; LOG(WARNING) << "Getting sat azimuth for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false; return false;
@ -752,7 +756,7 @@ bool Pvt_Solution::set_visible_satellites_Az(size_t index, double az)
double Pvt_Solution::get_visible_satellites_Az(size_t index) const double Pvt_Solution::get_visible_satellites_Az(size_t index) const
{ {
if(index >= PVT_MAX_CHANNELS) if (index >= PVT_MAX_CHANNELS)
{ {
LOG(WARNING) << "Getting sat azimuth for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; LOG(WARNING) << "Getting sat azimuth for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0.0; return 0.0;
@ -766,7 +770,7 @@ double Pvt_Solution::get_visible_satellites_Az(size_t index) const
bool Pvt_Solution::set_visible_satellites_Distance(size_t index, double dist) bool Pvt_Solution::set_visible_satellites_Distance(size_t index, double dist)
{ {
if(index >= PVT_MAX_CHANNELS) if (index >= PVT_MAX_CHANNELS)
{ {
LOG(WARNING) << "Setting sat distance for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; LOG(WARNING) << "Setting sat distance for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false; return false;
@ -781,7 +785,7 @@ bool Pvt_Solution::set_visible_satellites_Distance(size_t index, double dist)
double Pvt_Solution::get_visible_satellites_Distance(size_t index) const double Pvt_Solution::get_visible_satellites_Distance(size_t index) const
{ {
if(index >= PVT_MAX_CHANNELS) if (index >= PVT_MAX_CHANNELS)
{ {
LOG(WARNING) << "Getting sat distance for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; LOG(WARNING) << "Getting sat distance for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0.0; return 0.0;
@ -795,7 +799,7 @@ double Pvt_Solution::get_visible_satellites_Distance(size_t index) const
bool Pvt_Solution::set_visible_satellites_CN0_dB(size_t index, double cn0) bool Pvt_Solution::set_visible_satellites_CN0_dB(size_t index, double cn0)
{ {
if(index >= PVT_MAX_CHANNELS) if (index >= PVT_MAX_CHANNELS)
{ {
LOG(WARNING) << "Setting sat Cn0 for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; LOG(WARNING) << "Setting sat Cn0 for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false; return false;
@ -810,7 +814,7 @@ bool Pvt_Solution::set_visible_satellites_CN0_dB(size_t index, double cn0)
double Pvt_Solution::get_visible_satellites_CN0_dB(size_t index) const double Pvt_Solution::get_visible_satellites_CN0_dB(size_t index) const
{ {
if(index >= PVT_MAX_CHANNELS) if (index >= PVT_MAX_CHANNELS)
{ {
LOG(WARNING) << "Getting received CN0 for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; LOG(WARNING) << "Getting received CN0 for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0.0; return 0.0;
@ -822,7 +826,7 @@ double Pvt_Solution::get_visible_satellites_CN0_dB(size_t index) const
} }
void Pvt_Solution::set_Q(const arma::mat & Q) void Pvt_Solution::set_Q(const arma::mat &Q)
{ {
d_Q = Q; d_Q = Q;
} }

80
src/algorithms/PVT/libs/pvt_solution.h
View File

@ -47,15 +47,15 @@ const unsigned int PVT_MAX_PRN = 127; // 126 is SBAS
class Pvt_Solution class Pvt_Solution
{ {
private: private:
double d_rx_dt_s; // RX time offset [s] double d_rx_dt_s; // RX time offset [s]
double d_latitude_d; // RX position Latitude WGS84 [deg] double d_latitude_d; // RX position Latitude WGS84 [deg]
double d_longitude_d; // RX position Longitude WGS84 [deg] double d_longitude_d; // RX position Longitude WGS84 [deg]
double d_height_m; // RX position height WGS84 [m] double d_height_m; // RX position height WGS84 [m]
double d_avg_latitude_d; // Averaged latitude in degrees double d_avg_latitude_d; // Averaged latitude in degrees
double d_avg_longitude_d; // Averaged longitude in degrees double d_avg_longitude_d; // Averaged longitude in degrees
double d_avg_height_m; // Averaged height [m] double d_avg_height_m; // Averaged height [m]
bool b_valid_position; bool b_valid_position;
@ -64,7 +64,7 @@ private:
std::deque<double> d_hist_height_m; std::deque<double> d_hist_height_m;
bool d_flag_averaging; bool d_flag_averaging;
int d_averaging_depth; // Length of averaging window int d_averaging_depth; // Length of averaging window
arma::vec d_rx_pos; arma::vec d_rx_pos;
boost::posix_time::ptime d_position_UTC_time; boost::posix_time::ptime d_position_UTC_time;
@ -77,11 +77,11 @@ private:
double d_VDOP; double d_VDOP;
double d_TDOP; double d_TDOP;
int d_visible_satellites_IDs[PVT_MAX_CHANNELS] = {}; // Array with the IDs of the valid satellites int d_visible_satellites_IDs[PVT_MAX_CHANNELS] = {}; // Array with the IDs of the valid satellites
double d_visible_satellites_El[PVT_MAX_CHANNELS] = {}; // Array with the LOS Elevation of the valid satellites double d_visible_satellites_El[PVT_MAX_CHANNELS] = {}; // Array with the LOS Elevation of the valid satellites
double d_visible_satellites_Az[PVT_MAX_CHANNELS] = {}; // Array with the LOS Azimuth of the valid satellites double d_visible_satellites_Az[PVT_MAX_CHANNELS] = {}; // Array with the LOS Azimuth of the valid satellites
double d_visible_satellites_Distance[PVT_MAX_CHANNELS] = {}; // Array with the LOS Distance of the valid satellites double d_visible_satellites_Distance[PVT_MAX_CHANNELS] = {}; // Array with the LOS Distance of the valid satellites
double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS] = {}; // Array with the IDs of the valid satellites double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS] = {}; // Array with the IDs of the valid satellites
public: public:
Pvt_Solution(); Pvt_Solution();
@ -89,22 +89,22 @@ public:
double get_time_offset_s() const; //!< Get RX time offset [s] double get_time_offset_s() const; //!< Get RX time offset [s]
void set_time_offset_s(double offset); //!< Set RX time offset [s] void set_time_offset_s(double offset); //!< Set RX time offset [s]
double get_latitude() const; //!< Get RX position Latitude WGS84 [deg] double get_latitude() const; //!< Get RX position Latitude WGS84 [deg]
double get_longitude() const; //!< Get RX position Longitude WGS84 [deg] double get_longitude() const; //!< Get RX position Longitude WGS84 [deg]
double get_height() const; //!< Get RX position height WGS84 [m] double get_height() const; //!< Get RX position height WGS84 [m]
double get_avg_latitude() const; //!< Get RX position averaged Latitude WGS84 [deg] double get_avg_latitude() const; //!< Get RX position averaged Latitude WGS84 [deg]
double get_avg_longitude() const; //!< Get RX position averaged Longitude WGS84 [deg] double get_avg_longitude() const; //!< Get RX position averaged Longitude WGS84 [deg]
double get_avg_height() const; //!< Get RX position averaged height WGS84 [m] double get_avg_height() const; //!< Get RX position averaged height WGS84 [m]
void set_rx_pos(const arma::vec & pos); //!< Set position: Latitude [deg], longitude [deg], height [m] void set_rx_pos(const arma::vec &pos); //!< Set position: Latitude [deg], longitude [deg], height [m]
arma::vec get_rx_pos() const; arma::vec get_rx_pos() const;
bool is_valid_position() const; bool is_valid_position() const;
void set_valid_position(bool is_valid); void set_valid_position(bool is_valid);
boost::posix_time::ptime get_position_UTC_time() const; boost::posix_time::ptime get_position_UTC_time() const;
void set_position_UTC_time(const boost::posix_time::ptime & pt); void set_position_UTC_time(const boost::posix_time::ptime &pt);
int get_num_valid_observations() const; //!< Get the number of valid pseudorange observations (valid satellites) int get_num_valid_observations() const; //!< Get the number of valid pseudorange observations (valid satellites)
void set_num_valid_observations(int num); //!< Set the number of valid pseudorange observations (valid satellites) void set_num_valid_observations(int num); //!< Set the number of valid pseudorange observations (valid satellites)
@ -112,27 +112,27 @@ public:
bool set_visible_satellites_ID(size_t index, unsigned int prn); //!< Set the ID of the visible satellite index channel bool set_visible_satellites_ID(size_t index, unsigned int prn); //!< Set the ID of the visible satellite index channel
unsigned int get_visible_satellites_ID(size_t index) const; //!< Get the ID of the visible satellite index channel unsigned int get_visible_satellites_ID(size_t index) const; //!< Get the ID of the visible satellite index channel
bool set_visible_satellites_El(size_t index, double el); //!< Set the LOS Elevation, in degrees, of the visible satellite index channel bool set_visible_satellites_El(size_t index, double el); //!< Set the LOS Elevation, in degrees, of the visible satellite index channel
double get_visible_satellites_El(size_t index) const; //!< Get the LOS Elevation, in degrees, of the visible satellite index channel double get_visible_satellites_El(size_t index) const; //!< Get the LOS Elevation, in degrees, of the visible satellite index channel
bool set_visible_satellites_Az(size_t index, double az); //!< Set the LOS Azimuth, in degrees, of the visible satellite index channel bool set_visible_satellites_Az(size_t index, double az); //!< Set the LOS Azimuth, in degrees, of the visible satellite index channel
double get_visible_satellites_Az(size_t index) const; //!< Get the LOS Azimuth, in degrees, of the visible satellite index channel double get_visible_satellites_Az(size_t index) const; //!< Get the LOS Azimuth, in degrees, of the visible satellite index channel
bool set_visible_satellites_Distance(size_t index, double dist); //!< Set the LOS Distance of the visible satellite index channel bool set_visible_satellites_Distance(size_t index, double dist); //!< Set the LOS Distance of the visible satellite index channel
double get_visible_satellites_Distance(size_t index) const; //!< Get the LOS Distance of the visible satellite index channel double get_visible_satellites_Distance(size_t index) const; //!< Get the LOS Distance of the visible satellite index channel
bool set_visible_satellites_CN0_dB(size_t index, double cn0); //!< Set the CN0 in dB of the visible satellite index channel bool set_visible_satellites_CN0_dB(size_t index, double cn0); //!< Set the CN0 in dB of the visible satellite index channel
double get_visible_satellites_CN0_dB(size_t index) const; //!< Get the CN0 in dB of the visible satellite index channel double get_visible_satellites_CN0_dB(size_t index) const; //!< Get the CN0 in dB of the visible satellite index channel
//averaging //averaging
void perform_pos_averaging(); void perform_pos_averaging();
void set_averaging_depth(int depth); //!< Set length of averaging window void set_averaging_depth(int depth); //!< Set length of averaging window
bool is_averaging() const; bool is_averaging() const;
void set_averaging_flag(bool flag); void set_averaging_flag(bool flag);
// DOP estimations // DOP estimations
void set_Q(const arma::mat & Q); void set_Q(const arma::mat &Q);
int compute_DOP(); //!< Compute Dilution Of Precision parameters int compute_DOP(); //!< Compute Dilution Of Precision parameters
double get_GDOP() const; double get_GDOP() const;
double get_PDOP() const; double get_PDOP() const;
@ -140,7 +140,7 @@ public:
double get_VDOP() const; double get_VDOP() const;
double get_TDOP() const; double get_TDOP() const;
arma::vec rotateSatellite(double traveltime, const arma::vec & X_sat); arma::vec rotateSatellite(double traveltime, const arma::vec &X_sat);
/*! /*!
* \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical * \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical
@ -157,9 +157,9 @@ public:
* 4 - World Geodetic System 1984. * 4 - World Geodetic System 1984.
* *
*/ */
int cart2geo(double X, double Y, double Z, int elipsoid_selection); int cart2geo(double X, double Y, double Z, int elipsoid_selection);
/*! /*!
* \brief Transformation of vector dx into topocentric coordinate system with origin at x * \brief Transformation of vector dx into topocentric coordinate system with origin at x
* *
* \param[in] x Vector origin coordinates (in ECEF system [X; Y; Z;]) * \param[in] x Vector origin coordinates (in ECEF system [X; Y; Z;])
@ -171,9 +171,9 @@ public:
* *
* Based on a Matlab function by Kai Borre * Based on a Matlab function by Kai Borre
*/ */
int topocent(double *Az, double *El, double *D, const arma::vec & x, const arma::vec & dx); int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx);
/*! /*!
* \brief Subroutine to calculate geodetic coordinates latitude, longitude, * \brief Subroutine to calculate geodetic coordinates latitude, longitude,
* height given Cartesian coordinates X,Y,Z, and reference ellipsoid * height given Cartesian coordinates X,Y,Z, and reference ellipsoid
* values semi-major axis (a) and the inverse of flattening (finv). * values semi-major axis (a) and the inverse of flattening (finv).
@ -192,9 +192,9 @@ public:
* *
* Based in a Matlab function by Kai Borre * Based in a Matlab function by Kai Borre
*/ */
int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z); int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
/*! /*!
* \brief Tropospheric correction * \brief Tropospheric correction
* *
* \param[in] sinel - sin of elevation angle of satellite * \param[in] sinel - sin of elevation angle of satellite
@ -217,7 +217,7 @@ public:
* *
* Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre * Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre
*/ */
int tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km); int tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km);
}; };
#endif #endif

1535
src/algorithms/PVT/libs/rinex_printer.cc
View File

File diff suppressed because it is too large Load Diff

277
src/algorithms/PVT/libs/rinex_printer.h
View File

@ -85,122 +85,122 @@ public:
*/ */
~Rinex_Printer(); ~Rinex_Printer();
std::fstream obsFile ; //<! Output file stream for RINEX observation file std::fstream obsFile; //<! Output file stream for RINEX observation file
std::fstream navFile ; //<! Output file stream for RINEX navigation data file std::fstream navFile; //<! Output file stream for RINEX navigation data file
std::fstream sbsFile ; //<! Output file stream for RINEX SBAS raw data file std::fstream sbsFile; //<! Output file stream for RINEX SBAS raw data file
std::fstream navGalFile ; //<! Output file stream for RINEX Galileo navigation data file std::fstream navGalFile; //<! Output file stream for RINEX Galileo navigation data file
std::fstream navGloFile ; //<! Output file stream for RINEX GLONASS navigation data file std::fstream navGloFile; //<! Output file stream for RINEX GLONASS navigation data file
std::fstream navMixFile ; //<! Output file stream for RINEX Mixed navigation data file std::fstream navMixFile; //<! Output file stream for RINEX Mixed navigation data file
/*! /*!
* \brief Generates the GPS L1 C/A Navigation Data header * \brief Generates the GPS L1 C/A Navigation Data header
*/ */
void rinex_nav_header(std::fstream & out, const Gps_Iono & iono, const Gps_Utc_Model & utc_model); void rinex_nav_header(std::fstream& out, const Gps_Iono& iono, const Gps_Utc_Model& utc_model);
/*! /*!
* \brief Generates the GPS L2C(M) Navigation Data header * \brief Generates the GPS L2C(M) Navigation Data header
*/ */
void rinex_nav_header(std::fstream & out, const Gps_CNAV_Iono & iono, const Gps_CNAV_Utc_Model & utc_model); void rinex_nav_header(std::fstream& out, const Gps_CNAV_Iono& iono, const Gps_CNAV_Utc_Model& utc_model);
/*! /*!
* \brief Generates the Galileo Navigation Data header * \brief Generates the Galileo Navigation Data header
*/ */
void rinex_nav_header(std::fstream & out, const Galileo_Iono & iono, const Galileo_Utc_Model & utc_model, const Galileo_Almanac & galileo_almanac); void rinex_nav_header(std::fstream& out, const Galileo_Iono& iono, const Galileo_Utc_Model& utc_model, const Galileo_Almanac& galileo_almanac);
/*! /*!
* \brief Generates the Mixed (GPS/Galileo) Navigation Data header * \brief Generates the Mixed (GPS/Galileo) Navigation Data header
*/ */
void rinex_nav_header(std::fstream & out, const Gps_Iono & gps_iono, const Gps_Utc_Model & gps_utc_model, const Galileo_Iono & galileo_iono, const Galileo_Utc_Model & galileo_utc_model, const Galileo_Almanac & galileo_almanac); void rinex_nav_header(std::fstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac);
/*! /*!
* \brief Generates the GLONASS L1, L2 C/A Navigation Data header * \brief Generates the GLONASS L1, L2 C/A Navigation Data header
*/ */
void rinex_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & utc_model, const Glonass_Gnav_Ephemeris & glonass_gnav_eph); void rinex_nav_header(std::fstream& out, const Glonass_Gnav_Utc_Model& utc_model, const Glonass_Gnav_Ephemeris& glonass_gnav_eph);
/*! /*!
* \brief Generates the Mixed (Galileo/GLONASS) Navigation Data header * \brief Generates the Mixed (Galileo/GLONASS) Navigation Data header
*/ */
void rinex_nav_header(std::fstream & out, const Galileo_Iono & galileo_iono, const Galileo_Utc_Model & galileo_utc_model, const Galileo_Almanac & galileo_almanac, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); void rinex_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac);
/*! /*!
* \brief Generates the Mixed (GPS L1 C/A/GLONASS L1, L2) Navigation Data header * \brief Generates the Mixed (GPS L1 C/A/GLONASS L1, L2) Navigation Data header
*/ */
void rinex_nav_header(std::fstream & out, const Gps_Iono & gps_iono, const Gps_Utc_Model & gps_utc_model, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); void rinex_nav_header(std::fstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac);
/*! /*!
* \brief Generates the Mixed (GPS L2C C/A/GLONASS L1, L2) Navigation Data header * \brief Generates the Mixed (GPS L2C C/A/GLONASS L1, L2) Navigation Data header
*/ */
void rinex_nav_header(std::fstream & out, const Gps_CNAV_Iono & gps_iono, const Gps_CNAV_Utc_Model & gps_utc_model, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); void rinex_nav_header(std::fstream& out, const Gps_CNAV_Iono& gps_iono, const Gps_CNAV_Utc_Model& gps_utc_model, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac);
/*! /*!
* \brief Generates the GPS Observation data header * \brief Generates the GPS Observation data header
*/ */
void rinex_obs_header(std::fstream & out, const Gps_Ephemeris & eph, const double d_TOW_first_observation); void rinex_obs_header(std::fstream& out, const Gps_Ephemeris& eph, const double d_TOW_first_observation);
/*! /*!
* \brief Generates the GPS L2 Observation data header * \brief Generates the GPS L2 Observation data header
*/ */
void rinex_obs_header(std::fstream & out, const Gps_CNAV_Ephemeris & eph, const double d_TOW_first_observation); void rinex_obs_header(std::fstream& out, const Gps_CNAV_Ephemeris& eph, const double d_TOW_first_observation);
/*! /*!
* \brief Generates the dual frequency GPS L1 & L2 Observation data header * \brief Generates the dual frequency GPS L1 & L2 Observation data header
*/ */
void rinex_obs_header(std::fstream & out, const Gps_Ephemeris & eph, const Gps_CNAV_Ephemeris & eph_cnav, const double d_TOW_first_observation); void rinex_obs_header(std::fstream& out, const Gps_Ephemeris& eph, const Gps_CNAV_Ephemeris& eph_cnav, const double d_TOW_first_observation);
/*! /*!
* \brief Generates the Galileo Observation data header. Example: bands("1B"), bands("1B 5X"), bands("5X"), ... Default: "1B". * \brief Generates the Galileo Observation data header. Example: bands("1B"), bands("1B 5X"), bands("5X"), ... Default: "1B".
*/ */
void rinex_obs_header(std::fstream & out, const Galileo_Ephemeris & eph, const double d_TOW_first_observation, const std::string bands = "1B"); void rinex_obs_header(std::fstream& out, const Galileo_Ephemeris& eph, const double d_TOW_first_observation, const std::string bands = "1B");
/*! /*!
* \brief Generates the Mixed (GPS/Galileo) Observation data header. Example: galileo_bands("1B"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B". * \brief Generates the Mixed (GPS/Galileo) Observation data header. Example: galileo_bands("1B"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B".
*/ */
void rinex_obs_header(std::fstream & out, const Gps_Ephemeris & gps_eph, const Galileo_Ephemeris & galileo_eph, const double d_TOW_first_observation, const std::string galileo_bands = "1B"); void rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps_eph, const Galileo_Ephemeris& galileo_eph, const double d_TOW_first_observation, const std::string galileo_bands = "1B");
/*! /*!
* \brief Generates the GLONASS GNAV Observation data header. Example: bands("1C"), bands("1C 2C"), bands("2C"), ... Default: "1C". * \brief Generates the GLONASS GNAV Observation data header. Example: bands("1C"), bands("1C 2C"), bands("2C"), ... Default: "1C".
*/ */
void rinex_obs_header(std::fstream & out, const Glonass_Gnav_Ephemeris & eph, const double d_TOW_first_observation, const std::string bands = "1G"); void rinex_obs_header(std::fstream& out, const Glonass_Gnav_Ephemeris& eph, const double d_TOW_first_observation, const std::string bands = "1G");
/*! /*!
* \brief Generates the Mixed (GPS L1 C/A /GLONASS) Observation data header. Example: galileo_bands("1C"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B". * \brief Generates the Mixed (GPS L1 C/A /GLONASS) Observation data header. Example: galileo_bands("1C"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B".
*/ */
void rinex_obs_header(std::fstream & out, const Gps_Ephemeris & gps_eph, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const double d_TOW_first_observation, const std::string glo_bands = "1C"); void rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps_eph, const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const double d_TOW_first_observation, const std::string glo_bands = "1C");
/*! /*!
* \brief Generates the Mixed (Galileo/GLONASS) Observation data header. Example: galileo_bands("1C"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B". * \brief Generates the Mixed (Galileo/GLONASS) Observation data header. Example: galileo_bands("1C"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B".
*/ */
void rinex_obs_header(std::fstream & out, const Galileo_Ephemeris & galileo_eph, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const double d_TOW_first_observation, const std::string galileo_bands = "1B", const std::string glo_bands = "1C"); void rinex_obs_header(std::fstream& out, const Galileo_Ephemeris& galileo_eph, const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const double d_TOW_first_observation, const std::string galileo_bands = "1B", const std::string glo_bands = "1C");
/*! /*!
* \brief Generates the Mixed (GPS L2C/GLONASS) Observation data header. Example: galileo_bands("1G")... Default: "1G". * \brief Generates the Mixed (GPS L2C/GLONASS) Observation data header. Example: galileo_bands("1G")... Default: "1G".
*/ */
void rinex_obs_header(std::fstream & out, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const double d_TOW_first_observation, const std::string glo_bands = "1G"); void rinex_obs_header(std::fstream& out, const Gps_CNAV_Ephemeris& gps_cnav_eph, const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const double d_TOW_first_observation, const std::string glo_bands = "1G");
/*! /*!
* \brief Generates the SBAS raw data header * \brief Generates the SBAS raw data header
*/ */
void rinex_sbs_header(std::fstream & out); void rinex_sbs_header(std::fstream& out);
/*! /*!
* \brief Computes the UTC time and returns a boost::posix_time::ptime object * \brief Computes the UTC time and returns a boost::posix_time::ptime object
*/ */
boost::posix_time::ptime compute_UTC_time(const Gps_Navigation_Message & nav_msg); boost::posix_time::ptime compute_UTC_time(const Gps_Navigation_Message& nav_msg);
/*! /*!
* \brief Computes the GPS time and returns a boost::posix_time::ptime object * \brief Computes the GPS time and returns a boost::posix_time::ptime object
*/ */
boost::posix_time::ptime compute_GPS_time(const Gps_Ephemeris & eph, const double obs_time); boost::posix_time::ptime compute_GPS_time(const Gps_Ephemeris& eph, const double obs_time);
/*! /*!
* \brief Computes the GPS time and returns a boost::posix_time::ptime object * \brief Computes the GPS time and returns a boost::posix_time::ptime object
*/ */
boost::posix_time::ptime compute_GPS_time(const Gps_CNAV_Ephemeris & eph, const double obs_time); boost::posix_time::ptime compute_GPS_time(const Gps_CNAV_Ephemeris& eph, const double obs_time);
/*! /*!
* \brief Computes the Galileo time and returns a boost::posix_time::ptime object * \brief Computes the Galileo time and returns a boost::posix_time::ptime object
*/ */
boost::posix_time::ptime compute_Galileo_time(const Galileo_Ephemeris & eph, const double obs_time); boost::posix_time::ptime compute_Galileo_time(const Galileo_Ephemeris& eph, const double obs_time);
/*! /*!
* \brief Computes the UTC Time and returns a boost::posix_time::ptime object * \brief Computes the UTC Time and returns a boost::posix_time::ptime object
@ -209,7 +209,7 @@ public:
* \param eph GLONASS GNAV Ephemeris object * \param eph GLONASS GNAV Ephemeris object
* \param obs_time Observation time in GPS seconds of week * \param obs_time Observation time in GPS seconds of week
*/ */
boost::posix_time::ptime compute_UTC_time(const Glonass_Gnav_Ephemeris & eph, const double obs_time); boost::posix_time::ptime compute_UTC_time(const Glonass_Gnav_Ephemeris& eph, const double obs_time);
/*! /*!
* \brief Computes number of leap seconds of GPS relative to UTC * \brief Computes number of leap seconds of GPS relative to UTC
@ -221,126 +221,126 @@ public:
/*! /*!
* \brief Writes data from the GPS L1 C/A navigation message into the RINEX file * \brief Writes data from the GPS L1 C/A navigation message into the RINEX file
*/ */
void log_rinex_nav(std::fstream & out, const std::map<int, Gps_Ephemeris> & eph_map); void log_rinex_nav(std::fstream& out, const std::map<int, Gps_Ephemeris>& eph_map);
/*! /*!
* \brief Writes data from the GPS L2 navigation message into the RINEX file * \brief Writes data from the GPS L2 navigation message into the RINEX file
*/ */
void log_rinex_nav(std::fstream & out, const std::map<int, Gps_CNAV_Ephemeris> & eph_map); void log_rinex_nav(std::fstream& out, const std::map<int, Gps_CNAV_Ephemeris>& eph_map);
/*! /*!
* \brief Writes data from the Galileo navigation message into the RINEX file * \brief Writes data from the Galileo navigation message into the RINEX file
*/ */
void log_rinex_nav(std::fstream & out, const std::map<int, Galileo_Ephemeris> & eph_map); void log_rinex_nav(std::fstream& out, const std::map<int, Galileo_Ephemeris>& eph_map);
/*! /*!
* \brief Writes data from the Mixed (GPS/Galileo) navigation message into the RINEX file * \brief Writes data from the Mixed (GPS/Galileo) navigation message into the RINEX file
*/ */
void log_rinex_nav(std::fstream & out, const std::map<int, Gps_Ephemeris> & gps_eph_map, const std::map<int, Galileo_Ephemeris> & galileo_eph_map); void log_rinex_nav(std::fstream& out, const std::map<int, Gps_Ephemeris>& gps_eph_map, const std::map<int, Galileo_Ephemeris>& galileo_eph_map);
/*! /*!
* \brief Writes data from the GLONASS GNAV navigation message into the RINEX file * \brief Writes data from the GLONASS GNAV navigation message into the RINEX file
*/ */
void log_rinex_nav(std::fstream & out, const std::map<int, Glonass_Gnav_Ephemeris> & eph_map); void log_rinex_nav(std::fstream& out, const std::map<int, Glonass_Gnav_Ephemeris>& eph_map);
/*! /*!
* \brief Writes data from the Mixed (GPS/GLONASS GNAV) navigation message into the RINEX file * \brief Writes data from the Mixed (GPS/GLONASS GNAV) navigation message into the RINEX file
*/ */
void log_rinex_nav(std::fstream & out, const std::map<int, Gps_Ephemeris> & gps_eph_map, const std::map<int, Glonass_Gnav_Ephemeris> & glonass_gnav_eph_map); void log_rinex_nav(std::fstream& out, const std::map<int, Gps_Ephemeris>& gps_eph_map, const std::map<int, Glonass_Gnav_Ephemeris>& glonass_gnav_eph_map);
/*! /*!
* \brief Writes data from the Mixed (GPS/GLONASS GNAV) navigation message into the RINEX file * \brief Writes data from the Mixed (GPS/GLONASS GNAV) navigation message into the RINEX file
*/ */
void log_rinex_nav(std::fstream & out, const std::map<int, Gps_CNAV_Ephemeris> & gps_cnav_eph_map, const std::map<int, Glonass_Gnav_Ephemeris> & glonass_gnav_eph_map); void log_rinex_nav(std::fstream& out, const std::map<int, Gps_CNAV_Ephemeris>& gps_cnav_eph_map, const std::map<int, Glonass_Gnav_Ephemeris>& glonass_gnav_eph_map);
/*! /*!
* \brief Writes data from the Mixed (Galileo/ GLONASS GNAV) navigation message into the RINEX file * \brief Writes data from the Mixed (Galileo/ GLONASS GNAV) navigation message into the RINEX file
*/ */
void log_rinex_nav(std::fstream & out, const std::map<int, Galileo_Ephemeris> & galileo_eph_map, const std::map<int, Glonass_Gnav_Ephemeris> & glonass_gnav_eph_map); void log_rinex_nav(std::fstream& out, const std::map<int, Galileo_Ephemeris>& galileo_eph_map, const std::map<int, Glonass_Gnav_Ephemeris>& glonass_gnav_eph_map);
/*! /*!
* \brief Writes GPS L1 observables into the RINEX file * \brief Writes GPS L1 observables into the RINEX file
*/ */
void log_rinex_obs(std::fstream & out, const Gps_Ephemeris & eph, double obs_time, const std::map<int, Gnss_Synchro> & observables); void log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, double obs_time, const std::map<int, Gnss_Synchro>& observables);
/*! /*!
* \brief Writes GPS L2 observables into the RINEX file * \brief Writes GPS L2 observables into the RINEX file
*/ */
void log_rinex_obs(std::fstream & out, const Gps_CNAV_Ephemeris & eph, double obs_time, const std::map<int, Gnss_Synchro> & observables); void log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& eph, double obs_time, const std::map<int, Gnss_Synchro>& observables);
/*! /*!
* \brief Writes dual frequency GPS L1 and L2 observables into the RINEX file * \brief Writes dual frequency GPS L1 and L2 observables into the RINEX file
*/ */
void log_rinex_obs(std::fstream & out, const Gps_Ephemeris & eph, const Gps_CNAV_Ephemeris & eph_cnav, double obs_time, const std::map<int, Gnss_Synchro> & observables); void log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, const Gps_CNAV_Ephemeris& eph_cnav, double obs_time, const std::map<int, Gnss_Synchro>& observables);
/*! /*!
* \brief Writes Galileo observables into the RINEX file. Example: galileo_bands("1B"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B". * \brief Writes Galileo observables into the RINEX file. Example: galileo_bands("1B"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B".
*/ */
void log_rinex_obs(std::fstream & out, const Galileo_Ephemeris & eph, double obs_time, const std::map<int, Gnss_Synchro> & observables, const std::string galileo_bands = "1B"); void log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& eph, double obs_time, const std::map<int, Gnss_Synchro>& observables, const std::string galileo_bands = "1B");
/*! /*!
* \brief Writes Mixed GPS / Galileo observables into the RINEX file * \brief Writes Mixed GPS / Galileo observables into the RINEX file
*/ */
void log_rinex_obs(std::fstream & out, const Gps_Ephemeris & gps_eph, const Galileo_Ephemeris & galileo_eph, const double gps_obs_time, const std::map<int, Gnss_Synchro> & observables); void log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_eph, const Galileo_Ephemeris& galileo_eph, const double gps_obs_time, const std::map<int, Gnss_Synchro>& observables);
/*! /*!
* \brief Writes GLONASS GNAV observables into the RINEX file. Example: glonass_bands("1C"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B". * \brief Writes GLONASS GNAV observables into the RINEX file. Example: glonass_bands("1C"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B".
*/ */
void log_rinex_obs(std::fstream & out, const Glonass_Gnav_Ephemeris & eph, double obs_time, const std::map<int, Gnss_Synchro> & observables, const std::string glonass_bands = "1C"); void log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeris& eph, double obs_time, const std::map<int, Gnss_Synchro>& observables, const std::string glonass_bands = "1C");
/*! /*!
* \brief Writes Mixed GPS L1 C/A - GLONASS observables into the RINEX file * \brief Writes Mixed GPS L1 C/A - GLONASS observables into the RINEX file
*/ */
void log_rinex_obs(std::fstream & out, const Gps_Ephemeris & gps_eph, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const double gps_obs_time, const std::map<int, Gnss_Synchro> & observables); void log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_eph, const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const double gps_obs_time, const std::map<int, Gnss_Synchro>& observables);
/*! /*!
* \brief Writes Mixed GPS L2C - GLONASS observables into the RINEX file * \brief Writes Mixed GPS L2C - GLONASS observables into the RINEX file
*/ */
void log_rinex_obs(std::fstream & out, const Gps_CNAV_Ephemeris & gps_cnav_eph, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const double gps_obs_time, const std::map<int, Gnss_Synchro> & observables); void log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& gps_cnav_eph, const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const double gps_obs_time, const std::map<int, Gnss_Synchro>& observables);
/*! /*!
* \brief Writes Mixed Galileo/GLONASS observables into the RINEX file * \brief Writes Mixed Galileo/GLONASS observables into the RINEX file
*/ */
void log_rinex_obs(std::fstream & out, const Galileo_Ephemeris & galileo_eph, const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const double gps_obs_time, const std::map<int, Gnss_Synchro> & observables); void log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& galileo_eph, const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const double gps_obs_time, const std::map<int, Gnss_Synchro>& observables);
/*! /*!
* \brief Represents GPS time in the date time format. Leap years are considered, but leap seconds are not. * \brief Represents GPS time in the date time format. Leap years are considered, but leap seconds are not.
*/ */
void to_date_time(int gps_week, int gps_tow, int & year, int & month, int & day, int & hour, int & minute, int & second); void to_date_time(int gps_week, int gps_tow, int& year, int& month, int& day, int& hour, int& minute, int& second);
/*! /*!
* \brief Writes raw SBAS messages into the RINEX file * \brief Writes raw SBAS messages into the RINEX file
*/ */
//void log_rinex_sbs(std::fstream & out, const Sbas_Raw_Msg & sbs_message); //void log_rinex_sbs(std::fstream & out, const Sbas_Raw_Msg & sbs_message);
void update_nav_header(std::fstream & out, const Gps_Utc_Model & gps_utc, const Gps_Iono & gps_iono); void update_nav_header(std::fstream& out, const Gps_Utc_Model& gps_utc, const Gps_Iono& gps_iono);
void update_nav_header(std::fstream & out, const Gps_CNAV_Utc_Model & utc_model, const Gps_CNAV_Iono & iono); void update_nav_header(std::fstream& out, const Gps_CNAV_Utc_Model& utc_model, const Gps_CNAV_Iono& iono);
void update_nav_header(std::fstream & out, const Gps_Iono & gps_iono, const Gps_Utc_Model & gps_utc_model, const Galileo_Iono & galileo_iono, const Galileo_Utc_Model & galileo_utc_model, const Galileo_Almanac& galileo_almanac); void update_nav_header(std::fstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac);
void update_nav_header(std::fstream & out, const Galileo_Iono & galileo_iono, const Galileo_Utc_Model & utc_model, const Galileo_Almanac & galileo_almanac); void update_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& utc_model, const Galileo_Almanac& galileo_almanac);
void update_nav_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); void update_nav_header(std::fstream& out, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac);
void update_nav_header(std::fstream & out, const Gps_Iono & gps_iono, const Gps_Utc_Model & gps_utc, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); void update_nav_header(std::fstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac);
void update_nav_header(std::fstream & out, const Gps_CNAV_Iono & gps_cnav_iono, const Gps_CNAV_Utc_Model & gps_cnav_utc, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); void update_nav_header(std::fstream& out, const Gps_CNAV_Iono& gps_cnav_iono, const Gps_CNAV_Utc_Model& gps_cnav_utc, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac);
void update_nav_header(std::fstream & out, const Galileo_Iono & galileo_iono, const Galileo_Utc_Model & galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model, const Glonass_Gnav_Almanac & glonass_gnav_almanac); void update_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac);
void update_obs_header(std::fstream & out, const Gps_Utc_Model & utc_model); void update_obs_header(std::fstream& out, const Gps_Utc_Model& utc_model);
void update_obs_header(std::fstream & out, const Gps_CNAV_Utc_Model & utc_model); void update_obs_header(std::fstream& out, const Gps_CNAV_Utc_Model& utc_model);
void update_obs_header(std::fstream & out, const Galileo_Utc_Model & galileo_utc_model); void update_obs_header(std::fstream& out, const Galileo_Utc_Model& galileo_utc_model);
void update_obs_header(std::fstream & out, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model); void update_obs_header(std::fstream& out, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model);
std::map<std::string,std::string> satelliteSystem; //<! GPS, GLONASS, SBAS payload, Galileo or Compass std::map<std::string, std::string> satelliteSystem; //<! GPS, GLONASS, SBAS payload, Galileo or Compass
std::map<std::string,std::string> observationType; //<! PSEUDORANGE, CARRIER_PHASE, DOPPLER, SIGNAL_STRENGTH std::map<std::string, std::string> observationType; //<! PSEUDORANGE, CARRIER_PHASE, DOPPLER, SIGNAL_STRENGTH
std::map<std::string,std::string> observationCode; //<! GNSS observation descriptors std::map<std::string, std::string> observationCode; //<! GNSS observation descriptors
std::string stringVersion; //<! RINEX version (2.10/2.11 or 3.01/3.02) std::string stringVersion; //<! RINEX version (2.10/2.11 or 3.01/3.02)
std::string navfilename; std::string navfilename;
std::string obsfilename; std::string obsfilename;
@ -350,8 +350,8 @@ public:
std::string navMixfilename; std::string navMixfilename;
private: private:
int version ; // RINEX version (2 for 2.10/2.11 and 3 for 3.01) int version; // RINEX version (2 for 2.10/2.11 and 3 for 3.01)
int numberTypesObservations; // Number of available types of observable in the system. Should be public? int numberTypesObservations; // Number of available types of observable in the system. Should be public?
/* /*
* Generation of RINEX signal strength indicators * Generation of RINEX signal strength indicators
*/ */
@ -383,7 +383,7 @@ private:
/* /*
* Checks that the line is 80 characters length * Checks that the line is 80 characters length
*/ */
void lengthCheck(const std::string & line); void lengthCheck(const std::string& line);
double fake_cnav_iode; double fake_cnav_iode;
@ -400,9 +400,9 @@ private:
* \param[in] length new desired length of string. * \param[in] length new desired length of string.
* \param[in] pad character to pad string with (blank by default). * \param[in] pad character to pad string with (blank by default).
* \return a reference to \a s. */ * \return a reference to \a s. */
inline std::string & leftJustify(std::string & s, inline std::string& leftJustify(std::string& s,
const std::string::size_type length, const std::string::size_type length,
const char pad = ' '); const char pad = ' ');
/* /*
* If the string is bigger than length, truncate it from the right. * If the string is bigger than length, truncate it from the right.
@ -417,11 +417,12 @@ private:
* \param[in] length new desired length of string. * \param[in] length new desired length of string.
* \param[in] pad character to pad string with (blank by default). * \param[in] pad character to pad string with (blank by default).
* \return a reference to \a s. */ * \return a reference to \a s. */
inline std::string leftJustify(const std::string & s, inline std::string leftJustify(const std::string& s,
const std::string::size_type length, const std::string::size_type length,
const char pad = ' ') const char pad = ' ')
{ {
std::string t(s); return leftJustify(t, length, pad); std::string t(s);
return leftJustify(t, length, pad);
} }
@ -431,9 +432,9 @@ private:
* requested length (\a length), it is padded on the left with * requested length (\a length), it is padded on the left with
* the pad character (\a pad). The default pad * the pad character (\a pad). The default pad
* character is a blank. */ * character is a blank. */
inline std::string & rightJustify(std::string & s, inline std::string& rightJustify(std::string& s,
const std::string::size_type length, const std::string::size_type length,
const char pad = ' '); const char pad = ' ');
/* /*
* Right-justifies the receiver in a string of the specified * Right-justifies the receiver in a string of the specified
@ -441,11 +442,12 @@ private:
* requested length (\a length), it is padded on the left with * requested length (\a length), it is padded on the left with
* the pad character (\a pad). The default pad * the pad character (\a pad). The default pad
* character is a blank.*/ * character is a blank.*/
inline std::string rightJustify(const std::string & s, inline std::string rightJustify(const std::string& s,
const std::string::size_type length, const std::string::size_type length,
const char pad = ' ') const char pad = ' ')
{ {
std::string t(s); return rightJustify(t, length, pad); std::string t(s);
return rightJustify(t, length, pad);
} }
@ -459,11 +461,11 @@ private:
* exponentials above three characters in length. If false, it removes * exponentials above three characters in length. If false, it removes
* that check. * that check.
*/ */
inline std::string doub2sci(const double & d, inline std::string doub2sci(const double& d,
const std::string::size_type length, const std::string::size_type length,
const std::string::size_type expLen, const std::string::size_type expLen,
const bool showSign = true, const bool showSign = true,
const bool checkSwitch = true); const bool checkSwitch = true);
/* /*
@ -480,11 +482,11 @@ private:
* produce an exponential with an E instead of a D, and always have a leading * produce an exponential with an E instead of a D, and always have a leading
* zero. For example -> 0.87654E-0004 or -0.1234E00005. * zero. For example -> 0.87654E-0004 or -0.1234E00005.
*/ */
inline std::string & sci2for(std::string & aStr, inline std::string& sci2for(std::string& aStr,
const std::string::size_type startPos = 0, const std::string::size_type startPos = 0,
const std::string::size_type length = std::string::npos, const std::string::size_type length = std::string::npos,
const std::string::size_type expLen = 3, const std::string::size_type expLen = 3,
const bool checkSwitch = true); const bool checkSwitch = true);
/* /*
@ -499,10 +501,10 @@ private:
* that check. * that check.
* @return a string containing \a d in FORTRAN notation. * @return a string containing \a d in FORTRAN notation.
*/ */
inline std::string doub2for(const double & d, inline std::string doub2for(const double& d,
const std::string::size_type length, const std::string::size_type length,
const std::string::size_type expLen, const std::string::size_type expLen,
const bool checkSwitch = true); const bool checkSwitch = true);
/* /*
@ -510,7 +512,7 @@ private:
* @param s string containing a number. * @param s string containing a number.
* @return double representation of string. * @return double representation of string.
*/ */
inline double asDouble(const std::string & s) inline double asDouble(const std::string& s)
{ {
return strtod(s.c_str(), 0); return strtod(s.c_str(), 0);
} }
@ -523,7 +525,7 @@ private:
* @param s string containing a number. * @param s string containing a number.
* @return long integer representation of string. * @return long integer representation of string.
*/ */
inline long asInt(const std::string & s) inline long asInt(const std::string& s)
{ {
return strtol(s.c_str(), 0, 10); return strtol(s.c_str(), 0, 10);
} }
@ -536,7 +538,7 @@ private:
* @return string representation of \a x. * @return string representation of \a x.
*/ */
inline std::string asString(const double x, inline std::string asString(const double x,
const std::string::size_type precision = 17); const std::string::size_type precision = 17);
/* /*
@ -546,7 +548,7 @@ private:
* @return string representation of \a x. * @return string representation of \a x.
*/ */
inline std::string asString(const long double x, inline std::string asString(const long double x,
const std::string::size_type precision = 21); const std::string::size_type precision = 21);
/* /*
@ -555,26 +557,26 @@ private:
* @param x object to turn into a string. * @param x object to turn into a string.
* @return string representation of \a x. * @return string representation of \a x.
*/ */
template <class X> inline std::string asString(const X x); template <class X>
inline std::string asString(const X x);
inline std::string asFixWidthString(const int x, const int width, char fill_digit); inline std::string asFixWidthString(const int x, const int width, char fill_digit);
}; };
// Implementation of inline functions (modified versions from GPSTk http://www.gpstk.org) // Implementation of inline functions (modified versions from GPSTk http://www.gpstk.org)
inline std::string & Rinex_Printer::leftJustify(std::string & s, inline std::string& Rinex_Printer::leftJustify(std::string& s,
const std::string::size_type length, const std::string::size_type length,
const char pad) const char pad)
{ {
if(length < s.length()) if (length < s.length())
{ {
s = s.substr(0, length); s = s.substr(0, length);
} }
else else
{ {
s.append(length-s.length(), pad); s.append(length - s.length(), pad);
} }
return s; return s;
} }
@ -582,13 +584,13 @@ inline std::string & Rinex_Printer::leftJustify(std::string & s,
// if the string is bigger than length, truncate it from the left. // if the string is bigger than length, truncate it from the left.
// otherwise, add pad characters to its left. // otherwise, add pad characters to its left.
inline std::string & Rinex_Printer::rightJustify(std::string & s, inline std::string& Rinex_Printer::rightJustify(std::string& s,
const std::string::size_type length, const std::string::size_type length,
const char pad) const char pad)
{ {
if(length < s.length()) if (length < s.length())
{ {
s = s.substr(s.length()-length, std::string::npos); s = s.substr(s.length() - length, std::string::npos);
} }
else else
{ {
@ -598,11 +600,10 @@ inline std::string & Rinex_Printer::rightJustify(std::string & s,
} }
inline std::string Rinex_Printer::doub2for(const double& d,
inline std::string Rinex_Printer::doub2for(const double & d, const std::string::size_type length,
const std::string::size_type length, const std::string::size_type expLen,
const std::string::size_type expLen, const bool checkSwitch)
const bool checkSwitch)
{ {
short exponentLength = expLen; short exponentLength = expLen;
@ -617,11 +618,11 @@ inline std::string Rinex_Printer::doub2for(const double & d,
} }
inline std::string Rinex_Printer::doub2sci(const double & d, inline std::string Rinex_Printer::doub2sci(const double& d,
const std::string::size_type length, const std::string::size_type length,
const std::string::size_type expLen, const std::string::size_type expLen,
const bool showSign, const bool showSign,
const bool checkSwitch) const bool checkSwitch)
{ {
std::string toReturn; std::string toReturn;
short exponentLength = expLen; short exponentLength = expLen;
@ -648,11 +649,11 @@ inline std::string Rinex_Printer::doub2sci(const double & d,
} }
inline std::string & Rinex_Printer::sci2for(std::string & aStr, inline std::string& Rinex_Printer::sci2for(std::string& aStr,
const std::string::size_type startPos, const std::string::size_type startPos,
const std::string::size_type length, const std::string::size_type length,
const std::string::size_type expLen, const std::string::size_type expLen,
const bool checkSwitch) const bool checkSwitch)
{ {
std::string::size_type idx = aStr.find('.', startPos); std::string::size_type idx = aStr.find('.', startPos);
int expAdd = 0; int expAdd = 0;
@ -660,7 +661,7 @@ inline std::string & Rinex_Printer::sci2for(std::string & aStr,
long iexp; long iexp;
//If checkSwitch is false, always redo the exponential. Otherwise, //If checkSwitch is false, always redo the exponential. Otherwise,
//set it to false. //set it to false.
bool redoexp =! checkSwitch; bool redoexp = !checkSwitch;
// Check for decimal place within specified boundaries // Check for decimal place within specified boundaries
if ((idx <= 0) || (idx >= (startPos + length - expLen - 1))) if ((idx <= 0) || (idx >= (startPos + length - expLen - 1)))
@ -712,11 +713,11 @@ inline std::string & Rinex_Printer::sci2for(std::string & aStr,
if (iexp < 0) if (iexp < 0)
{ {
aStr += "-"; aStr += "-";
iexp -= iexp*2; iexp -= iexp * 2;
} }
else else
aStr += "+"; aStr += "+";
aStr += Rinex_Printer::rightJustify(asString(iexp),expLen,'0'); aStr += Rinex_Printer::rightJustify(asString(iexp), expLen, '0');
} }
// if the number is positive, append a space // if the number is positive, append a space
@ -736,11 +737,10 @@ inline std::string & Rinex_Printer::sci2for(std::string & aStr,
} // end sci2for } // end sci2for
inline std::string asString(const long double x, const std::string::size_type precision) inline std::string asString(const long double x, const std::string::size_type precision)
{ {
std::ostringstream ss; std::ostringstream ss;
ss << std::fixed << std::setprecision(precision) << x ; ss << std::fixed << std::setprecision(precision) << x;
return ss.str(); return ss.str();
} }
@ -761,7 +761,7 @@ inline std::string Rinex_Printer::asFixWidthString(const int x, const int width,
} }
inline long asInt(const std::string & s) inline long asInt(const std::string& s)
{ {
return strtol(s.c_str(), 0, 10); return strtol(s.c_str(), 0, 10);
} }
@ -771,16 +771,17 @@ inline int Rinex_Printer::toInt(std::string bitString, int sLength)
{ {
int tempInt; int tempInt;
int num = 0; int num = 0;
for(int i = 0; i < sLength; i++) for (int i = 0; i < sLength; i++)
{ {
tempInt = bitString[i]-'0'; tempInt = bitString[i] - '0';
num |= (1 << (sLength - 1 - i)) * tempInt; num |= (1 << (sLength - 1 - i)) * tempInt;
} }
return num; return num;
} }
template<class X> inline std::string Rinex_Printer::asString(const X x) template <class X>
inline std::string Rinex_Printer::asString(const X x)
{ {
std::ostringstream ss; std::ostringstream ss;
ss << x; ss << x;

120
src/algorithms/PVT/libs/rtcm_printer.cc
View File

@ -53,37 +53,37 @@ Rtcm_Printer::Rtcm_Printer(std::string filename, bool flag_rtcm_server, bool fla
const int year = timeinfo.tm_year - 100; const int year = timeinfo.tm_year - 100;
strm0 << year; strm0 << year;
const int month = timeinfo.tm_mon + 1; const int month = timeinfo.tm_mon + 1;
if(month < 10) if (month < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << month; strm0 << month;
const int day = timeinfo.tm_mday; const int day = timeinfo.tm_mday;
if(day < 10) if (day < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << day << "_"; strm0 << day << "_";
const int hour = timeinfo.tm_hour; const int hour = timeinfo.tm_hour;
if(hour < 10) if (hour < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << hour; strm0 << hour;
const int min = timeinfo.tm_min; const int min = timeinfo.tm_min;
if(min < 10) if (min < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << min; strm0 << min;
const int sec = timeinfo.tm_sec; const int sec = timeinfo.tm_sec;
if(sec < 10) if (sec < 10)
{ {
strm0 << "0"; strm0 << "0";
} }
strm0 << sec; strm0 << sec;
rtcm_filename = filename + "_" + strm0.str() + ".rtcm"; rtcm_filename = filename + "_" + strm0.str() + ".rtcm";
} }
else else
{ {
@ -115,7 +115,7 @@ Rtcm_Printer::Rtcm_Printer(std::string filename, bool flag_rtcm_server, bool fla
rtcm = std::make_shared<Rtcm>(port); rtcm = std::make_shared<Rtcm>(port);
if(flag_rtcm_server) if (flag_rtcm_server)
{ {
rtcm->run_server(); rtcm->run_server();
} }
@ -124,20 +124,20 @@ Rtcm_Printer::Rtcm_Printer(std::string filename, bool flag_rtcm_server, bool fla
Rtcm_Printer::~Rtcm_Printer() Rtcm_Printer::~Rtcm_Printer()
{ {
if(rtcm->is_server_running()) if (rtcm->is_server_running())
{ {
try try
{ {
rtcm->stop_server(); rtcm->stop_server();
} }
catch(const boost::exception & e) catch (const boost::exception& e)
{ {
LOG(WARNING) << "Boost exception: " << boost::diagnostic_information(e); LOG(WARNING) << "Boost exception: " << boost::diagnostic_information(e);
} }
catch(const std::exception & ex) catch (const std::exception& ex)
{ {
LOG(WARNING) << "STD exception: " << ex.what(); LOG(WARNING) << "STD exception: " << ex.what();
} }
} }
if (rtcm_file_descriptor.is_open()) if (rtcm_file_descriptor.is_open())
{ {
@ -146,14 +146,14 @@ Rtcm_Printer::~Rtcm_Printer()
rtcm_file_descriptor.close(); rtcm_file_descriptor.close();
if (pos == 0) if (pos == 0)
{ {
if(remove(rtcm_filename.c_str()) != 0) LOG(INFO) << "Error deleting temporary RTCM file"; if (remove(rtcm_filename.c_str()) != 0) LOG(INFO) << "Error deleting temporary RTCM file";
} }
} }
close_serial(); close_serial();
} }
bool Rtcm_Printer::Print_Rtcm_MT1001(const Gps_Ephemeris& gps_eph, double obs_time, const std::map<int, Gnss_Synchro> & observables) bool Rtcm_Printer::Print_Rtcm_MT1001(const Gps_Ephemeris& gps_eph, double obs_time, const std::map<int, Gnss_Synchro>& observables)
{ {
std::string m1001 = rtcm->print_MT1001(gps_eph, obs_time, observables, station_id); std::string m1001 = rtcm->print_MT1001(gps_eph, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1001); Rtcm_Printer::Print_Message(m1001);
@ -161,7 +161,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1001(const Gps_Ephemeris& gps_eph, double obs_ti
} }
bool Rtcm_Printer::Print_Rtcm_MT1002(const Gps_Ephemeris& gps_eph, double obs_time, const std::map<int, Gnss_Synchro> & observables) bool Rtcm_Printer::Print_Rtcm_MT1002(const Gps_Ephemeris& gps_eph, double obs_time, const std::map<int, Gnss_Synchro>& observables)
{ {
std::string m1002 = rtcm->print_MT1002(gps_eph, obs_time, observables, station_id); std::string m1002 = rtcm->print_MT1002(gps_eph, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1002); Rtcm_Printer::Print_Message(m1002);
@ -169,7 +169,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1002(const Gps_Ephemeris& gps_eph, double obs_ti
} }
bool Rtcm_Printer::Print_Rtcm_MT1003(const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& cnav_eph, double obs_time, const std::map<int, Gnss_Synchro> & observables) bool Rtcm_Printer::Print_Rtcm_MT1003(const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& cnav_eph, double obs_time, const std::map<int, Gnss_Synchro>& observables)
{ {
std::string m1003 = rtcm->print_MT1003(gps_eph, cnav_eph, obs_time, observables, station_id); std::string m1003 = rtcm->print_MT1003(gps_eph, cnav_eph, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1003); Rtcm_Printer::Print_Message(m1003);
@ -177,7 +177,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1003(const Gps_Ephemeris& gps_eph, const Gps_CNA
} }
bool Rtcm_Printer::Print_Rtcm_MT1004(const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& cnav_eph, double obs_time, const std::map<int, Gnss_Synchro> & observables) bool Rtcm_Printer::Print_Rtcm_MT1004(const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& cnav_eph, double obs_time, const std::map<int, Gnss_Synchro>& observables)
{ {
std::string m1003 = rtcm->print_MT1004(gps_eph, cnav_eph, obs_time, observables, station_id); std::string m1003 = rtcm->print_MT1004(gps_eph, cnav_eph, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1003); Rtcm_Printer::Print_Message(m1003);
@ -185,7 +185,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1004(const Gps_Ephemeris& gps_eph, const Gps_CNA
} }
bool Rtcm_Printer::Print_Rtcm_MT1009(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map<int, Gnss_Synchro> & observables) bool Rtcm_Printer::Print_Rtcm_MT1009(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map<int, Gnss_Synchro>& observables)
{ {
std::string m1009 = rtcm->print_MT1009(glonass_gnav_eph, obs_time, observables, station_id); std::string m1009 = rtcm->print_MT1009(glonass_gnav_eph, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1009); Rtcm_Printer::Print_Message(m1009);
@ -193,7 +193,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1009(const Glonass_Gnav_Ephemeris& glonass_gnav_
} }
bool Rtcm_Printer::Print_Rtcm_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map<int, Gnss_Synchro> & observables) bool Rtcm_Printer::Print_Rtcm_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map<int, Gnss_Synchro>& observables)
{ {
std::string m1010 = rtcm->print_MT1010(glonass_gnav_eph, obs_time, observables, station_id); std::string m1010 = rtcm->print_MT1010(glonass_gnav_eph, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1010); Rtcm_Printer::Print_Message(m1010);
@ -201,7 +201,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_
} }
bool Rtcm_Printer::Print_Rtcm_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map<int, Gnss_Synchro> & observables) bool Rtcm_Printer::Print_Rtcm_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map<int, Gnss_Synchro>& observables)
{ {
std::string m1011 = rtcm->print_MT1011(glonass_gnav_ephL1, glonass_gnav_ephL2, obs_time, observables, station_id); std::string m1011 = rtcm->print_MT1011(glonass_gnav_ephL1, glonass_gnav_ephL2, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1011); Rtcm_Printer::Print_Message(m1011);
@ -209,7 +209,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_
} }
bool Rtcm_Printer::Print_Rtcm_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map<int, Gnss_Synchro> & observables) bool Rtcm_Printer::Print_Rtcm_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map<int, Gnss_Synchro>& observables)
{ {
std::string m1012 = rtcm->print_MT1012(glonass_gnav_ephL1, glonass_gnav_ephL2, obs_time, observables, station_id); std::string m1012 = rtcm->print_MT1012(glonass_gnav_ephL1, glonass_gnav_ephL2, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1012); Rtcm_Printer::Print_Message(m1012);
@ -217,7 +217,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_
} }
bool Rtcm_Printer::Print_Rtcm_MT1019(const Gps_Ephemeris & gps_eph) bool Rtcm_Printer::Print_Rtcm_MT1019(const Gps_Ephemeris& gps_eph)
{ {
std::string m1019 = rtcm->print_MT1019(gps_eph); std::string m1019 = rtcm->print_MT1019(gps_eph);
Rtcm_Printer::Print_Message(m1019); Rtcm_Printer::Print_Message(m1019);
@ -225,7 +225,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1019(const Gps_Ephemeris & gps_eph)
} }
bool Rtcm_Printer::Print_Rtcm_MT1020(const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Glonass_Gnav_Utc_Model & glonass_gnav_utc_model) bool Rtcm_Printer::Print_Rtcm_MT1020(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model)
{ {
std::string m1020 = rtcm->print_MT1020(glonass_gnav_eph, glonass_gnav_utc_model); std::string m1020 = rtcm->print_MT1020(glonass_gnav_eph, glonass_gnav_utc_model);
Rtcm_Printer::Print_Message(m1020); Rtcm_Printer::Print_Message(m1020);
@ -233,7 +233,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1020(const Glonass_Gnav_Ephemeris & glonass_gnav
} }
bool Rtcm_Printer::Print_Rtcm_MT1045(const Galileo_Ephemeris & gal_eph) bool Rtcm_Printer::Print_Rtcm_MT1045(const Galileo_Ephemeris& gal_eph)
{ {
std::string m1045 = rtcm->print_MT1045(gal_eph); std::string m1045 = rtcm->print_MT1045(gal_eph);
Rtcm_Printer::Print_Message(m1045); Rtcm_Printer::Print_Message(m1045);
@ -241,44 +241,44 @@ bool Rtcm_Printer::Print_Rtcm_MT1045(const Galileo_Ephemeris & gal_eph)
} }
bool Rtcm_Printer::Print_Rtcm_MSM(unsigned int msm_number, const Gps_Ephemeris & gps_eph, bool Rtcm_Printer::Print_Rtcm_MSM(unsigned int msm_number, const Gps_Ephemeris& gps_eph,
const Gps_CNAV_Ephemeris & gps_cnav_eph, const Gps_CNAV_Ephemeris& gps_cnav_eph,
const Galileo_Ephemeris & gal_eph, const Galileo_Ephemeris& gal_eph,
const Glonass_Gnav_Ephemeris & glo_gnav_eph, const Glonass_Gnav_Ephemeris& glo_gnav_eph,
double obs_time, double obs_time,
const std::map<int, Gnss_Synchro> & observables, const std::map<int, Gnss_Synchro>& observables,
unsigned int clock_steering_indicator, unsigned int clock_steering_indicator,
unsigned int external_clock_indicator, unsigned int external_clock_indicator,
int smooth_int, int smooth_int,
bool divergence_free, bool divergence_free,
bool more_messages) bool more_messages)
{ {
std::string msm; std::string msm;
if(msm_number == 1) if (msm_number == 1)
{ {
msm = rtcm->print_MSM_1(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); msm = rtcm->print_MSM_1(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages);
} }
else if(msm_number == 2) else if (msm_number == 2)
{ {
msm = rtcm->print_MSM_2(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); msm = rtcm->print_MSM_2(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages);
} }
else if(msm_number == 3) else if (msm_number == 3)
{ {
msm = rtcm->print_MSM_3(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); msm = rtcm->print_MSM_3(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages);
} }
else if(msm_number == 4) else if (msm_number == 4)
{ {
msm = rtcm->print_MSM_4(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); msm = rtcm->print_MSM_4(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages);
} }
else if(msm_number == 5) else if (msm_number == 5)
{ {
msm = rtcm->print_MSM_5(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); msm = rtcm->print_MSM_5(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages);
} }
else if(msm_number == 6) else if (msm_number == 6)
{ {
msm = rtcm->print_MSM_6(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); msm = rtcm->print_MSM_6(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages);
} }
else if(msm_number == 7) else if (msm_number == 7)
{ {
msm = rtcm->print_MSM_7(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages); msm = rtcm->print_MSM_7(gps_eph, gps_cnav_eph, gal_eph, glo_gnav_eph, obs_time, observables, station_id, clock_steering_indicator, external_clock_indicator, smooth_int, divergence_free, more_messages);
} }
@ -308,10 +308,10 @@ int Rtcm_Printer::init_serial(std::string serial_device)
fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
if (fd == -1) return fd; // failed to open TTY port if (fd == -1) return fd; // failed to open TTY port
if(fcntl(fd, F_SETFL, 0) == -1) LOG(INFO) << "Error enabling direct I/O"; // clear all flags on descriptor, enable direct I/O if (fcntl(fd, F_SETFL, 0) == -1) LOG(INFO) << "Error enabling direct I/O"; // clear all flags on descriptor, enable direct I/O
tcgetattr(fd, &options); // read serial port options tcgetattr(fd, &options); // read serial port options
BAUD = B9600; BAUD = B9600;
//BAUD = B38400; //BAUD = B38400;
DATABITS = CS8; DATABITS = CS8;
STOPBITS = 0; STOPBITS = 0;
@ -338,23 +338,23 @@ void Rtcm_Printer::close_serial()
} }
bool Rtcm_Printer::Print_Message(const std::string & message) bool Rtcm_Printer::Print_Message(const std::string& message)
{ {
//write to file //write to file
try try
{ {
rtcm_file_descriptor << message << std::endl; rtcm_file_descriptor << message << std::endl;
} }
catch(const std::exception & ex) catch (const std::exception& ex)
{ {
DLOG(INFO) << "RTCM printer cannot write on the output file " << rtcm_filename.c_str(); DLOG(INFO) << "RTCM printer cannot write on the output file " << rtcm_filename.c_str();
return false; return false;
} }
//write to serial device //write to serial device
if (rtcm_dev_descriptor != -1) if (rtcm_dev_descriptor != -1)
{ {
if(write(rtcm_dev_descriptor, message.c_str(), message.length()) == -1) if (write(rtcm_dev_descriptor, message.c_str(), message.length()) == -1)
{ {
DLOG(INFO) << "RTCM printer cannot write on serial device " << rtcm_devname.c_str(); DLOG(INFO) << "RTCM printer cannot write on serial device " << rtcm_devname.c_str();
std::cout << "RTCM printer cannot write on serial device " << rtcm_devname.c_str() << std::endl; std::cout << "RTCM printer cannot write on serial device " << rtcm_devname.c_str() << std::endl;
@ -372,25 +372,25 @@ std::string Rtcm_Printer::print_MT1005_test()
} }
unsigned int Rtcm_Printer::lock_time(const Gps_Ephemeris& eph, double obs_time, const Gnss_Synchro & gnss_synchro) unsigned int Rtcm_Printer::lock_time(const Gps_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro)
{ {
return rtcm->lock_time(eph, obs_time, gnss_synchro); return rtcm->lock_time(eph, obs_time, gnss_synchro);
} }
unsigned int Rtcm_Printer::lock_time(const Gps_CNAV_Ephemeris& eph, double obs_time, const Gnss_Synchro & gnss_synchro) unsigned int Rtcm_Printer::lock_time(const Gps_CNAV_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro)
{ {
return rtcm->lock_time(eph, obs_time, gnss_synchro); return rtcm->lock_time(eph, obs_time, gnss_synchro);
} }
unsigned int Rtcm_Printer::lock_time(const Galileo_Ephemeris& eph, double obs_time, const Gnss_Synchro & gnss_synchro) unsigned int Rtcm_Printer::lock_time(const Galileo_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro)
{ {
return rtcm->lock_time(eph, obs_time, gnss_synchro); return rtcm->lock_time(eph, obs_time, gnss_synchro);
} }
unsigned int Rtcm_Printer::lock_time(const Glonass_Gnav_Ephemeris& eph, double obs_time, const Gnss_Synchro & gnss_synchro) unsigned int Rtcm_Printer::lock_time(const Glonass_Gnav_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro)
{ {
return rtcm->lock_time(eph, obs_time, gnss_synchro); return rtcm->lock_time(eph, obs_time, gnss_synchro);
} }

66
src/algorithms/PVT/libs/rtcm_printer.h
View File

@ -55,10 +55,10 @@ public:
*/ */
~Rtcm_Printer(); ~Rtcm_Printer();
bool Print_Rtcm_MT1001(const Gps_Ephemeris& gps_eph, double obs_time, const std::map<int, Gnss_Synchro> & observables); bool Print_Rtcm_MT1001(const Gps_Ephemeris& gps_eph, double obs_time, const std::map<int, Gnss_Synchro>& observables);
bool Print_Rtcm_MT1002(const Gps_Ephemeris& gps_eph, double obs_time, const std::map<int, Gnss_Synchro> & observables); bool Print_Rtcm_MT1002(const Gps_Ephemeris& gps_eph, double obs_time, const std::map<int, Gnss_Synchro>& observables);
bool Print_Rtcm_MT1003(const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& cnav_eph, double obs_time, const std::map<int, Gnss_Synchro> & observables); bool Print_Rtcm_MT1003(const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& cnav_eph, double obs_time, const std::map<int, Gnss_Synchro>& observables);
bool Print_Rtcm_MT1004(const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& cnav_eph, double obs_time, const std::map<int, Gnss_Synchro> & observables); bool Print_Rtcm_MT1004(const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& cnav_eph, double obs_time, const std::map<int, Gnss_Synchro>& observables);
/*! /*!
* \brief Prints L1-Only GLONASS RTK Observables * \brief Prints L1-Only GLONASS RTK Observables
* \details This GLONASS message type is not generally used or supported; type 1012 is to be preferred. * \details This GLONASS message type is not generally used or supported; type 1012 is to be preferred.
@ -68,7 +68,7 @@ public:
* \param observables Set of observables as defined by the platform * \param observables Set of observables as defined by the platform
* \return true or false upon operation success * \return true or false upon operation success
*/ */
bool Print_Rtcm_MT1009(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map<int, Gnss_Synchro> & observables); bool Print_Rtcm_MT1009(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map<int, Gnss_Synchro>& observables);
/*! /*!
* \brief Prints Extended L1-Only GLONASS RTK Observables * \brief Prints Extended L1-Only GLONASS RTK Observables
* \details This GLONASS message type is used when only L1 data is present and bandwidth is very tight, often 1012 is used in such cases. * \details This GLONASS message type is used when only L1 data is present and bandwidth is very tight, often 1012 is used in such cases.
@ -78,7 +78,7 @@ public:
* \param observables Set of observables as defined by the platform * \param observables Set of observables as defined by the platform
* \return true or false upon operation success * \return true or false upon operation success
*/ */
bool Print_Rtcm_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map<int, Gnss_Synchro> & observables); bool Print_Rtcm_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map<int, Gnss_Synchro>& observables);
/*! /*!
* \brief Prints L1&L2 GLONASS RTK Observables * \brief Prints L1&L2 GLONASS RTK Observables
* \details This GLONASS message type is not generally used or supported; type 1012 is to be preferred * \details This GLONASS message type is not generally used or supported; type 1012 is to be preferred
@ -89,7 +89,7 @@ public:
* \param observables Set of observables as defined by the platform * \param observables Set of observables as defined by the platform
* \return true or false upon operation success * \return true or false upon operation success
*/ */
bool Print_Rtcm_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map<int, Gnss_Synchro> & observables); bool Print_Rtcm_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map<int, Gnss_Synchro>& observables);
/*! /*!
* \brief Prints Extended L1&L2 GLONASS RTK Observables * \brief Prints Extended L1&L2 GLONASS RTK Observables
* \details This GLONASS message type is the most common observational message type, with L1/L2/SNR content. This is one of the most common messages found. * \details This GLONASS message type is the most common observational message type, with L1/L2/SNR content. This is one of the most common messages found.
@ -100,10 +100,10 @@ public:
* \param observables Set of observables as defined by the platform * \param observables Set of observables as defined by the platform
* \return true or false upon operation success * \return true or false upon operation success
*/ */
bool Print_Rtcm_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map<int, Gnss_Synchro> & observables); bool Print_Rtcm_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map<int, Gnss_Synchro>& observables);
bool Print_Rtcm_MT1019(const Gps_Ephemeris & gps_eph); //<! GPS Ephemeris, should be broadcast in the event that the IODC does not match the IODE, and every 2 minutes. bool Print_Rtcm_MT1019(const Gps_Ephemeris& gps_eph); //<! GPS Ephemeris, should be broadcast in the event that the IODC does not match the IODE, and every 2 minutes.
bool Print_Rtcm_MT1045(const Galileo_Ephemeris & gal_eph); //<! Galileo Ephemeris, should be broadcast every 2 minutes bool Print_Rtcm_MT1045(const Galileo_Ephemeris& gal_eph); //<! Galileo Ephemeris, should be broadcast every 2 minutes
/*! /*!
* \brief Prints GLONASS GNAV Ephemeris * \brief Prints GLONASS GNAV Ephemeris
* \details This GLONASS message should be broadcast every 2 minutes * \details This GLONASS message should be broadcast every 2 minutes
@ -112,25 +112,25 @@ public:
* \param utc_model GLONASS GNAV Clock Information broadcast in string 5 * \param utc_model GLONASS GNAV Clock Information broadcast in string 5
* \return true or false upon operation success * \return true or false upon operation success
*/ */
bool Print_Rtcm_MT1020(const Glonass_Gnav_Ephemeris & glo_gnav_eph, const Glonass_Gnav_Utc_Model & utc_model); bool Print_Rtcm_MT1020(const Glonass_Gnav_Ephemeris& glo_gnav_eph, const Glonass_Gnav_Utc_Model& utc_model);
bool Print_Rtcm_MSM(unsigned int msm_number, bool Print_Rtcm_MSM(unsigned int msm_number,
const Gps_Ephemeris & gps_eph, const Gps_Ephemeris& gps_eph,
const Gps_CNAV_Ephemeris & gps_cnav_eph, const Gps_CNAV_Ephemeris& gps_cnav_eph,
const Galileo_Ephemeris & gal_eph, const Galileo_Ephemeris& gal_eph,
const Glonass_Gnav_Ephemeris & glo_gnav_eph, const Glonass_Gnav_Ephemeris& glo_gnav_eph,
double obs_time, double obs_time,
const std::map<int, Gnss_Synchro> & observables, const std::map<int, Gnss_Synchro>& observables,
unsigned int clock_steering_indicator, unsigned int clock_steering_indicator,
unsigned int external_clock_indicator, unsigned int external_clock_indicator,
int smooth_int, int smooth_int,
bool divergence_free, bool divergence_free,
bool more_messages); bool more_messages);
std::string print_MT1005_test(); //<! For testing purposes std::string print_MT1005_test(); //<! For testing purposes
unsigned int lock_time(const Gps_Ephemeris& eph, double obs_time, const Gnss_Synchro & gnss_synchro); unsigned int lock_time(const Gps_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro);
unsigned int lock_time(const Gps_CNAV_Ephemeris& eph, double obs_time, const Gnss_Synchro & gnss_synchro); unsigned int lock_time(const Gps_CNAV_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro);
unsigned int lock_time(const Galileo_Ephemeris& eph, double obs_time, const Gnss_Synchro & gnss_synchro); unsigned int lock_time(const Galileo_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro);
/*! /*!
* \brief Locks time for logging given GLONASS GNAV Broadcast Ephemeris * \brief Locks time for logging given GLONASS GNAV Broadcast Ephemeris
* \note Code added as part of GSoC 2017 program * \note Code added as part of GSoC 2017 program
@ -139,19 +139,19 @@ public:
* \params observables Set of observables as defined by the platform * \params observables Set of observables as defined by the platform
* \return locked time during logging process * \return locked time during logging process
*/ */
unsigned int lock_time(const Glonass_Gnav_Ephemeris& eph, double obs_time, const Gnss_Synchro & gnss_synchro); unsigned int lock_time(const Glonass_Gnav_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro);
private: private:
std::string rtcm_filename; // String with the RTCM log filename std::string rtcm_filename; // String with the RTCM log filename
std::ofstream rtcm_file_descriptor; // Output file stream for RTCM log file std::ofstream rtcm_file_descriptor; // Output file stream for RTCM log file
std::string rtcm_devname; std::string rtcm_devname;
unsigned short port; unsigned short port;
unsigned short station_id; unsigned short station_id;
int rtcm_dev_descriptor; // RTCM serial device descriptor (i.e. COM port) int rtcm_dev_descriptor; // RTCM serial device descriptor (i.e. COM port)
int init_serial (std::string serial_device); //serial port control int init_serial(std::string serial_device); //serial port control
void close_serial (); void close_serial();
std::shared_ptr<Rtcm> rtcm; std::shared_ptr<Rtcm> rtcm;
bool Print_Message(const std::string & message); bool Print_Message(const std::string& message);
}; };
#endif #endif

538
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -61,7 +61,7 @@
using google::LogMessage; using google::LogMessage;
rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, rtk_t & rtk) rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, rtk_t& rtk)
{ {
// init empty ephemeris for all the available GNSS channels // init empty ephemeris for all the available GNSS channels
d_nchannels = nchannels; d_nchannels = nchannels;
@ -71,7 +71,7 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
this->set_averaging_flag(false); this->set_averaging_flag(false);
rtk_ = rtk; rtk_ = rtk;
pvt_sol = {{0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, '0', '0', '0', 0, 0, 0 }; pvt_sol = {{0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, '0', '0', '0', 0, 0, 0};
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true) if (d_flag_dump_enabled == true)
@ -79,15 +79,15 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
if (d_dump_file.is_open() == false) if (d_dump_file.is_open() == false)
{ {
try try
{ {
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str(); LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
} }
catch (const std::ifstream::failure &e) catch (const std::ifstream::failure& e)
{ {
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what(); LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
} }
} }
} }
} }
@ -98,24 +98,24 @@ rtklib_solver::~rtklib_solver()
if (d_dump_file.is_open() == true) if (d_dump_file.is_open() == true)
{ {
try try
{ {
d_dump_file.close(); d_dump_file.close();
} }
catch(const std::exception & ex) catch (const std::exception& ex)
{ {
LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what();
} }
} }
} }
bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_map, double Rx_time, bool flag_averaging) bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double Rx_time, bool flag_averaging)
{ {
std::map<int,Gnss_Synchro>::const_iterator gnss_observables_iter; std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
std::map<int,Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter; std::map<int, Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter;
std::map<int,Gps_Ephemeris>::const_iterator gps_ephemeris_iter; std::map<int, Gps_Ephemeris>::const_iterator gps_ephemeris_iter;
std::map<int,Gps_CNAV_Ephemeris>::const_iterator gps_cnav_ephemeris_iter; std::map<int, Gps_CNAV_Ephemeris>::const_iterator gps_cnav_ephemeris_iter;
std::map<int,Glonass_Gnav_Ephemeris>::const_iterator glonass_gnav_ephemeris_iter; std::map<int, Glonass_Gnav_Ephemeris>::const_iterator glonass_gnav_ephemeris_iter;
const Glonass_Gnav_Utc_Model gnav_utc = this->glonass_gnav_utc_model; const Glonass_Gnav_Utc_Model gnav_utc = this->glonass_gnav_utc_model;
this->set_averaging_flag(flag_averaging); this->set_averaging_flag(flag_averaging);
@ -123,283 +123,280 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
// ******************************************************************************** // ********************************************************************************
// ****** PREPARE THE DATA (SV EPHEMERIS AND OBSERVATIONS) ************************ // ****** PREPARE THE DATA (SV EPHEMERIS AND OBSERVATIONS) ************************
// ******************************************************************************** // ********************************************************************************
int valid_obs = 0; //valid observations counter int valid_obs = 0; //valid observations counter
int glo_valid_obs = 0; //GLONASS L1/L2 valid observations counter int glo_valid_obs = 0; //GLONASS L1/L2 valid observations counter
obsd_t obs_data[MAXOBS]; obsd_t obs_data[MAXOBS];
eph_t eph_data[MAXOBS]; eph_t eph_data[MAXOBS];
geph_t geph_data[MAXOBS]; geph_t geph_data[MAXOBS];
for(gnss_observables_iter = gnss_observables_map.cbegin(); for (gnss_observables_iter = gnss_observables_map.cbegin();
gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter != gnss_observables_map.cend();
gnss_observables_iter++) gnss_observables_iter++)
{ {
switch(gnss_observables_iter->second.System) switch (gnss_observables_iter->second.System)
{
case 'E':
{ {
std::string sig_(gnss_observables_iter->second.Signal); case 'E':
// Galileo E1 {
if(sig_.compare("1B") == 0) std::string sig_(gnss_observables_iter->second.Signal);
{ // Galileo E1
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key if (sig_.compare("1B") == 0)
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); {
if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
{ galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
//convert ephemeris from GNSS-SDR class to RTKLIB structure if (galileo_ephemeris_iter != galileo_ephemeris_map.cend())
eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); {
//convert observation from GNSS-SDR class to RTKLIB structure //convert ephemeris from GNSS-SDR class to RTKLIB structure
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
obs_data[valid_obs+glo_valid_obs] = insert_obs_to_rtklib(newobs, //convert observation from GNSS-SDR class to RTKLIB structure
obsd_t newobs = {{0, 0}, '0', '0', {}, {}, {}, {}, {}, {}};
obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
galileo_ephemeris_iter->second.WN_5, galileo_ephemeris_iter->second.WN_5,
0); 0);
valid_obs++; valid_obs++;
} }
else // the ephemeris are not available for this SV else // the ephemeris are not available for this SV
{ {
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
} }
} }
// Galileo E5 // Galileo E5
if(sig_.compare("5X") == 0) if (sig_.compare("5X") == 0)
{ {
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) if (galileo_ephemeris_iter != galileo_ephemeris_map.cend())
{ {
bool found_E1_obs = false; bool found_E1_obs = false;
for (int i = 0; i < valid_obs; i++) for (int i = 0; i < valid_obs; i++)
{ {
if (eph_data[i].sat == (static_cast<int>(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO))) if (eph_data[i].sat == (static_cast<int>(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO)))
{ {
obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(obs_data[i + glo_valid_obs], obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(obs_data[i + glo_valid_obs],
gnss_observables_iter->second, gnss_observables_iter->second,
galileo_ephemeris_iter->second.WN_5, galileo_ephemeris_iter->second.WN_5,
2);//Band 3 (L5/E5) 2); //Band 3 (L5/E5)
found_E1_obs = true; found_E1_obs = true;
break; break;
} }
} }
if (!found_E1_obs) if (!found_E1_obs)
{ {
//insert Galileo E5 obs as new obs and also insert its ephemeris //insert Galileo E5 obs as new obs and also insert its ephemeris
//convert ephemeris from GNSS-SDR class to RTKLIB structure //convert ephemeris from GNSS-SDR class to RTKLIB structure
eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
//convert observation from GNSS-SDR class to RTKLIB structure //convert observation from GNSS-SDR class to RTKLIB structure
unsigned char default_code_ = static_cast<unsigned char>(CODE_NONE); unsigned char default_code_ = static_cast<unsigned char>(CODE_NONE);
obsd_t newobs = {{0,0}, '0', '0', {}, {}, obsd_t newobs = {{0, 0}, '0', '0', {}, {},
{default_code_, default_code_, default_code_}, {default_code_, default_code_, default_code_},
{}, {0.0, 0.0, 0.0}, {}}; {}, {0.0, 0.0, 0.0}, {}};
obs_data[valid_obs+glo_valid_obs] = insert_obs_to_rtklib(newobs, obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
galileo_ephemeris_iter->second.WN_5, galileo_ephemeris_iter->second.WN_5,
2); //Band 3 (L5/E5) 2); //Band 3 (L5/E5)
valid_obs++; valid_obs++;
} }
} }
else // the ephemeris are not available for this SV else // the ephemeris are not available for this SV
{ {
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
} }
} }
break; break;
} }
case 'G': case 'G':
{ {
// GPS L1 // GPS L1
// 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key // 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key
std::string sig_(gnss_observables_iter->second.Signal); std::string sig_(gnss_observables_iter->second.Signal);
if(sig_.compare("1C") == 0) if (sig_.compare("1C") == 0)
{ {
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_ephemeris_iter != gps_ephemeris_map.cend()) if (gps_ephemeris_iter != gps_ephemeris_map.cend())
{ {
//convert ephemeris from GNSS-SDR class to RTKLIB structure //convert ephemeris from GNSS-SDR class to RTKLIB structure
eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second); eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second);
//convert observation from GNSS-SDR class to RTKLIB structure //convert observation from GNSS-SDR class to RTKLIB structure
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; obsd_t newobs = {{0, 0}, '0', '0', {}, {}, {}, {}, {}, {}};
obs_data[valid_obs+glo_valid_obs] = insert_obs_to_rtklib(newobs, obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
gps_ephemeris_iter->second.i_GPS_week, gps_ephemeris_iter->second.i_GPS_week,
0); 0);
valid_obs++; valid_obs++;
} }
else // the ephemeris are not available for this SV else // the ephemeris are not available for this SV
{ {
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first; DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first;
} }
} }
//GPS L2 //GPS L2
if(sig_.compare("2S") == 0) if (sig_.compare("2S") == 0)
{ {
gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN); gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.cend()) if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.cend())
{ {
// 1. Find the same satellite in GPS L1 band // 1. Find the same satellite in GPS L1 band
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_ephemeris_iter != gps_ephemeris_map.cend()) if (gps_ephemeris_iter != gps_ephemeris_map.cend())
{ {
// 2. If found, replace the existing GPS L1 ephemeris with the GPS L2 ephemeris // 2. If found, replace the existing GPS L1 ephemeris with the GPS L2 ephemeris
// (more precise!), and attach the L2 observation to the L1 observation in RTKLIB structure // (more precise!), and attach the L2 observation to the L1 observation in RTKLIB structure
for (int i = 0; i < valid_obs; i++) for (int i = 0; i < valid_obs; i++)
{ {
if (eph_data[i].sat == static_cast<int>(gnss_observables_iter->second.PRN)) if (eph_data[i].sat == static_cast<int>(gnss_observables_iter->second.PRN))
{ {
eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second);
obs_data[i+glo_valid_obs] = insert_obs_to_rtklib(obs_data[i+glo_valid_obs], obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(obs_data[i + glo_valid_obs],
gnss_observables_iter->second, gnss_observables_iter->second,
gps_cnav_ephemeris_iter->second.i_GPS_week, gps_cnav_ephemeris_iter->second.i_GPS_week,
1);//Band 2 (L2) 1); //Band 2 (L2)
break; break;
} }
} }
} }
else else
{ {
// 3. If not found, insert the GPS L2 ephemeris and the observation // 3. If not found, insert the GPS L2 ephemeris and the observation
//convert ephemeris from GNSS-SDR class to RTKLIB structure //convert ephemeris from GNSS-SDR class to RTKLIB structure
eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second);
//convert observation from GNSS-SDR class to RTKLIB structure //convert observation from GNSS-SDR class to RTKLIB structure
unsigned char default_code_ = static_cast<unsigned char>(CODE_NONE); unsigned char default_code_ = static_cast<unsigned char>(CODE_NONE);
obsd_t newobs = {{0,0}, '0', '0', {}, {}, obsd_t newobs = {{0, 0}, '0', '0', {}, {},
{default_code_, default_code_, default_code_}, {default_code_, default_code_, default_code_},
{}, {0.0, 0.0, 0.0}, {}}; {}, {0.0, 0.0, 0.0}, {}};
obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
gps_cnav_ephemeris_iter->second.i_GPS_week, gps_cnav_ephemeris_iter->second.i_GPS_week,
1);//Band 2 (L2) 1); //Band 2 (L2)
valid_obs++; valid_obs++;
} }
} }
else // the ephemeris are not available for this SV else // the ephemeris are not available for this SV
{ {
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
} }
} }
//GPS L5 //GPS L5
if(sig_.compare("L5") == 0) if (sig_.compare("L5") == 0)
{ {
gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN); gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.cend()) if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.cend())
{ {
// 1. Find the same satellite in GPS L1 band // 1. Find the same satellite in GPS L1 band
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_ephemeris_iter != gps_ephemeris_map.cend()) if (gps_ephemeris_iter != gps_ephemeris_map.cend())
{ {
// 2. If found, replace the existing GPS L1 ephemeris with the GPS L5 ephemeris // 2. If found, replace the existing GPS L1 ephemeris with the GPS L5 ephemeris
// (more precise!), and attach the L5 observation to the L1 observation in RTKLIB structure // (more precise!), and attach the L5 observation to the L1 observation in RTKLIB structure
for (int i = 0; i < valid_obs; i++) for (int i = 0; i < valid_obs; i++)
{ {
if (eph_data[i].sat == static_cast<int>(gnss_observables_iter->second.PRN)) if (eph_data[i].sat == static_cast<int>(gnss_observables_iter->second.PRN))
{ {
eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second);
obs_data[i+glo_valid_obs] = insert_obs_to_rtklib(obs_data[i], obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(obs_data[i],
gnss_observables_iter->second, gnss_observables_iter->second,
gps_cnav_ephemeris_iter->second.i_GPS_week, gps_cnav_ephemeris_iter->second.i_GPS_week,
2);//Band 3 (L5) 2); //Band 3 (L5)
break; break;
} }
} }
} }
else else
{ {
// 3. If not found, insert the GPS L5 ephemeris and the observation // 3. If not found, insert the GPS L5 ephemeris and the observation
//convert ephemeris from GNSS-SDR class to RTKLIB structure //convert ephemeris from GNSS-SDR class to RTKLIB structure
eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second);
//convert observation from GNSS-SDR class to RTKLIB structure //convert observation from GNSS-SDR class to RTKLIB structure
unsigned char default_code_ = static_cast<unsigned char>(CODE_NONE); unsigned char default_code_ = static_cast<unsigned char>(CODE_NONE);
obsd_t newobs = {{0,0}, '0', '0', {}, {}, obsd_t newobs = {{0, 0}, '0', '0', {}, {},
{default_code_, default_code_, default_code_}, {default_code_, default_code_, default_code_},
{}, {0.0, 0.0, 0.0}, {}}; {}, {0.0, 0.0, 0.0}, {}};
obs_data[valid_obs+glo_valid_obs] = insert_obs_to_rtklib(newobs, obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
gps_cnav_ephemeris_iter->second.i_GPS_week, gps_cnav_ephemeris_iter->second.i_GPS_week,
2);//Band 3 (L5) 2); //Band 3 (L5)
valid_obs++; valid_obs++;
} }
} }
else // the ephemeris are not available for this SV else // the ephemeris are not available for this SV
{ {
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
} }
} }
break; break;
} }
case 'R': //TODO This should be using rtk lib nomenclature case 'R': //TODO This should be using rtk lib nomenclature
{ {
std::string sig_(gnss_observables_iter->second.Signal); std::string sig_(gnss_observables_iter->second.Signal);
// GLONASS GNAV L1 // GLONASS GNAV L1
if(sig_.compare("1G") == 0) if (sig_.compare("1G") == 0)
{ {
// 1 Glo - find the ephemeris for the current GLONASS SV observation. The SV Slot Number (PRN ID) is the map key // 1 Glo - find the ephemeris for the current GLONASS SV observation. The SV Slot Number (PRN ID) is the map key
glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend()) if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend())
{ {
//convert ephemeris from GNSS-SDR class to RTKLIB structure //convert ephemeris from GNSS-SDR class to RTKLIB structure
geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc); geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc);
//convert observation from GNSS-SDR class to RTKLIB structure //convert observation from GNSS-SDR class to RTKLIB structure
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; obsd_t newobs = {{0, 0}, '0', '0', {}, {}, {}, {}, {}, {}};
obs_data[valid_obs+glo_valid_obs] = insert_obs_to_rtklib(newobs, obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
glonass_gnav_ephemeris_iter->second.d_WN, glonass_gnav_ephemeris_iter->second.d_WN,
0);//Band 0 (L1) 0); //Band 0 (L1)
glo_valid_obs++; glo_valid_obs++;
} }
else // the ephemeris are not available for this SV else // the ephemeris are not available for this SV
{ {
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
} }
}
} // GLONASS GNAV L2
// GLONASS GNAV L2 if (sig_.compare("2G") == 0)
if(sig_.compare("2G") == 0) {
{ // 1 GLONASS - find the ephemeris for the current GLONASS SV observation. The SV PRN ID is the map key
// 1 GLONASS - find the ephemeris for the current GLONASS SV observation. The SV PRN ID is the map key glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend())
if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend()) {
{ bool found_L1_obs = false;
bool found_L1_obs = false; for (int i = 0; i < glo_valid_obs; i++)
for (int i = 0; i < glo_valid_obs; i++) {
{ if (geph_data[i].sat == (static_cast<int>(gnss_observables_iter->second.PRN + NSATGPS)))
if (geph_data[i].sat == (static_cast<int>(gnss_observables_iter->second.PRN+NSATGPS))) {
{ obs_data[i + valid_obs] = insert_obs_to_rtklib(obs_data[i + valid_obs],
obs_data[i+valid_obs] = insert_obs_to_rtklib(obs_data[i+valid_obs],
gnss_observables_iter->second, gnss_observables_iter->second,
glonass_gnav_ephemeris_iter->second.d_WN, glonass_gnav_ephemeris_iter->second.d_WN,
1);//Band 1 (L2) 1); //Band 1 (L2)
found_L1_obs = true; found_L1_obs = true;
break; break;
} }
} }
if (!found_L1_obs) if (!found_L1_obs)
{ {
//insert GLONASS GNAV L2 obs as new obs and also insert its ephemeris //insert GLONASS GNAV L2 obs as new obs and also insert its ephemeris
//convert ephemeris from GNSS-SDR class to RTKLIB structure //convert ephemeris from GNSS-SDR class to RTKLIB structure
geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc); geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc);
//convert observation from GNSS-SDR class to RTKLIB structure //convert observation from GNSS-SDR class to RTKLIB structure
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; obsd_t newobs = {{0, 0}, '0', '0', {}, {}, {}, {}, {}, {}};
obs_data[valid_obs+glo_valid_obs] = insert_obs_to_rtklib(newobs, obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
glonass_gnav_ephemeris_iter->second.d_WN, glonass_gnav_ephemeris_iter->second.d_WN,
1); //Band 1 (L2) 1); //Band 1 (L2)
glo_valid_obs++; glo_valid_obs++;
} }
} }
else // the ephemeris are not available for this SV else // the ephemeris are not available for this SV
{ {
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
} }
}
break;
} }
default:
DLOG(INFO) << "Hybrid observables: Unknown GNSS";
break; break;
} }
default :
DLOG(INFO) << "Hybrid observables: Unknown GNSS";
break;
}
} }
// ********************************************************************** // **********************************************************************
@ -425,16 +422,16 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
result = rtkpos(&rtk_, obs_data, valid_obs + glo_valid_obs, &nav_data); result = rtkpos(&rtk_, obs_data, valid_obs + glo_valid_obs, &nav_data);
if(result == 0) if (result == 0)
{ {
LOG(INFO) << "RTKLIB rtkpos error"; LOG(INFO) << "RTKLIB rtkpos error";
DLOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf; DLOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf;
this->set_time_offset_s(0.0); //reset rx time estimation this->set_time_offset_s(0.0); //reset rx time estimation
this->set_num_valid_observations(0); this->set_num_valid_observations(0);
} }
else else
{ {
this->set_num_valid_observations(rtk_.sol.ns); //record the number of valid satellites used by the PVT solver this->set_num_valid_observations(rtk_.sol.ns); //record the number of valid satellites used by the PVT solver
pvt_sol = rtk_.sol; pvt_sol = rtk_.sol;
this->set_valid_position(true); this->set_valid_position(true);
arma::vec rx_position_and_time(4); arma::vec rx_position_and_time(4);
@ -442,9 +439,9 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
rx_position_and_time(1) = pvt_sol.rr[1]; rx_position_and_time(1) = pvt_sol.rr[1];
rx_position_and_time(2) = pvt_sol.rr[2]; rx_position_and_time(2) = pvt_sol.rr[2];
rx_position_and_time(3) = pvt_sol.dtr[0]; rx_position_and_time(3) = pvt_sol.dtr[0];
this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration
double offset_s = this->get_time_offset_s(); double offset_s = this->get_time_offset_s();
this->set_time_offset_s(offset_s + (rx_position_and_time(3) / GPS_C_m_s)); // accumulate the rx time error for the next iteration [meters]->[seconds] this->set_time_offset_s(offset_s + (rx_position_and_time(3) / GPS_C_m_s)); // accumulate the rx time error for the next iteration [meters]->[seconds]
DLOG(INFO) << "RTKLIB Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; DLOG(INFO) << "RTKLIB Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
boost::posix_time::ptime p_time; boost::posix_time::ptime p_time;
@ -456,14 +453,15 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
DLOG(INFO) << "RTKLIB Position at " << boost::posix_time::to_simple_string(p_time) DLOG(INFO) << "RTKLIB Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude() << " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude()
<< " [deg], Height= " << this->get_height() << " [m]" << " RX time offset= " << this->get_time_offset_s() << " [s]"; << " [deg], Height= " << this->get_height() << " [m]"
<< " RX time offset= " << this->get_time_offset_s() << " [s]";
// ######## LOG FILE ######### // ######## LOG FILE #########
if(d_flag_dump_enabled == true) if (d_flag_dump_enabled == true)
{ {
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file
try try
{ {
double tmp_double; double tmp_double;
// PVT GPS time // PVT GPS time
tmp_double = Rx_time; tmp_double = Rx_time;
@ -489,13 +487,13 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
// GEO user position Height [m] // GEO user position Height [m]
tmp_double = this->get_height(); tmp_double = this->get_height();
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
} }
catch (const std::ifstream::failure& e) catch (const std::ifstream::failure& e)
{ {
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what(); LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
} }
} }
} }
} }
return this->is_valid_position(); return this->is_valid_position();
} }

16
src/algorithms/PVT/libs/rtklib_solver.h
View File

@ -80,15 +80,15 @@ private:
bool d_flag_dump_enabled; bool d_flag_dump_enabled;
int d_nchannels; // Number of available channels for positioning int d_nchannels; // Number of available channels for positioning
public: public:
rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, rtk_t & rtk); rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, rtk_t& rtk);
~rtklib_solver(); ~rtklib_solver();
bool get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_map, double Rx_time, bool flag_averaging); bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double Rx_time, bool flag_averaging);
std::map<int,Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris std::map<int, Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris
std::map<int,Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new GPS_Ephemeris std::map<int, Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new GPS_Ephemeris
std::map<int,Gps_CNAV_Ephemeris> gps_cnav_ephemeris_map; //!< Map storing new GPS_CNAV_Ephemeris std::map<int, Gps_CNAV_Ephemeris> gps_cnav_ephemeris_map; //!< Map storing new GPS_CNAV_Ephemeris
std::map<int,Glonass_Gnav_Ephemeris> glonass_gnav_ephemeris_map; //!< Map storing new GLONASS GNAV Ephmeris std::map<int, Glonass_Gnav_Ephemeris> glonass_gnav_ephemeris_map; //!< Map storing new GLONASS GNAV Ephmeris
Galileo_Utc_Model galileo_utc_model; Galileo_Utc_Model galileo_utc_model;
Galileo_Iono galileo_iono; Galileo_Iono galileo_iono;
@ -100,8 +100,8 @@ public:
Gps_CNAV_Iono gps_cnav_iono; Gps_CNAV_Iono gps_cnav_iono;
Gps_CNAV_Utc_Model gps_cnav_utc_model; Gps_CNAV_Utc_Model gps_cnav_utc_model;
Glonass_Gnav_Utc_Model glonass_gnav_utc_model; //!< Map storing GLONASS GNAV UTC Model Glonass_Gnav_Utc_Model glonass_gnav_utc_model; //!< Map storing GLONASS GNAV UTC Model
Glonass_Gnav_Almanac glonass_gnav_almanac; //!< Map storing GLONASS GNAV Almanac Model Glonass_Gnav_Almanac glonass_gnav_almanac; //!< Map storing GLONASS GNAV Almanac Model
int count_valid_position; int count_valid_position;
}; };

53
src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.cc
View File

@ -41,9 +41,8 @@
using google::LogMessage; using google::LogMessage;
GalileoE1Pcps8msAmbiguousAcquisition::GalileoE1Pcps8msAmbiguousAcquisition( GalileoE1Pcps8msAmbiguousAcquisition::GalileoE1Pcps8msAmbiguousAcquisition(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
@ -52,14 +51,14 @@ GalileoE1Pcps8msAmbiguousAcquisition::GalileoE1Pcps8msAmbiguousAcquisition(
DLOG(INFO) << "role " << role; DLOG(INFO) << "role " << role;
item_type_ = configuration_->property(role + ".item_type", item_type_ = configuration_->property(role + ".item_type",
default_item_type); default_item_type);
long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 4000000); long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 4000000);
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
if_ = configuration_->property(role + ".if", 0); if_ = configuration_->property(role + ".if", 0);
dump_ = configuration_->property(role + ".dump", false); dump_ = configuration_->property(role + ".dump", false);
doppler_max_ = configuration_->property(role + ".doppler_max", 5000); doppler_max_ = configuration_->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 4); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 4);
if (sampled_ms_ % 4 != 0) if (sampled_ms_ % 4 != 0)
@ -73,13 +72,11 @@ GalileoE1Pcps8msAmbiguousAcquisition::GalileoE1Pcps8msAmbiguousAcquisition(
max_dwells_ = configuration_->property(role + ".max_dwells", 1); max_dwells_ = configuration_->property(role + ".max_dwells", 1);
dump_filename_ = configuration_->property(role + ".dump_filename", dump_filename_ = configuration_->property(role + ".dump_filename",
default_dump_filename); default_dump_filename);
//--- Find number of samples per spreading code (4 ms) ----------------- //--- Find number of samples per spreading code (4 ms) -----------------
code_length_ = round( code_length_ = round(
fs_in_ fs_in_ / (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS));
/ (Galileo_E1_CODE_CHIP_RATE_HZ
/ Galileo_E1_B_CODE_LENGTH_CHIPS));
vector_length_ = code_length_ * static_cast<int>(sampled_ms_ / 4); vector_length_ = code_length_ * static_cast<int>(sampled_ms_ / 4);
@ -91,13 +88,13 @@ GalileoE1Pcps8msAmbiguousAcquisition::GalileoE1Pcps8msAmbiguousAcquisition(
{ {
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
acquisition_cc_ = galileo_pcps_8ms_make_acquisition_cc(sampled_ms_, max_dwells_, acquisition_cc_ = galileo_pcps_8ms_make_acquisition_cc(sampled_ms_, max_dwells_,
doppler_max_, if_, fs_in_, samples_per_ms, code_length_, doppler_max_, if_, fs_in_, samples_per_ms, code_length_,
dump_, dump_filename_); dump_, dump_filename_);
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
DLOG(INFO) << "stream_to_vector(" DLOG(INFO) << "stream_to_vector("
<< stream_to_vector_->unique_id() << ")"; << stream_to_vector_->unique_id() << ")";
DLOG(INFO) << "acquisition(" << acquisition_cc_->unique_id() DLOG(INFO) << "acquisition(" << acquisition_cc_->unique_id()
<< ")"; << ")";
} }
else else
{ {
@ -130,11 +127,11 @@ void GalileoE1Pcps8msAmbiguousAcquisition::set_channel(unsigned int channel)
void GalileoE1Pcps8msAmbiguousAcquisition::set_threshold(float threshold) void GalileoE1Pcps8msAmbiguousAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_+ boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0); float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0);
if(pfa == 0.0) pfa = configuration_->property(role_ + ".pfa", 0.0); if (pfa == 0.0) pfa = configuration_->property(role_ + ".pfa", 0.0);
if(pfa == 0.0) if (pfa == 0.0)
{ {
threshold_ = threshold; threshold_ = threshold;
} }
@ -174,7 +171,7 @@ void GalileoE1Pcps8msAmbiguousAcquisition::set_doppler_step(unsigned int doppler
void GalileoE1Pcps8msAmbiguousAcquisition::set_gnss_synchro( void GalileoE1Pcps8msAmbiguousAcquisition::set_gnss_synchro(
Gnss_Synchro* gnss_synchro) Gnss_Synchro* gnss_synchro)
{ {
gnss_synchro_ = gnss_synchro; gnss_synchro_ = gnss_synchro;
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
@ -209,18 +206,17 @@ void GalileoE1Pcps8msAmbiguousAcquisition::set_local_code()
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
bool cboc = configuration_->property( bool cboc = configuration_->property(
"Acquisition" + boost::lexical_cast<std::string>(channel_) "Acquisition" + boost::lexical_cast<std::string>(channel_) + ".cboc", false);
+ ".cboc", false);
std::complex<float> * code = new std::complex<float>[code_length_]; std::complex<float>* code = new std::complex<float>[code_length_];
galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal,
cboc, gnss_synchro_->PRN, fs_in_, 0, false); cboc, gnss_synchro_->PRN, fs_in_, 0, false);
for (unsigned int i = 0; i < sampled_ms_/4; i++) for (unsigned int i = 0; i < sampled_ms_ / 4; i++)
{ {
memcpy(&(code_[i*code_length_]), code, memcpy(&(code_[i * code_length_]), code,
sizeof(gr_complex)*code_length_); sizeof(gr_complex) * code_length_);
} }
acquisition_cc_->set_local_code(code_); acquisition_cc_->set_local_code(code_);
@ -248,12 +244,12 @@ float GalileoE1Pcps8msAmbiguousAcquisition::calculate_threshold(float pfa)
DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa; DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
unsigned int ncells = vector_length_*frequency_bins; unsigned int ncells = vector_length_ * frequency_bins;
double exponent = 1 / static_cast<double>(ncells); double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0 - pfa,exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist(lambda);
float threshold = static_cast<float>(quantile(mydist,val)); float threshold = static_cast<float>(quantile(mydist, val));
return threshold; return threshold;
} }
@ -287,4 +283,3 @@ gr::basic_block_sptr GalileoE1Pcps8msAmbiguousAcquisition::get_right_block()
{ {
return acquisition_cc_; return acquisition_cc_;
} }

10
src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.h
View File

@ -45,12 +45,12 @@ class ConfigurationInterface;
* \brief Adapts a PCPS 8ms acquisition block to an * \brief Adapts a PCPS 8ms acquisition block to an
* AcquisitionInterface for Galileo E1 Signals * AcquisitionInterface for Galileo E1 Signals
*/ */
class GalileoE1Pcps8msAmbiguousAcquisition: public AcquisitionInterface class GalileoE1Pcps8msAmbiguousAcquisition : public AcquisitionInterface
{ {
public: public:
GalileoE1Pcps8msAmbiguousAcquisition(ConfigurationInterface* configuration, GalileoE1Pcps8msAmbiguousAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GalileoE1Pcps8msAmbiguousAcquisition(); virtual ~GalileoE1Pcps8msAmbiguousAcquisition();
@ -142,8 +142,8 @@ private:
long if_; long if_;
bool dump_; bool dump_;
std::string dump_filename_; std::string dump_filename_;
std::complex<float> * code_; std::complex<float>* code_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

52
src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc
View File

@ -42,9 +42,8 @@
using google::LogMessage; using google::LogMessage;
GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition( GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
@ -60,7 +59,7 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition(
dump_ = configuration_->property(role + ".dump", false); dump_ = configuration_->property(role + ".dump", false);
blocking_ = configuration_->property(role + ".blocking", true); blocking_ = configuration_->property(role + ".blocking", true);
doppler_max_ = configuration_->property(role + ".doppler_max", 5000); doppler_max_ = configuration_->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 4); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 4);
if (sampled_ms_ % 4 != 0) if (sampled_ms_ % 4 != 0)
@ -72,8 +71,8 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition(
} }
bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false);
use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions
acquire_pilot_ = configuration_->property(role + ".acquire_pilot", false); //will be true in future versions acquire_pilot_ = configuration_->property(role + ".acquire_pilot", false); //will be true in future versions
max_dwells_ = configuration_->property(role + ".max_dwells", 1); max_dwells_ = configuration_->property(role + ".max_dwells", 1);
@ -84,22 +83,25 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition(
int samples_per_ms = round(code_length_ / 4.0); int samples_per_ms = round(code_length_ / 4.0);
vector_length_ = sampled_ms_ * samples_per_ms; vector_length_ = sampled_ms_ * samples_per_ms;
if( bit_transition_flag_ ) if (bit_transition_flag_)
{ {
vector_length_ *= 2; vector_length_ *= 2;
} }
code_ = new gr_complex[vector_length_]; code_ = new gr_complex[vector_length_];
if (item_type_.compare("cshort") == 0 ) if (item_type_.compare("cshort") == 0)
{ {
item_size_ = sizeof(lv_16sc_t); item_size_ = sizeof(lv_16sc_t);
} }
else { item_size_ = sizeof(gr_complex); } else
{
item_size_ = sizeof(gr_complex);
}
acquisition_ = pcps_make_acquisition(sampled_ms_, max_dwells_, acquisition_ = pcps_make_acquisition(sampled_ms_, max_dwells_,
doppler_max_, if_, fs_in_, samples_per_ms, code_length_, doppler_max_, if_, fs_in_, samples_per_ms, code_length_,
bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_, bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_,
dump_filename_, item_size_); dump_filename_, item_size_);
DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")";
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
@ -133,11 +135,11 @@ void GalileoE1PcpsAmbiguousAcquisition::set_channel(unsigned int channel)
void GalileoE1PcpsAmbiguousAcquisition::set_threshold(float threshold) void GalileoE1PcpsAmbiguousAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_+ boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0); float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0);
if(pfa == 0.0) pfa = configuration_->property(role_ + ".pfa", 0.0); if (pfa == 0.0) pfa = configuration_->property(role_ + ".pfa", 0.0);
if(pfa == 0.0) if (pfa == 0.0)
{ {
threshold_ = threshold; threshold_ = threshold;
} }
@ -192,28 +194,27 @@ void GalileoE1PcpsAmbiguousAcquisition::init()
void GalileoE1PcpsAmbiguousAcquisition::set_local_code() void GalileoE1PcpsAmbiguousAcquisition::set_local_code()
{ {
bool cboc = configuration_->property( bool cboc = configuration_->property(
"Acquisition" + boost::lexical_cast<std::string>(channel_) "Acquisition" + boost::lexical_cast<std::string>(channel_) + ".cboc", false);
+ ".cboc", false);
std::complex<float> * code = new std::complex<float>[code_length_]; std::complex<float>* code = new std::complex<float>[code_length_];
if (acquire_pilot_ == true) if (acquire_pilot_ == true)
{ {
//set local signal generator to Galileo E1 pilot component (1C) //set local signal generator to Galileo E1 pilot component (1C)
char pilot_signal[3] = "1C"; char pilot_signal[3] = "1C";
galileo_e1_code_gen_complex_sampled(code, pilot_signal, galileo_e1_code_gen_complex_sampled(code, pilot_signal,
cboc, gnss_synchro_->PRN, fs_in_, 0, false); cboc, gnss_synchro_->PRN, fs_in_, 0, false);
} }
else else
{ {
galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal,
cboc, gnss_synchro_->PRN, fs_in_, 0, false); cboc, gnss_synchro_->PRN, fs_in_, 0, false);
} }
for (unsigned int i = 0; i < sampled_ms_ / 4; i++) for (unsigned int i = 0; i < sampled_ms_ / 4; i++)
{ {
memcpy(&(code_[i*code_length_]), code, sizeof(gr_complex)*code_length_); memcpy(&(code_[i * code_length_]), code, sizeof(gr_complex) * code_length_);
} }
acquisition_->set_local_code(code_); acquisition_->set_local_code(code_);
@ -241,14 +242,14 @@ float GalileoE1PcpsAmbiguousAcquisition::calculate_threshold(float pfa)
frequency_bins++; frequency_bins++;
} }
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa; DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
unsigned int ncells = vector_length_ * frequency_bins; unsigned int ncells = vector_length_ * frequency_bins;
double exponent = 1 / static_cast<double>(ncells); double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0 - pfa,exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist(lambda);
float threshold = static_cast<float>(quantile(mydist,val)); float threshold = static_cast<float>(quantile(mydist, val));
return threshold; return threshold;
} }
@ -330,4 +331,3 @@ gr::basic_block_sptr GalileoE1PcpsAmbiguousAcquisition::get_right_block()
{ {
return acquisition_; return acquisition_;
} }

10
src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h
View File

@ -48,12 +48,12 @@ class ConfigurationInterface;
* \brief This class adapts a PCPS acquisition block to an * \brief This class adapts a PCPS acquisition block to an
* AcquisitionInterface for Galileo E1 Signals * AcquisitionInterface for Galileo E1 Signals
*/ */
class GalileoE1PcpsAmbiguousAcquisition: public AcquisitionInterface class GalileoE1PcpsAmbiguousAcquisition : public AcquisitionInterface
{ {
public: public:
GalileoE1PcpsAmbiguousAcquisition(ConfigurationInterface* configuration, GalileoE1PcpsAmbiguousAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GalileoE1PcpsAmbiguousAcquisition(); virtual ~GalileoE1PcpsAmbiguousAcquisition();
@ -156,8 +156,8 @@ private:
bool dump_; bool dump_;
bool blocking_; bool blocking_;
std::string dump_filename_; std::string dump_filename_;
std::complex<float> * code_; std::complex<float>* code_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

45
src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.cc
View File

@ -41,9 +41,8 @@
using google::LogMessage; using google::LogMessage;
GalileoE1PcpsCccwsrAmbiguousAcquisition::GalileoE1PcpsCccwsrAmbiguousAcquisition( GalileoE1PcpsCccwsrAmbiguousAcquisition::GalileoE1PcpsCccwsrAmbiguousAcquisition(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
@ -58,47 +57,45 @@ GalileoE1PcpsCccwsrAmbiguousAcquisition::GalileoE1PcpsCccwsrAmbiguousAcquisition
if_ = configuration_->property(role + ".if", 0); if_ = configuration_->property(role + ".if", 0);
dump_ = configuration_->property(role + ".dump", false); dump_ = configuration_->property(role + ".dump", false);
doppler_max_ = configuration_->property(role + ".doppler_max", 5000); doppler_max_ = configuration_->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 4); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 4);
if (sampled_ms_ % 4 != 0) if (sampled_ms_ % 4 != 0)
{ {
sampled_ms_ = static_cast<int>(sampled_ms_ / 4) * 4; sampled_ms_ = static_cast<int>(sampled_ms_ / 4) * 4;
LOG(WARNING) << "coherent_integration_time should be multiple of " LOG(WARNING) << "coherent_integration_time should be multiple of "
<< "Galileo code length (4 ms). coherent_integration_time = " << "Galileo code length (4 ms). coherent_integration_time = "
<< sampled_ms_ << " ms will be used."; << sampled_ms_ << " ms will be used.";
} }
max_dwells_ = configuration_->property(role + ".max_dwells", 1); max_dwells_ = configuration_->property(role + ".max_dwells", 1);
dump_filename_ = configuration_->property(role + ".dump_filename", dump_filename_ = configuration_->property(role + ".dump_filename",
default_dump_filename); default_dump_filename);
//--- Find number of samples per spreading code (4 ms) ----------------- //--- Find number of samples per spreading code (4 ms) -----------------
code_length_ = round( code_length_ = round(
fs_in_ fs_in_ / (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS));
/ (Galileo_E1_CODE_CHIP_RATE_HZ
/ Galileo_E1_B_CODE_LENGTH_CHIPS));
vector_length_ = code_length_ * static_cast<int>(sampled_ms_ / 4); vector_length_ = code_length_ * static_cast<int>(sampled_ms_ / 4);
int samples_per_ms = code_length_ / 4; int samples_per_ms = code_length_ / 4;
code_data_ = new gr_complex[vector_length_]; code_data_ = new gr_complex[vector_length_];
code_pilot_ = new gr_complex[vector_length_]; code_pilot_ = new gr_complex[vector_length_];
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
acquisition_cc_ = pcps_cccwsr_make_acquisition_cc(sampled_ms_, max_dwells_, acquisition_cc_ = pcps_cccwsr_make_acquisition_cc(sampled_ms_, max_dwells_,
doppler_max_, if_, fs_in_, samples_per_ms, code_length_, doppler_max_, if_, fs_in_, samples_per_ms, code_length_,
dump_, dump_filename_); dump_, dump_filename_);
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
DLOG(INFO) << "stream_to_vector(" DLOG(INFO) << "stream_to_vector("
<< stream_to_vector_->unique_id() << ")"; << stream_to_vector_->unique_id() << ")";
DLOG(INFO) << "acquisition(" << acquisition_cc_->unique_id() DLOG(INFO) << "acquisition(" << acquisition_cc_->unique_id()
<< ")"; << ")";
} }
else else
{ {
@ -147,7 +144,7 @@ void GalileoE1PcpsCccwsrAmbiguousAcquisition::set_threshold(float threshold)
threshold_ = threshold; threshold_ = threshold;
DLOG(INFO) <<"Channel "<<channel_<<" Threshold = " << threshold_; DLOG(INFO) << "Channel " << channel_ << " Threshold = " << threshold_;
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
@ -177,7 +174,7 @@ void GalileoE1PcpsCccwsrAmbiguousAcquisition::set_doppler_step(unsigned int dopp
} }
void GalileoE1PcpsCccwsrAmbiguousAcquisition::set_gnss_synchro( void GalileoE1PcpsCccwsrAmbiguousAcquisition::set_gnss_synchro(
Gnss_Synchro* gnss_synchro) Gnss_Synchro* gnss_synchro)
{ {
gnss_synchro_ = gnss_synchro; gnss_synchro_ = gnss_synchro;
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
@ -212,20 +209,19 @@ void GalileoE1PcpsCccwsrAmbiguousAcquisition::set_local_code()
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
bool cboc = configuration_->property( bool cboc = configuration_->property(
"Acquisition" + boost::lexical_cast<std::string>(channel_) "Acquisition" + boost::lexical_cast<std::string>(channel_) + ".cboc", false);
+ ".cboc", false);
char signal[3]; char signal[3];
strcpy(signal, "1B"); strcpy(signal, "1B");
galileo_e1_code_gen_complex_sampled(code_data_, signal, galileo_e1_code_gen_complex_sampled(code_data_, signal,
cboc, gnss_synchro_->PRN, fs_in_, 0, false); cboc, gnss_synchro_->PRN, fs_in_, 0, false);
strcpy(signal, "1C"); strcpy(signal, "1C");
galileo_e1_code_gen_complex_sampled(code_pilot_, signal, galileo_e1_code_gen_complex_sampled(code_pilot_, signal,
cboc, gnss_synchro_->PRN, fs_in_, 0, false); cboc, gnss_synchro_->PRN, fs_in_, 0, false);
acquisition_cc_->set_local_code(code_data_, code_pilot_); acquisition_cc_->set_local_code(code_data_, code_pilot_);
} }
@ -246,10 +242,11 @@ void GalileoE1PcpsCccwsrAmbiguousAcquisition::set_state(int state)
} }
float GalileoE1PcpsCccwsrAmbiguousAcquisition::calculate_threshold(float pfa) float GalileoE1PcpsCccwsrAmbiguousAcquisition::calculate_threshold(float pfa)
{ {
if(pfa){ /* Not implemented*/}; if (pfa)
{ /* Not implemented*/
};
return 0.0; return 0.0;
} }
@ -260,7 +257,6 @@ void GalileoE1PcpsCccwsrAmbiguousAcquisition::connect(gr::top_block_sptr top_blo
{ {
top_block->connect(stream_to_vector_, 0, acquisition_cc_, 0); top_block->connect(stream_to_vector_, 0, acquisition_cc_, 0);
} }
} }
@ -283,4 +279,3 @@ gr::basic_block_sptr GalileoE1PcpsCccwsrAmbiguousAcquisition::get_right_block()
{ {
return acquisition_cc_; return acquisition_cc_;
} }

12
src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.h
View File

@ -45,12 +45,12 @@ class ConfigurationInterface;
* \brief Adapts a PCPS CCCWSR acquisition block to an AcquisitionInterface * \brief Adapts a PCPS CCCWSR acquisition block to an AcquisitionInterface
* for Galileo E1 Signals * for Galileo E1 Signals
*/ */
class GalileoE1PcpsCccwsrAmbiguousAcquisition: public AcquisitionInterface class GalileoE1PcpsCccwsrAmbiguousAcquisition : public AcquisitionInterface
{ {
public: public:
GalileoE1PcpsCccwsrAmbiguousAcquisition(ConfigurationInterface* configuration, GalileoE1PcpsCccwsrAmbiguousAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GalileoE1PcpsCccwsrAmbiguousAcquisition(); virtual ~GalileoE1PcpsCccwsrAmbiguousAcquisition();
@ -145,9 +145,9 @@ private:
long if_; long if_;
bool dump_; bool dump_;
std::string dump_filename_; std::string dump_filename_;
std::complex<float> * code_data_; std::complex<float>* code_data_;
std::complex<float> * code_pilot_; std::complex<float>* code_pilot_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

122
src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.cc
View File

@ -41,9 +41,8 @@
using google::LogMessage; using google::LogMessage;
GalileoE1PcpsQuickSyncAmbiguousAcquisition::GalileoE1PcpsQuickSyncAmbiguousAcquisition( GalileoE1PcpsQuickSyncAmbiguousAcquisition::GalileoE1PcpsQuickSyncAmbiguousAcquisition(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
@ -52,21 +51,19 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::GalileoE1PcpsQuickSyncAmbiguousAcqui
DLOG(INFO) << "role " << role; DLOG(INFO) << "role " << role;
item_type_ = configuration_->property(role + ".item_type", item_type_ = configuration_->property(role + ".item_type",
default_item_type); default_item_type);
long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 4000000); long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 4000000);
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
if_ = configuration_->property(role + ".if", 0); if_ = configuration_->property(role + ".if", 0);
dump_ = configuration_->property(role + ".dump", false); dump_ = configuration_->property(role + ".dump", false);
doppler_max_ = configuration_->property(role + ".doppler_max", 5000); doppler_max_ = configuration_->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 8); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 8);
/*--- Find number of samples per spreading code (4 ms) -----------------*/ /*--- Find number of samples per spreading code (4 ms) -----------------*/
code_length_ = round( code_length_ = round(
fs_in_ fs_in_ / (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS));
/ (Galileo_E1_CODE_CHIP_RATE_HZ
/ Galileo_E1_B_CODE_LENGTH_CHIPS));
int samples_per_ms = round(code_length_ / 4.0); int samples_per_ms = round(code_length_ / 4.0);
@ -79,24 +76,23 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::GalileoE1PcpsQuickSyncAmbiguousAcqui
//folding_factor_ = static_cast<unsigned int>(ceil(sqrt(log2(code_length_)))); //folding_factor_ = static_cast<unsigned int>(ceil(sqrt(log2(code_length_))));
folding_factor_ = configuration_->property(role + ".folding_factor", 2); folding_factor_ = configuration_->property(role + ".folding_factor", 2);
if (sampled_ms_ % (folding_factor_*4) != 0) if (sampled_ms_ % (folding_factor_ * 4) != 0)
{ {
LOG(WARNING) << "QuickSync Algorithm requires a coherent_integration_time" LOG(WARNING) << "QuickSync Algorithm requires a coherent_integration_time"
<< " multiple of "<<(folding_factor_*4)<<"ms, Value entered " << " multiple of " << (folding_factor_ * 4) << "ms, Value entered "
<<sampled_ms_<<" ms"; << sampled_ms_ << " ms";
if(sampled_ms_ < (folding_factor_*4)) if (sampled_ms_ < (folding_factor_ * 4))
{ {
sampled_ms_ = static_cast<int>(folding_factor_ * 4); sampled_ms_ = static_cast<int>(folding_factor_ * 4);
} }
else else
{ {
sampled_ms_ = static_cast<int>(sampled_ms_/(folding_factor_*4)) * (folding_factor_*4); sampled_ms_ = static_cast<int>(sampled_ms_ / (folding_factor_ * 4)) * (folding_factor_ * 4);
} }
LOG(WARNING) << "coherent_integration_time should be multiple of " LOG(WARNING) << "coherent_integration_time should be multiple of "
<< "Galileo code length (4 ms). coherent_integration_time = " << "Galileo code length (4 ms). coherent_integration_time = "
<< sampled_ms_ << " ms will be used."; << sampled_ms_ << " ms will be used.";
} }
// vector_length_ = (sampled_ms_/folding_factor_) * code_length_; // vector_length_ = (sampled_ms_/folding_factor_) * code_length_;
vector_length_ = sampled_ms_ * samples_per_ms; vector_length_ = sampled_ms_ * samples_per_ms;
@ -112,27 +108,27 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::GalileoE1PcpsQuickSyncAmbiguousAcqui
} }
dump_filename_ = configuration_->property(role + ".dump_filename", dump_filename_ = configuration_->property(role + ".dump_filename",
default_dump_filename); default_dump_filename);
code_ = new gr_complex[code_length_]; code_ = new gr_complex[code_length_];
LOG(INFO) << "Vector Length: " << vector_length_ LOG(INFO) << "Vector Length: " << vector_length_
<< ", Samples per ms: " << samples_per_ms << ", Samples per ms: " << samples_per_ms
<< ", Folding factor: " << folding_factor_ << ", Folding factor: " << folding_factor_
<< ", Sampled ms: " << sampled_ms_ << ", Sampled ms: " << sampled_ms_
<< ", Code Length: " << code_length_; << ", Code Length: " << code_length_;
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
acquisition_cc_ = pcps_quicksync_make_acquisition_cc(folding_factor_, acquisition_cc_ = pcps_quicksync_make_acquisition_cc(folding_factor_,
sampled_ms_, max_dwells_, doppler_max_, if_, fs_in_, sampled_ms_, max_dwells_, doppler_max_, if_, fs_in_,
samples_per_ms, code_length_, bit_transition_flag_, samples_per_ms, code_length_, bit_transition_flag_,
dump_, dump_filename_); dump_, dump_filename_);
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_,
vector_length_); vector_length_);
DLOG(INFO) << "stream_to_vector_quicksync(" DLOG(INFO) << "stream_to_vector_quicksync("
<< stream_to_vector_->unique_id() << ")"; << stream_to_vector_->unique_id() << ")";
DLOG(INFO) << "acquisition_quicksync(" << acquisition_cc_->unique_id() DLOG(INFO) << "acquisition_quicksync(" << acquisition_cc_->unique_id()
<< ")"; << ")";
} }
else else
{ {
@ -153,8 +149,7 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::~GalileoE1PcpsQuickSyncAmbiguousAcqu
} }
void void GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_channel(unsigned int channel)
GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_channel(unsigned int channel)
{ {
channel_ = channel; channel_ = channel;
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
@ -164,15 +159,13 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_channel(unsigned int channel)
} }
void void GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_threshold(float threshold)
GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0);
float pfa = configuration_->property(role_+ boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0); if (pfa == 0.0) pfa = configuration_->property(role_ + ".pfa", 0.0);
if(pfa==0.0) pfa = configuration_->property(role_+".pfa", 0.0); if (pfa == 0.0)
if(pfa==0.0)
{ {
threshold_ = threshold; threshold_ = threshold;
} }
@ -181,7 +174,7 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_threshold(float threshold)
threshold_ = calculate_threshold(pfa); threshold_ = calculate_threshold(pfa);
} }
DLOG(INFO) <<"Channel "<<channel_<<" Threshold = " << threshold_; DLOG(INFO) << "Channel " << channel_ << " Threshold = " << threshold_;
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
@ -190,8 +183,7 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_threshold(float threshold)
} }
void void GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_doppler_max(unsigned int doppler_max)
GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_doppler_max(unsigned int doppler_max)
{ {
doppler_max_ = doppler_max; doppler_max_ = doppler_max;
@ -202,8 +194,7 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_doppler_max(unsigned int doppler
} }
void void GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_doppler_step(unsigned int doppler_step)
GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_doppler_step(unsigned int doppler_step)
{ {
doppler_step_ = doppler_step; doppler_step_ = doppler_step;
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
@ -212,9 +203,8 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_doppler_step(unsigned int dopple
} }
} }
void void GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_gnss_synchro(
GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_gnss_synchro( Gnss_Synchro* gnss_synchro)
Gnss_Synchro* gnss_synchro)
{ {
gnss_synchro_ = gnss_synchro; gnss_synchro_ = gnss_synchro;
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
@ -238,36 +228,33 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::mag()
} }
void void GalileoE1PcpsQuickSyncAmbiguousAcquisition::init()
GalileoE1PcpsQuickSyncAmbiguousAcquisition::init()
{ {
acquisition_cc_->init(); acquisition_cc_->init();
//set_local_code(); //set_local_code();
} }
void void GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_local_code()
GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_local_code()
{ {
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
bool cboc = configuration_->property( bool cboc = configuration_->property(
"Acquisition" + boost::lexical_cast<std::string>(channel_) "Acquisition" + boost::lexical_cast<std::string>(channel_) + ".cboc", false);
+ ".cboc", false);
std::complex<float> * code = new std::complex<float>[code_length_]; std::complex<float>* code = new std::complex<float>[code_length_];
galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal,
cboc, gnss_synchro_->PRN, fs_in_, 0, false); cboc, gnss_synchro_->PRN, fs_in_, 0, false);
for (unsigned int i = 0; i < (sampled_ms_/(folding_factor_*4)); i++) for (unsigned int i = 0; i < (sampled_ms_ / (folding_factor_ * 4)); i++)
{ {
memcpy(&(code_[i*code_length_]), code, memcpy(&(code_[i * code_length_]), code,
sizeof(gr_complex)*code_length_); sizeof(gr_complex) * code_length_);
} }
// memcpy(code_, code,sizeof(gr_complex)*code_length_); // memcpy(code_, code,sizeof(gr_complex)*code_length_);
acquisition_cc_->set_local_code(code_); acquisition_cc_->set_local_code(code_);
delete[] code; delete[] code;
@ -276,8 +263,7 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_local_code()
} }
void void GalileoE1PcpsQuickSyncAmbiguousAcquisition::reset()
GalileoE1PcpsQuickSyncAmbiguousAcquisition::reset()
{ {
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
@ -288,13 +274,12 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::reset()
void GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_state(int state) void GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_state(int state)
{ {
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
acquisition_cc_->set_state(state); acquisition_cc_->set_state(state);
} }
} }
float GalileoE1PcpsQuickSyncAmbiguousAcquisition::calculate_threshold(float pfa) float GalileoE1PcpsQuickSyncAmbiguousAcquisition::calculate_threshold(float pfa)
{ {
unsigned int frequency_bins = 0; unsigned int frequency_bins = 0;
@ -309,15 +294,14 @@ float GalileoE1PcpsQuickSyncAmbiguousAcquisition::calculate_threshold(float pfa)
double exponent = 1.0 / static_cast<double>(ncells); double exponent = 1.0 / static_cast<double>(ncells);
double val = pow(1.0 - pfa, exponent); double val = pow(1.0 - pfa, exponent);
double lambda = static_cast<double>(code_length_) / static_cast<double>(folding_factor_); double lambda = static_cast<double>(code_length_) / static_cast<double>(folding_factor_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist(lambda);
float threshold = static_cast<float>(quantile(mydist,val)); float threshold = static_cast<float>(quantile(mydist, val));
return threshold; return threshold;
} }
void void GalileoE1PcpsQuickSyncAmbiguousAcquisition::connect(gr::top_block_sptr top_block)
GalileoE1PcpsQuickSyncAmbiguousAcquisition::connect(gr::top_block_sptr top_block)
{ {
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
@ -326,8 +310,7 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::connect(gr::top_block_sptr top_block
} }
void void GalileoE1PcpsQuickSyncAmbiguousAcquisition::disconnect(gr::top_block_sptr top_block)
GalileoE1PcpsQuickSyncAmbiguousAcquisition::disconnect(gr::top_block_sptr top_block)
{ {
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
@ -346,4 +329,3 @@ gr::basic_block_sptr GalileoE1PcpsQuickSyncAmbiguousAcquisition::get_right_block
{ {
return acquisition_cc_; return acquisition_cc_;
} }

10
src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.h
View File

@ -45,12 +45,12 @@ class ConfigurationInterface;
* \brief This class adapts a PCPS acquisition block to an * \brief This class adapts a PCPS acquisition block to an
* AcquisitionInterface for Galileo E1 Signals * AcquisitionInterface for Galileo E1 Signals
*/ */
class GalileoE1PcpsQuickSyncAmbiguousAcquisition: public AcquisitionInterface class GalileoE1PcpsQuickSyncAmbiguousAcquisition : public AcquisitionInterface
{ {
public: public:
GalileoE1PcpsQuickSyncAmbiguousAcquisition(ConfigurationInterface* configuration, GalileoE1PcpsQuickSyncAmbiguousAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GalileoE1PcpsQuickSyncAmbiguousAcquisition(); virtual ~GalileoE1PcpsQuickSyncAmbiguousAcquisition();
@ -149,8 +149,8 @@ private:
long if_; long if_;
bool dump_; bool dump_;
std::string dump_filename_; std::string dump_filename_;
std::complex<float> * code_; std::complex<float>* code_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

59
src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.cc
View File

@ -41,9 +41,8 @@
using google::LogMessage; using google::LogMessage;
GalileoE1PcpsTongAmbiguousAcquisition::GalileoE1PcpsTongAmbiguousAcquisition( GalileoE1PcpsTongAmbiguousAcquisition::GalileoE1PcpsTongAmbiguousAcquisition(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
@ -52,22 +51,22 @@ GalileoE1PcpsTongAmbiguousAcquisition::GalileoE1PcpsTongAmbiguousAcquisition(
DLOG(INFO) << "role " << role; DLOG(INFO) << "role " << role;
item_type_ = configuration_->property(role + ".item_type", item_type_ = configuration_->property(role + ".item_type",
default_item_type); default_item_type);
long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 4000000); long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 4000000);
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
if_ = configuration_->property(role + ".if", 0); if_ = configuration_->property(role + ".if", 0);
dump_ = configuration_->property(role + ".dump", false); dump_ = configuration_->property(role + ".dump", false);
doppler_max_ = configuration_->property(role + ".doppler_max", 5000); doppler_max_ = configuration_->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 4); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 4);
if (sampled_ms_ % 4 != 0) if (sampled_ms_ % 4 != 0)
{ {
sampled_ms_ = static_cast<int>(sampled_ms_ / 4) * 4; sampled_ms_ = static_cast<int>(sampled_ms_ / 4) * 4;
LOG(WARNING) << "coherent_integration_time should be multiple of " LOG(WARNING) << "coherent_integration_time should be multiple of "
<< "Galileo code length (4 ms). coherent_integration_time = " << "Galileo code length (4 ms). coherent_integration_time = "
<< sampled_ms_ << " ms will be used."; << sampled_ms_ << " ms will be used.";
} }
tong_init_val_ = configuration->property(role + ".tong_init_val", 1); tong_init_val_ = configuration->property(role + ".tong_init_val", 1);
@ -75,14 +74,12 @@ GalileoE1PcpsTongAmbiguousAcquisition::GalileoE1PcpsTongAmbiguousAcquisition(
tong_max_dwells_ = configuration->property(role + ".tong_max_dwells", tong_max_val_ + 1); tong_max_dwells_ = configuration->property(role + ".tong_max_dwells", tong_max_val_ + 1);
dump_filename_ = configuration_->property(role + ".dump_filename", dump_filename_ = configuration_->property(role + ".dump_filename",
default_dump_filename); default_dump_filename);
//--- Find number of samples per spreading code (4 ms) ----------------- //--- Find number of samples per spreading code (4 ms) -----------------
code_length_ = round( code_length_ = round(
fs_in_ fs_in_ / (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS));
/ (Galileo_E1_CODE_CHIP_RATE_HZ
/ Galileo_E1_B_CODE_LENGTH_CHIPS));
vector_length_ = code_length_ * static_cast<int>(sampled_ms_ / 4); vector_length_ = code_length_ * static_cast<int>(sampled_ms_ / 4);
@ -94,14 +91,14 @@ GalileoE1PcpsTongAmbiguousAcquisition::GalileoE1PcpsTongAmbiguousAcquisition(
{ {
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
acquisition_cc_ = pcps_tong_make_acquisition_cc(sampled_ms_, doppler_max_, acquisition_cc_ = pcps_tong_make_acquisition_cc(sampled_ms_, doppler_max_,
if_, fs_in_, samples_per_ms, code_length_, tong_init_val_, if_, fs_in_, samples_per_ms, code_length_, tong_init_val_,
tong_max_val_, tong_max_dwells_, dump_, dump_filename_); tong_max_val_, tong_max_dwells_, dump_, dump_filename_);
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
DLOG(INFO) << "stream_to_vector(" DLOG(INFO) << "stream_to_vector("
<< stream_to_vector_->unique_id() << ")"; << stream_to_vector_->unique_id() << ")";
DLOG(INFO) << "acquisition(" << acquisition_cc_->unique_id() DLOG(INFO) << "acquisition(" << acquisition_cc_->unique_id()
<< ")"; << ")";
} }
else else
{ {
@ -134,12 +131,11 @@ void GalileoE1PcpsTongAmbiguousAcquisition::set_channel(unsigned int channel)
void GalileoE1PcpsTongAmbiguousAcquisition::set_threshold(float threshold) void GalileoE1PcpsTongAmbiguousAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0);
float pfa = configuration_->property(role_+ boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0); if (pfa == 0.0) pfa = configuration_->property(role_ + ".pfa", 0.0);
if(pfa == 0.0) pfa = configuration_->property(role_+".pfa", 0.0); if (pfa == 0.0)
if(pfa == 0.0)
{ {
threshold_ = threshold; threshold_ = threshold;
} }
@ -148,7 +144,7 @@ void GalileoE1PcpsTongAmbiguousAcquisition::set_threshold(float threshold)
threshold_ = calculate_threshold(pfa); threshold_ = calculate_threshold(pfa);
} }
DLOG(INFO) <<"Channel "<<channel_<<" Threshold = " << threshold_; DLOG(INFO) << "Channel " << channel_ << " Threshold = " << threshold_;
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
@ -175,12 +171,11 @@ void GalileoE1PcpsTongAmbiguousAcquisition::set_doppler_step(unsigned int dopple
{ {
acquisition_cc_->set_doppler_step(doppler_step_); acquisition_cc_->set_doppler_step(doppler_step_);
} }
} }
void GalileoE1PcpsTongAmbiguousAcquisition::set_gnss_synchro( void GalileoE1PcpsTongAmbiguousAcquisition::set_gnss_synchro(
Gnss_Synchro* gnss_synchro) Gnss_Synchro* gnss_synchro)
{ {
gnss_synchro_ = gnss_synchro; gnss_synchro_ = gnss_synchro;
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
@ -215,18 +210,17 @@ void GalileoE1PcpsTongAmbiguousAcquisition::set_local_code()
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
bool cboc = configuration_->property( bool cboc = configuration_->property(
"Acquisition" + boost::lexical_cast<std::string>(channel_) "Acquisition" + boost::lexical_cast<std::string>(channel_) + ".cboc", false);
+ ".cboc", false);
std::complex<float> * code = new std::complex<float>[code_length_]; std::complex<float>* code = new std::complex<float>[code_length_];
galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal,
cboc, gnss_synchro_->PRN, fs_in_, 0, false); cboc, gnss_synchro_->PRN, fs_in_, 0, false);
for (unsigned int i = 0; i < sampled_ms_/4; i++) for (unsigned int i = 0; i < sampled_ms_ / 4; i++)
{ {
memcpy(&(code_[i*code_length_]), code, memcpy(&(code_[i * code_length_]), code,
sizeof(gr_complex)*code_length_); sizeof(gr_complex) * code_length_);
} }
acquisition_cc_->set_local_code(code_); acquisition_cc_->set_local_code(code_);
@ -262,10 +256,10 @@ float GalileoE1PcpsTongAmbiguousAcquisition::calculate_threshold(float pfa)
unsigned int ncells = vector_length_ * frequency_bins; unsigned int ncells = vector_length_ * frequency_bins;
double exponent = 1 / static_cast<double>(ncells); double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0-pfa,exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist(lambda);
float threshold = static_cast<float>(quantile(mydist,val)); float threshold = static_cast<float>(quantile(mydist, val));
return threshold; return threshold;
} }
@ -299,4 +293,3 @@ gr::basic_block_sptr GalileoE1PcpsTongAmbiguousAcquisition::get_right_block()
{ {
return acquisition_cc_; return acquisition_cc_;
} }

10
src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.h
View File

@ -45,12 +45,12 @@ class ConfigurationInterface;
* \brief Adapts a PCPS Tong acquisition block to an AcquisitionInterface * \brief Adapts a PCPS Tong acquisition block to an AcquisitionInterface
* for Galileo E1 Signals * for Galileo E1 Signals
*/ */
class GalileoE1PcpsTongAmbiguousAcquisition: public AcquisitionInterface class GalileoE1PcpsTongAmbiguousAcquisition : public AcquisitionInterface
{ {
public: public:
GalileoE1PcpsTongAmbiguousAcquisition(ConfigurationInterface* configuration, GalileoE1PcpsTongAmbiguousAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GalileoE1PcpsTongAmbiguousAcquisition(); virtual ~GalileoE1PcpsTongAmbiguousAcquisition();
@ -149,8 +149,8 @@ private:
long if_; long if_;
bool dump_; bool dump_;
std::string dump_filename_; std::string dump_filename_;
std::complex<float> * code_; std::complex<float>* code_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

70
src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.cc
View File

@ -47,9 +47,8 @@
using google::LogMessage; using google::LogMessage;
GalileoE5aNoncoherentIQAcquisitionCaf::GalileoE5aNoncoherentIQAcquisitionCaf( GalileoE5aNoncoherentIQAcquisitionCaf::GalileoE5aNoncoherentIQAcquisitionCaf(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
@ -64,9 +63,9 @@ GalileoE5aNoncoherentIQAcquisitionCaf::GalileoE5aNoncoherentIQAcquisitionCaf(
if_ = configuration_->property(role + ".if", 0); if_ = configuration_->property(role + ".if", 0);
dump_ = configuration_->property(role + ".dump", false); dump_ = configuration_->property(role + ".dump", false);
doppler_max_ = configuration_->property(role + ".doppler_max", 5000); doppler_max_ = configuration_->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
CAF_window_hz_ = configuration_->property(role + ".CAF_window_hz",0); CAF_window_hz_ = configuration_->property(role + ".CAF_window_hz", 0);
Zero_padding = configuration_->property(role + ".Zero_padding",0); Zero_padding = configuration_->property(role + ".Zero_padding", 0);
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1);
if (sampled_ms_ > 3) if (sampled_ms_ > 3)
{ {
@ -90,8 +89,8 @@ GalileoE5aNoncoherentIQAcquisitionCaf::GalileoE5aNoncoherentIQAcquisitionCaf(
vector_length_ = code_length_ * sampled_ms_; vector_length_ = code_length_ * sampled_ms_;
codeI_= new gr_complex[vector_length_]; codeI_ = new gr_complex[vector_length_];
codeQ_= new gr_complex[vector_length_]; codeQ_ = new gr_complex[vector_length_];
both_signal_components = false; both_signal_components = false;
std::string sig_ = configuration_->property("Channel.signal", std::string("5X")); std::string sig_ = configuration_->property("Channel.signal", std::string("5X"));
@ -103,13 +102,13 @@ GalileoE5aNoncoherentIQAcquisitionCaf::GalileoE5aNoncoherentIQAcquisitionCaf(
{ {
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
acquisition_cc_ = galileo_e5a_noncoherentIQ_make_acquisition_caf_cc(sampled_ms_, max_dwells_, acquisition_cc_ = galileo_e5a_noncoherentIQ_make_acquisition_caf_cc(sampled_ms_, max_dwells_,
doppler_max_, if_, fs_in_, code_length_, code_length_, bit_transition_flag_, doppler_max_, if_, fs_in_, code_length_, code_length_, bit_transition_flag_,
dump_, dump_filename_, both_signal_components, CAF_window_hz_,Zero_padding); dump_, dump_filename_, both_signal_components, CAF_window_hz_, Zero_padding);
} }
else else
{ {
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
LOG(WARNING) << item_type_ << " unknown acquisition item type"; LOG(WARNING) << item_type_ << " unknown acquisition item type";
} }
channel_ = 0; channel_ = 0;
@ -138,12 +137,11 @@ void GalileoE5aNoncoherentIQAcquisitionCaf::set_channel(unsigned int channel)
void GalileoE5aNoncoherentIQAcquisitionCaf::set_threshold(float threshold) void GalileoE5aNoncoherentIQAcquisitionCaf::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0);
float pfa = configuration_->property(role_+ boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0); if (pfa == 0.0) pfa = configuration_->property(role_ + ".pfa", 0.0);
if(pfa == 0.0) pfa = configuration_->property(role_ + ".pfa", 0.0); if (pfa == 0.0)
if(pfa == 0.0)
{ {
threshold_ = threshold; threshold_ = threshold;
} }
@ -183,7 +181,7 @@ void GalileoE5aNoncoherentIQAcquisitionCaf::set_doppler_step(unsigned int dopple
void GalileoE5aNoncoherentIQAcquisitionCaf::set_gnss_synchro( void GalileoE5aNoncoherentIQAcquisitionCaf::set_gnss_synchro(
Gnss_Synchro* gnss_synchro) Gnss_Synchro* gnss_synchro)
{ {
gnss_synchro_ = gnss_synchro; gnss_synchro_ = gnss_synchro;
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
@ -223,31 +221,31 @@ void GalileoE5aNoncoherentIQAcquisitionCaf::set_local_code()
if (gnss_synchro_->Signal[0] == '5' && gnss_synchro_->Signal[1] == 'X') if (gnss_synchro_->Signal[0] == '5' && gnss_synchro_->Signal[1] == 'X')
{ {
char a[3]; char a[3];
strcpy(a,"5I"); strcpy(a, "5I");
galileo_e5_a_code_gen_complex_sampled(codeI, a, galileo_e5_a_code_gen_complex_sampled(codeI, a,
gnss_synchro_->PRN, fs_in_, 0); gnss_synchro_->PRN, fs_in_, 0);
strcpy(a,"5Q"); strcpy(a, "5Q");
galileo_e5_a_code_gen_complex_sampled(codeQ, a, galileo_e5_a_code_gen_complex_sampled(codeQ, a,
gnss_synchro_->PRN, fs_in_, 0); gnss_synchro_->PRN, fs_in_, 0);
} }
else else
{ {
galileo_e5_a_code_gen_complex_sampled(codeI, gnss_synchro_->Signal, galileo_e5_a_code_gen_complex_sampled(codeI, gnss_synchro_->Signal,
gnss_synchro_->PRN, fs_in_, 0); gnss_synchro_->PRN, fs_in_, 0);
} }
// WARNING: 3ms are coherently integrated. Secondary sequence (1,1,1) // WARNING: 3ms are coherently integrated. Secondary sequence (1,1,1)
// is generated, and modulated in the 'block'. // is generated, and modulated in the 'block'.
if (Zero_padding == 0) // if no zero_padding if (Zero_padding == 0) // if no zero_padding
{ {
for (unsigned int i = 0; i < sampled_ms_; i++) for (unsigned int i = 0; i < sampled_ms_; i++)
{ {
memcpy(&(codeI_[i*code_length_]), codeI, memcpy(&(codeI_[i * code_length_]), codeI,
sizeof(gr_complex)*code_length_); sizeof(gr_complex) * code_length_);
if (gnss_synchro_->Signal[0] == '5' && gnss_synchro_->Signal[1] == 'X') if (gnss_synchro_->Signal[0] == '5' && gnss_synchro_->Signal[1] == 'X')
{ {
memcpy(&(codeQ_[i*code_length_]), codeQ, memcpy(&(codeQ_[i * code_length_]), codeQ,
sizeof(gr_complex)*code_length_); sizeof(gr_complex) * code_length_);
} }
} }
} }
@ -255,20 +253,18 @@ void GalileoE5aNoncoherentIQAcquisitionCaf::set_local_code()
{ {
// 1ms code + 1ms zero padding // 1ms code + 1ms zero padding
memcpy(&(codeI_[0]), codeI, memcpy(&(codeI_[0]), codeI,
sizeof(gr_complex)*code_length_); sizeof(gr_complex) * code_length_);
if (gnss_synchro_->Signal[0] == '5' && gnss_synchro_->Signal[1] == 'X') if (gnss_synchro_->Signal[0] == '5' && gnss_synchro_->Signal[1] == 'X')
{ {
memcpy(&(codeQ_[0]), codeQ, memcpy(&(codeQ_[0]), codeQ,
sizeof(gr_complex)*code_length_); sizeof(gr_complex) * code_length_);
} }
} }
acquisition_cc_->set_local_code(codeI_,codeQ_); acquisition_cc_->set_local_code(codeI_, codeQ_);
delete[] codeI; delete[] codeI;
delete[] codeQ; delete[] codeQ;
} }
} }
@ -294,8 +290,8 @@ float GalileoE5aNoncoherentIQAcquisitionCaf::calculate_threshold(float pfa)
double exponent = 1 / static_cast<double>(ncells); double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0 - pfa, exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist(lambda);
float threshold = static_cast<float>(quantile(mydist,val)); float threshold = static_cast<float>(quantile(mydist, val));
return threshold; return threshold;
} }
@ -309,14 +305,18 @@ void GalileoE5aNoncoherentIQAcquisitionCaf::set_state(int state)
void GalileoE5aNoncoherentIQAcquisitionCaf::connect(gr::top_block_sptr top_block) void GalileoE5aNoncoherentIQAcquisitionCaf::connect(gr::top_block_sptr top_block)
{ {
if(top_block) { /* top_block is not null */}; if (top_block)
{ /* top_block is not null */
};
// Nothing to connect internally // Nothing to connect internally
} }
void GalileoE5aNoncoherentIQAcquisitionCaf::disconnect(gr::top_block_sptr top_block) void GalileoE5aNoncoherentIQAcquisitionCaf::disconnect(gr::top_block_sptr top_block)
{ {
if(top_block) { /* top_block is not null */}; if (top_block)
{ /* top_block is not null */
};
// Nothing to disconnect internally // Nothing to disconnect internally
} }

12
src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.h
View File

@ -45,12 +45,12 @@
class ConfigurationInterface; class ConfigurationInterface;
class GalileoE5aNoncoherentIQAcquisitionCaf: public AcquisitionInterface class GalileoE5aNoncoherentIQAcquisitionCaf : public AcquisitionInterface
{ {
public: public:
GalileoE5aNoncoherentIQAcquisitionCaf(ConfigurationInterface* configuration, GalileoE5aNoncoherentIQAcquisitionCaf(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GalileoE5aNoncoherentIQAcquisitionCaf(); virtual ~GalileoE5aNoncoherentIQAcquisitionCaf();
@ -151,10 +151,10 @@ private:
std::string dump_filename_; std::string dump_filename_;
int Zero_padding; int Zero_padding;
int CAF_window_hz_; int CAF_window_hz_;
std::complex<float> * codeI_; std::complex<float>* codeI_;
std::complex<float> * codeQ_; std::complex<float>* codeQ_;
bool both_signal_components; bool both_signal_components;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

65
src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc
View File

@ -42,8 +42,7 @@
using google::LogMessage; using google::LogMessage;
GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* configuration, GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, unsigned int out_streams) : std::string role, unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
@ -57,10 +56,13 @@ GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* con
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
acq_pilot_ = configuration_->property(role + ".acquire_pilot", false); acq_pilot_ = configuration_->property(role + ".acquire_pilot", false);
acq_iq_ = configuration_->property(role + ".acquire_iq", false); acq_iq_ = configuration_->property(role + ".acquire_iq", false);
if(acq_iq_) { acq_pilot_ = false; } if (acq_iq_)
{
acq_pilot_ = false;
}
dump_ = configuration_->property(role + ".dump", false); dump_ = configuration_->property(role + ".dump", false);
doppler_max_ = configuration_->property(role + ".doppler_max", 5000); doppler_max_ = configuration_->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1);
max_dwells_ = configuration_->property(role + ".max_dwells", 1); max_dwells_ = configuration_->property(role + ".max_dwells", 1);
dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename); dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename);
@ -74,23 +76,23 @@ GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* con
code_ = new gr_complex[vector_length_]; code_ = new gr_complex[vector_length_];
if(item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
} }
else if(item_type_.compare("cshort") == 0) else if (item_type_.compare("cshort") == 0)
{ {
item_size_ = sizeof(lv_16sc_t); item_size_ = sizeof(lv_16sc_t);
} }
else else
{ {
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
LOG(WARNING) << item_type_ << " unknown acquisition item type"; LOG(WARNING) << item_type_ << " unknown acquisition item type";
} }
acquisition_ = pcps_make_acquisition(sampled_ms_, max_dwells_, doppler_max_, 0, fs_in_, acquisition_ = pcps_make_acquisition(sampled_ms_, max_dwells_, doppler_max_, 0, fs_in_,
code_length_, code_length_, bit_transition_flag_, use_CFAR_, dump_, blocking_, code_length_, code_length_, bit_transition_flag_, use_CFAR_, dump_, blocking_,
dump_filename_, item_size_); dump_filename_, item_size_);
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
channel_ = 0; channel_ = 0;
@ -115,14 +117,22 @@ void GalileoE5aPcpsAcquisition::set_channel(unsigned int channel)
void GalileoE5aPcpsAcquisition::set_threshold(float threshold) void GalileoE5aPcpsAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0);
float pfa = configuration_->property(role_+ boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0); if (pfa == 0.0)
{
pfa = configuration_->property(role_ + ".pfa", 0.0);
}
if(pfa == 0.0) { pfa = configuration_->property(role_ + ".pfa", 0.0); } if (pfa == 0.0)
{
threshold_ = threshold;
}
if(pfa == 0.0) { threshold_ = threshold; } else
{
else { threshold_ = calculate_threshold(pfa); } threshold_ = calculate_threshold(pfa);
}
DLOG(INFO) << "Channel " << channel_ << " Threshold = " << threshold_; DLOG(INFO) << "Channel " << channel_ << " Threshold = " << threshold_;
@ -168,16 +178,25 @@ void GalileoE5aPcpsAcquisition::set_local_code()
gr_complex* code = new gr_complex[code_length_]; gr_complex* code = new gr_complex[code_length_];
char signal_[3]; char signal_[3];
if(acq_iq_) { strcpy(signal_, "5X"); } if (acq_iq_)
else if(acq_pilot_) { strcpy(signal_, "5Q"); } {
else { strcpy(signal_, "5I"); } strcpy(signal_, "5X");
}
else if (acq_pilot_)
{
strcpy(signal_, "5Q");
}
else
{
strcpy(signal_, "5I");
}
galileo_e5_a_code_gen_complex_sampled(code, signal_, gnss_synchro_->PRN, fs_in_, 0); galileo_e5_a_code_gen_complex_sampled(code, signal_, gnss_synchro_->PRN, fs_in_, 0);
for(unsigned int i = 0; i < sampled_ms_; i++) for (unsigned int i = 0; i < sampled_ms_; i++)
{ {
memcpy(code_ + (i * code_length_), code, sizeof(gr_complex) * code_length_); memcpy(code_ + (i * code_length_), code, sizeof(gr_complex) * code_length_);
} }
acquisition_->set_local_code(code_); acquisition_->set_local_code(code_);
delete[] code; delete[] code;
@ -202,8 +221,8 @@ float GalileoE5aPcpsAcquisition::calculate_threshold(float pfa)
double exponent = 1 / static_cast<double>(ncells); double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0 - pfa, exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist(lambda);
float threshold = static_cast<float>(quantile(mydist,val)); float threshold = static_cast<float>(quantile(mydist, val));
return threshold; return threshold;
} }

8
src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.h
View File

@ -40,12 +40,12 @@
class ConfigurationInterface; class ConfigurationInterface;
class GalileoE5aPcpsAcquisition: public AcquisitionInterface class GalileoE5aPcpsAcquisition : public AcquisitionInterface
{ {
public: public:
GalileoE5aPcpsAcquisition(ConfigurationInterface* configuration, GalileoE5aPcpsAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GalileoE5aPcpsAcquisition(); virtual ~GalileoE5aPcpsAcquisition();
@ -124,7 +124,6 @@ public:
void set_state(int state); void set_state(int state);
private: private:
float calculate_threshold(float pfa); float calculate_threshold(float pfa);
ConfigurationInterface* configuration_; ConfigurationInterface* configuration_;
@ -167,6 +166,5 @@ private:
gr_complex* code_; gr_complex* code_;
Gnss_Synchro* gnss_synchro_; Gnss_Synchro* gnss_synchro_;
}; };
#endif /* GALILEO_E5A_PCPS_ACQUISITION_H_ */ #endif /* GALILEO_E5A_PCPS_ACQUISITION_H_ */

31
src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc
View File

@ -43,9 +43,8 @@
using google::LogMessage; using google::LogMessage;
GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition( GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
@ -61,11 +60,11 @@ GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition(
dump_ = configuration_->property(role + ".dump", false); dump_ = configuration_->property(role + ".dump", false);
blocking_ = configuration_->property(role + ".blocking", true); blocking_ = configuration_->property(role + ".blocking", true);
doppler_max_ = configuration_->property(role + ".doppler_max", 5000); doppler_max_ = configuration_->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1);
bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false);
use_CFAR_algorithm_flag_=configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions
max_dwells_ = configuration_->property(role + ".max_dwells", 1); max_dwells_ = configuration_->property(role + ".max_dwells", 1);
@ -76,14 +75,14 @@ GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition(
vector_length_ = code_length_ * sampled_ms_; vector_length_ = code_length_ * sampled_ms_;
if( bit_transition_flag_ ) if (bit_transition_flag_)
{ {
vector_length_ *= 2; vector_length_ *= 2;
} }
code_ = new gr_complex[vector_length_]; code_ = new gr_complex[vector_length_];
if (item_type_.compare("cshort") == 0 ) if (item_type_.compare("cshort") == 0)
{ {
item_size_ = sizeof(lv_16sc_t); item_size_ = sizeof(lv_16sc_t);
} }
@ -92,8 +91,8 @@ GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition(
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
} }
acquisition_ = pcps_make_acquisition(sampled_ms_, max_dwells_, acquisition_ = pcps_make_acquisition(sampled_ms_, max_dwells_,
doppler_max_, if_, fs_in_, code_length_, code_length_, doppler_max_, if_, fs_in_, code_length_, code_length_,
bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_, dump_filename_, item_size_); bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_, dump_filename_, item_size_);
DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")";
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
@ -129,7 +128,7 @@ void GlonassL1CaPcpsAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + ".pfa", 0.0); float pfa = configuration_->property(role_ + ".pfa", 0.0);
if(pfa == 0.0) if (pfa == 0.0)
{ {
threshold_ = threshold; threshold_ = threshold;
} }
@ -186,12 +185,12 @@ void GlonassL1CaPcpsAcquisition::set_local_code()
{ {
std::complex<float>* code = new std::complex<float>[code_length_]; std::complex<float>* code = new std::complex<float>[code_length_];
glonass_l1_ca_code_gen_complex_sampled(code,/* gnss_synchro_->PRN,*/ fs_in_, 0); glonass_l1_ca_code_gen_complex_sampled(code, /* gnss_synchro_->PRN,*/ fs_in_, 0);
for (unsigned int i = 0; i < sampled_ms_; i++) for (unsigned int i = 0; i < sampled_ms_; i++)
{ {
memcpy(&(code_[i*code_length_]), code, memcpy(&(code_[i * code_length_]), code,
sizeof(gr_complex)*code_length_); sizeof(gr_complex) * code_length_);
} }
acquisition_->set_local_code(code_); acquisition_->set_local_code(code_);
@ -222,15 +221,15 @@ float GlonassL1CaPcpsAcquisition::calculate_threshold(float pfa)
} }
*/ */
frequency_bins = (2*doppler_max_ + doppler_step_)/doppler_step_; frequency_bins = (2 * doppler_max_ + doppler_step_) / doppler_step_;
DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa; DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
unsigned int ncells = vector_length_ * frequency_bins; unsigned int ncells = vector_length_ * frequency_bins;
double exponent = 1 / static_cast<double>(ncells); double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0 - pfa, exponent); double val = pow(1.0 - pfa, exponent);
double lambda = static_cast<double>(vector_length_); double lambda = static_cast<double>(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist(lambda);
float threshold = static_cast<float>(quantile(mydist,val)); float threshold = static_cast<float>(quantile(mydist, val));
return threshold; return threshold;
} }

10
src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.h
View File

@ -48,12 +48,12 @@ class ConfigurationInterface;
* \brief This class adapts a PCPS acquisition block to an AcquisitionInterface * \brief This class adapts a PCPS acquisition block to an AcquisitionInterface
* for GPS L1 C/A signals * for GPS L1 C/A signals
*/ */
class GlonassL1CaPcpsAcquisition: public AcquisitionInterface class GlonassL1CaPcpsAcquisition : public AcquisitionInterface
{ {
public: public:
GlonassL1CaPcpsAcquisition(ConfigurationInterface* configuration, GlonassL1CaPcpsAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GlonassL1CaPcpsAcquisition(); virtual ~GlonassL1CaPcpsAcquisition();
@ -155,8 +155,8 @@ private:
bool dump_; bool dump_;
bool blocking_; bool blocking_;
std::string dump_filename_; std::string dump_filename_;
std::complex<float> * code_; std::complex<float>* code_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

30
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc
View File

@ -42,13 +42,11 @@
#include <glog/logging.h> #include <glog/logging.h>
using google::LogMessage; using google::LogMessage;
GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition( GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
@ -63,11 +61,11 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition(
dump_ = configuration_->property(role + ".dump", false); dump_ = configuration_->property(role + ".dump", false);
blocking_ = configuration_->property(role + ".blocking", true); blocking_ = configuration_->property(role + ".blocking", true);
doppler_max_ = configuration_->property(role + ".doppler_max", 5000); doppler_max_ = configuration_->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1);
bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false);
use_CFAR_algorithm_flag_=configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions
max_dwells_ = configuration_->property(role + ".max_dwells", 1); max_dwells_ = configuration_->property(role + ".max_dwells", 1);
@ -78,14 +76,14 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition(
vector_length_ = code_length_ * sampled_ms_; vector_length_ = code_length_ * sampled_ms_;
if( bit_transition_flag_ ) if (bit_transition_flag_)
{ {
vector_length_ *= 2; vector_length_ *= 2;
} }
code_ = new gr_complex[vector_length_]; code_ = new gr_complex[vector_length_];
if (item_type_.compare("cshort") == 0 ) if (item_type_.compare("cshort") == 0)
{ {
item_size_ = sizeof(lv_16sc_t); item_size_ = sizeof(lv_16sc_t);
} }
@ -94,8 +92,8 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition(
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
} }
acquisition_ = pcps_make_acquisition(sampled_ms_, max_dwells_, acquisition_ = pcps_make_acquisition(sampled_ms_, max_dwells_,
doppler_max_, if_, fs_in_, code_length_, code_length_, doppler_max_, if_, fs_in_, code_length_, code_length_,
bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_, dump_filename_, item_size_); bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_, dump_filename_, item_size_);
DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")";
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
@ -131,7 +129,7 @@ void GpsL1CaPcpsAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + ".pfa", 0.0); float pfa = configuration_->property(role_ + ".pfa", 0.0);
if(pfa == 0.0) if (pfa == 0.0)
{ {
threshold_ = threshold; threshold_ = threshold;
} }
@ -191,8 +189,8 @@ void GpsL1CaPcpsAcquisition::set_local_code()
for (unsigned int i = 0; i < sampled_ms_; i++) for (unsigned int i = 0; i < sampled_ms_; i++)
{ {
memcpy(&(code_[i*code_length_]), code, memcpy(&(code_[i * code_length_]), code,
sizeof(gr_complex)*code_length_); sizeof(gr_complex) * code_length_);
} }
acquisition_->set_local_code(code_); acquisition_->set_local_code(code_);
@ -212,7 +210,6 @@ void GpsL1CaPcpsAcquisition::set_state(int state)
} }
float GpsL1CaPcpsAcquisition::calculate_threshold(float pfa) float GpsL1CaPcpsAcquisition::calculate_threshold(float pfa)
{ {
//Calculate the threshold //Calculate the threshold
@ -226,8 +223,8 @@ float GpsL1CaPcpsAcquisition::calculate_threshold(float pfa)
double exponent = 1 / static_cast<double>(ncells); double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0 - pfa, exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist(lambda);
float threshold = static_cast<float>(quantile(mydist,val)); float threshold = static_cast<float>(quantile(mydist, val));
return threshold; return threshold;
} }
@ -309,4 +306,3 @@ gr::basic_block_sptr GpsL1CaPcpsAcquisition::get_right_block()
{ {
return acquisition_; return acquisition_;
} }

10
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.h
View File

@ -52,12 +52,12 @@ class ConfigurationInterface;
* \brief This class adapts a PCPS acquisition block to an AcquisitionInterface * \brief This class adapts a PCPS acquisition block to an AcquisitionInterface
* for GPS L1 C/A signals * for GPS L1 C/A signals
*/ */
class GpsL1CaPcpsAcquisition: public AcquisitionInterface class GpsL1CaPcpsAcquisition : public AcquisitionInterface
{ {
public: public:
GpsL1CaPcpsAcquisition(ConfigurationInterface* configuration, GpsL1CaPcpsAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GpsL1CaPcpsAcquisition(); virtual ~GpsL1CaPcpsAcquisition();
@ -159,8 +159,8 @@ private:
bool dump_; bool dump_;
bool blocking_; bool blocking_;
std::string dump_filename_; std::string dump_filename_;
std::complex<float> * code_; std::complex<float>* code_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

31
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc
View File

@ -42,9 +42,8 @@
using google::LogMessage; using google::LogMessage;
GpsL1CaPcpsAcquisitionFineDoppler::GpsL1CaPcpsAcquisitionFineDoppler( GpsL1CaPcpsAcquisitionFineDoppler::GpsL1CaPcpsAcquisitionFineDoppler(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
std::string default_dump_filename = "./data/acquisition.dat"; std::string default_dump_filename = "./data/acquisition.dat";
@ -58,23 +57,22 @@ GpsL1CaPcpsAcquisitionFineDoppler::GpsL1CaPcpsAcquisitionFineDoppler(
dump_ = configuration->property(role + ".dump", false); dump_ = configuration->property(role + ".dump", false);
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
doppler_max_ = configuration->property(role + ".doppler_max", 5000); doppler_max_ = configuration->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
doppler_min_ = configuration->property(role + ".doppler_min", - doppler_max_); doppler_min_ = configuration->property(role + ".doppler_min", -doppler_max_);
sampled_ms_ = configuration->property(role + ".coherent_integration_time_ms", 1); sampled_ms_ = configuration->property(role + ".coherent_integration_time_ms", 1);
max_dwells_= configuration->property(role + ".max_dwells", 1); max_dwells_ = configuration->property(role + ".max_dwells", 1);
//--- Find number of samples per spreading code ------------------------- //--- Find number of samples per spreading code -------------------------
vector_length_ = round(fs_in_ vector_length_ = round(fs_in_ / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
/ (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
code_ = new gr_complex[vector_length_]; code_ = new gr_complex[vector_length_];
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
acquisition_cc_ = pcps_make_acquisition_fine_doppler_cc(max_dwells_,sampled_ms_, acquisition_cc_ = pcps_make_acquisition_fine_doppler_cc(max_dwells_, sampled_ms_,
doppler_max_, doppler_min_, if_, fs_in_, vector_length_, doppler_max_, doppler_min_, if_, fs_in_, vector_length_,
dump_, dump_filename_); dump_, dump_filename_);
} }
else else
{ {
@ -132,7 +130,7 @@ void GpsL1CaPcpsAcquisitionFineDoppler::set_gnss_synchro(Gnss_Synchro* gnss_sync
signed int GpsL1CaPcpsAcquisitionFineDoppler::mag() signed int GpsL1CaPcpsAcquisitionFineDoppler::mag()
{ {
return acquisition_cc_->mag(); return acquisition_cc_->mag();
} }
@ -158,14 +156,18 @@ void GpsL1CaPcpsAcquisitionFineDoppler::reset()
void GpsL1CaPcpsAcquisitionFineDoppler::connect(boost::shared_ptr<gr::top_block> top_block) void GpsL1CaPcpsAcquisitionFineDoppler::connect(boost::shared_ptr<gr::top_block> top_block)
{ {
if(top_block) { /* top_block is not null */}; if (top_block)
{ /* top_block is not null */
};
//nothing to disconnect, now the tracking uses gr_sync_decimator //nothing to disconnect, now the tracking uses gr_sync_decimator
} }
void GpsL1CaPcpsAcquisitionFineDoppler::disconnect(boost::shared_ptr<gr::top_block> top_block) void GpsL1CaPcpsAcquisitionFineDoppler::disconnect(boost::shared_ptr<gr::top_block> top_block)
{ {
if(top_block) { /* top_block is not null */}; if (top_block)
{ /* top_block is not null */
};
//nothing to disconnect, now the tracking uses gr_sync_decimator //nothing to disconnect, now the tracking uses gr_sync_decimator
} }
@ -180,4 +182,3 @@ boost::shared_ptr<gr::basic_block> GpsL1CaPcpsAcquisitionFineDoppler::get_right_
{ {
return acquisition_cc_; return acquisition_cc_;
} }

11
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.h
View File

@ -40,19 +40,18 @@
#include "pcps_acquisition_fine_doppler_cc.h" #include "pcps_acquisition_fine_doppler_cc.h"
class ConfigurationInterface; class ConfigurationInterface;
/*! /*!
* \brief This class Adapts a PCPS acquisition block with fine Doppler estimation to an AcquisitionInterface for * \brief This class Adapts a PCPS acquisition block with fine Doppler estimation to an AcquisitionInterface for
* GPS L1 C/A signals * GPS L1 C/A signals
*/ */
class GpsL1CaPcpsAcquisitionFineDoppler: public AcquisitionInterface class GpsL1CaPcpsAcquisitionFineDoppler : public AcquisitionInterface
{ {
public: public:
GpsL1CaPcpsAcquisitionFineDoppler(ConfigurationInterface* configuration, GpsL1CaPcpsAcquisitionFineDoppler(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GpsL1CaPcpsAcquisitionFineDoppler(); virtual ~GpsL1CaPcpsAcquisitionFineDoppler();
@ -139,8 +138,8 @@ private:
long if_; long if_;
bool dump_; bool dump_;
std::string dump_filename_; std::string dump_filename_;
std::complex<float> * code_; std::complex<float>* code_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

6
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h
View File

@ -53,8 +53,8 @@ class GpsL1CaPcpsAcquisitionFpga : public AcquisitionInterface
{ {
public: public:
GpsL1CaPcpsAcquisitionFpga(ConfigurationInterface* configuration, GpsL1CaPcpsAcquisitionFpga(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GpsL1CaPcpsAcquisitionFpga(); virtual ~GpsL1CaPcpsAcquisitionFpga();
@ -144,7 +144,7 @@ private:
unsigned int doppler_max_; unsigned int doppler_max_;
unsigned int doppler_step_; unsigned int doppler_step_;
unsigned int max_dwells_; unsigned int max_dwells_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

27
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.cc
View File

@ -43,9 +43,8 @@
using google::LogMessage; using google::LogMessage;
GpsL1CaPcpsAssistedAcquisition::GpsL1CaPcpsAssistedAcquisition( GpsL1CaPcpsAssistedAcquisition::GpsL1CaPcpsAssistedAcquisition(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
std::string default_dump_filename = "./data/acquisition.dat"; std::string default_dump_filename = "./data/acquisition.dat";
@ -58,15 +57,14 @@ GpsL1CaPcpsAssistedAcquisition::GpsL1CaPcpsAssistedAcquisition(
if_ = configuration->property(role + ".if", 0); if_ = configuration->property(role + ".if", 0);
dump_ = configuration->property(role + ".dump", false); dump_ = configuration->property(role + ".dump", false);
doppler_max_ = configuration->property(role + ".doppler_max", 5000); doppler_max_ = configuration->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
doppler_min_ = configuration->property(role + ".doppler_min", -doppler_max_); doppler_min_ = configuration->property(role + ".doppler_min", -doppler_max_);
sampled_ms_ = configuration->property(role + ".coherent_integration_time_ms", 1); sampled_ms_ = configuration->property(role + ".coherent_integration_time_ms", 1);
max_dwells_= configuration->property(role + ".max_dwells", 1); max_dwells_ = configuration->property(role + ".max_dwells", 1);
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
//--- Find number of samples per spreading code ------------------------- //--- Find number of samples per spreading code -------------------------
vector_length_ = round(fs_in_ vector_length_ = round(fs_in_ / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
/ (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
code_ = new gr_complex[vector_length_]; code_ = new gr_complex[vector_length_];
@ -74,8 +72,8 @@ GpsL1CaPcpsAssistedAcquisition::GpsL1CaPcpsAssistedAcquisition(
{ {
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
acquisition_cc_ = pcps_make_assisted_acquisition_cc(max_dwells_, sampled_ms_, acquisition_cc_ = pcps_make_assisted_acquisition_cc(max_dwells_, sampled_ms_,
doppler_max_, doppler_min_, if_, fs_in_, vector_length_, doppler_max_, doppler_min_, if_, fs_in_, vector_length_,
dump_, dump_filename_); dump_, dump_filename_);
} }
else else
{ {
@ -133,7 +131,7 @@ void GpsL1CaPcpsAssistedAcquisition::set_gnss_synchro(Gnss_Synchro* gnss_synchro
signed int GpsL1CaPcpsAssistedAcquisition::mag() signed int GpsL1CaPcpsAssistedAcquisition::mag()
{ {
return acquisition_cc_->mag(); return acquisition_cc_->mag();
} }
@ -157,14 +155,18 @@ void GpsL1CaPcpsAssistedAcquisition::reset()
void GpsL1CaPcpsAssistedAcquisition::connect(gr::top_block_sptr top_block) void GpsL1CaPcpsAssistedAcquisition::connect(gr::top_block_sptr top_block)
{ {
if(top_block) { /* top_block is not null */}; if (top_block)
{ /* top_block is not null */
};
//nothing to disconnect, now the tracking uses gr_sync_decimator //nothing to disconnect, now the tracking uses gr_sync_decimator
} }
void GpsL1CaPcpsAssistedAcquisition::disconnect(gr::top_block_sptr top_block) void GpsL1CaPcpsAssistedAcquisition::disconnect(gr::top_block_sptr top_block)
{ {
if(top_block) { /* top_block is not null */}; if (top_block)
{ /* top_block is not null */
};
//nothing to disconnect, now the tracking uses gr_sync_decimator //nothing to disconnect, now the tracking uses gr_sync_decimator
} }
@ -179,4 +181,3 @@ gr::basic_block_sptr GpsL1CaPcpsAssistedAcquisition::get_right_block()
{ {
return acquisition_cc_; return acquisition_cc_;
} }

11
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.h
View File

@ -40,19 +40,18 @@
#include "pcps_assisted_acquisition_cc.h" #include "pcps_assisted_acquisition_cc.h"
class ConfigurationInterface; class ConfigurationInterface;
/*! /*!
* \brief This class adapts a PCPS acquisition block to an AcquisitionInterface * \brief This class adapts a PCPS acquisition block to an AcquisitionInterface
* for GPS L1 C/A signals * for GPS L1 C/A signals
*/ */
class GpsL1CaPcpsAssistedAcquisition: public AcquisitionInterface class GpsL1CaPcpsAssistedAcquisition : public AcquisitionInterface
{ {
public: public:
GpsL1CaPcpsAssistedAcquisition(ConfigurationInterface* configuration, GpsL1CaPcpsAssistedAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GpsL1CaPcpsAssistedAcquisition(); virtual ~GpsL1CaPcpsAssistedAcquisition();
@ -140,8 +139,8 @@ private:
long if_; long if_;
bool dump_; bool dump_;
std::string dump_filename_; std::string dump_filename_;
std::complex<float> * code_; std::complex<float>* code_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

32
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc
View File

@ -41,9 +41,8 @@
using google::LogMessage; using google::LogMessage;
GpsL1CaPcpsOpenClAcquisition::GpsL1CaPcpsOpenClAcquisition( GpsL1CaPcpsOpenClAcquisition::GpsL1CaPcpsOpenClAcquisition(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
@ -52,14 +51,14 @@ GpsL1CaPcpsOpenClAcquisition::GpsL1CaPcpsOpenClAcquisition(
DLOG(INFO) << "role " << role; DLOG(INFO) << "role " << role;
item_type_ = configuration_->property(role + ".item_type", item_type_ = configuration_->property(role + ".item_type",
default_item_type); default_item_type);
long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000); long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000);
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
if_ = configuration_->property(role + ".if", 0); if_ = configuration_->property(role + ".if", 0);
dump_ = configuration_->property(role + ".dump", false); dump_ = configuration_->property(role + ".dump", false);
doppler_max_ = configuration->property(role + ".doppler_max", 5000); doppler_max_ = configuration->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1);
bit_transition_flag_ = configuration_->property("Acquisition.bit_transition_flag", false); bit_transition_flag_ = configuration_->property("Acquisition.bit_transition_flag", false);
@ -74,11 +73,10 @@ GpsL1CaPcpsOpenClAcquisition::GpsL1CaPcpsOpenClAcquisition(
} }
dump_filename_ = configuration_->property(role + ".dump_filename", dump_filename_ = configuration_->property(role + ".dump_filename",
default_dump_filename); default_dump_filename);
//--- Find number of samples per spreading code ------------------------- //--- Find number of samples per spreading code -------------------------
code_length_ = round(fs_in_ code_length_ = round(fs_in_ / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
/ (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
vector_length_ = code_length_ * sampled_ms_; vector_length_ = code_length_ * sampled_ms_;
@ -88,8 +86,8 @@ GpsL1CaPcpsOpenClAcquisition::GpsL1CaPcpsOpenClAcquisition(
{ {
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
acquisition_cc_ = pcps_make_opencl_acquisition_cc(sampled_ms_, max_dwells_, acquisition_cc_ = pcps_make_opencl_acquisition_cc(sampled_ms_, max_dwells_,
doppler_max_, if_, fs_in_, code_length_, code_length_, doppler_max_, if_, fs_in_, code_length_, code_length_,
bit_transition_flag_, dump_, dump_filename_); bit_transition_flag_, dump_, dump_filename_);
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
@ -129,11 +127,11 @@ void GpsL1CaPcpsOpenClAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0); float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0);
if(pfa == 0.0) if (pfa == 0.0)
{ {
pfa = configuration_->property(role_ + ".pfa", 0.0); pfa = configuration_->property(role_ + ".pfa", 0.0);
} }
if(pfa == 0.0) if (pfa == 0.0)
{ {
threshold_ = threshold; threshold_ = threshold;
} }
@ -168,7 +166,6 @@ void GpsL1CaPcpsOpenClAcquisition::set_doppler_step(unsigned int doppler_step)
{ {
acquisition_cc_->set_doppler_step(doppler_step_); acquisition_cc_->set_doppler_step(doppler_step_);
} }
} }
@ -212,8 +209,8 @@ void GpsL1CaPcpsOpenClAcquisition::set_local_code()
for (unsigned int i = 0; i < sampled_ms_; i++) for (unsigned int i = 0; i < sampled_ms_; i++)
{ {
memcpy(&(code_[i*code_length_]), code, memcpy(&(code_[i * code_length_]), code,
sizeof(gr_complex)*code_length_); sizeof(gr_complex) * code_length_);
} }
acquisition_cc_->set_local_code(code_); acquisition_cc_->set_local_code(code_);
@ -248,8 +245,8 @@ float GpsL1CaPcpsOpenClAcquisition::calculate_threshold(float pfa)
double exponent = 1 / static_cast<double>(ncells); double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0 - pfa, exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist(lambda);
float threshold = static_cast<float>(quantile(mydist,val)); float threshold = static_cast<float>(quantile(mydist, val));
return threshold; return threshold;
} }
@ -283,4 +280,3 @@ gr::basic_block_sptr GpsL1CaPcpsOpenClAcquisition::get_right_block()
{ {
return acquisition_cc_; return acquisition_cc_;
} }

11
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.h
View File

@ -39,19 +39,18 @@
#include "pcps_opencl_acquisition_cc.h" #include "pcps_opencl_acquisition_cc.h"
class ConfigurationInterface; class ConfigurationInterface;
/*! /*!
* \brief This class adapts an OpenCL PCPS acquisition block to an * \brief This class adapts an OpenCL PCPS acquisition block to an
* AcquisitionInterface for GPS L1 C/A signals * AcquisitionInterface for GPS L1 C/A signals
*/ */
class GpsL1CaPcpsOpenClAcquisition: public AcquisitionInterface class GpsL1CaPcpsOpenClAcquisition : public AcquisitionInterface
{ {
public: public:
GpsL1CaPcpsOpenClAcquisition(ConfigurationInterface* configuration, GpsL1CaPcpsOpenClAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GpsL1CaPcpsOpenClAcquisition(); virtual ~GpsL1CaPcpsOpenClAcquisition();
@ -144,8 +143,8 @@ private:
long if_; long if_;
bool dump_; bool dump_;
std::string dump_filename_; std::string dump_filename_;
std::complex<float> * code_; std::complex<float>* code_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

55
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc
View File

@ -42,9 +42,8 @@
using google::LogMessage; using google::LogMessage;
GpsL1CaPcpsQuickSyncAcquisition::GpsL1CaPcpsQuickSyncAcquisition( GpsL1CaPcpsQuickSyncAcquisition::GpsL1CaPcpsQuickSyncAcquisition(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
@ -58,23 +57,22 @@ GpsL1CaPcpsQuickSyncAcquisition::GpsL1CaPcpsQuickSyncAcquisition(
if_ = configuration_->property(role + ".if", 0); if_ = configuration_->property(role + ".if", 0);
dump_ = configuration_->property(role + ".dump", false); dump_ = configuration_->property(role + ".dump", false);
doppler_max_ = configuration->property(role + ".doppler_max", 5000); doppler_max_ = configuration->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 4); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 4);
//--- Find number of samples per spreading code ------------------------- //--- Find number of samples per spreading code -------------------------
code_length_ = round(fs_in_ code_length_ = round(fs_in_ / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
/ (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
/*Calculate the folding factor value based on the calculations*/ /*Calculate the folding factor value based on the calculations*/
unsigned int temp = static_cast<unsigned int>(ceil(sqrt(log2(code_length_)))); unsigned int temp = static_cast<unsigned int>(ceil(sqrt(log2(code_length_))));
folding_factor_ = configuration_->property(role + ".folding_factor", temp); folding_factor_ = configuration_->property(role + ".folding_factor", temp);
if ( sampled_ms_ % folding_factor_ != 0) if (sampled_ms_ % folding_factor_ != 0)
{ {
LOG(WARNING) << "QuickSync Algorithm requires a coherent_integration_time" LOG(WARNING) << "QuickSync Algorithm requires a coherent_integration_time"
<< " multiple of " << folding_factor_ << "ms, Value entered " << " multiple of " << folding_factor_ << "ms, Value entered "
<< sampled_ms_ << " ms"; << sampled_ms_ << " ms";
if(sampled_ms_ < folding_factor_) if (sampled_ms_ < folding_factor_)
{ {
sampled_ms_ = static_cast<int>(folding_factor_); sampled_ms_ = static_cast<int>(folding_factor_);
} }
@ -105,22 +103,22 @@ GpsL1CaPcpsQuickSyncAcquisition::GpsL1CaPcpsQuickSyncAcquisition(
code_ = new gr_complex[code_length_](); code_ = new gr_complex[code_length_]();
/*Object relevant information for debugging*/ /*Object relevant information for debugging*/
LOG(INFO) << "Implementation: " << this->implementation() LOG(INFO) << "Implementation: " << this->implementation()
<< ", Vector Length: " << vector_length_ << ", Vector Length: " << vector_length_
<< ", Samples per ms: " << samples_per_ms << ", Samples per ms: " << samples_per_ms
<< ", Folding factor: " << folding_factor_ << ", Folding factor: " << folding_factor_
<< ", Sampled ms: " << sampled_ms_ << ", Sampled ms: " << sampled_ms_
<< ", Code Length: " << code_length_; << ", Code Length: " << code_length_;
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
acquisition_cc_ = pcps_quicksync_make_acquisition_cc(folding_factor_, acquisition_cc_ = pcps_quicksync_make_acquisition_cc(folding_factor_,
sampled_ms_, max_dwells_,doppler_max_, if_, fs_in_, sampled_ms_, max_dwells_, doppler_max_, if_, fs_in_,
samples_per_ms, code_length_,bit_transition_flag_, samples_per_ms, code_length_, bit_transition_flag_,
dump_, dump_filename_); dump_, dump_filename_);
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_,
code_length_*folding_factor_); code_length_ * folding_factor_);
DLOG(INFO) << "stream_to_vector_quicksync(" << stream_to_vector_->unique_id() << ")"; DLOG(INFO) << "stream_to_vector_quicksync(" << stream_to_vector_->unique_id() << ")";
DLOG(INFO) << "acquisition(" << acquisition_cc_->unique_id() << ")"; DLOG(INFO) << "acquisition(" << acquisition_cc_->unique_id() << ")";
@ -157,13 +155,14 @@ void GpsL1CaPcpsQuickSyncAcquisition::set_channel(unsigned int channel)
void GpsL1CaPcpsQuickSyncAcquisition::set_threshold(float threshold) void GpsL1CaPcpsQuickSyncAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + float pfa = configuration_->property(role_ +
boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0); boost::lexical_cast<std::string>(channel_) + ".pfa",
0.0);
if(pfa == 0.0) if (pfa == 0.0)
{ {
pfa = configuration_->property(role_ + ".pfa", 0.0); pfa = configuration_->property(role_ + ".pfa", 0.0);
} }
if(pfa == 0.0) if (pfa == 0.0)
{ {
threshold_ = threshold; threshold_ = threshold;
} }
@ -172,7 +171,7 @@ void GpsL1CaPcpsQuickSyncAcquisition::set_threshold(float threshold)
threshold_ = calculate_threshold(pfa); threshold_ = calculate_threshold(pfa);
} }
DLOG(INFO) << "Channel "<< channel_ << " Threshold = " << threshold_; DLOG(INFO) << "Channel " << channel_ << " Threshold = " << threshold_;
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
@ -240,10 +239,10 @@ void GpsL1CaPcpsQuickSyncAcquisition::set_local_code()
gps_l1_ca_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_, 0); gps_l1_ca_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_, 0);
for (unsigned int i = 0; i < (sampled_ms_/folding_factor_); i++) for (unsigned int i = 0; i < (sampled_ms_ / folding_factor_); i++)
{ {
memcpy(&(code_[i*code_length_]), code, memcpy(&(code_[i * code_length_]), code,
sizeof(gr_complex)*code_length_); sizeof(gr_complex) * code_length_);
} }
//memcpy(code_, code,sizeof(gr_complex)*code_length_); //memcpy(code_, code,sizeof(gr_complex)*code_length_);
@ -280,13 +279,13 @@ float GpsL1CaPcpsQuickSyncAcquisition::calculate_threshold(float pfa)
{ {
frequency_bins++; frequency_bins++;
} }
DLOG(INFO) << "Channel " << channel_<< " Pfa = " << pfa; DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
unsigned int ncells = (code_length_ / folding_factor_) * frequency_bins; unsigned int ncells = (code_length_ / folding_factor_) * frequency_bins;
double exponent = 1.0 / static_cast<double>(ncells); double exponent = 1.0 / static_cast<double>(ncells);
double val = pow(1.0 - pfa, exponent); double val = pow(1.0 - pfa, exponent);
double lambda = static_cast<double>(code_length_) / static_cast<double>(folding_factor_); double lambda = static_cast<double>(code_length_) / static_cast<double>(folding_factor_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist(lambda);
float threshold = static_cast<float>(quantile(mydist,val)); float threshold = static_cast<float>(quantile(mydist, val));
return threshold; return threshold;
} }
@ -320,5 +319,3 @@ gr::basic_block_sptr GpsL1CaPcpsQuickSyncAcquisition::get_right_block()
{ {
return acquisition_cc_; return acquisition_cc_;
} }

13
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.h
View File

@ -41,19 +41,18 @@
#include "configuration_interface.h" #include "configuration_interface.h"
class ConfigurationInterface; class ConfigurationInterface;
/*! /*!
* \brief This class adapts a PCPS acquisition block to an AcquisitionInterface * \brief This class adapts a PCPS acquisition block to an AcquisitionInterface
* for GPS L1 C/A signals * for GPS L1 C/A signals
*/ */
class GpsL1CaPcpsQuickSyncAcquisition: public AcquisitionInterface class GpsL1CaPcpsQuickSyncAcquisition : public AcquisitionInterface
{ {
public: public:
GpsL1CaPcpsQuickSyncAcquisition(ConfigurationInterface* configuration, GpsL1CaPcpsQuickSyncAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GpsL1CaPcpsQuickSyncAcquisition(); virtual ~GpsL1CaPcpsQuickSyncAcquisition();
@ -131,6 +130,7 @@ public:
* \brief If state = 1, it forces the block to start acquiring from the first sample * \brief If state = 1, it forces the block to start acquiring from the first sample
*/ */
void set_state(int state); void set_state(int state);
private: private:
ConfigurationInterface* configuration_; ConfigurationInterface* configuration_;
pcps_quicksync_acquisition_cc_sptr acquisition_cc_; pcps_quicksync_acquisition_cc_sptr acquisition_cc_;
@ -151,14 +151,13 @@ private:
long if_; long if_;
bool dump_; bool dump_;
std::string dump_filename_; std::string dump_filename_;
std::complex<float> * code_; std::complex<float>* code_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;
float calculate_threshold(float pfa); float calculate_threshold(float pfa);
}; };
#endif /* GNSS_SDR_GPS_L1_CA_PCPS_QUICKSYNC_ACQUISITION_H_ */ #endif /* GNSS_SDR_GPS_L1_CA_PCPS_QUICKSYNC_ACQUISITION_H_ */

63
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.cc
View File

@ -41,9 +41,8 @@
using google::LogMessage; using google::LogMessage;
GpsL1CaPcpsTongAcquisition::GpsL1CaPcpsTongAcquisition( GpsL1CaPcpsTongAcquisition::GpsL1CaPcpsTongAcquisition(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
@ -58,7 +57,7 @@ GpsL1CaPcpsTongAcquisition::GpsL1CaPcpsTongAcquisition(
if_ = configuration_->property(role + ".if", 0); if_ = configuration_->property(role + ".if", 0);
dump_ = configuration_->property(role + ".dump", false); dump_ = configuration_->property(role + ".dump", false);
doppler_max_ = configuration->property(role + ".doppler_max", 5000); doppler_max_ = configuration->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1);
tong_init_val_ = configuration->property(role + ".tong_init_val", 1); tong_init_val_ = configuration->property(role + ".tong_init_val", 1);
@ -68,8 +67,7 @@ GpsL1CaPcpsTongAcquisition::GpsL1CaPcpsTongAcquisition(
dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename); dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename);
//--- Find number of samples per spreading code ------------------------- //--- Find number of samples per spreading code -------------------------
code_length_ = round(fs_in_ code_length_ = round(fs_in_ / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
/ (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
vector_length_ = code_length_ * sampled_ms_; vector_length_ = code_length_ * sampled_ms_;
@ -79,8 +77,8 @@ GpsL1CaPcpsTongAcquisition::GpsL1CaPcpsTongAcquisition(
{ {
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
acquisition_cc_ = pcps_tong_make_acquisition_cc(sampled_ms_, doppler_max_, if_, fs_in_, acquisition_cc_ = pcps_tong_make_acquisition_cc(sampled_ms_, doppler_max_, if_, fs_in_,
code_length_, code_length_, tong_init_val_, tong_max_val_, tong_max_dwells_, code_length_, code_length_, tong_init_val_, tong_max_val_, tong_max_dwells_,
dump_, dump_filename_); dump_, dump_filename_);
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
@ -110,9 +108,9 @@ void GpsL1CaPcpsTongAcquisition::set_channel(unsigned int channel)
{ {
channel_ = channel; channel_ = channel;
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
acquisition_cc_->set_channel(channel_); acquisition_cc_->set_channel(channel_);
} }
} }
@ -120,11 +118,11 @@ void GpsL1CaPcpsTongAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0); float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0);
if(pfa == 0.0) if (pfa == 0.0)
{ {
pfa = configuration_->property(role_+".pfa", 0.0); pfa = configuration_->property(role_ + ".pfa", 0.0);
} }
if(pfa == 0.0) if (pfa == 0.0)
{ {
threshold_ = threshold; threshold_ = threshold;
} }
@ -159,7 +157,6 @@ void GpsL1CaPcpsTongAcquisition::set_doppler_step(unsigned int doppler_step)
{ {
acquisition_cc_->set_doppler_step(doppler_step_); acquisition_cc_->set_doppler_step(doppler_step_);
} }
} }
@ -195,21 +192,21 @@ void GpsL1CaPcpsTongAcquisition::init()
void GpsL1CaPcpsTongAcquisition::set_local_code() void GpsL1CaPcpsTongAcquisition::set_local_code()
{ {
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
std::complex<float>* code = new std::complex<float>[code_length_]; std::complex<float>* code = new std::complex<float>[code_length_];
gps_l1_ca_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_, 0); gps_l1_ca_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_, 0);
for (unsigned int i = 0; i < sampled_ms_; i++) for (unsigned int i = 0; i < sampled_ms_; i++)
{ {
memcpy(&(code_[i*code_length_]), code, memcpy(&(code_[i * code_length_]), code,
sizeof(gr_complex)*code_length_); sizeof(gr_complex) * code_length_);
} }
acquisition_cc_->set_local_code(code_); acquisition_cc_->set_local_code(code_);
delete[] code; delete[] code;
} }
} }
@ -225,9 +222,9 @@ void GpsL1CaPcpsTongAcquisition::reset()
void GpsL1CaPcpsTongAcquisition::set_state(int state) void GpsL1CaPcpsTongAcquisition::set_state(int state)
{ {
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
acquisition_cc_->set_state(state); acquisition_cc_->set_state(state);
} }
} }
@ -240,13 +237,13 @@ float GpsL1CaPcpsTongAcquisition::calculate_threshold(float pfa)
frequency_bins++; frequency_bins++;
} }
DLOG(INFO) << "Channel "<< channel_ <<" Pfa = "<< pfa; DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
unsigned int ncells = vector_length_ * frequency_bins; unsigned int ncells = vector_length_ * frequency_bins;
double exponent = 1 / static_cast<double>(ncells); double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0 - pfa,exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist(lambda);
float threshold = static_cast<float>(quantile(mydist, val)); float threshold = static_cast<float>(quantile(mydist, val));
return threshold; return threshold;
@ -259,7 +256,6 @@ void GpsL1CaPcpsTongAcquisition::connect(gr::top_block_sptr top_block)
{ {
top_block->connect(stream_to_vector_, 0, acquisition_cc_, 0); top_block->connect(stream_to_vector_, 0, acquisition_cc_, 0);
} }
} }
@ -282,4 +278,3 @@ gr::basic_block_sptr GpsL1CaPcpsTongAcquisition::get_right_block()
{ {
return acquisition_cc_; return acquisition_cc_;
} }

10
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.h
View File

@ -45,12 +45,12 @@ class ConfigurationInterface;
* \brief This class adapts a PCPS Tong acquisition block to an * \brief This class adapts a PCPS Tong acquisition block to an
* AcquisitionInterface for GPS L1 C/A signals * AcquisitionInterface for GPS L1 C/A signals
*/ */
class GpsL1CaPcpsTongAcquisition: public AcquisitionInterface class GpsL1CaPcpsTongAcquisition : public AcquisitionInterface
{ {
public: public:
GpsL1CaPcpsTongAcquisition(ConfigurationInterface* configuration, GpsL1CaPcpsTongAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GpsL1CaPcpsTongAcquisition(); virtual ~GpsL1CaPcpsTongAcquisition();
@ -149,8 +149,8 @@ private:
long if_; long if_;
bool dump_; bool dump_;
std::string dump_filename_; std::string dump_filename_;
std::complex<float> * code_; std::complex<float>* code_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

39
src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc
View File

@ -43,9 +43,8 @@
using google::LogMessage; using google::LogMessage;
GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition( GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
@ -62,29 +61,28 @@ GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition(
dump_ = configuration_->property(role + ".dump", false); dump_ = configuration_->property(role + ".dump", false);
blocking_ = configuration_->property(role + ".blocking", true); blocking_ = configuration_->property(role + ".blocking", true);
doppler_max_ = configuration->property(role + ".doppler_max", 5000); doppler_max_ = configuration->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false);
use_CFAR_algorithm_flag_=configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions
max_dwells_ = configuration_->property(role + ".max_dwells", 1); max_dwells_ = configuration_->property(role + ".max_dwells", 1);
dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename); dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename);
//--- Find number of samples per spreading code ------------------------- //--- Find number of samples per spreading code -------------------------
code_length_ = round(static_cast<double>(fs_in_) code_length_ = round(static_cast<double>(fs_in_) / (GPS_L2_M_CODE_RATE_HZ / static_cast<double>(GPS_L2_M_CODE_LENGTH_CHIPS)));
/ (GPS_L2_M_CODE_RATE_HZ / static_cast<double>(GPS_L2_M_CODE_LENGTH_CHIPS)));
vector_length_ = code_length_; vector_length_ = code_length_;
if( bit_transition_flag_ ) if (bit_transition_flag_)
{ {
vector_length_ *= 2; vector_length_ *= 2;
} }
code_ = new gr_complex[vector_length_]; code_ = new gr_complex[vector_length_];
if (item_type_.compare("cshort") == 0 ) if (item_type_.compare("cshort") == 0)
{ {
item_size_ = sizeof(lv_16sc_t); item_size_ = sizeof(lv_16sc_t);
} }
@ -93,10 +91,10 @@ GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition(
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
} }
acquisition_ = pcps_make_acquisition(1, max_dwells_, acquisition_ = pcps_make_acquisition(1, max_dwells_,
doppler_max_, if_, fs_in_, code_length_, code_length_, doppler_max_, if_, fs_in_, code_length_, code_length_,
bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_, bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_,
dump_filename_, item_size_); dump_filename_, item_size_);
DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")";
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")"; DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")";
@ -131,11 +129,11 @@ void GpsL2MPcpsAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0); float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0);
if(pfa == 0.0) if (pfa == 0.0)
{ {
pfa = configuration_->property(role_ + ".pfa", 0.0); pfa = configuration_->property(role_ + ".pfa", 0.0);
} }
if(pfa == 0.0) if (pfa == 0.0)
{ {
threshold_ = threshold; threshold_ = threshold;
} }
@ -144,7 +142,7 @@ void GpsL2MPcpsAcquisition::set_threshold(float threshold)
threshold_ = calculate_threshold(pfa); threshold_ = calculate_threshold(pfa);
} }
DLOG(INFO) << "Channel " << channel_ <<" Threshold = " << threshold_; DLOG(INFO) << "Channel " << channel_ << " Threshold = " << threshold_;
acquisition_->set_threshold(threshold_); acquisition_->set_threshold(threshold_);
} }
@ -191,7 +189,6 @@ void GpsL2MPcpsAcquisition::init()
void GpsL2MPcpsAcquisition::set_local_code() void GpsL2MPcpsAcquisition::set_local_code()
{ {
gps_l2c_m_code_gen_complex_sampled(code_, gnss_synchro_->PRN, fs_in_); gps_l2c_m_code_gen_complex_sampled(code_, gnss_synchro_->PRN, fs_in_);
acquisition_->set_local_code(code_); acquisition_->set_local_code(code_);
@ -209,7 +206,6 @@ void GpsL2MPcpsAcquisition::set_state(int state)
} }
float GpsL2MPcpsAcquisition::calculate_threshold(float pfa) float GpsL2MPcpsAcquisition::calculate_threshold(float pfa)
{ {
//Calculate the threshold //Calculate the threshold
@ -218,13 +214,13 @@ float GpsL2MPcpsAcquisition::calculate_threshold(float pfa)
{ {
frequency_bins++; frequency_bins++;
} }
DLOG(INFO) << "Channel " << channel_<< " Pfa = " << pfa; DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
unsigned int ncells = vector_length_ * frequency_bins; unsigned int ncells = vector_length_ * frequency_bins;
double exponent = 1.0 / static_cast<double>(ncells); double exponent = 1.0 / static_cast<double>(ncells);
double val = pow(1.0 - pfa, exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist(lambda);
float threshold = static_cast<float>(quantile(mydist,val)); float threshold = static_cast<float>(quantile(mydist, val));
return threshold; return threshold;
} }
@ -306,4 +302,3 @@ gr::basic_block_sptr GpsL2MPcpsAcquisition::get_right_block()
{ {
return acquisition_; return acquisition_;
} }

11
src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.h
View File

@ -44,19 +44,18 @@
#include <string> #include <string>
class ConfigurationInterface; class ConfigurationInterface;
/*! /*!
* \brief This class adapts a PCPS acquisition block to an AcquisitionInterface * \brief This class adapts a PCPS acquisition block to an AcquisitionInterface
* for GPS L2 M signals * for GPS L2 M signals
*/ */
class GpsL2MPcpsAcquisition: public AcquisitionInterface class GpsL2MPcpsAcquisition : public AcquisitionInterface
{ {
public: public:
GpsL2MPcpsAcquisition(ConfigurationInterface* configuration, GpsL2MPcpsAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GpsL2MPcpsAcquisition(); virtual ~GpsL2MPcpsAcquisition();
@ -157,8 +156,8 @@ private:
bool dump_; bool dump_;
bool blocking_; bool blocking_;
std::string dump_filename_; std::string dump_filename_;
std::complex<float> * code_; std::complex<float>* code_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

37
src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc
View File

@ -43,9 +43,8 @@
using google::LogMessage; using google::LogMessage;
GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition( GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
ConfigurationInterface* configuration, std::string role, ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
@ -61,29 +60,28 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
dump_ = configuration_->property(role + ".dump", false); dump_ = configuration_->property(role + ".dump", false);
blocking_ = configuration_->property(role + ".blocking", true); blocking_ = configuration_->property(role + ".blocking", true);
doppler_max_ = configuration->property(role + ".doppler_max", 5000); doppler_max_ = configuration->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0 ) doppler_max_ = FLAGS_doppler_max; if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false);
use_CFAR_algorithm_flag_=configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions
max_dwells_ = configuration_->property(role + ".max_dwells", 1); max_dwells_ = configuration_->property(role + ".max_dwells", 1);
dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename); dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename);
//--- Find number of samples per spreading code ------------------------- //--- Find number of samples per spreading code -------------------------
code_length_ = round(static_cast<double>(fs_in_) code_length_ = round(static_cast<double>(fs_in_) / (GPS_L5i_CODE_RATE_HZ / static_cast<double>(GPS_L5i_CODE_LENGTH_CHIPS)));
/ (GPS_L5i_CODE_RATE_HZ / static_cast<double>(GPS_L5i_CODE_LENGTH_CHIPS)));
vector_length_ = code_length_; vector_length_ = code_length_;
if( bit_transition_flag_ ) if (bit_transition_flag_)
{ {
vector_length_ *= 2; vector_length_ *= 2;
} }
code_ = new gr_complex[vector_length_]; code_ = new gr_complex[vector_length_];
if (item_type_.compare("cshort") == 0 ) if (item_type_.compare("cshort") == 0)
{ {
item_size_ = sizeof(lv_16sc_t); item_size_ = sizeof(lv_16sc_t);
} }
@ -92,9 +90,9 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
} }
acquisition_ = pcps_make_acquisition(1, max_dwells_, acquisition_ = pcps_make_acquisition(1, max_dwells_,
doppler_max_, if_, fs_in_, code_length_, code_length_, doppler_max_, if_, fs_in_, code_length_, code_length_,
bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_, bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_,
dump_filename_, item_size_); dump_filename_, item_size_);
DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")";
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
@ -130,11 +128,11 @@ void GpsL5iPcpsAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0); float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0);
if(pfa == 0.0) if (pfa == 0.0)
{ {
pfa = configuration_->property(role_ + ".pfa", 0.0); pfa = configuration_->property(role_ + ".pfa", 0.0);
} }
if(pfa == 0.0) if (pfa == 0.0)
{ {
threshold_ = threshold; threshold_ = threshold;
} }
@ -143,7 +141,7 @@ void GpsL5iPcpsAcquisition::set_threshold(float threshold)
threshold_ = calculate_threshold(pfa); threshold_ = calculate_threshold(pfa);
} }
DLOG(INFO) << "Channel " << channel_ <<" Threshold = " << threshold_; DLOG(INFO) << "Channel " << channel_ << " Threshold = " << threshold_;
acquisition_->set_threshold(threshold_); acquisition_->set_threshold(threshold_);
} }
@ -188,7 +186,6 @@ void GpsL5iPcpsAcquisition::init()
void GpsL5iPcpsAcquisition::set_local_code() void GpsL5iPcpsAcquisition::set_local_code()
{ {
gps_l5i_code_gen_complex_sampled(code_, gnss_synchro_->PRN, fs_in_); gps_l5i_code_gen_complex_sampled(code_, gnss_synchro_->PRN, fs_in_);
acquisition_->set_local_code(code_); acquisition_->set_local_code(code_);
@ -206,7 +203,6 @@ void GpsL5iPcpsAcquisition::set_state(int state)
} }
float GpsL5iPcpsAcquisition::calculate_threshold(float pfa) float GpsL5iPcpsAcquisition::calculate_threshold(float pfa)
{ {
//Calculate the threshold //Calculate the threshold
@ -215,13 +211,13 @@ float GpsL5iPcpsAcquisition::calculate_threshold(float pfa)
{ {
frequency_bins++; frequency_bins++;
} }
DLOG(INFO) << "Channel " << channel_<< " Pfa = " << pfa; DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
unsigned int ncells = vector_length_ * frequency_bins; unsigned int ncells = vector_length_ * frequency_bins;
double exponent = 1.0 / static_cast<double>(ncells); double exponent = 1.0 / static_cast<double>(ncells);
double val = pow(1.0 - pfa, exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist(lambda);
float threshold = static_cast<float>(quantile(mydist,val)); float threshold = static_cast<float>(quantile(mydist, val));
return threshold; return threshold;
} }
@ -303,4 +299,3 @@ gr::basic_block_sptr GpsL5iPcpsAcquisition::get_right_block()
{ {
return acquisition_; return acquisition_;
} }

10
src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.h
View File

@ -50,12 +50,12 @@ class ConfigurationInterface;
* \brief This class adapts a PCPS acquisition block to an AcquisitionInterface * \brief This class adapts a PCPS acquisition block to an AcquisitionInterface
* for GPS L5i signals * for GPS L5i signals
*/ */
class GpsL5iPcpsAcquisition: public AcquisitionInterface class GpsL5iPcpsAcquisition : public AcquisitionInterface
{ {
public: public:
GpsL5iPcpsAcquisition(ConfigurationInterface* configuration, GpsL5iPcpsAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~GpsL5iPcpsAcquisition(); virtual ~GpsL5iPcpsAcquisition();
@ -156,8 +156,8 @@ private:
bool dump_; bool dump_;
bool blocking_; bool blocking_;
std::string dump_filename_; std::string dump_filename_;
std::complex<float> * code_; std::complex<float>* code_;
Gnss_Synchro * gnss_synchro_; Gnss_Synchro* gnss_synchro_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

981
src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc
View File

File diff suppressed because it is too large Load Diff

167
src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.h
View File

@ -51,15 +51,15 @@ typedef boost::shared_ptr<galileo_e5a_noncoherentIQ_acquisition_caf_cc> galileo_
galileo_e5a_noncoherentIQ_acquisition_caf_cc_sptr galileo_e5a_noncoherentIQ_acquisition_caf_cc_sptr
galileo_e5a_noncoherentIQ_make_acquisition_caf_cc(unsigned int sampled_ms, galileo_e5a_noncoherentIQ_make_acquisition_caf_cc(unsigned int sampled_ms,
unsigned int max_dwells, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
bool dump, bool dump,
std::string dump_filename, std::string dump_filename,
bool both_signal_components_, bool both_signal_components_,
int CAF_window_hz_, int CAF_window_hz_,
int Zero_padding_); int Zero_padding_);
/*! /*!
* \brief This class implements a Parallel Code Phase Search Acquisition. * \brief This class implements a Parallel Code Phase Search Acquisition.
@ -67,37 +67,37 @@ galileo_e5a_noncoherentIQ_make_acquisition_caf_cc(unsigned int sampled_ms,
* Check \ref Navitec2012 "An Open Source Galileo E1 Software Receiver", * Check \ref Navitec2012 "An Open Source Galileo E1 Software Receiver",
* Algorithm 1, for a pseudocode description of this implementation. * Algorithm 1, for a pseudocode description of this implementation.
*/ */
class galileo_e5a_noncoherentIQ_acquisition_caf_cc: public gr::block class galileo_e5a_noncoherentIQ_acquisition_caf_cc : public gr::block
{ {
private: private:
friend galileo_e5a_noncoherentIQ_acquisition_caf_cc_sptr friend galileo_e5a_noncoherentIQ_acquisition_caf_cc_sptr
galileo_e5a_noncoherentIQ_make_acquisition_caf_cc( galileo_e5a_noncoherentIQ_make_acquisition_caf_cc(
unsigned int sampled_ms, unsigned int sampled_ms,
unsigned int max_dwells, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
bool dump, bool dump,
std::string dump_filename, std::string dump_filename,
bool both_signal_components_, bool both_signal_components_,
int CAF_window_hz_, int CAF_window_hz_,
int Zero_padding_); int Zero_padding_);
galileo_e5a_noncoherentIQ_acquisition_caf_cc( galileo_e5a_noncoherentIQ_acquisition_caf_cc(
unsigned int sampled_ms, unsigned int sampled_ms,
unsigned int max_dwells, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
bool dump, bool dump,
std::string dump_filename, std::string dump_filename,
bool both_signal_components_, bool both_signal_components_,
int CAF_window_hz_, int CAF_window_hz_,
int Zero_padding_); int Zero_padding_);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift, void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset); int doppler_offset);
float estimate_input_power(gr_complex *in ); float estimate_input_power(gr_complex* in);
long d_fs_in; long d_fs_in;
long d_freq; long d_freq;
@ -122,7 +122,7 @@ private:
gr_complex* d_inbuffer; gr_complex* d_inbuffer;
gr::fft::fft_complex* d_fft_if; gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft; gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro; Gnss_Synchro* d_gnss_synchro;
unsigned int d_code_phase; unsigned int d_code_phase;
float d_doppler_freq; float d_doppler_freq;
float d_mag; float d_mag;
@ -138,14 +138,14 @@ private:
int d_state; int d_state;
bool d_dump; bool d_dump;
bool d_both_signal_components; bool d_both_signal_components;
// bool d_CAF_filter; // bool d_CAF_filter;
int d_CAF_window_hz; int d_CAF_window_hz;
float* d_CAF_vector; float* d_CAF_vector;
float* d_CAF_vector_I; float* d_CAF_vector_I;
float* d_CAF_vector_Q; float* d_CAF_vector_Q;
// double* d_CAF_vector; // double* d_CAF_vector;
// double* d_CAF_vector_I; // double* d_CAF_vector_I;
// double* d_CAF_vector_Q; // double* d_CAF_vector_Q;
unsigned int d_channel; unsigned int d_channel;
std::string d_dump_filename; std::string d_dump_filename;
unsigned int d_buffer_count; unsigned int d_buffer_count;
@ -155,97 +155,96 @@ public:
/*! /*!
* \brief Default destructor. * \brief Default destructor.
*/ */
~galileo_e5a_noncoherentIQ_acquisition_caf_cc(); ~galileo_e5a_noncoherentIQ_acquisition_caf_cc();
/*! /*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer * \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks. * to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks. * \param p_gnss_synchro Satellite information shared by the processing blocks.
*/ */
inline void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) inline void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{ {
d_gnss_synchro = p_gnss_synchro; d_gnss_synchro = p_gnss_synchro;
} }
/*! /*!
* \brief Returns the maximum peak of grid search. * \brief Returns the maximum peak of grid search.
*/ */
inline unsigned int mag() const inline unsigned int mag() const
{ {
return d_mag; return d_mag;
} }
/*! /*!
* \brief Initializes acquisition algorithm. * \brief Initializes acquisition algorithm.
*/ */
void init(); void init();
/*! /*!
* \brief Sets local code for PCPS acquisition algorithm. * \brief Sets local code for PCPS acquisition algorithm.
* \param code - Pointer to the PRN code. * \param code - Pointer to the PRN code.
*/ */
void set_local_code(std::complex<float> * code, std::complex<float> * codeQ); void set_local_code(std::complex<float>* code, std::complex<float>* codeQ);
/*! /*!
* \brief Starts acquisition algorithm, turning from standby mode to * \brief Starts acquisition algorithm, turning from standby mode to
* active mode * active mode
* \param active - bool that activates/deactivates the block. * \param active - bool that activates/deactivates the block.
*/ */
inline void set_active(bool active) inline void set_active(bool active)
{ {
d_active = active; d_active = active;
} }
/*! /*!
* \brief If set to 1, ensures that acquisition starts at the * \brief If set to 1, ensures that acquisition starts at the
* first available sample. * first available sample.
* \param state - int=1 forces start of acquisition * \param state - int=1 forces start of acquisition
*/ */
void set_state(int state); void set_state(int state);
/*! /*!
* \brief Set acquisition channel unique ID * \brief Set acquisition channel unique ID
* \param channel - receiver channel. * \param channel - receiver channel.
*/ */
inline void set_channel(unsigned int channel) inline void set_channel(unsigned int channel)
{ {
d_channel = channel; d_channel = channel;
} }
/*! /*!
* \brief Set statistics threshold of PCPS algorithm. * \brief Set statistics threshold of PCPS algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012, * \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold). * Algorithm 1, for a definition of this threshold).
*/ */
inline void set_threshold(float threshold) inline void set_threshold(float threshold)
{ {
d_threshold = threshold; d_threshold = threshold;
} }
/*! /*!
* \brief Set maximum Doppler grid search * \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz]. * \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/ */
inline void set_doppler_max(unsigned int doppler_max) inline void set_doppler_max(unsigned int doppler_max)
{ {
d_doppler_max = doppler_max; d_doppler_max = doppler_max;
} }
/*! /*!
* \brief Set Doppler steps for the grid search * \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz]. * \param doppler_step - Frequency bin of the search grid [Hz].
*/ */
inline void set_doppler_step(unsigned int doppler_step) inline void set_doppler_step(unsigned int doppler_step)
{ {
d_doppler_step = doppler_step; d_doppler_step = doppler_step;
} }
/*! /*!
* \brief Parallel Code Phase Search Acquisition signal processing. * \brief Parallel Code Phase Search Acquisition signal processing.
*/ */
int general_work(int noutput_items, gr_vector_int &ninput_items, int general_work(int noutput_items, gr_vector_int& ninput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star& input_items,
gr_vector_void_star &output_items); gr_vector_void_star& output_items);
}; };
#endif /* GALILEO_E5A_NONCOHERENT_IQ_ACQUISITION_CAF_CC_H_ */ #endif /* GALILEO_E5A_NONCOHERENT_IQ_ACQUISITION_CAF_CC_H_ */

461
src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc
View File

@ -40,28 +40,27 @@
using google::LogMessage; using google::LogMessage;
galileo_pcps_8ms_acquisition_cc_sptr galileo_pcps_8ms_make_acquisition_cc( galileo_pcps_8ms_acquisition_cc_sptr galileo_pcps_8ms_make_acquisition_cc(
unsigned int sampled_ms, unsigned int max_dwells, unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool dump, std::string dump_filename) bool dump, std::string dump_filename)
{ {
return galileo_pcps_8ms_acquisition_cc_sptr( return galileo_pcps_8ms_acquisition_cc_sptr(
new galileo_pcps_8ms_acquisition_cc(sampled_ms, max_dwells, doppler_max, freq, fs_in, samples_per_ms, new galileo_pcps_8ms_acquisition_cc(sampled_ms, max_dwells, doppler_max, freq, fs_in, samples_per_ms,
samples_per_code, dump, dump_filename)); samples_per_code, dump, dump_filename));
} }
galileo_pcps_8ms_acquisition_cc::galileo_pcps_8ms_acquisition_cc( galileo_pcps_8ms_acquisition_cc::galileo_pcps_8ms_acquisition_cc(
unsigned int sampled_ms, unsigned int max_dwells, unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool dump, std::string dump_filename) : bool dump,
gr::block("galileo_pcps_8ms_acquisition_cc", std::string dump_filename) : gr::block("galileo_pcps_8ms_acquisition_cc",
gr::io_signature::make(1, 1, sizeof(gr_complex) * sampled_ms * samples_per_ms), gr::io_signature::make(1, 1, sizeof(gr_complex) * sampled_ms * samples_per_ms),
gr::io_signature::make(0, 0, sizeof(gr_complex) * sampled_ms * samples_per_ms)) gr::io_signature::make(0, 0, sizeof(gr_complex) * sampled_ms * samples_per_ms))
{ {
this->message_port_register_out(pmt::mp("events")); this->message_port_register_out(pmt::mp("events"));
d_sample_counter = 0; // SAMPLE COUNTER d_sample_counter = 0; // SAMPLE COUNTER
d_active = false; d_active = false;
d_state = 0; d_state = 0;
d_freq = freq; d_freq = freq;
@ -77,9 +76,9 @@ galileo_pcps_8ms_acquisition_cc::galileo_pcps_8ms_acquisition_cc(
d_input_power = 0.0; d_input_power = 0.0;
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
d_fft_code_A = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_fft_code_A = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_fft_code_B = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_fft_code_B = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitude = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); d_magnitude = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@ -126,22 +125,22 @@ galileo_pcps_8ms_acquisition_cc::~galileo_pcps_8ms_acquisition_cc()
} }
} }
void galileo_pcps_8ms_acquisition_cc::set_local_code(std::complex<float> * code) void galileo_pcps_8ms_acquisition_cc::set_local_code(std::complex<float> *code)
{ {
// code A: two replicas of a primary code // code A: two replicas of a primary code
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_A, d_fft_if->get_outbuf(), d_fft_size); volk_32fc_conjugate_32fc(d_fft_code_A, d_fft_if->get_outbuf(), d_fft_size);
// code B: two replicas of a primary code; the second replica is inverted. // code B: two replicas of a primary code; the second replica is inverted.
volk_32fc_s32fc_multiply_32fc(&(d_fft_if->get_inbuf())[d_samples_per_code], volk_32fc_s32fc_multiply_32fc(&(d_fft_if->get_inbuf())[d_samples_per_code],
&code[d_samples_per_code], gr_complex(-1,0), &code[d_samples_per_code], gr_complex(-1, 0),
d_samples_per_code); d_samples_per_code);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_B, d_fft_if->get_outbuf(), d_fft_size); volk_32fc_conjugate_32fc(d_fft_code_B, d_fft_if->get_outbuf(), d_fft_size);
@ -163,22 +162,22 @@ void galileo_pcps_8ms_acquisition_cc::init()
// Count the number of bins // Count the number of bins
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
for (int doppler = static_cast<int>(-d_doppler_max); for (int doppler = static_cast<int>(-d_doppler_max);
doppler <= static_cast<int>(d_doppler_max); doppler <= static_cast<int>(d_doppler_max);
doppler += d_doppler_step) doppler += d_doppler_step)
{ {
d_num_doppler_bins++; d_num_doppler_bins++;
} }
// Create the carrier Doppler wipeoff signals // Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_bins];
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index; int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = static_cast<float>(GALILEO_TWO_PI) * (d_freq + doppler) / static_cast<float>(d_fs_in); float phase_step_rad = static_cast<float>(GALILEO_TWO_PI) * (d_freq + doppler) / static_cast<float>(d_fs_in);
float _phase[1]; float _phase[1];
_phase[0] = 0; _phase[0] = 0;
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], - phase_step_rad, _phase, d_fft_size); volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase, d_fft_size);
} }
} }
@ -197,7 +196,8 @@ void galileo_pcps_8ms_acquisition_cc::set_state(int state)
d_test_statistics = 0.0; d_test_statistics = 0.0;
} }
else if (d_state == 0) else if (d_state == 0)
{} {
}
else else
{ {
LOG(ERROR) << "State can only be set to 0 or 1"; LOG(ERROR) << "State can only be set to 0 or 1";
@ -205,216 +205,215 @@ void galileo_pcps_8ms_acquisition_cc::set_state(int state)
} }
int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items, int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused))) gr_vector_void_star &output_items __attribute__((unused)))
{ {
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
switch (d_state) switch (d_state)
{
case 0:
{ {
if (d_active) case 0:
{ {
//restart acquisition variables if (d_active)
d_gnss_synchro->Acq_delay_samples = 0.0; {
d_gnss_synchro->Acq_doppler_hz = 0.0; //restart acquisition variables
d_gnss_synchro->Acq_samplestamp_samples = 0; d_gnss_synchro->Acq_delay_samples = 0.0;
d_well_count = 0; d_gnss_synchro->Acq_doppler_hz = 0.0;
d_mag = 0.0; d_gnss_synchro->Acq_samplestamp_samples = 0;
d_input_power = 0.0; d_well_count = 0;
d_test_statistics = 0.0; d_mag = 0.0;
d_input_power = 0.0;
d_test_statistics = 0.0;
d_state = 1; d_state = 1;
} }
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]); consume_each(ninput_items[0]);
break; break;
}
case 1:
{
// initialize acquisition algorithm
int doppler;
uint32_t indext = 0;
uint32_t indext_A = 0;
uint32_t indext_B = 0;
float magt = 0.0;
float magt_A = 0.0;
float magt_B = 0.0;
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_input_power = 0.0;
d_mag = 0.0;
d_sample_counter += d_fft_size; // sample counter
d_well_count++;
DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN
<< " ,sample stamp: " << d_sample_counter << ", threshold: "
<< d_threshold << ", doppler_max: " << d_doppler_max
<< ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
// doppler search steps
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal
d_fft_if->execute();
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code A reference using SIMD operations with
// VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_A, d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// Search maximum
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_A, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW
magt_A = d_magnitude[indext_A] / (fft_normalization_factor * fft_normalization_factor);
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code B reference using SIMD operations with
// VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_B, d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// Search maximum
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_B, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW
magt_B = d_magnitude[indext_B] / (fft_normalization_factor * fft_normalization_factor);
// Take the greater magnitude
if (magt_A >= magt_B)
{
magt = magt_A;
indext = indext_A;
}
else
{
magt = magt_B;
indext = indext_B;
}
// 4- record the maximum peak and the associated synchronization parameters
if (d_mag < magt)
{
d_mag = magt;
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
}
// Record results to file if required
if (d_dump)
{
std::stringstream filename;
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<< "_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
}
}
// 5- Compute the test statistics and compare to the threshold
//d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;
d_test_statistics = d_mag / d_input_power;
if (d_test_statistics > d_threshold)
{
d_state = 2; // Positive acquisition
}
else if (d_well_count == d_max_dwells)
{
d_state = 3; // Negative acquisition
}
consume_each(1);
break;
}
case 2:
{
// 6.1- Declare positive acquisition using a message port
DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 1;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
case 3:
{
// 6.2- Declare negative acquisition using a message port
DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 2;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
} }
case 1:
{
// initialize acquisition algorithm
int doppler;
uint32_t indext = 0;
uint32_t indext_A = 0;
uint32_t indext_B = 0;
float magt = 0.0;
float magt_A = 0.0;
float magt_B = 0.0;
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_input_power = 0.0;
d_mag = 0.0;
d_sample_counter += d_fft_size; // sample counter
d_well_count++;
DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " "<< d_gnss_synchro->PRN
<< " ,sample stamp: " << d_sample_counter << ", threshold: "
<< d_threshold << ", doppler_max: " << d_doppler_max
<< ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
// doppler search steps
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal
d_fft_if->execute();
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code A reference using SIMD operations with
// VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_A, d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// Search maximum
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_A, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW
magt_A = d_magnitude[indext_A] / (fft_normalization_factor * fft_normalization_factor);
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code B reference using SIMD operations with
// VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_B, d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// Search maximum
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_B, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW
magt_B = d_magnitude[indext_B] / (fft_normalization_factor * fft_normalization_factor);
// Take the greater magnitude
if (magt_A >= magt_B)
{
magt = magt_A;
indext = indext_A;
}
else
{
magt = magt_B;
indext = indext_B;
}
// 4- record the maximum peak and the associated synchronization parameters
if (d_mag < magt)
{
d_mag = magt;
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
}
// Record results to file if required
if (d_dump)
{
std::stringstream filename;
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<<"_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char*>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
}
}
// 5- Compute the test statistics and compare to the threshold
//d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;
d_test_statistics = d_mag / d_input_power;
if (d_test_statistics > d_threshold)
{
d_state = 2; // Positive acquisition
}
else if (d_well_count == d_max_dwells)
{
d_state = 3; // Negative acquisition
}
consume_each(1);
break;
}
case 2:
{
// 6.1- Declare positive acquisition using a message port
DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 1;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
case 3:
{
// 6.2- Declare negative acquisition using a message port
DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 2;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
}
return noutput_items; return noutput_items;
} }

32
src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.h
View File

@ -45,31 +45,31 @@ typedef boost::shared_ptr<galileo_pcps_8ms_acquisition_cc> galileo_pcps_8ms_acqu
galileo_pcps_8ms_acquisition_cc_sptr galileo_pcps_8ms_acquisition_cc_sptr
galileo_pcps_8ms_make_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, galileo_pcps_8ms_make_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool dump, std::string dump_filename); bool dump, std::string dump_filename);
/*! /*!
* \brief This class implements a Parallel Code Phase Search Acquisition for * \brief This class implements a Parallel Code Phase Search Acquisition for
* Galileo E1 signals with coherent integration time = 8 ms (two codes) * Galileo E1 signals with coherent integration time = 8 ms (two codes)
*/ */
class galileo_pcps_8ms_acquisition_cc: public gr::block class galileo_pcps_8ms_acquisition_cc : public gr::block
{ {
private: private:
friend galileo_pcps_8ms_acquisition_cc_sptr friend galileo_pcps_8ms_acquisition_cc_sptr
galileo_pcps_8ms_make_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, galileo_pcps_8ms_make_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool dump, std::string dump_filename); bool dump, std::string dump_filename);
galileo_pcps_8ms_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, galileo_pcps_8ms_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool dump, std::string dump_filename); bool dump, std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift, void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset); int doppler_offset);
long d_fs_in; long d_fs_in;
long d_freq; long d_freq;
@ -91,7 +91,7 @@ private:
gr_complex* d_fft_code_B; gr_complex* d_fft_code_B;
gr::fft::fft_complex* d_fft_if; gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft; gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro; Gnss_Synchro* d_gnss_synchro;
unsigned int d_code_phase; unsigned int d_code_phase;
float d_doppler_freq; float d_doppler_freq;
float d_mag; float d_mag;
@ -138,7 +138,7 @@ public:
* \brief Sets local code for PCPS acquisition algorithm. * \brief Sets local code for PCPS acquisition algorithm.
* \param code - Pointer to the PRN code. * \param code - Pointer to the PRN code.
*/ */
void set_local_code(std::complex<float> * code); void set_local_code(std::complex<float>* code);
/*! /*!
* \brief Starts acquisition algorithm, turning from standby mode to * \brief Starts acquisition algorithm, turning from standby mode to
@ -197,9 +197,9 @@ public:
/*! /*!
* \brief Parallel Code Phase Search Acquisition signal processing. * \brief Parallel Code Phase Search Acquisition signal processing.
*/ */
int general_work(int noutput_items, gr_vector_int &ninput_items, int general_work(int noutput_items, gr_vector_int& ninput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star& input_items,
gr_vector_void_star &output_items); gr_vector_void_star& output_items);
}; };
#endif /* GNSS_SDR_PCPS_8MS_ACQUISITION_CC_H_*/ #endif /* GNSS_SDR_PCPS_8MS_ACQUISITION_CC_H_*/

201
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc
View File

@ -34,8 +34,8 @@
*/ */
#include "pcps_acquisition.h" #include "pcps_acquisition.h"
#include "GPS_L1_CA.h" // for GPS_TWO_PI #include "GPS_L1_CA.h" // for GPS_TWO_PI
#include "GLONASS_L1_CA.h" // for GLONASS_TWO_PI #include "GLONASS_L1_CA.h" // for GLONASS_TWO_PI
#include <glog/logging.h> #include <glog/logging.h>
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <matio.h> #include <matio.h>
@ -46,33 +46,33 @@
using google::LogMessage; using google::LogMessage;
pcps_acquisition_sptr pcps_make_acquisition( pcps_acquisition_sptr pcps_make_acquisition(
unsigned int sampled_ms, unsigned int max_dwells, unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool use_CFAR_algorithm_flag, bool bit_transition_flag, bool use_CFAR_algorithm_flag,
bool dump, bool blocking, bool dump, bool blocking,
std::string dump_filename, size_t it_size) std::string dump_filename, size_t it_size)
{ {
return pcps_acquisition_sptr( return pcps_acquisition_sptr(
new pcps_acquisition(sampled_ms, max_dwells, doppler_max, freq, fs_in, samples_per_ms, new pcps_acquisition(sampled_ms, max_dwells, doppler_max, freq, fs_in, samples_per_ms,
samples_per_code, bit_transition_flag, use_CFAR_algorithm_flag, dump, blocking, dump_filename, it_size)); samples_per_code, bit_transition_flag, use_CFAR_algorithm_flag, dump, blocking, dump_filename, it_size));
} }
pcps_acquisition::pcps_acquisition( pcps_acquisition::pcps_acquisition(
unsigned int sampled_ms, unsigned int max_dwells, unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool use_CFAR_algorithm_flag, bool bit_transition_flag, bool use_CFAR_algorithm_flag,
bool dump, bool blocking, bool dump, bool blocking,
std::string dump_filename, size_t it_size) : std::string dump_filename,
gr::block("pcps_acquisition", size_t it_size) : gr::block("pcps_acquisition",
gr::io_signature::make(1, 1, it_size * sampled_ms * samples_per_ms * ( bit_transition_flag ? 2 : 1 )), gr::io_signature::make(1, 1, it_size * sampled_ms * samples_per_ms * (bit_transition_flag ? 2 : 1)),
gr::io_signature::make(0, 0, it_size * sampled_ms * samples_per_ms * ( bit_transition_flag ? 2 : 1 )) ) gr::io_signature::make(0, 0, it_size * sampled_ms * samples_per_ms * (bit_transition_flag ? 2 : 1)))
{ {
this->message_port_register_out(pmt::mp("events")); this->message_port_register_out(pmt::mp("events"));
d_sample_counter = 0; // SAMPLE COUNTER d_sample_counter = 0; // SAMPLE COUNTER
d_active = false; d_active = false;
d_state = 0; d_state = 0;
d_freq = freq; d_freq = freq;
@ -95,8 +95,14 @@ pcps_acquisition::pcps_acquisition(
d_code_phase = 0; d_code_phase = 0;
d_test_statistics = 0.0; d_test_statistics = 0.0;
d_channel = 0; d_channel = 0;
if(it_size == sizeof(gr_complex)) { d_cshort = false; } if (it_size == sizeof(gr_complex))
else { d_cshort = true; } {
d_cshort = false;
}
else
{
d_cshort = true;
}
// COD: // COD:
// Experimenting with the overlap/save technique for handling bit trannsitions // Experimenting with the overlap/save technique for handling bit trannsitions
@ -108,10 +114,10 @@ pcps_acquisition::pcps_acquisition(
// //
// We can avoid this by doing linear correlation, effectively doubling the // We can avoid this by doing linear correlation, effectively doubling the
// size of the input buffer and padding the code with zeros. // size of the input buffer and padding the code with zeros.
if( d_bit_transition_flag ) if (d_bit_transition_flag)
{ {
d_fft_size *= 2; d_fft_size *= 2;
d_max_dwells = 1; //Activation of d_bit_transition_flag invalidates the value of d_max_dwells d_max_dwells = 1; //Activation of d_bit_transition_flag invalidates the value of d_max_dwells
} }
d_fft_codes = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_fft_codes = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
@ -131,7 +137,7 @@ pcps_acquisition::pcps_acquisition(
d_blocking = blocking; d_blocking = blocking;
d_worker_active = false; d_worker_active = false;
d_data_buffer = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_data_buffer = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
if(d_cshort) if (d_cshort)
{ {
d_data_buffer_sc = static_cast<lv_16sc_t*>(volk_gnsssdr_malloc(d_fft_size * sizeof(lv_16sc_t), volk_gnsssdr_get_alignment())); d_data_buffer_sc = static_cast<lv_16sc_t*>(volk_gnsssdr_malloc(d_fft_size * sizeof(lv_16sc_t), volk_gnsssdr_get_alignment()));
} }
@ -158,16 +164,19 @@ pcps_acquisition::~pcps_acquisition()
delete d_ifft; delete d_ifft;
delete d_fft_if; delete d_fft_if;
volk_gnsssdr_free(d_data_buffer); volk_gnsssdr_free(d_data_buffer);
if(d_cshort) { volk_gnsssdr_free(d_data_buffer_sc); } if (d_cshort)
{
volk_gnsssdr_free(d_data_buffer_sc);
}
} }
void pcps_acquisition::set_local_code(std::complex<float> * code) void pcps_acquisition::set_local_code(std::complex<float>* code)
{ {
// reset the intermediate frequency // reset the intermediate frequency
d_freq = d_old_freq; d_freq = d_old_freq;
// This will check if it's fdma, if yes will update the intermediate frequency and the doppler grid // This will check if it's fdma, if yes will update the intermediate frequency and the doppler grid
if( is_fdma() ) if (is_fdma())
{ {
update_grid_doppler_wipeoffs(); update_grid_doppler_wipeoffs();
} }
@ -175,11 +184,11 @@ void pcps_acquisition::set_local_code(std::complex<float> * code)
// Here we want to create a buffer that looks like this: // Here we want to create a buffer that looks like this:
// [ 0 0 0 ... 0 c_0 c_1 ... c_L] // [ 0 0 0 ... 0 c_0 c_1 ... c_L]
// where c_i is the local code and there are L zeros and L chips // where c_i is the local code and there are L zeros and L chips
gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler
if( d_bit_transition_flag ) if (d_bit_transition_flag)
{ {
int offset = d_fft_size / 2; int offset = d_fft_size / 2;
std::fill_n( d_fft_if->get_inbuf(), offset, gr_complex( 0.0, 0.0 ) ); std::fill_n(d_fft_if->get_inbuf(), offset, gr_complex(0.0, 0.0));
memcpy(d_fft_if->get_inbuf() + offset, code, sizeof(gr_complex) * offset); memcpy(d_fft_if->get_inbuf() + offset, code, sizeof(gr_complex) * offset);
} }
else else
@ -187,7 +196,7 @@ void pcps_acquisition::set_local_code(std::complex<float> * code)
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size); memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size);
} }
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size); volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
} }
@ -195,7 +204,7 @@ void pcps_acquisition::set_local_code(std::complex<float> * code)
bool pcps_acquisition::is_fdma() bool pcps_acquisition::is_fdma()
{ {
// Dealing with FDMA system // Dealing with FDMA system
if( strcmp(d_gnss_synchro->Signal,"1G") == 0 ) if (strcmp(d_gnss_synchro->Signal, "1G") == 0)
{ {
d_freq += DFRQ1_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN); d_freq += DFRQ1_GLO * GLONASS_PRN.at(d_gnss_synchro->PRN);
LOG(INFO) << "Trying to acquire SV PRN " << d_gnss_synchro->PRN << " with freq " << d_freq << " in Glonass Channel " << GLONASS_PRN.at(d_gnss_synchro->PRN) << std::endl; LOG(INFO) << "Trying to acquire SV PRN " << d_gnss_synchro->PRN << " with freq " << d_freq << " in Glonass Channel " << GLONASS_PRN.at(d_gnss_synchro->PRN) << std::endl;
@ -213,7 +222,7 @@ void pcps_acquisition::update_local_carrier(gr_complex* carrier_vector, int corr
float phase_step_rad = GPS_TWO_PI * freq / static_cast<float>(d_fs_in); float phase_step_rad = GPS_TWO_PI * freq / static_cast<float>(d_fs_in);
float _phase[1]; float _phase[1];
_phase[0] = 0; _phase[0] = 0;
volk_gnsssdr_s32f_sincos_32fc(carrier_vector, - phase_step_rad, _phase, correlator_length_samples); volk_gnsssdr_s32f_sincos_32fc(carrier_vector, -phase_step_rad, _phase, correlator_length_samples);
} }
@ -230,7 +239,7 @@ void pcps_acquisition::init()
d_mag = 0.0; d_mag = 0.0;
d_input_power = 0.0; d_input_power = 0.0;
d_num_doppler_bins = static_cast<unsigned int>(std::ceil( static_cast<double>(static_cast<int>(d_doppler_max) - static_cast<int>(-d_doppler_max)) / static_cast<double>(d_doppler_step))); d_num_doppler_bins = static_cast<unsigned int>(std::ceil(static_cast<double>(static_cast<int>(d_doppler_max) - static_cast<int>(-d_doppler_max)) / static_cast<double>(d_doppler_step)));
// Create the carrier Doppler wipeoff signals // Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
@ -243,7 +252,7 @@ void pcps_acquisition::init()
} }
d_worker_active = false; d_worker_active = false;
if(d_dump) if (d_dump)
{ {
unsigned int effective_fft_size = (d_bit_transition_flag ? (d_fft_size / 2) : d_fft_size); unsigned int effective_fft_size = (d_bit_transition_flag ? (d_fft_size / 2) : d_fft_size);
grid_ = arma::fmat(effective_fft_size, d_num_doppler_bins, arma::fill::zeros); grid_ = arma::fmat(effective_fft_size, d_num_doppler_bins, arma::fill::zeros);
@ -264,7 +273,7 @@ void pcps_acquisition::update_grid_doppler_wipeoffs()
void pcps_acquisition::set_state(int state) void pcps_acquisition::set_state(int state)
{ {
gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler
d_state = state; d_state = state;
if (d_state == 1) if (d_state == 1)
{ {
@ -278,7 +287,8 @@ void pcps_acquisition::set_state(int state)
d_active = true; d_active = true;
} }
else if (d_state == 0) else if (d_state == 0)
{} {
}
else else
{ {
LOG(ERROR) << "State can only be set to 0 or 1"; LOG(ERROR) << "State can only be set to 0 or 1";
@ -323,8 +333,8 @@ void pcps_acquisition::send_negative_acquisition()
int pcps_acquisition::general_work(int noutput_items __attribute__((unused)), int pcps_acquisition::general_work(int noutput_items __attribute__((unused)),
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_int& ninput_items, gr_vector_const_void_star& input_items,
gr_vector_void_star &output_items __attribute__((unused))) gr_vector_void_star& output_items __attribute__((unused)))
{ {
/* /*
* By J.Arribas, L.Esteve and M.Molina * By J.Arribas, L.Esteve and M.Molina
@ -338,56 +348,62 @@ int pcps_acquisition::general_work(int noutput_items __attribute__((unused)),
*/ */
gr::thread::scoped_lock lk(d_setlock); gr::thread::scoped_lock lk(d_setlock);
if(!d_active || d_worker_active) if (!d_active || d_worker_active)
{ {
d_sample_counter += d_fft_size * ninput_items[0]; d_sample_counter += d_fft_size * ninput_items[0];
consume_each(ninput_items[0]); consume_each(ninput_items[0]);
return 0; return 0;
} }
switch(d_state) switch (d_state)
{
case 0:
{ {
//restart acquisition variables case 0:
d_gnss_synchro->Acq_delay_samples = 0.0; {
d_gnss_synchro->Acq_doppler_hz = 0.0; //restart acquisition variables
d_gnss_synchro->Acq_samplestamp_samples = 0; d_gnss_synchro->Acq_delay_samples = 0.0;
d_well_count = 0; d_gnss_synchro->Acq_doppler_hz = 0.0;
d_mag = 0.0; d_gnss_synchro->Acq_samplestamp_samples = 0;
d_input_power = 0.0; d_well_count = 0;
d_test_statistics = 0.0; d_mag = 0.0;
d_state = 1; d_input_power = 0.0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter d_test_statistics = 0.0;
consume_each(ninput_items[0]); d_state = 1;
break; d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
} consume_each(ninput_items[0]);
break;
}
case 1: case 1:
{ {
// Copy the data to the core and let it know that new data is available // Copy the data to the core and let it know that new data is available
if(d_cshort) { memcpy(d_data_buffer_sc, input_items[0], d_fft_size * sizeof(lv_16sc_t)); } if (d_cshort)
else { memcpy(d_data_buffer, input_items[0], d_fft_size * sizeof(gr_complex)); } {
if(d_blocking) memcpy(d_data_buffer_sc, input_items[0], d_fft_size * sizeof(lv_16sc_t));
{ }
lk.unlock(); else
acquisition_core(d_sample_counter); {
} memcpy(d_data_buffer, input_items[0], d_fft_size * sizeof(gr_complex));
else }
{ if (d_blocking)
gr::thread::thread d_worker(&pcps_acquisition::acquisition_core, this, d_sample_counter); {
d_worker_active = true; lk.unlock();
} acquisition_core(d_sample_counter);
d_sample_counter += d_fft_size; }
consume_each(1); else
break; {
gr::thread::thread d_worker(&pcps_acquisition::acquisition_core, this, d_sample_counter);
d_worker_active = true;
}
d_sample_counter += d_fft_size;
consume_each(1);
break;
}
} }
}
return 0; return 0;
} }
void pcps_acquisition::acquisition_core( unsigned long int samp_count ) void pcps_acquisition::acquisition_core(unsigned long int samp_count)
{ {
gr::thread::scoped_lock lk(d_setlock); gr::thread::scoped_lock lk(d_setlock);
@ -395,9 +411,12 @@ void pcps_acquisition::acquisition_core( unsigned long int samp_count )
int doppler; int doppler;
uint32_t indext = 0; uint32_t indext = 0;
float magt = 0.0; float magt = 0.0;
const gr_complex* in = d_data_buffer; //Get the input samples pointer const gr_complex* in = d_data_buffer; //Get the input samples pointer
int effective_fft_size = ( d_bit_transition_flag ? d_fft_size/2 : d_fft_size ); int effective_fft_size = (d_bit_transition_flag ? d_fft_size / 2 : d_fft_size);
if(d_cshort) { volk_gnsssdr_16ic_convert_32fc(d_data_buffer, d_data_buffer_sc, d_fft_size); } if (d_cshort)
{
volk_gnsssdr_16ic_convert_32fc(d_data_buffer, d_data_buffer_sc, d_fft_size);
}
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size); float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_input_power = 0.0; d_input_power = 0.0;
@ -409,7 +428,7 @@ void pcps_acquisition::acquisition_core( unsigned long int samp_count )
<< " ,sample stamp: " << samp_count << ", threshold: " << " ,sample stamp: " << samp_count << ", threshold: "
<< d_threshold << ", doppler_max: " << d_doppler_max << d_threshold << ", doppler_max: " << d_doppler_max
<< ", doppler_step: " << d_doppler_step << ", doppler_step: " << d_doppler_step
<< ", use_CFAR_algorithm_flag: " << ( d_use_CFAR_algorithm_flag ? "true" : "false" ); << ", use_CFAR_algorithm_flag: " << (d_use_CFAR_algorithm_flag ? "true" : "false");
lk.unlock(); lk.unlock();
if (d_use_CFAR_algorithm_flag) if (d_use_CFAR_algorithm_flag)
@ -439,7 +458,7 @@ void pcps_acquisition::acquisition_core( unsigned long int samp_count )
d_ifft->execute(); d_ifft->execute();
// Search maximum // Search maximum
size_t offset = ( d_bit_transition_flag ? effective_fft_size : 0 ); size_t offset = (d_bit_transition_flag ? effective_fft_size : 0);
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf() + offset, effective_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf() + offset, effective_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude, effective_fft_size); volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude, effective_fft_size);
magt = d_magnitude[indext]; magt = d_magnitude[indext];
@ -484,7 +503,7 @@ void pcps_acquisition::acquisition_core( unsigned long int samp_count )
if (d_dump) if (d_dump)
{ {
memcpy(grid_.colptr(doppler_index), d_magnitude, sizeof(float) * effective_fft_size); memcpy(grid_.colptr(doppler_index), d_magnitude, sizeof(float) * effective_fft_size);
if(doppler_index == (d_num_doppler_bins - 1)) if (doppler_index == (d_num_doppler_bins - 1))
{ {
std::string filename = d_dump_filename; std::string filename = d_dump_filename;
filename.append("_"); filename.append("_");
@ -496,7 +515,7 @@ void pcps_acquisition::acquisition_core( unsigned long int samp_count )
filename.append(std::to_string(d_gnss_synchro->PRN)); filename.append(std::to_string(d_gnss_synchro->PRN));
filename.append(".mat"); filename.append(".mat");
mat_t* matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); mat_t* matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73);
if(matfp == NULL) if (matfp == NULL)
{ {
std::cout << "Unable to create or open Acquisition dump file" << std::endl; std::cout << "Unable to create or open Acquisition dump file" << std::endl;
d_dump = false; d_dump = false;
@ -505,17 +524,17 @@ void pcps_acquisition::acquisition_core( unsigned long int samp_count )
{ {
size_t dims[2] = {static_cast<size_t>(effective_fft_size), static_cast<size_t>(d_num_doppler_bins)}; size_t dims[2] = {static_cast<size_t>(effective_fft_size), static_cast<size_t>(d_num_doppler_bins)};
matvar_t* matvar = Mat_VarCreate("grid", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, grid_.memptr(), 0); matvar_t* matvar = Mat_VarCreate("grid", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, grid_.memptr(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
dims[0] = static_cast<size_t>(1); dims[0] = static_cast<size_t>(1);
dims[1] = static_cast<size_t>(1); dims[1] = static_cast<size_t>(1);
matvar = Mat_VarCreate("doppler_max", MAT_C_SINGLE, MAT_T_UINT32, 1, dims, &d_doppler_max, 0); matvar = Mat_VarCreate("doppler_max", MAT_C_SINGLE, MAT_T_UINT32, 1, dims, &d_doppler_max, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
matvar = Mat_VarCreate("doppler_step", MAT_C_SINGLE, MAT_T_UINT32, 1, dims, &d_doppler_step, 0); matvar = Mat_VarCreate("doppler_step", MAT_C_SINGLE, MAT_T_UINT32, 1, dims, &d_doppler_step, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
Mat_Close(matfp); Mat_Close(matfp);
@ -528,7 +547,7 @@ void pcps_acquisition::acquisition_core( unsigned long int samp_count )
{ {
if (d_test_statistics > d_threshold) if (d_test_statistics > d_threshold)
{ {
d_state = 0; // Positive acquisition d_state = 0; // Positive acquisition
d_active = false; d_active = false;
send_positive_acquisition(); send_positive_acquisition();
} }
@ -541,17 +560,17 @@ void pcps_acquisition::acquisition_core( unsigned long int samp_count )
} }
else else
{ {
if (d_well_count == d_max_dwells) // d_max_dwells = 2 if (d_well_count == d_max_dwells) // d_max_dwells = 2
{ {
if (d_test_statistics > d_threshold) if (d_test_statistics > d_threshold)
{ {
d_state = 0; // Positive acquisition d_state = 0; // Positive acquisition
d_active = false; d_active = false;
send_positive_acquisition(); send_positive_acquisition();
} }
else else
{ {
d_state = 0; // Negative acquisition d_state = 0; // Negative acquisition
d_active = false; d_active = false;
send_negative_acquisition(); send_negative_acquisition();
} }

75
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h
View File

@ -66,11 +66,11 @@ typedef boost::shared_ptr<pcps_acquisition> pcps_acquisition_sptr;
pcps_acquisition_sptr pcps_acquisition_sptr
pcps_make_acquisition(unsigned int sampled_ms, unsigned int max_dwells, pcps_make_acquisition(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool use_CFAR_algorithm_flag, bool bit_transition_flag, bool use_CFAR_algorithm_flag,
bool dump, bool blocking, bool dump, bool blocking,
std::string dump_filename, size_t it_size); std::string dump_filename, size_t it_size);
/*! /*!
* \brief This class implements a Parallel Code Phase Search Acquisition. * \brief This class implements a Parallel Code Phase Search Acquisition.
@ -78,29 +78,29 @@ pcps_make_acquisition(unsigned int sampled_ms, unsigned int max_dwells,
* Check \ref Navitec2012 "An Open Source Galileo E1 Software Receiver", * Check \ref Navitec2012 "An Open Source Galileo E1 Software Receiver",
* Algorithm 1, for a pseudocode description of this implementation. * Algorithm 1, for a pseudocode description of this implementation.
*/ */
class pcps_acquisition: public gr::block class pcps_acquisition : public gr::block
{ {
private: private:
friend pcps_acquisition_sptr friend pcps_acquisition_sptr
pcps_make_acquisition(unsigned int sampled_ms, unsigned int max_dwells, pcps_make_acquisition(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool use_CFAR_algorithm_flag, bool bit_transition_flag, bool use_CFAR_algorithm_flag,
bool dump, bool blocking, bool dump, bool blocking,
std::string dump_filename, size_t it_size); std::string dump_filename, size_t it_size);
pcps_acquisition(unsigned int sampled_ms, unsigned int max_dwells, pcps_acquisition(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool use_CFAR_algorithm_flag, bool bit_transition_flag, bool use_CFAR_algorithm_flag,
bool dump, bool blocking, bool dump, bool blocking,
std::string dump_filename, size_t it_size); std::string dump_filename, size_t it_size);
void update_local_carrier(gr_complex* carrier_vector, int correlator_length_samples, float freq); void update_local_carrier(gr_complex* carrier_vector, int correlator_length_samples, float freq);
void update_grid_doppler_wipeoffs(); void update_grid_doppler_wipeoffs();
bool is_fdma(); bool is_fdma();
void acquisition_core( unsigned long int samp_count ); void acquisition_core(unsigned long int samp_count);
void send_negative_acquisition(); void send_negative_acquisition();
@ -147,14 +147,14 @@ private:
public: public:
~pcps_acquisition(); ~pcps_acquisition();
/*! /*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer * \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks. * to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks. * \param p_gnss_synchro Satellite information shared by the processing blocks.
*/ */
inline void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) inline void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{ {
gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler
d_gnss_synchro = p_gnss_synchro; d_gnss_synchro = p_gnss_synchro;
} }
@ -166,83 +166,82 @@ public:
return d_mag; return d_mag;
} }
/*! /*!
* \brief Initializes acquisition algorithm. * \brief Initializes acquisition algorithm.
*/ */
void init(); void init();
/*! /*!
* \brief Sets local code for PCPS acquisition algorithm. * \brief Sets local code for PCPS acquisition algorithm.
* \param code - Pointer to the PRN code. * \param code - Pointer to the PRN code.
*/ */
void set_local_code(std::complex<float> * code); void set_local_code(std::complex<float>* code);
/*! /*!
* \brief Starts acquisition algorithm, turning from standby mode to * \brief Starts acquisition algorithm, turning from standby mode to
* active mode * active mode
* \param active - bool that activates/deactivates the block. * \param active - bool that activates/deactivates the block.
*/ */
inline void set_active(bool active) inline void set_active(bool active)
{ {
gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler
d_active = active; d_active = active;
} }
/*! /*!
* \brief If set to 1, ensures that acquisition starts at the * \brief If set to 1, ensures that acquisition starts at the
* first available sample. * first available sample.
* \param state - int=1 forces start of acquisition * \param state - int=1 forces start of acquisition
*/ */
void set_state(int state); void set_state(int state);
/*! /*!
* \brief Set acquisition channel unique ID * \brief Set acquisition channel unique ID
* \param channel - receiver channel. * \param channel - receiver channel.
*/ */
inline void set_channel(unsigned int channel) inline void set_channel(unsigned int channel)
{ {
gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler
d_channel = channel; d_channel = channel;
} }
/*! /*!
* \brief Set statistics threshold of PCPS algorithm. * \brief Set statistics threshold of PCPS algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012, * \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold). * Algorithm 1, for a definition of this threshold).
*/ */
inline void set_threshold(float threshold) inline void set_threshold(float threshold)
{ {
gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler
d_threshold = threshold; d_threshold = threshold;
} }
/*! /*!
* \brief Set maximum Doppler grid search * \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz]. * \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/ */
inline void set_doppler_max(unsigned int doppler_max) inline void set_doppler_max(unsigned int doppler_max)
{ {
gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler
d_doppler_max = doppler_max; d_doppler_max = doppler_max;
} }
/*! /*!
* \brief Set Doppler steps for the grid search * \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz]. * \param doppler_step - Frequency bin of the search grid [Hz].
*/ */
inline void set_doppler_step(unsigned int doppler_step) inline void set_doppler_step(unsigned int doppler_step)
{ {
gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler
d_doppler_step = doppler_step; d_doppler_step = doppler_step;
} }
/*! /*!
* \brief Parallel Code Phase Search Acquisition signal processing. * \brief Parallel Code Phase Search Acquisition signal processing.
*/ */
int general_work(int noutput_items, gr_vector_int &ninput_items, int general_work(int noutput_items, gr_vector_int& ninput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star& input_items,
gr_vector_void_star &output_items); gr_vector_void_star& output_items);
}; };
#endif /* GNSS_SDR_PCPS_ACQUISITION_H_*/ #endif /* GNSS_SDR_PCPS_ACQUISITION_H_*/

241
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc
View File

@ -38,34 +38,32 @@
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <volk/volk.h> #include <volk/volk.h>
#include <volk_gnsssdr/volk_gnsssdr.h> #include <volk_gnsssdr/volk_gnsssdr.h>
#include <algorithm> // std::rotate, std::fill_n #include <algorithm> // std::rotate, std::fill_n
#include <sstream> #include <sstream>
using google::LogMessage; using google::LogMessage;
pcps_acquisition_fine_doppler_cc_sptr pcps_make_acquisition_fine_doppler_cc( pcps_acquisition_fine_doppler_cc_sptr pcps_make_acquisition_fine_doppler_cc(
int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq, int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq,
long fs_in, int samples_per_ms, bool dump, long fs_in, int samples_per_ms, bool dump,
std::string dump_filename) std::string dump_filename)
{ {
return pcps_acquisition_fine_doppler_cc_sptr( return pcps_acquisition_fine_doppler_cc_sptr(
new pcps_acquisition_fine_doppler_cc(max_dwells, sampled_ms, doppler_max, doppler_min, freq, new pcps_acquisition_fine_doppler_cc(max_dwells, sampled_ms, doppler_max, doppler_min, freq,
fs_in, samples_per_ms, dump, dump_filename)); fs_in, samples_per_ms, dump, dump_filename));
} }
pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc( pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(
int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq, int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq,
long fs_in, int samples_per_ms, bool dump, long fs_in, int samples_per_ms, bool dump,
std::string dump_filename) : std::string dump_filename) : gr::block("pcps_acquisition_fine_doppler_cc",
gr::block("pcps_acquisition_fine_doppler_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(0, 0, sizeof(gr_complex)))
gr::io_signature::make(0, 0, sizeof(gr_complex)))
{ {
this->message_port_register_out(pmt::mp("events")); this->message_port_register_out(pmt::mp("events"));
d_sample_counter = 0; // SAMPLE COUNTER d_sample_counter = 0; // SAMPLE COUNTER
d_active = false; d_active = false;
d_freq = freq; d_freq = freq;
d_fs_in = fs_in; d_fs_in = fs_in;
@ -79,9 +77,9 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(
d_gnuradio_forecast_samples = d_fft_size; d_gnuradio_forecast_samples = d_fft_size;
d_input_power = 0.0; d_input_power = 0.0;
d_state = 0; d_state = 0;
d_carrier = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_carrier = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_fft_codes = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_fft_codes = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitude = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); d_magnitude = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@ -115,10 +113,10 @@ void pcps_acquisition_fine_doppler_cc::set_doppler_step(unsigned int doppler_ste
d_num_doppler_points = floor(std::abs(d_config_doppler_max - d_config_doppler_min) / d_doppler_step); d_num_doppler_points = floor(std::abs(d_config_doppler_max - d_config_doppler_min) / d_doppler_step);
d_grid_data = new float*[d_num_doppler_points]; d_grid_data = new float *[d_num_doppler_points];
for (int i = 0; i < d_num_doppler_points; i++) for (int i = 0; i < d_num_doppler_points; i++)
{ {
d_grid_data[i] = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); d_grid_data[i] = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
} }
update_carrier_wipeoff(); update_carrier_wipeoff();
} }
@ -151,10 +149,10 @@ pcps_acquisition_fine_doppler_cc::~pcps_acquisition_fine_doppler_cc()
} }
void pcps_acquisition_fine_doppler_cc::set_local_code(std::complex<float> * code) void pcps_acquisition_fine_doppler_cc::set_local_code(std::complex<float> *code)
{ {
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size); memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size); volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
} }
@ -175,12 +173,12 @@ void pcps_acquisition_fine_doppler_cc::init()
} }
void pcps_acquisition_fine_doppler_cc::forecast (int noutput_items, void pcps_acquisition_fine_doppler_cc::forecast(int noutput_items,
gr_vector_int &ninput_items_required) gr_vector_int &ninput_items_required)
{ {
if (noutput_items != 0) if (noutput_items != 0)
{ {
ninput_items_required[0] = d_gnuradio_forecast_samples ; //set the required available samples in each call ninput_items_required[0] = d_gnuradio_forecast_samples; //set the required available samples in each call
} }
} }
@ -203,17 +201,17 @@ void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff()
// create the carrier Doppler wipeoff signals // create the carrier Doppler wipeoff signals
int doppler_hz; int doppler_hz;
float phase_step_rad; float phase_step_rad;
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_points]; d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_points];
for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++) for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
{ {
doppler_hz = d_config_doppler_min + d_doppler_step*doppler_index; doppler_hz = d_config_doppler_min + d_doppler_step * doppler_index;
// doppler search steps // doppler search steps
// compute the carrier doppler wipe-off signal and store it // compute the carrier doppler wipe-off signal and store it
phase_step_rad = static_cast<float>(GPS_TWO_PI) * ( d_freq + doppler_hz ) / static_cast<float>(d_fs_in); phase_step_rad = static_cast<float>(GPS_TWO_PI) * (d_freq + doppler_hz) / static_cast<float>(d_fs_in);
d_grid_doppler_wipeoffs[doppler_index] = new gr_complex[d_fft_size]; d_grid_doppler_wipeoffs[doppler_index] = new gr_complex[d_fft_size];
float _phase[1]; float _phase[1];
_phase[0] = 0; _phase[0] = 0;
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], - phase_step_rad, _phase, d_fft_size); volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase, d_fft_size);
} }
} }
@ -226,7 +224,7 @@ double pcps_acquisition_fine_doppler_cc::search_maximum()
uint32_t tmp_intex_t = 0; uint32_t tmp_intex_t = 0;
uint32_t index_time = 0; uint32_t index_time = 0;
for (int i=0;i<d_num_doppler_points;i++) for (int i = 0; i < d_num_doppler_points; i++)
{ {
volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_grid_data[i], d_fft_size); volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_grid_data[i], d_fft_size);
if (d_grid_data[i][tmp_intex_t] > magt) if (d_grid_data[i][tmp_intex_t] > magt)
@ -243,7 +241,7 @@ double pcps_acquisition_fine_doppler_cc::search_maximum()
magt = magt / (fft_normalization_factor * fft_normalization_factor); magt = magt / (fft_normalization_factor * fft_normalization_factor);
// 5- Compute the test statistics and compare to the threshold // 5- Compute the test statistics and compare to the threshold
d_test_statistics = magt/(d_input_power*std::sqrt(d_well_count)); d_test_statistics = magt / (d_input_power * std::sqrt(d_well_count));
// 4- record the maximum peak and the associated synchronization parameters // 4- record the maximum peak and the associated synchronization parameters
d_gnss_synchro->Acq_delay_samples = static_cast<double>(index_time); d_gnss_synchro->Acq_delay_samples = static_cast<double>(index_time);
@ -254,14 +252,13 @@ double pcps_acquisition_fine_doppler_cc::search_maximum()
if (d_dump) if (d_dump)
{ {
std::stringstream filename; std::stringstream filename;
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str(""); filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System filename << "../data/test_statistics_" << d_gnss_synchro->System
<<"_" << d_gnss_synchro->Signal << "_sat_" << "_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << d_gnss_synchro->Acq_doppler_hz << ".dat"; << d_gnss_synchro->PRN << "_doppler_" << d_gnss_synchro->Acq_doppler_hz << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
| std::ios::binary); d_dump_file.write(reinterpret_cast<char *>(d_grid_data[index_doppler]), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.write(reinterpret_cast<char*>(d_grid_data[index_doppler]), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close(); d_dump_file.close();
} }
@ -271,7 +268,7 @@ double pcps_acquisition_fine_doppler_cc::search_maximum()
float pcps_acquisition_fine_doppler_cc::estimate_input_power(gr_vector_const_void_star &input_items) float pcps_acquisition_fine_doppler_cc::estimate_input_power(gr_vector_const_void_star &input_items)
{ {
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
// Compute the input signal power estimation // Compute the input signal power estimation
float power = 0; float power = 0;
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
@ -284,16 +281,16 @@ float pcps_acquisition_fine_doppler_cc::estimate_input_power(gr_vector_const_voi
int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items) int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items)
{ {
// initialize acquisition algorithm // initialize acquisition algorithm
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
DLOG(INFO) << "Channel: " << d_channel DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " "<< d_gnss_synchro->PRN << " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN
<< " ,sample stamp: " << d_sample_counter << ", threshold: " << " ,sample stamp: " << d_sample_counter << ", threshold: "
<< d_threshold << ", doppler_max: " << d_config_doppler_max << d_threshold << ", doppler_max: " << d_config_doppler_max
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
float* p_tmp_vector = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); float *p_tmp_vector = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++) for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
{ {
@ -314,7 +311,7 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons
// save the grid matrix delay file // save the grid matrix delay file
volk_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size);
const float* old_vector = d_grid_data[doppler_index]; const float *old_vector = d_grid_data[doppler_index];
volk_32f_x2_add_32f(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size); volk_32f_x2_add_32f(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size);
} }
@ -334,7 +331,7 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
std::fill_n(fft_operator->get_inbuf(), fft_size_extended, gr_complex(0.0, 0.0)); std::fill_n(fft_operator->get_inbuf(), fft_size_extended, gr_complex(0.0, 0.0));
//1. generate local code aligned with the acquisition code phase estimation //1. generate local code aligned with the acquisition code phase estimation
gr_complex *code_replica = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); gr_complex *code_replica = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
gps_l1_ca_code_gen_complex_sampled(code_replica, d_gnss_synchro->PRN, d_fs_in, 0); gps_l1_ca_code_gen_complex_sampled(code_replica, d_gnss_synchro->PRN, d_fs_in, 0);
@ -347,7 +344,7 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
} }
//2. Perform code wipe-off //2. Perform code wipe-off
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
volk_32fc_x2_multiply_32fc(fft_operator->get_inbuf(), in, code_replica, d_fft_size); volk_32fc_x2_multiply_32fc(fft_operator->get_inbuf(), in, code_replica, d_fft_size);
@ -355,7 +352,7 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
fft_operator->execute(); fft_operator->execute();
// 4. Compute the magnitude and find the maximum // 4. Compute the magnitude and find the maximum
float* p_tmp_vector = static_cast<float*>(volk_gnsssdr_malloc(fft_size_extended * sizeof(float), volk_gnsssdr_get_alignment())); float *p_tmp_vector = static_cast<float *>(volk_gnsssdr_malloc(fft_size_extended * sizeof(float), volk_gnsssdr_get_alignment()));
volk_32fc_magnitude_squared_32f(p_tmp_vector, fft_operator->get_outbuf(), fft_size_extended); volk_32fc_magnitude_squared_32f(p_tmp_vector, fft_operator->get_outbuf(), fft_size_extended);
@ -389,7 +386,7 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
else else
{ {
DLOG(INFO) << "Abs(Grid Doppler - FFT Doppler)=" << std::abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz); DLOG(INFO) << "Abs(Grid Doppler - FFT Doppler)=" << std::abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz);
DLOG(INFO) << "Error estimating fine frequency Doppler"; DLOG(INFO) << "Error estimating fine frequency Doppler";
//debug log //debug log
// //
// std::cout<<"FFT maximum present at "<<fftFreqBins[tmp_index_freq]<<" [Hz]"<<std::endl; // std::cout<<"FFT maximum present at "<<fftFreqBins[tmp_index_freq]<<" [Hz]"<<std::endl;
@ -429,8 +426,8 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items, int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items, gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused))) gr_vector_void_star &output_items __attribute__((unused)))
{ {
/*! /*!
* TODO: High sensitivity acquisition algorithm: * TODO: High sensitivity acquisition algorithm:
@ -447,82 +444,82 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
*/ */
switch (d_state) switch (d_state)
{ {
case 0: // S0. StandBy case 0: // S0. StandBy
//DLOG(INFO) <<"S0"<<std::endl; //DLOG(INFO) <<"S0"<<std::endl;
if (d_active == true) if (d_active == true)
{ {
reset_grid(); reset_grid();
d_state = 1; d_state = 1;
} }
break; break;
case 1: // S1. ComputeGrid case 1: // S1. ComputeGrid
//DLOG(INFO) <<"S1"<<std::endl; //DLOG(INFO) <<"S1"<<std::endl;
compute_and_accumulate_grid(input_items); compute_and_accumulate_grid(input_items);
d_well_count++; d_well_count++;
if (d_well_count >= d_max_dwells) if (d_well_count >= d_max_dwells)
{ {
d_state = 2; d_state = 2;
} }
break; break;
case 2: // Compute test statistics and decide case 2: // Compute test statistics and decide
//DLOG(INFO) <<"S2"<<std::endl; //DLOG(INFO) <<"S2"<<std::endl;
d_input_power = estimate_input_power(input_items); d_input_power = estimate_input_power(input_items);
d_test_statistics = search_maximum(); d_test_statistics = search_maximum();
if (d_test_statistics > d_threshold) if (d_test_statistics > d_threshold)
{ {
d_state = 3; //perform fine doppler estimation d_state = 3; //perform fine doppler estimation
} }
else else
{ {
d_state = 5; //negative acquisition d_state = 5; //negative acquisition
} }
break; break;
case 3: // Fine doppler estimation case 3: // Fine doppler estimation
//DLOG(INFO) <<"S3"<<std::endl; //DLOG(INFO) <<"S3"<<std::endl;
DLOG(INFO) << "Performing fine Doppler estimation"; DLOG(INFO) << "Performing fine Doppler estimation";
estimate_Doppler(input_items); //disabled in repo estimate_Doppler(input_items); //disabled in repo
d_state = 4; d_state = 4;
break; break;
case 4: // Positive_Acq case 4: // Positive_Acq
//DLOG(INFO) <<"S4"<<std::endl; //DLOG(INFO) <<"S4"<<std::endl;
DLOG(INFO) << "positive acquisition"; DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN; DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter; DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics; DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold; DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples; DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz; DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "input signal power " << d_input_power; DLOG(INFO) << "input signal power " << d_input_power;
d_active = false; d_active = false;
// Send message to channel port //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL // Send message to channel port //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
this->message_port_pub(pmt::mp("events"), pmt::from_long(1)); this->message_port_pub(pmt::mp("events"), pmt::from_long(1));
d_state = 0; d_state = 0;
break; break;
case 5: // Negative_Acq case 5: // Negative_Acq
//DLOG(INFO) <<"S5"<<std::endl; //DLOG(INFO) <<"S5"<<std::endl;
DLOG(INFO) << "negative acquisition"; DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN; DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter; DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics; DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold; DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples; DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz; DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "input signal power " << d_input_power; DLOG(INFO) << "input signal power " << d_input_power;
d_active = false; d_active = false;
// Send message to channel port //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL // Send message to channel port //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
this->message_port_pub(pmt::mp("events"), pmt::from_long(2)); this->message_port_pub(pmt::mp("events"), pmt::from_long(2));
d_state = 0; d_state = 0;
break; break;
default: default:
d_state = 0; d_state = 0;
break; break;
} }
//DLOG(INFO)<<"d_sample_counter="<<d_sample_counter<<std::endl; //DLOG(INFO)<<"d_sample_counter="<<d_sample_counter<<std::endl;
d_sample_counter += d_fft_size; // sample counter d_sample_counter += d_fft_size; // sample counter
consume_each(d_fft_size); consume_each(d_fft_size);
return noutput_items; return noutput_items;
} }

40
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h
View File

@ -58,12 +58,12 @@
class pcps_acquisition_fine_doppler_cc; class pcps_acquisition_fine_doppler_cc;
typedef boost::shared_ptr<pcps_acquisition_fine_doppler_cc> typedef boost::shared_ptr<pcps_acquisition_fine_doppler_cc>
pcps_acquisition_fine_doppler_cc_sptr; pcps_acquisition_fine_doppler_cc_sptr;
pcps_acquisition_fine_doppler_cc_sptr pcps_acquisition_fine_doppler_cc_sptr
pcps_make_acquisition_fine_doppler_cc(int max_dwells, unsigned int sampled_ms, pcps_make_acquisition_fine_doppler_cc(int max_dwells, unsigned int sampled_ms,
int doppler_max, int doppler_min, long freq, long fs_in, int samples_per_ms, int doppler_max, int doppler_min, long freq, long fs_in, int samples_per_ms,
bool dump, std::string dump_filename); bool dump, std::string dump_filename);
/*! /*!
* \brief This class implements a Parallel Code Phase Search Acquisition. * \brief This class implements a Parallel Code Phase Search Acquisition.
@ -72,26 +72,26 @@ pcps_make_acquisition_fine_doppler_cc(int max_dwells, unsigned int sampled_ms,
* Algorithm 1, for a pseudocode description of this implementation. * Algorithm 1, for a pseudocode description of this implementation.
*/ */
class pcps_acquisition_fine_doppler_cc: public gr::block class pcps_acquisition_fine_doppler_cc : public gr::block
{ {
private: private:
friend pcps_acquisition_fine_doppler_cc_sptr friend pcps_acquisition_fine_doppler_cc_sptr
pcps_make_acquisition_fine_doppler_cc(int max_dwells, unsigned int sampled_ms, pcps_make_acquisition_fine_doppler_cc(int max_dwells, unsigned int sampled_ms,
int doppler_max, int doppler_min, long freq, long fs_in, int doppler_max, int doppler_min, long freq, long fs_in,
int samples_per_ms, bool dump, int samples_per_ms, bool dump,
std::string dump_filename); std::string dump_filename);
pcps_acquisition_fine_doppler_cc(int max_dwells, unsigned int sampled_ms, pcps_acquisition_fine_doppler_cc(int max_dwells, unsigned int sampled_ms,
int doppler_max, int doppler_min, long freq, long fs_in, int doppler_max, int doppler_min, long freq, long fs_in,
int samples_per_ms, bool dump, int samples_per_ms, bool dump,
std::string dump_filename); std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift, void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset); int doppler_offset);
int compute_and_accumulate_grid(gr_vector_const_void_star &input_items); int compute_and_accumulate_grid(gr_vector_const_void_star& input_items);
int estimate_Doppler(gr_vector_const_void_star &input_items); int estimate_Doppler(gr_vector_const_void_star& input_items);
float estimate_input_power(gr_vector_const_void_star &input_items); float estimate_input_power(gr_vector_const_void_star& input_items);
double search_maximum(); double search_maximum();
void reset_grid(); void reset_grid();
void update_carrier_wipeoff(); void update_carrier_wipeoff();
@ -122,7 +122,7 @@ private:
gr::fft::fft_complex* d_fft_if; gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft; gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro; Gnss_Synchro* d_gnss_synchro;
unsigned int d_code_phase; unsigned int d_code_phase;
float d_doppler_freq; float d_doppler_freq;
float d_input_power; float d_input_power;
@ -169,7 +169,7 @@ public:
* \brief Sets local code for PCPS acquisition algorithm. * \brief Sets local code for PCPS acquisition algorithm.
* \param code - Pointer to the PRN code. * \param code - Pointer to the PRN code.
*/ */
void set_local_code(std::complex<float> * code); void set_local_code(std::complex<float>* code);
/*! /*!
* \brief Starts acquisition algorithm, turning from standby mode to * \brief Starts acquisition algorithm, turning from standby mode to
@ -218,11 +218,11 @@ public:
/*! /*!
* \brief Parallel Code Phase Search Acquisition signal processing. * \brief Parallel Code Phase Search Acquisition signal processing.
*/ */
int general_work(int noutput_items, gr_vector_int &ninput_items, int general_work(int noutput_items, gr_vector_int& ninput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star& input_items,
gr_vector_void_star &output_items); gr_vector_void_star& output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required); void forecast(int noutput_items, gr_vector_int& ninput_items_required);
}; };
#endif /* pcps_acquisition_fine_doppler_cc*/ #endif /* pcps_acquisition_fine_doppler_cc*/

293
src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc
View File

@ -46,27 +46,25 @@ extern concurrent_map<Gps_Acq_Assist> global_gps_acq_assist_map;
using google::LogMessage; using google::LogMessage;
pcps_assisted_acquisition_cc_sptr pcps_make_assisted_acquisition_cc( pcps_assisted_acquisition_cc_sptr pcps_make_assisted_acquisition_cc(
int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq, int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq,
long fs_in, int samples_per_ms, bool dump, long fs_in, int samples_per_ms, bool dump,
std::string dump_filename) std::string dump_filename)
{ {
return pcps_assisted_acquisition_cc_sptr( return pcps_assisted_acquisition_cc_sptr(
new pcps_assisted_acquisition_cc(max_dwells, sampled_ms, doppler_max, doppler_min, freq, new pcps_assisted_acquisition_cc(max_dwells, sampled_ms, doppler_max, doppler_min, freq,
fs_in, samples_per_ms, dump, dump_filename)); fs_in, samples_per_ms, dump, dump_filename));
} }
pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc( pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc(
int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq, int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq,
long fs_in, int samples_per_ms, bool dump, long fs_in, int samples_per_ms, bool dump,
std::string dump_filename) : std::string dump_filename) : gr::block("pcps_assisted_acquisition_cc",
gr::block("pcps_assisted_acquisition_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(0, 0, sizeof(gr_complex)))
gr::io_signature::make(0, 0, sizeof(gr_complex)))
{ {
this->message_port_register_out(pmt::mp("events")); this->message_port_register_out(pmt::mp("events"));
d_sample_counter = 0; // SAMPLE COUNTER d_sample_counter = 0; // SAMPLE COUNTER
d_active = false; d_active = false;
d_freq = freq; d_freq = freq;
d_fs_in = fs_in; d_fs_in = fs_in;
@ -77,12 +75,12 @@ pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc(
d_fft_size = d_sampled_ms * d_samples_per_ms; d_fft_size = d_sampled_ms * d_samples_per_ms;
// HS Acquisition // HS Acquisition
d_max_dwells = max_dwells; d_max_dwells = max_dwells;
d_gnuradio_forecast_samples = d_fft_size*4; d_gnuradio_forecast_samples = d_fft_size * 4;
d_input_power = 0.0; d_input_power = 0.0;
d_state = 0; d_state = 0;
d_disable_assist = false; d_disable_assist = false;
d_fft_codes = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_fft_codes = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_carrier = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_carrier = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@ -111,14 +109,12 @@ pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc(
} }
void pcps_assisted_acquisition_cc::set_doppler_step(unsigned int doppler_step) void pcps_assisted_acquisition_cc::set_doppler_step(unsigned int doppler_step)
{ {
d_doppler_step = doppler_step; d_doppler_step = doppler_step;
} }
void pcps_assisted_acquisition_cc::free_grid_memory() void pcps_assisted_acquisition_cc::free_grid_memory()
{ {
for (int i = 0; i < d_num_doppler_points; i++) for (int i = 0; i < d_num_doppler_points; i++)
@ -130,7 +126,6 @@ void pcps_assisted_acquisition_cc::free_grid_memory()
} }
pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc() pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc()
{ {
volk_gnsssdr_free(d_carrier); volk_gnsssdr_free(d_carrier);
@ -144,14 +139,12 @@ pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc()
} }
void pcps_assisted_acquisition_cc::set_local_code(std::complex<float> *code)
void pcps_assisted_acquisition_cc::set_local_code(std::complex<float> * code)
{ {
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size);
} }
void pcps_assisted_acquisition_cc::init() void pcps_assisted_acquisition_cc::init()
{ {
d_gnss_synchro->Flag_valid_acquisition = false; d_gnss_synchro->Flag_valid_acquisition = false;
@ -165,35 +158,33 @@ void pcps_assisted_acquisition_cc::init()
d_input_power = 0.0; d_input_power = 0.0;
d_state = 0; d_state = 0;
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size); volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
} }
void pcps_assisted_acquisition_cc::forecast(int noutput_items,
void pcps_assisted_acquisition_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
gr_vector_int &ninput_items_required)
{ {
if (noutput_items != 0) if (noutput_items != 0)
{ {
ninput_items_required[0] = d_gnuradio_forecast_samples ; //set the required available samples in each call ninput_items_required[0] = d_gnuradio_forecast_samples; //set the required available samples in each call
} }
} }
void pcps_assisted_acquisition_cc::get_assistance() void pcps_assisted_acquisition_cc::get_assistance()
{ {
Gps_Acq_Assist gps_acq_assisistance; Gps_Acq_Assist gps_acq_assisistance;
if (global_gps_acq_assist_map.read(this->d_gnss_synchro->PRN, gps_acq_assisistance)==true) if (global_gps_acq_assist_map.read(this->d_gnss_synchro->PRN, gps_acq_assisistance) == true)
{ {
//TODO: use the LO tolerance here //TODO: use the LO tolerance here
if (gps_acq_assisistance.dopplerUncertainty >= 1000) if (gps_acq_assisistance.dopplerUncertainty >= 1000)
{ {
d_doppler_max = gps_acq_assisistance.d_Doppler0 + gps_acq_assisistance.dopplerUncertainty*2; d_doppler_max = gps_acq_assisistance.d_Doppler0 + gps_acq_assisistance.dopplerUncertainty * 2;
d_doppler_min = gps_acq_assisistance.d_Doppler0 - gps_acq_assisistance.dopplerUncertainty*2; d_doppler_min = gps_acq_assisistance.d_Doppler0 - gps_acq_assisistance.dopplerUncertainty * 2;
} }
else else
{ {
@ -201,18 +192,17 @@ void pcps_assisted_acquisition_cc::get_assistance()
d_doppler_min = gps_acq_assisistance.d_Doppler0 - 1000; d_doppler_min = gps_acq_assisistance.d_Doppler0 - 1000;
} }
this->d_disable_assist = false; this->d_disable_assist = false;
std::cout << "Acq assist ENABLED for GPS SV "<< this->d_gnss_synchro->PRN <<" (Doppler max,Doppler min)=(" std::cout << "Acq assist ENABLED for GPS SV " << this->d_gnss_synchro->PRN << " (Doppler max,Doppler min)=("
<< d_doppler_max << "," << d_doppler_min << ")" << std::endl; << d_doppler_max << "," << d_doppler_min << ")" << std::endl;
} }
else else
{ {
this->d_disable_assist = true; this->d_disable_assist = true;
std::cout << "Acq assist DISABLED for GPS SV "<< this->d_gnss_synchro->PRN << std::endl; std::cout << "Acq assist DISABLED for GPS SV " << this->d_gnss_synchro->PRN << std::endl;
} }
} }
void pcps_assisted_acquisition_cc::reset_grid() void pcps_assisted_acquisition_cc::reset_grid()
{ {
d_well_count = 0; d_well_count = 0;
@ -226,7 +216,6 @@ void pcps_assisted_acquisition_cc::reset_grid()
} }
void pcps_assisted_acquisition_cc::redefine_grid() void pcps_assisted_acquisition_cc::redefine_grid()
{ {
if (this->d_disable_assist == true) if (this->d_disable_assist == true)
@ -237,7 +226,7 @@ void pcps_assisted_acquisition_cc::redefine_grid()
// Create the search grid array // Create the search grid array
d_num_doppler_points = floor(std::abs(d_doppler_max - d_doppler_min) / d_doppler_step); d_num_doppler_points = floor(std::abs(d_doppler_max - d_doppler_min) / d_doppler_step);
d_grid_data = new float*[d_num_doppler_points]; d_grid_data = new float *[d_num_doppler_points];
for (int i = 0; i < d_num_doppler_points; i++) for (int i = 0; i < d_num_doppler_points; i++)
{ {
d_grid_data[i] = new float[d_fft_size]; d_grid_data[i] = new float[d_fft_size];
@ -246,22 +235,21 @@ void pcps_assisted_acquisition_cc::redefine_grid()
// create the carrier Doppler wipeoff signals // create the carrier Doppler wipeoff signals
int doppler_hz; int doppler_hz;
float phase_step_rad; float phase_step_rad;
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_points]; d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_points];
for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++) for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
{ {
doppler_hz = d_doppler_min + d_doppler_step*doppler_index; doppler_hz = d_doppler_min + d_doppler_step * doppler_index;
// doppler search steps // doppler search steps
// compute the carrier doppler wipe-off signal and store it // compute the carrier doppler wipe-off signal and store it
phase_step_rad = static_cast<float>(GPS_TWO_PI) * doppler_hz / static_cast<float>(d_fs_in); phase_step_rad = static_cast<float>(GPS_TWO_PI) * doppler_hz / static_cast<float>(d_fs_in);
d_grid_doppler_wipeoffs[doppler_index] = new gr_complex[d_fft_size]; d_grid_doppler_wipeoffs[doppler_index] = new gr_complex[d_fft_size];
float _phase[1]; float _phase[1];
_phase[0] = 0; _phase[0] = 0;
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], - phase_step_rad, _phase, d_fft_size); volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase, d_fft_size);
} }
} }
double pcps_assisted_acquisition_cc::search_maximum() double pcps_assisted_acquisition_cc::search_maximum()
{ {
float magt = 0.0; float magt = 0.0;
@ -270,9 +258,9 @@ double pcps_assisted_acquisition_cc::search_maximum()
uint32_t tmp_intex_t = 0; uint32_t tmp_intex_t = 0;
uint32_t index_time = 0; uint32_t index_time = 0;
for (int i=0;i<d_num_doppler_points;i++) for (int i = 0; i < d_num_doppler_points; i++)
{ {
volk_gnsssdr_32f_index_max_32u(&tmp_intex_t,d_grid_data[i],d_fft_size); volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_grid_data[i], d_fft_size);
if (d_grid_data[i][tmp_intex_t] > magt) if (d_grid_data[i][tmp_intex_t] > magt)
{ {
magt = d_grid_data[i][index_time]; magt = d_grid_data[i][index_time];
@ -297,13 +285,13 @@ double pcps_assisted_acquisition_cc::search_maximum()
if (d_dump) if (d_dump)
{ {
std::stringstream filename; std::stringstream filename;
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str(""); filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System filename << "../data/test_statistics_" << d_gnss_synchro->System
<< "_" << d_gnss_synchro->Signal << "_sat_" << "_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << d_gnss_synchro->Acq_doppler_hz << ".dat"; << d_gnss_synchro->PRN << "_doppler_" << d_gnss_synchro->Acq_doppler_hz << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char*>(d_grid_data[index_doppler]), n); //write directly |abs(x)|^2 in this Doppler bin? d_dump_file.write(reinterpret_cast<char *>(d_grid_data[index_doppler]), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close(); d_dump_file.close();
} }
@ -311,28 +299,26 @@ double pcps_assisted_acquisition_cc::search_maximum()
} }
float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_star &input_items) float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_star &input_items)
{ {
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
float* p_tmp_vector = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); float *p_tmp_vector = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
volk_32fc_magnitude_squared_32f(p_tmp_vector, in, d_fft_size); volk_32fc_magnitude_squared_32f(p_tmp_vector, in, d_fft_size);
const float* p_const_tmp_vector = p_tmp_vector; const float *p_const_tmp_vector = p_tmp_vector;
float power; float power;
volk_32f_accumulator_s32f(&power, p_const_tmp_vector, d_fft_size); volk_32f_accumulator_s32f(&power, p_const_tmp_vector, d_fft_size);
volk_gnsssdr_free(p_tmp_vector); volk_gnsssdr_free(p_tmp_vector);
return ( power / static_cast<float>(d_fft_size)); return (power / static_cast<float>(d_fft_size));
} }
int pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items) int pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items)
{ {
// initialize acquisition algorithm // initialize acquisition algorithm
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
DLOG(INFO) << "Channel: " << d_channel DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << " , doing acquisition of satellite: " << d_gnss_synchro->System << " "
@ -342,7 +328,7 @@ int pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_vo
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
float* p_tmp_vector = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); float *p_tmp_vector = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++) for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
{ {
@ -362,7 +348,7 @@ int pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_vo
// save the grid matrix delay file // save the grid matrix delay file
volk_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size);
const float* old_vector = d_grid_data[doppler_index]; const float *old_vector = d_grid_data[doppler_index];
volk_32f_x2_add_32f(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size); volk_32f_x2_add_32f(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size);
} }
volk_gnsssdr_free(p_tmp_vector); volk_gnsssdr_free(p_tmp_vector);
@ -370,10 +356,9 @@ int pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_vo
} }
int pcps_assisted_acquisition_cc::general_work(int noutput_items, int pcps_assisted_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused))) gr_vector_void_star &output_items __attribute__((unused)))
{ {
/*! /*!
* TODO: High sensitivity acquisition algorithm: * TODO: High sensitivity acquisition algorithm:
@ -393,102 +378,102 @@ int pcps_assisted_acquisition_cc::general_work(int noutput_items,
*/ */
switch (d_state) switch (d_state)
{ {
case 0: // S0. StandBy case 0: // S0. StandBy
if (d_active == true) d_state = 1; if (d_active == true) d_state = 1;
d_sample_counter += ninput_items[0]; // sample counter d_sample_counter += ninput_items[0]; // sample counter
consume_each(ninput_items[0]); consume_each(ninput_items[0]);
break; break;
case 1: // S1. GetAssist case 1: // S1. GetAssist
get_assistance(); get_assistance();
redefine_grid(); redefine_grid();
reset_grid(); reset_grid();
d_sample_counter += ninput_items[0]; // sample counter d_sample_counter += ninput_items[0]; // sample counter
consume_each(ninput_items[0]); consume_each(ninput_items[0]);
d_state = 2; d_state = 2;
break; break;
case 2: // S2. ComputeGrid case 2: // S2. ComputeGrid
int consumed_samples; int consumed_samples;
consumed_samples = compute_and_accumulate_grid(input_items); consumed_samples = compute_and_accumulate_grid(input_items);
d_well_count++; d_well_count++;
if (d_well_count >= d_max_dwells) if (d_well_count >= d_max_dwells)
{ {
d_state=3; d_state = 3;
} }
d_sample_counter += consumed_samples; d_sample_counter += consumed_samples;
consume_each(consumed_samples); consume_each(consumed_samples);
break; break;
case 3: // Compute test statistics and decide case 3: // Compute test statistics and decide
d_input_power = estimate_input_power(input_items); d_input_power = estimate_input_power(input_items);
d_test_statistics = search_maximum(); d_test_statistics = search_maximum();
if (d_test_statistics > d_threshold) if (d_test_statistics > d_threshold)
{ {
d_state = 5; d_state = 5;
} }
else else
{ {
if (d_disable_assist == false) if (d_disable_assist == false)
{ {
d_disable_assist = true; d_disable_assist = true;
std::cout << "Acq assist DISABLED for GPS SV "<< this->d_gnss_synchro->PRN << std::endl; std::cout << "Acq assist DISABLED for GPS SV " << this->d_gnss_synchro->PRN << std::endl;
d_state = 4; d_state = 4;
} }
else else
{ {
d_state = 6; d_state = 6;
} }
} }
d_sample_counter += ninput_items[0]; // sample counter d_sample_counter += ninput_items[0]; // sample counter
consume_each(ninput_items[0]); consume_each(ninput_items[0]);
break; break;
case 4: // RedefineGrid case 4: // RedefineGrid
free_grid_memory(); free_grid_memory();
redefine_grid(); redefine_grid();
reset_grid(); reset_grid();
d_sample_counter += ninput_items[0]; // sample counter d_sample_counter += ninput_items[0]; // sample counter
consume_each(ninput_items[0]); consume_each(ninput_items[0]);
d_state = 2; d_state = 2;
break; break;
case 5: // Positive_Acq case 5: // Positive_Acq
DLOG(INFO) << "positive acquisition"; DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN; DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter; DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics; DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold; DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples; DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz; DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "input signal power " << d_input_power; DLOG(INFO) << "input signal power " << d_input_power;
d_active = false; d_active = false;
// Send message to channel port //0=STOP_CHANNEL 1=ACQ_SUCCESS 2=ACQ_FAIL // Send message to channel port //0=STOP_CHANNEL 1=ACQ_SUCCESS 2=ACQ_FAIL
this->message_port_pub(pmt::mp("events"), pmt::from_long(1)); this->message_port_pub(pmt::mp("events"), pmt::from_long(1));
free_grid_memory(); free_grid_memory();
// consume samples to not block the GNU Radio flowgraph // consume samples to not block the GNU Radio flowgraph
d_sample_counter += ninput_items[0]; // sample counter d_sample_counter += ninput_items[0]; // sample counter
consume_each(ninput_items[0]); consume_each(ninput_items[0]);
d_state = 0; d_state = 0;
break; break;
case 6: // Negative_Acq case 6: // Negative_Acq
DLOG(INFO) << "negative acquisition"; DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN; DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter; DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics; DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold; DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples; DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz; DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "input signal power " << d_input_power; DLOG(INFO) << "input signal power " << d_input_power;
d_active = false; d_active = false;
// Send message to channel port //0=STOP_CHANNEL 1=ACQ_SUCCESS 2=ACQ_FAIL // Send message to channel port //0=STOP_CHANNEL 1=ACQ_SUCCESS 2=ACQ_FAIL
this->message_port_pub(pmt::mp("events"), pmt::from_long(2)); this->message_port_pub(pmt::mp("events"), pmt::from_long(2));
free_grid_memory(); free_grid_memory();
// consume samples to not block the GNU Radio flowgraph // consume samples to not block the GNU Radio flowgraph
d_sample_counter += ninput_items[0]; // sample counter d_sample_counter += ninput_items[0]; // sample counter
consume_each(ninput_items[0]); consume_each(ninput_items[0]);
d_state = 0; d_state = 0;
break; break;
default: default:
d_state = 0; d_state = 0;
break; break;
} }
return noutput_items; return noutput_items;
} }

38
src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h
View File

@ -58,12 +58,12 @@
class pcps_assisted_acquisition_cc; class pcps_assisted_acquisition_cc;
typedef boost::shared_ptr<pcps_assisted_acquisition_cc> typedef boost::shared_ptr<pcps_assisted_acquisition_cc>
pcps_assisted_acquisition_cc_sptr; pcps_assisted_acquisition_cc_sptr;
pcps_assisted_acquisition_cc_sptr pcps_assisted_acquisition_cc_sptr
pcps_make_assisted_acquisition_cc(int max_dwells, unsigned int sampled_ms, pcps_make_assisted_acquisition_cc(int max_dwells, unsigned int sampled_ms,
int doppler_max, int doppler_min, long freq, long fs_in, int samples_per_ms, int doppler_max, int doppler_min, long freq, long fs_in, int samples_per_ms,
bool dump, std::string dump_filename); bool dump, std::string dump_filename);
/*! /*!
* \brief This class implements a Parallel Code Phase Search Acquisition. * \brief This class implements a Parallel Code Phase Search Acquisition.
@ -71,25 +71,25 @@ pcps_make_assisted_acquisition_cc(int max_dwells, unsigned int sampled_ms,
* Check \ref Navitec2012 "An Open Source Galileo E1 Software Receiver", * Check \ref Navitec2012 "An Open Source Galileo E1 Software Receiver",
* Algorithm 1, for a pseudocode description of this implementation. * Algorithm 1, for a pseudocode description of this implementation.
*/ */
class pcps_assisted_acquisition_cc: public gr::block class pcps_assisted_acquisition_cc : public gr::block
{ {
private: private:
friend pcps_assisted_acquisition_cc_sptr friend pcps_assisted_acquisition_cc_sptr
pcps_make_assisted_acquisition_cc(int max_dwells, unsigned int sampled_ms, pcps_make_assisted_acquisition_cc(int max_dwells, unsigned int sampled_ms,
int doppler_max, int doppler_min, long freq, long fs_in, int doppler_max, int doppler_min, long freq, long fs_in,
int samples_per_ms, bool dump, int samples_per_ms, bool dump,
std::string dump_filename); std::string dump_filename);
pcps_assisted_acquisition_cc(int max_dwells, unsigned int sampled_ms, pcps_assisted_acquisition_cc(int max_dwells, unsigned int sampled_ms,
int doppler_max, int doppler_min, long freq, long fs_in, int doppler_max, int doppler_min, long freq, long fs_in,
int samples_per_ms, bool dump, int samples_per_ms, bool dump,
std::string dump_filename); std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift, void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset); int doppler_offset);
int compute_and_accumulate_grid(gr_vector_const_void_star &input_items); int compute_and_accumulate_grid(gr_vector_const_void_star& input_items);
float estimate_input_power(gr_vector_const_void_star &input_items); float estimate_input_power(gr_vector_const_void_star& input_items);
double search_maximum(); double search_maximum();
void get_assistance(); void get_assistance();
void reset_grid(); void reset_grid();
@ -122,7 +122,7 @@ private:
gr::fft::fft_complex* d_fft_if; gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft; gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro; Gnss_Synchro* d_gnss_synchro;
unsigned int d_code_phase; unsigned int d_code_phase;
float d_doppler_freq; float d_doppler_freq;
float d_input_power; float d_input_power;
@ -170,7 +170,7 @@ public:
* \brief Sets local code for PCPS acquisition algorithm. * \brief Sets local code for PCPS acquisition algorithm.
* \param code - Pointer to the PRN code. * \param code - Pointer to the PRN code.
*/ */
void set_local_code(std::complex<float> * code); void set_local_code(std::complex<float>* code);
/*! /*!
* \brief Starts acquisition algorithm, turning from standby mode to * \brief Starts acquisition algorithm, turning from standby mode to
@ -219,11 +219,11 @@ public:
/*! /*!
* \brief Parallel Code Phase Search Acquisition signal processing. * \brief Parallel Code Phase Search Acquisition signal processing.
*/ */
int general_work(int noutput_items, gr_vector_int &ninput_items, int general_work(int noutput_items, gr_vector_int& ninput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star& input_items,
gr_vector_void_star &output_items); gr_vector_void_star& output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required); void forecast(int noutput_items, gr_vector_int& ninput_items_required);
}; };
#endif /* GNSS_SDR_PCPS_assisted_acquisition_cc_H_*/ #endif /* GNSS_SDR_PCPS_assisted_acquisition_cc_H_*/

484
src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc
View File

@ -41,33 +41,33 @@
#include <volk/volk.h> #include <volk/volk.h>
#include <volk_gnsssdr/volk_gnsssdr.h> #include <volk_gnsssdr/volk_gnsssdr.h>
#include "control_message_factory.h" #include "control_message_factory.h"
#include "GPS_L1_CA.h" //GPS_TWO_PI #include "GPS_L1_CA.h" //GPS_TWO_PI
using google::LogMessage; using google::LogMessage;
pcps_cccwsr_acquisition_cc_sptr pcps_cccwsr_make_acquisition_cc( pcps_cccwsr_acquisition_cc_sptr pcps_cccwsr_make_acquisition_cc(
unsigned int sampled_ms, unsigned int max_dwells, unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool dump, std::string dump_filename) bool dump, std::string dump_filename)
{ {
return pcps_cccwsr_acquisition_cc_sptr( return pcps_cccwsr_acquisition_cc_sptr(
new pcps_cccwsr_acquisition_cc(sampled_ms, max_dwells, doppler_max, freq, fs_in, new pcps_cccwsr_acquisition_cc(sampled_ms, max_dwells, doppler_max, freq, fs_in,
samples_per_ms, samples_per_code, dump, dump_filename)); samples_per_ms, samples_per_code, dump, dump_filename));
} }
pcps_cccwsr_acquisition_cc::pcps_cccwsr_acquisition_cc( pcps_cccwsr_acquisition_cc::pcps_cccwsr_acquisition_cc(
unsigned int sampled_ms, unsigned int max_dwells, unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool dump, std::string dump_filename) : bool dump,
gr::block("pcps_cccwsr_acquisition_cc", std::string dump_filename) : gr::block("pcps_cccwsr_acquisition_cc",
gr::io_signature::make(1, 1, sizeof(gr_complex) * sampled_ms * samples_per_ms), gr::io_signature::make(1, 1, sizeof(gr_complex) * sampled_ms * samples_per_ms),
gr::io_signature::make(0, 0, sizeof(gr_complex) * sampled_ms * samples_per_ms)) gr::io_signature::make(0, 0, sizeof(gr_complex) * sampled_ms * samples_per_ms))
{ {
this->message_port_register_out(pmt::mp("events")); this->message_port_register_out(pmt::mp("events"));
d_sample_counter = 0; // SAMPLE COUNTER d_sample_counter = 0; // SAMPLE COUNTER
d_active = false; d_active = false;
d_state = 0; d_state = 0;
d_freq = freq; d_freq = freq;
@ -83,13 +83,13 @@ pcps_cccwsr_acquisition_cc::pcps_cccwsr_acquisition_cc(
d_input_power = 0.0; d_input_power = 0.0;
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
d_fft_code_data = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_fft_code_data = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_fft_code_pilot = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_fft_code_pilot = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_data_correlation = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_data_correlation = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_pilot_correlation = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_pilot_correlation = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_correlation_plus = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_correlation_plus = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_correlation_minus = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_correlation_minus = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitude = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); d_magnitude = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@ -140,21 +140,21 @@ pcps_cccwsr_acquisition_cc::~pcps_cccwsr_acquisition_cc()
} }
} }
void pcps_cccwsr_acquisition_cc::set_local_code(std::complex<float>* code_data, void pcps_cccwsr_acquisition_cc::set_local_code(std::complex<float> *code_data,
std::complex<float>* code_pilot) std::complex<float> *code_pilot)
{ {
// Data code (E1B) // Data code (E1B)
memcpy(d_fft_if->get_inbuf(), code_data, sizeof(gr_complex) * d_fft_size); memcpy(d_fft_if->get_inbuf(), code_data, sizeof(gr_complex) * d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_data,d_fft_if->get_outbuf(),d_fft_size); volk_32fc_conjugate_32fc(d_fft_code_data, d_fft_if->get_outbuf(), d_fft_size);
// Pilot code (E1C) // Pilot code (E1C)
memcpy(d_fft_if->get_inbuf(), code_pilot, sizeof(gr_complex) * d_fft_size); memcpy(d_fft_if->get_inbuf(), code_pilot, sizeof(gr_complex) * d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code, //Conjugate the local code,
volk_32fc_conjugate_32fc(d_fft_code_pilot, d_fft_if->get_outbuf(), d_fft_size); volk_32fc_conjugate_32fc(d_fft_code_pilot, d_fft_if->get_outbuf(), d_fft_size);
@ -178,21 +178,21 @@ void pcps_cccwsr_acquisition_cc::init()
for (int doppler = static_cast<int>(-d_doppler_max); for (int doppler = static_cast<int>(-d_doppler_max);
doppler <= static_cast<int>(d_doppler_max); doppler <= static_cast<int>(d_doppler_max);
doppler += d_doppler_step) doppler += d_doppler_step)
{ {
d_num_doppler_bins++; d_num_doppler_bins++;
} }
// Create the carrier Doppler wipeoff signals // Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_bins];
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index; int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = GPS_TWO_PI * (d_freq + doppler) / static_cast<float>(d_fs_in); float phase_step_rad = GPS_TWO_PI * (d_freq + doppler) / static_cast<float>(d_fs_in);
float _phase[1]; float _phase[1];
_phase[0] = 0; _phase[0] = 0;
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], - phase_step_rad, _phase, d_fft_size); volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase, d_fft_size);
} }
} }
@ -211,7 +211,8 @@ void pcps_cccwsr_acquisition_cc::set_state(int state)
d_test_statistics = 0.0; d_test_statistics = 0.0;
} }
else if (d_state == 0) else if (d_state == 0)
{} {
}
else else
{ {
LOG(ERROR) << "State can only be set to 0 or 1"; LOG(ERROR) << "State can only be set to 0 or 1";
@ -220,227 +221,226 @@ void pcps_cccwsr_acquisition_cc::set_state(int state)
int pcps_cccwsr_acquisition_cc::general_work(int noutput_items, int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused))) gr_vector_void_star &output_items __attribute__((unused)))
{ {
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
switch (d_state) switch (d_state)
{
case 0:
{ {
if (d_active) case 0:
{ {
//restart acquisition variables if (d_active)
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_well_count = 0;
d_mag = 0.0;
d_input_power = 0.0;
d_test_statistics = 0.0;
d_state = 1;
}
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
break;
}
case 1:
{
// initialize acquisition algorithm
int doppler;
uint32_t indext = 0;
uint32_t indext_plus = 0;
uint32_t indext_minus = 0;
float magt = 0.0;
float magt_plus = 0.0;
float magt_minus = 0.0;
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_sample_counter += d_fft_size; // sample counter
d_well_count++;
DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " "<< d_gnss_synchro->PRN
<< " ,sample stamp: " << d_sample_counter << ", threshold: "
<< d_threshold << ", doppler_max: " << d_doppler_max
<< ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
// doppler search steps
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal
d_fft_if->execute();
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd data code reference (E1B) using SIMD operations
// with VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_data, d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// Copy the result of the correlation between wiped--off signal and data code in
// d_data_correlation.
memcpy(d_data_correlation, d_ifft->get_outbuf(), sizeof(gr_complex)*d_fft_size);
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd pilot code reference (E1C) using SIMD operations
// with VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_pilot, d_fft_size);
// Compute the inverse FFT
d_ifft->execute();
// Copy the result of the correlation between wiped--off signal and pilot code in
// d_data_correlation.
memcpy(d_pilot_correlation, d_ifft->get_outbuf(), sizeof(gr_complex)*d_fft_size);
for (unsigned int i = 0; i < d_fft_size; i++)
{
d_correlation_plus[i] = std::complex<float>(
d_data_correlation[i].real() - d_pilot_correlation[i].imag(),
d_data_correlation[i].imag() + d_pilot_correlation[i].real());
d_correlation_minus[i] = std::complex<float>(
d_data_correlation[i].real() + d_pilot_correlation[i].imag(),
d_data_correlation[i].imag() - d_pilot_correlation[i].real());
}
volk_32fc_magnitude_squared_32f(d_magnitude, d_correlation_plus, d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_plus, d_magnitude, d_fft_size);
magt_plus = d_magnitude[indext_plus] / (fft_normalization_factor * fft_normalization_factor);
volk_32fc_magnitude_squared_32f(d_magnitude, d_correlation_minus, d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_minus, d_magnitude, d_fft_size);
magt_minus = d_magnitude[indext_minus] / (fft_normalization_factor * fft_normalization_factor);
if (magt_plus >= magt_minus)
{ {
magt = magt_plus; //restart acquisition variables
indext = indext_plus; d_gnss_synchro->Acq_delay_samples = 0.0;
} d_gnss_synchro->Acq_doppler_hz = 0.0;
else d_gnss_synchro->Acq_samplestamp_samples = 0;
{ d_well_count = 0;
magt = magt_minus; d_mag = 0.0;
indext = indext_minus; d_input_power = 0.0;
d_test_statistics = 0.0;
d_state = 1;
} }
// 4- record the maximum peak and the associated synchronization parameters d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
if (d_mag < magt) consume_each(ninput_items[0]);
{
d_mag = magt;
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
}
// Record results to file if required break;
if (d_dump) }
{ case 1:
std::stringstream filename; {
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write // initialize acquisition algorithm
filename.str(""); int doppler;
filename << "../data/test_statistics_" << d_gnss_synchro->System
<<"_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char*>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
}
}
// 5- Compute the test statistics and compare to the threshold uint32_t indext = 0;
//d_test_statistics = 2 * d_fft_size * d_mag / d_input_power; uint32_t indext_plus = 0;
d_test_statistics = d_mag / d_input_power; uint32_t indext_minus = 0;
float magt = 0.0;
float magt_plus = 0.0;
float magt_minus = 0.0;
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
// 6- Declare positive or negative acquisition using a message port d_sample_counter += d_fft_size; // sample counter
if (d_test_statistics > d_threshold)
{
d_state = 2; // Positive acquisition
}
else if (d_well_count == d_max_dwells)
{
d_state = 3; // Negative acquisition
}
consume_each(1); d_well_count++;
break; DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN
<< " ,sample stamp: " << d_sample_counter << ", threshold: "
<< d_threshold << ", doppler_max: " << d_doppler_max
<< ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
// doppler search steps
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal
d_fft_if->execute();
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd data code reference (E1B) using SIMD operations
// with VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_data, d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// Copy the result of the correlation between wiped--off signal and data code in
// d_data_correlation.
memcpy(d_data_correlation, d_ifft->get_outbuf(), sizeof(gr_complex) * d_fft_size);
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd pilot code reference (E1C) using SIMD operations
// with VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_pilot, d_fft_size);
// Compute the inverse FFT
d_ifft->execute();
// Copy the result of the correlation between wiped--off signal and pilot code in
// d_data_correlation.
memcpy(d_pilot_correlation, d_ifft->get_outbuf(), sizeof(gr_complex) * d_fft_size);
for (unsigned int i = 0; i < d_fft_size; i++)
{
d_correlation_plus[i] = std::complex<float>(
d_data_correlation[i].real() - d_pilot_correlation[i].imag(),
d_data_correlation[i].imag() + d_pilot_correlation[i].real());
d_correlation_minus[i] = std::complex<float>(
d_data_correlation[i].real() + d_pilot_correlation[i].imag(),
d_data_correlation[i].imag() - d_pilot_correlation[i].real());
}
volk_32fc_magnitude_squared_32f(d_magnitude, d_correlation_plus, d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_plus, d_magnitude, d_fft_size);
magt_plus = d_magnitude[indext_plus] / (fft_normalization_factor * fft_normalization_factor);
volk_32fc_magnitude_squared_32f(d_magnitude, d_correlation_minus, d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext_minus, d_magnitude, d_fft_size);
magt_minus = d_magnitude[indext_minus] / (fft_normalization_factor * fft_normalization_factor);
if (magt_plus >= magt_minus)
{
magt = magt_plus;
indext = indext_plus;
}
else
{
magt = magt_minus;
indext = indext_minus;
}
// 4- record the maximum peak and the associated synchronization parameters
if (d_mag < magt)
{
d_mag = magt;
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
}
// Record results to file if required
if (d_dump)
{
std::stringstream filename;
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<< "_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
}
}
// 5- Compute the test statistics and compare to the threshold
//d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;
d_test_statistics = d_mag / d_input_power;
// 6- Declare positive or negative acquisition using a message port
if (d_test_statistics > d_threshold)
{
d_state = 2; // Positive acquisition
}
else if (d_well_count == d_max_dwells)
{
d_state = 3; // Negative acquisition
}
consume_each(1);
break;
}
case 2:
{
// 6.1- Declare positive acquisition using a message port
DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 1;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
case 3:
{
// 6.2- Declare negative acquisition using a message port
DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 2;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
} }
case 2:
{
// 6.1- Declare positive acquisition using a message port
DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 1;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
case 3:
{
// 6.2- Declare negative acquisition using a message port
DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 2;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
}
return noutput_items; return noutput_items;
} }

114
src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h
View File

@ -51,30 +51,30 @@ typedef boost::shared_ptr<pcps_cccwsr_acquisition_cc> pcps_cccwsr_acquisition_cc
pcps_cccwsr_acquisition_cc_sptr pcps_cccwsr_acquisition_cc_sptr
pcps_cccwsr_make_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, pcps_cccwsr_make_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool dump, std::string dump_filename); bool dump, std::string dump_filename);
/*! /*!
* \brief This class implements a Parallel Code Phase Search Acquisition with * \brief This class implements a Parallel Code Phase Search Acquisition with
* Coherent Channel Combining With Sign Recovery scheme. * Coherent Channel Combining With Sign Recovery scheme.
*/ */
class pcps_cccwsr_acquisition_cc: public gr::block class pcps_cccwsr_acquisition_cc : public gr::block
{ {
private: private:
friend pcps_cccwsr_acquisition_cc_sptr friend pcps_cccwsr_acquisition_cc_sptr
pcps_cccwsr_make_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, pcps_cccwsr_make_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool dump, std::string dump_filename); bool dump, std::string dump_filename);
pcps_cccwsr_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, pcps_cccwsr_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool dump, std::string dump_filename); bool dump, std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift, void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset); int doppler_offset);
long d_fs_in; long d_fs_in;
long d_freq; long d_freq;
@ -96,7 +96,7 @@ private:
gr_complex* d_fft_code_pilot; gr_complex* d_fft_code_pilot;
gr::fft::fft_complex* d_fft_if; gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft; gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro; Gnss_Synchro* d_gnss_synchro;
unsigned int d_code_phase; unsigned int d_code_phase;
float d_doppler_freq; float d_doppler_freq;
float d_mag; float d_mag;
@ -118,98 +118,98 @@ public:
/*! /*!
* \brief Default destructor. * \brief Default destructor.
*/ */
~pcps_cccwsr_acquisition_cc(); ~pcps_cccwsr_acquisition_cc();
/*! /*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer * \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks. * to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks. * \param p_gnss_synchro Satellite information shared by the processing blocks.
*/ */
inline void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) inline void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{ {
d_gnss_synchro = p_gnss_synchro; d_gnss_synchro = p_gnss_synchro;
} }
/*! /*!
* \brief Returns the maximum peak of grid search. * \brief Returns the maximum peak of grid search.
*/ */
inline unsigned int mag() const inline unsigned int mag() const
{ {
return d_mag; return d_mag;
} }
/*! /*!
* \brief Initializes acquisition algorithm. * \brief Initializes acquisition algorithm.
*/ */
void init(); void init();
/*! /*!
* \brief Sets local code for CCCWSR acquisition algorithm. * \brief Sets local code for CCCWSR acquisition algorithm.
* \param data_code - Pointer to the data PRN code. * \param data_code - Pointer to the data PRN code.
* \param pilot_code - Pointer to the pilot PRN code. * \param pilot_code - Pointer to the pilot PRN code.
*/ */
void set_local_code(std::complex<float> * code_data, std::complex<float> * code_pilot); void set_local_code(std::complex<float>* code_data, std::complex<float>* code_pilot);
/*! /*!
* \brief Starts acquisition algorithm, turning from standby mode to * \brief Starts acquisition algorithm, turning from standby mode to
* active mode * active mode
* \param active - bool that activates/deactivates the block. * \param active - bool that activates/deactivates the block.
*/ */
inline void set_active(bool active) inline void set_active(bool active)
{ {
d_active = active; d_active = active;
} }
/*! /*!
* \brief If set to 1, ensures that acquisition starts at the * \brief If set to 1, ensures that acquisition starts at the
* first available sample. * first available sample.
* \param state - int=1 forces start of acquisition * \param state - int=1 forces start of acquisition
*/ */
void set_state(int state); void set_state(int state);
/*! /*!
* \brief Set acquisition channel unique ID * \brief Set acquisition channel unique ID
* \param channel - receiver channel. * \param channel - receiver channel.
*/ */
inline void set_channel(unsigned int channel) inline void set_channel(unsigned int channel)
{ {
d_channel = channel; d_channel = channel;
} }
/*! /*!
* \brief Set statistics threshold of CCCWSR algorithm. * \brief Set statistics threshold of CCCWSR algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012, * \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold). * Algorithm 1, for a definition of this threshold).
*/ */
inline void set_threshold(float threshold) inline void set_threshold(float threshold)
{ {
d_threshold = threshold; d_threshold = threshold;
} }
/*! /*!
* \brief Set maximum Doppler grid search * \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz]. * \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/ */
inline void set_doppler_max(unsigned int doppler_max) inline void set_doppler_max(unsigned int doppler_max)
{ {
d_doppler_max = doppler_max; d_doppler_max = doppler_max;
} }
/*! /*!
* \brief Set Doppler steps for the grid search * \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz]. * \param doppler_step - Frequency bin of the search grid [Hz].
*/ */
inline void set_doppler_step(unsigned int doppler_step) inline void set_doppler_step(unsigned int doppler_step)
{ {
d_doppler_step = doppler_step; d_doppler_step = doppler_step;
} }
/*! /*!
* \brief Coherent Channel Combining With Sign Recovery Acquisition signal processing. * \brief Coherent Channel Combining With Sign Recovery Acquisition signal processing.
*/ */
int general_work(int noutput_items, gr_vector_int &ninput_items, int general_work(int noutput_items, gr_vector_int& ninput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star& input_items,
gr_vector_void_star &output_items); gr_vector_void_star& output_items);
}; };
#endif /* GNSS_SDR_PCPS_CCCWSR_ACQUISITION_CC_H_*/ #endif /* GNSS_SDR_PCPS_CCCWSR_ACQUISITION_CC_H_*/

600
src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc
View File

@ -58,40 +58,38 @@
#include <volk/volk.h> #include <volk/volk.h>
#include <volk_gnsssdr/volk_gnsssdr.h> #include <volk_gnsssdr/volk_gnsssdr.h>
#include "control_message_factory.h" #include "control_message_factory.h"
#include "fft_base_kernels.h" #include "opencl/fft_base_kernels.h"
#include "fft_internal.h" #include "opencl/fft_internal.h"
#include "GPS_L1_CA.h" //GPS_TWO_PI #include "GPS_L1_CA.h" //GPS_TWO_PI
using google::LogMessage; using google::LogMessage;
pcps_opencl_acquisition_cc_sptr pcps_make_opencl_acquisition_cc( pcps_opencl_acquisition_cc_sptr pcps_make_opencl_acquisition_cc(
unsigned int sampled_ms, unsigned int max_dwells, unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
bool dump, bool dump,
std::string dump_filename) std::string dump_filename)
{ {
return pcps_opencl_acquisition_cc_sptr( return pcps_opencl_acquisition_cc_sptr(
new pcps_opencl_acquisition_cc(sampled_ms, max_dwells, doppler_max, freq, fs_in, samples_per_ms, new pcps_opencl_acquisition_cc(sampled_ms, max_dwells, doppler_max, freq, fs_in, samples_per_ms,
samples_per_code, bit_transition_flag, dump, dump_filename)); samples_per_code, bit_transition_flag, dump, dump_filename));
} }
pcps_opencl_acquisition_cc::pcps_opencl_acquisition_cc( pcps_opencl_acquisition_cc::pcps_opencl_acquisition_cc(
unsigned int sampled_ms, unsigned int max_dwells, unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
bool dump, bool dump,
std::string dump_filename) : std::string dump_filename) : gr::block("pcps_opencl_acquisition_cc",
gr::block("pcps_opencl_acquisition_cc", gr::io_signature::make(1, 1, sizeof(gr_complex) * sampled_ms * samples_per_ms),
gr::io_signature::make(1, 1, sizeof(gr_complex) * sampled_ms * samples_per_ms), gr::io_signature::make(0, 0, sizeof(gr_complex) * sampled_ms * samples_per_ms))
gr::io_signature::make(0, 0, sizeof(gr_complex) * sampled_ms * samples_per_ms))
{ {
this->message_port_register_out(pmt::mp("events")); this->message_port_register_out(pmt::mp("events"));
d_sample_counter = 0; // SAMPLE COUNTER d_sample_counter = 0; // SAMPLE COUNTER
d_active = false; d_active = false;
d_state = 0; d_state = 0;
d_core_working = false; d_core_working = false;
@ -112,39 +110,37 @@ pcps_opencl_acquisition_cc::pcps_opencl_acquisition_cc(
d_in_dwell_count = 0; d_in_dwell_count = 0;
d_cl_fft_batch_size = 1; d_cl_fft_batch_size = 1;
d_in_buffer = new gr_complex*[d_max_dwells]; d_in_buffer = new gr_complex *[d_max_dwells];
for (unsigned int i = 0; i < d_max_dwells; i++) for (unsigned int i = 0; i < d_max_dwells; i++)
{ {
d_in_buffer[i] = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_in_buffer[i] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
} }
d_magnitude = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); d_magnitude = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
d_fft_codes = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size_pow2 * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_fft_codes = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size_pow2 * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_zero_vector = static_cast<gr_complex*>(volk_gnsssdr_malloc((d_fft_size_pow2 - d_fft_size) * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_zero_vector = static_cast<gr_complex *>(volk_gnsssdr_malloc((d_fft_size_pow2 - d_fft_size) * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
for (unsigned int i = 0; i < (d_fft_size_pow2-d_fft_size); i++) for (unsigned int i = 0; i < (d_fft_size_pow2 - d_fft_size); i++)
{ {
d_zero_vector[i] = gr_complex(0.0,0.0); d_zero_vector[i] = gr_complex(0.0, 0.0);
} }
d_opencl = init_opencl_environment("math_kernel.cl"); d_opencl = init_opencl_environment("math_kernel.cl");
if (d_opencl != 0) if (d_opencl != 0)
{ {
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
// Inverse FFT // Inverse FFT
d_ifft = new gr::fft::fft_complex(d_fft_size, false); d_ifft = new gr::fft::fft_complex(d_fft_size, false);
} }
// For dumping samples into a file // For dumping samples into a file
d_dump = dump; d_dump = dump;
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
} }
pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc() pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc()
{ {
if (d_num_doppler_bins > 0) if (d_num_doppler_bins > 0)
@ -174,7 +170,7 @@ pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc()
delete d_cl_buffer_2; delete d_cl_buffer_2;
delete d_cl_buffer_magnitude; delete d_cl_buffer_magnitude;
delete d_cl_buffer_fft_codes; delete d_cl_buffer_fft_codes;
if(d_num_doppler_bins > 0) if (d_num_doppler_bins > 0)
{ {
delete[] d_cl_buffer_grid_doppler_wipeoffs; delete[] d_cl_buffer_grid_doppler_wipeoffs;
} }
@ -194,20 +190,19 @@ pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc()
} }
int pcps_opencl_acquisition_cc::init_opencl_environment(std::string kernel_filename) int pcps_opencl_acquisition_cc::init_opencl_environment(std::string kernel_filename)
{ {
//get all platforms (drivers) //get all platforms (drivers)
std::vector<cl::Platform> all_platforms; std::vector<cl::Platform> all_platforms;
cl::Platform::get(&all_platforms); cl::Platform::get(&all_platforms);
if(all_platforms.size()==0) if (all_platforms.size() == 0)
{ {
std::cout << "No OpenCL platforms found. Check OpenCL installation!" << std::endl; std::cout << "No OpenCL platforms found. Check OpenCL installation!" << std::endl;
return 1; return 1;
} }
d_cl_platform = all_platforms[0]; //get default platform d_cl_platform = all_platforms[0]; //get default platform
std::cout << "Using platform: " << d_cl_platform.getInfo<CL_PLATFORM_NAME>() std::cout << "Using platform: " << d_cl_platform.getInfo<CL_PLATFORM_NAME>()
<< std::endl; << std::endl;
@ -215,11 +210,11 @@ int pcps_opencl_acquisition_cc::init_opencl_environment(std::string kernel_filen
std::vector<cl::Device> gpu_devices; std::vector<cl::Device> gpu_devices;
d_cl_platform.getDevices(CL_DEVICE_TYPE_GPU, &gpu_devices); d_cl_platform.getDevices(CL_DEVICE_TYPE_GPU, &gpu_devices);
if(gpu_devices.size()==0) if (gpu_devices.size() == 0)
{ {
std::cout << "No GPU devices found. Check OpenCL installation!" << std::endl; std::cout << "No GPU devices found. Check OpenCL installation!" << std::endl;
return 2; return 2;
} }
d_cl_device = gpu_devices[0]; d_cl_device = gpu_devices[0];
@ -240,53 +235,52 @@ int pcps_opencl_acquisition_cc::init_opencl_environment(std::string kernel_filen
cl::Program::Sources sources; cl::Program::Sources sources;
sources.push_back({kernel_code.c_str(),kernel_code.length()}); sources.push_back({kernel_code.c_str(), kernel_code.length()});
cl::Program program(context,sources); cl::Program program(context, sources);
if(program.build(device)!=CL_SUCCESS) if (program.build(device) != CL_SUCCESS)
{ {
std::cout << " Error building: " std::cout << " Error building: "
<< program.getBuildInfo<CL_PROGRAM_BUILD_LOG>(device[0]) << program.getBuildInfo<CL_PROGRAM_BUILD_LOG>(device[0])
<< std::endl; << std::endl;
return 3; return 3;
} }
d_cl_program = program; d_cl_program = program;
// create buffers on the device // create buffers on the device
d_cl_buffer_in = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex)*d_fft_size); d_cl_buffer_in = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex) * d_fft_size);
d_cl_buffer_fft_codes = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex)*d_fft_size_pow2); d_cl_buffer_fft_codes = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex) * d_fft_size_pow2);
d_cl_buffer_1 = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex)*d_fft_size_pow2); d_cl_buffer_1 = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex) * d_fft_size_pow2);
d_cl_buffer_2 = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex)*d_fft_size_pow2); d_cl_buffer_2 = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex) * d_fft_size_pow2);
d_cl_buffer_magnitude = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(float)*d_fft_size); d_cl_buffer_magnitude = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(float) * d_fft_size);
//create queue to which we will push commands for the device. //create queue to which we will push commands for the device.
d_cl_queue = new cl::CommandQueue(d_cl_context,d_cl_device); d_cl_queue = new cl::CommandQueue(d_cl_context, d_cl_device);
//create FFT plan //create FFT plan
cl_int err; cl_int err;
clFFT_Dim3 dim = {d_fft_size_pow2, 1, 1}; clFFT_Dim3 dim = {d_fft_size_pow2, 1, 1};
d_cl_fft_plan = clFFT_CreatePlan(d_cl_context(), dim, clFFT_1D, d_cl_fft_plan = clFFT_CreatePlan(d_cl_context(), dim, clFFT_1D,
clFFT_InterleavedComplexFormat, &err); clFFT_InterleavedComplexFormat, &err);
if (err != 0) if (err != 0)
{ {
delete d_cl_queue; delete d_cl_queue;
delete d_cl_buffer_in; delete d_cl_buffer_in;
delete d_cl_buffer_1; delete d_cl_buffer_1;
delete d_cl_buffer_2; delete d_cl_buffer_2;
delete d_cl_buffer_magnitude; delete d_cl_buffer_magnitude;
delete d_cl_buffer_fft_codes; delete d_cl_buffer_fft_codes;
std::cout << "Error creating OpenCL FFT plan." << std::endl; std::cout << "Error creating OpenCL FFT plan." << std::endl;
return 4; return 4;
} }
return 0; return 0;
} }
void pcps_opencl_acquisition_cc::init() void pcps_opencl_acquisition_cc::init()
{ {
d_gnss_synchro->Flag_valid_acquisition = false; d_gnss_synchro->Flag_valid_acquisition = false;
@ -305,76 +299,75 @@ void pcps_opencl_acquisition_cc::init()
for (int doppler = static_cast<int>(-d_doppler_max); for (int doppler = static_cast<int>(-d_doppler_max);
doppler <= static_cast<int>(d_doppler_max); doppler <= static_cast<int>(d_doppler_max);
doppler += d_doppler_step) doppler += d_doppler_step)
{ {
d_num_doppler_bins++; d_num_doppler_bins++;
} }
// Create the carrier Doppler wipeoff signals // Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_bins];
if (d_opencl == 0) if (d_opencl == 0)
{ {
d_cl_buffer_grid_doppler_wipeoffs = new cl::Buffer*[d_num_doppler_bins]; d_cl_buffer_grid_doppler_wipeoffs = new cl::Buffer *[d_num_doppler_bins];
} }
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index; int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = static_cast<float>(GPS_TWO_PI) * (d_freq + doppler) / static_cast<float>(d_fs_in); float phase_step_rad = static_cast<float>(GPS_TWO_PI) * (d_freq + doppler) / static_cast<float>(d_fs_in);
float _phase[1]; float _phase[1];
_phase[0] = 0; _phase[0] = 0;
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], - phase_step_rad, _phase, d_fft_size); volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase, d_fft_size);
if (d_opencl == 0) if (d_opencl == 0)
{ {
d_cl_buffer_grid_doppler_wipeoffs[doppler_index] = d_cl_buffer_grid_doppler_wipeoffs[doppler_index] =
new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex)*d_fft_size); new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex) * d_fft_size);
d_cl_queue->enqueueWriteBuffer(*(d_cl_buffer_grid_doppler_wipeoffs[doppler_index]), d_cl_queue->enqueueWriteBuffer(*(d_cl_buffer_grid_doppler_wipeoffs[doppler_index]),
CL_TRUE, 0, sizeof(gr_complex)*d_fft_size, CL_TRUE, 0, sizeof(gr_complex) * d_fft_size,
d_grid_doppler_wipeoffs[doppler_index]); d_grid_doppler_wipeoffs[doppler_index]);
} }
} }
// zero padding in buffer_1 (FFT input) // zero padding in buffer_1 (FFT input)
if (d_opencl == 0) if (d_opencl == 0)
{ {
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_1, CL_TRUE, sizeof(gr_complex)*d_fft_size, d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_1, CL_TRUE, sizeof(gr_complex) * d_fft_size,
sizeof(gr_complex)*(d_fft_size_pow2 - d_fft_size), d_zero_vector); sizeof(gr_complex) * (d_fft_size_pow2 - d_fft_size), d_zero_vector);
} }
} }
void pcps_opencl_acquisition_cc::set_local_code(std::complex<float> * code) void pcps_opencl_acquisition_cc::set_local_code(std::complex<float> *code)
{ {
if(d_opencl == 0) if (d_opencl == 0)
{ {
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, 0, d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, 0,
sizeof(gr_complex)*d_fft_size, code); sizeof(gr_complex) * d_fft_size, code);
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex)*d_fft_size, d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex) * d_fft_size,
sizeof(gr_complex)*(d_fft_size_pow2 - 2*d_fft_size), sizeof(gr_complex) * (d_fft_size_pow2 - 2 * d_fft_size),
d_zero_vector); d_zero_vector);
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex) d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex) * (d_fft_size_pow2 - d_fft_size),
*(d_fft_size_pow2 - d_fft_size), sizeof(gr_complex) * d_fft_size, code);
sizeof(gr_complex)*d_fft_size, code);
clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size, clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size,
clFFT_Forward, (*d_cl_buffer_2)(), (*d_cl_buffer_2)(), clFFT_Forward, (*d_cl_buffer_2)(), (*d_cl_buffer_2)(),
0, NULL, NULL); 0, NULL, NULL);
//Conjucate the local code //Conjucate the local code
cl::Kernel kernel = cl::Kernel(d_cl_program, "conj_vector"); cl::Kernel kernel = cl::Kernel(d_cl_program, "conj_vector");
kernel.setArg(0, *d_cl_buffer_2); //input kernel.setArg(0, *d_cl_buffer_2); //input
kernel.setArg(1, *d_cl_buffer_fft_codes); //output kernel.setArg(1, *d_cl_buffer_fft_codes); //output
d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size_pow2), cl::NullRange); d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size_pow2), cl::NullRange);
} }
else else
{ {
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size); volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
@ -388,7 +381,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
uint32_t indext = 0; uint32_t indext = 0;
float magt = 0.0; float magt = 0.0;
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size); float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
gr_complex* in = d_in_buffer[d_well_count]; gr_complex *in = d_in_buffer[d_well_count];
unsigned long int samplestamp = d_sample_counter_buffer[d_well_count]; unsigned long int samplestamp = d_sample_counter_buffer[d_well_count];
d_input_power = 0.0; d_input_power = 0.0;
@ -397,10 +390,10 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
d_well_count++; d_well_count++;
DLOG(INFO) << "Channel: " << d_channel DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " "<< d_gnss_synchro->PRN << " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN
<< " ,sample stamp: " << d_sample_counter << ", threshold: " << " ,sample stamp: " << d_sample_counter << ", threshold: "
<< d_threshold << ", doppler_max: " << d_doppler_max << d_threshold << ", doppler_max: " << d_doppler_max
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
@ -414,7 +407,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index; doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size); d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal // Compute the FFT of the carrier wiped--off incoming signal
@ -423,7 +416,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
@ -448,28 +441,28 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
// current d_mag/d_input_power). Note that d_test_statistics is not // current d_mag/d_input_power). Note that d_test_statistics is not
// restarted between consecutive dwells in multidwell operation. // restarted between consecutive dwells in multidwell operation.
if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag) if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag)
{ {
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code); d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler); d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = samplestamp; d_gnss_synchro->Acq_samplestamp_samples = samplestamp;
// 5- Compute the test statistics and compare to the threshold // 5- Compute the test statistics and compare to the threshold
//d_test_statistics = 2 * d_fft_size * d_mag / d_input_power; //d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;
d_test_statistics = d_mag / d_input_power; d_test_statistics = d_mag / d_input_power;
} }
} }
// Record results to file if required // Record results to file if required
if (d_dump) if (d_dump)
{ {
std::stringstream filename; std::stringstream filename;
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str(""); filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System filename << "../data/test_statistics_" << d_gnss_synchro->System
<<"_" << d_gnss_synchro->Signal << "_sat_" << "_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat"; << d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char*>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin? d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close(); d_dump_file.close();
} }
} }
@ -478,24 +471,24 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
{ {
if (d_test_statistics > d_threshold) if (d_test_statistics > d_threshold)
{ {
d_state = 2; // Positive acquisition d_state = 2; // Positive acquisition
} }
else if (d_well_count == d_max_dwells) else if (d_well_count == d_max_dwells)
{ {
d_state = 3; // Negative acquisition d_state = 3; // Negative acquisition
} }
} }
else else
{ {
if (d_well_count == d_max_dwells) // d_max_dwells = 2 if (d_well_count == d_max_dwells) // d_max_dwells = 2
{ {
if (d_test_statistics > d_threshold) if (d_test_statistics > d_threshold)
{ {
d_state = 2; // Positive acquisition d_state = 2; // Positive acquisition
} }
else else
{ {
d_state = 3; // Negative acquisition d_state = 3; // Negative acquisition
} }
} }
} }
@ -509,30 +502,30 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
int doppler; int doppler;
uint32_t indext = 0; uint32_t indext = 0;
float magt = 0.0; float magt = 0.0;
float fft_normalization_factor = (static_cast<float>(d_fft_size_pow2) * static_cast<float>(d_fft_size)); //This works, but I am not sure why. float fft_normalization_factor = (static_cast<float>(d_fft_size_pow2) * static_cast<float>(d_fft_size)); //This works, but I am not sure why.
gr_complex* in = d_in_buffer[d_well_count]; gr_complex *in = d_in_buffer[d_well_count];
unsigned long int samplestamp = d_sample_counter_buffer[d_well_count]; unsigned long int samplestamp = d_sample_counter_buffer[d_well_count];
d_input_power = 0.0; d_input_power = 0.0;
d_mag = 0.0; d_mag = 0.0;
// write input vector in buffer of OpenCL device // write input vector in buffer of OpenCL device
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_in, CL_TRUE, 0, sizeof(gr_complex)*d_fft_size, in); d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_in, CL_TRUE, 0, sizeof(gr_complex) * d_fft_size, in);
d_well_count++; d_well_count++;
// struct timeval tv; // struct timeval tv;
// long long int begin = 0; // long long int begin = 0;
// long long int end = 0; // long long int end = 0;
// gettimeofday(&tv, NULL); // gettimeofday(&tv, NULL);
// begin = tv.tv_sec *1e6 + tv.tv_usec; // begin = tv.tv_sec *1e6 + tv.tv_usec;
DLOG(INFO) << "Channel: " << d_channel DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN << " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN
<< " ,sample stamp: " << d_sample_counter << ", threshold: " << " ,sample stamp: " << d_sample_counter << ", threshold: "
<< d_threshold << ", doppler_max: " << d_doppler_max << d_threshold << ", doppler_max: " << d_doppler_max
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
@ -546,49 +539,49 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
{ {
// doppler search steps // doppler search steps
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step*doppler_index; doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
//Multiply input signal with doppler wipe-off //Multiply input signal with doppler wipe-off
kernel = cl::Kernel(d_cl_program, "mult_vectors"); kernel = cl::Kernel(d_cl_program, "mult_vectors");
kernel.setArg(0, *d_cl_buffer_in); //input 1 kernel.setArg(0, *d_cl_buffer_in); //input 1
kernel.setArg(1, *d_cl_buffer_grid_doppler_wipeoffs[doppler_index]); //input 2 kernel.setArg(1, *d_cl_buffer_grid_doppler_wipeoffs[doppler_index]); //input 2
kernel.setArg(2, *d_cl_buffer_1); //output kernel.setArg(2, *d_cl_buffer_1); //output
d_cl_queue->enqueueNDRangeKernel(kernel,cl::NullRange, cl::NDRange(d_fft_size), d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size),
cl::NullRange); cl::NullRange);
// In the previous operation, we store the result in the first d_fft_size positions // In the previous operation, we store the result in the first d_fft_size positions
// of d_cl_buffer_1. The rest d_fft_size_pow2-d_fft_size already have zeros // of d_cl_buffer_1. The rest d_fft_size_pow2-d_fft_size already have zeros
// (zero-padding is made in init() for optimization purposes). // (zero-padding is made in init() for optimization purposes).
clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size, clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size,
clFFT_Forward,(*d_cl_buffer_1)(), (*d_cl_buffer_2)(), clFFT_Forward, (*d_cl_buffer_1)(), (*d_cl_buffer_2)(),
0, NULL, NULL); 0, NULL, NULL);
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference // with the local FFT'd code reference
kernel = cl::Kernel(d_cl_program, "mult_vectors"); kernel = cl::Kernel(d_cl_program, "mult_vectors");
kernel.setArg(0, *d_cl_buffer_2); //input 1 kernel.setArg(0, *d_cl_buffer_2); //input 1
kernel.setArg(1, *d_cl_buffer_fft_codes); //input 2 kernel.setArg(1, *d_cl_buffer_fft_codes); //input 2
kernel.setArg(2, *d_cl_buffer_2); //output kernel.setArg(2, *d_cl_buffer_2); //output
d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size_pow2), d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size_pow2),
cl::NullRange); cl::NullRange);
// compute the inverse FFT // compute the inverse FFT
clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size, clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size,
clFFT_Inverse, (*d_cl_buffer_2)(), (*d_cl_buffer_2)(), clFFT_Inverse, (*d_cl_buffer_2)(), (*d_cl_buffer_2)(),
0, NULL, NULL); 0, NULL, NULL);
// Compute magnitude // Compute magnitude
kernel = cl::Kernel(d_cl_program, "magnitude_squared"); kernel = cl::Kernel(d_cl_program, "magnitude_squared");
kernel.setArg(0, *d_cl_buffer_2); //input 1 kernel.setArg(0, *d_cl_buffer_2); //input 1
kernel.setArg(1, *d_cl_buffer_magnitude); //output kernel.setArg(1, *d_cl_buffer_magnitude); //output
d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size), d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size),
cl::NullRange); cl::NullRange);
// This is the only function that blocks this thread until all previously enqueued // This is the only function that blocks this thread until all previously enqueued
// OpenCL commands are completed. // OpenCL commands are completed.
d_cl_queue->enqueueReadBuffer(*d_cl_buffer_magnitude, CL_TRUE, 0, d_cl_queue->enqueueReadBuffer(*d_cl_buffer_magnitude, CL_TRUE, 0,
sizeof(float)*d_fft_size,d_magnitude); sizeof(float) * d_fft_size, d_magnitude);
// Search maximum // Search maximum
// @TODO: find an efficient way to search the maximum with OpenCL in the GPU. // @TODO: find an efficient way to search the maximum with OpenCL in the GPU.
@ -610,58 +603,58 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
// current d_mag/d_input_power). Note that d_test_statistics is not // current d_mag/d_input_power). Note that d_test_statistics is not
// restarted between consecutive dwells in multidwell operation. // restarted between consecutive dwells in multidwell operation.
if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag) if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag)
{ {
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code); d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler); d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = samplestamp; d_gnss_synchro->Acq_samplestamp_samples = samplestamp;
// 5- Compute the test statistics and compare to the threshold // 5- Compute the test statistics and compare to the threshold
//d_test_statistics = 2 * d_fft_size * d_mag / d_input_power; //d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;
d_test_statistics = d_mag / d_input_power; d_test_statistics = d_mag / d_input_power;
} }
} }
// Record results to file if required // Record results to file if required
if (d_dump) if (d_dump)
{ {
std::stringstream filename; std::stringstream filename;
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str(""); filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System filename << "../data/test_statistics_" << d_gnss_synchro->System
<< "_" << d_gnss_synchro->Signal << "_sat_" << "_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat"; << d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char*>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin? d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close(); d_dump_file.close();
} }
} }
// gettimeofday(&tv, NULL); // gettimeofday(&tv, NULL);
// end = tv.tv_sec *1e6 + tv.tv_usec; // end = tv.tv_sec *1e6 + tv.tv_usec;
// std::cout << "Acq time = " << (end-begin) << " us" << std::endl; // std::cout << "Acq time = " << (end-begin) << " us" << std::endl;
if (!d_bit_transition_flag) if (!d_bit_transition_flag)
{ {
if (d_test_statistics > d_threshold) if (d_test_statistics > d_threshold)
{ {
d_state = 2; // Positive acquisition d_state = 2; // Positive acquisition
} }
else if (d_well_count == d_max_dwells) else if (d_well_count == d_max_dwells)
{ {
d_state = 3; // Negative acquisition d_state = 3; // Negative acquisition
} }
} }
else else
{ {
if (d_well_count == d_max_dwells) // d_max_dwells = 2 if (d_well_count == d_max_dwells) // d_max_dwells = 2
{ {
if (d_test_statistics > d_threshold) if (d_test_statistics > d_threshold)
{ {
d_state = 2; // Positive acquisition d_state = 2; // Positive acquisition
} }
else else
{ {
d_state = 3; // Negative acquisition d_state = 3; // Negative acquisition
} }
} }
} }
@ -686,7 +679,8 @@ void pcps_opencl_acquisition_cc::set_state(int state)
d_sample_counter_buffer.clear(); d_sample_counter_buffer.clear();
} }
else if (d_state == 0) else if (d_state == 0)
{} {
}
else else
{ {
LOG(ERROR) << "State can only be set to 0 or 1"; LOG(ERROR) << "State can only be set to 0 or 1";
@ -694,137 +688,137 @@ void pcps_opencl_acquisition_cc::set_state(int state)
} }
int pcps_opencl_acquisition_cc::general_work(int noutput_items, int pcps_opencl_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused))) gr_vector_void_star &output_items __attribute__((unused)))
{ {
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
switch (d_state) switch (d_state)
{
case 0:
{ {
if (d_active) case 0:
{ {
//restart acquisition variables if (d_active)
d_gnss_synchro->Acq_delay_samples = 0.0; {
d_gnss_synchro->Acq_doppler_hz = 0.0; //restart acquisition variables
d_gnss_synchro->Acq_samplestamp_samples = 0; d_gnss_synchro->Acq_delay_samples = 0.0;
d_well_count = 0; d_gnss_synchro->Acq_doppler_hz = 0.0;
d_mag = 0.0; d_gnss_synchro->Acq_samplestamp_samples = 0;
d_input_power = 0.0; d_well_count = 0;
d_test_statistics = 0.0; d_mag = 0.0;
d_in_dwell_count = 0; d_input_power = 0.0;
d_sample_counter_buffer.clear(); d_test_statistics = 0.0;
d_in_dwell_count = 0;
d_sample_counter_buffer.clear();
d_state = 1; d_state = 1;
} }
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
break; break;
}
case 1:
{
if (d_in_dwell_count < d_max_dwells)
{
// Fill internal buffer with d_max_dwells signal blocks. This step ensures that
// consecutive signal blocks will be processed in multi-dwell operation. This is
// essential when d_bit_transition_flag = true.
unsigned int num_dwells = std::min(static_cast<int>(d_max_dwells - d_in_dwell_count), ninput_items[0]);
for (unsigned int i = 0; i < num_dwells; i++)
{
memcpy(d_in_buffer[d_in_dwell_count++], static_cast<const gr_complex *>(input_items[i]),
sizeof(gr_complex) * d_fft_size);
d_sample_counter += d_fft_size;
d_sample_counter_buffer.push_back(d_sample_counter);
}
if (ninput_items[0] > static_cast<int>(num_dwells))
{
d_sample_counter += d_fft_size * (ninput_items[0] - num_dwells);
}
}
else
{
// We already have d_max_dwells consecutive blocks in the internal buffer,
// just skip input blocks.
d_sample_counter += d_fft_size * ninput_items[0];
}
// We create a new thread to process next block if the following
// conditions are fulfilled:
// 1. There are new blocks in d_in_buffer that have not been processed yet
// (d_well_count < d_in_dwell_count).
// 2. No other acquisition_core thead is working (!d_core_working).
// 3. d_state==1. We need to check again d_state because it can be modified at any
// moment by the external thread (may have changed since checked in the switch()).
// If the external thread has already declared positive (d_state=2) or negative
// (d_state=3) acquisition, we don't have to process next block!!
if ((d_well_count < d_in_dwell_count) && !d_core_working && d_state == 1)
{
d_core_working = true;
if (d_opencl == 0)
{ // Use OpenCL implementation
boost::thread(&pcps_opencl_acquisition_cc::acquisition_core_opencl, this);
}
else
{ // Use Volk implementation
boost::thread(&pcps_opencl_acquisition_cc::acquisition_core_volk, this);
}
}
break;
}
case 2:
{
// Declare positive acquisition using a message port
DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
acquisition_message = 1;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
case 3:
{
// Declare negative acquisition using a message port
DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
acquisition_message = 2;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
} }
case 1:
{
if (d_in_dwell_count < d_max_dwells)
{
// Fill internal buffer with d_max_dwells signal blocks. This step ensures that
// consecutive signal blocks will be processed in multi-dwell operation. This is
// essential when d_bit_transition_flag = true.
unsigned int num_dwells = std::min(static_cast<int>(d_max_dwells - d_in_dwell_count), ninput_items[0]);
for (unsigned int i = 0; i < num_dwells; i++)
{
memcpy(d_in_buffer[d_in_dwell_count++], static_cast<const gr_complex*>(input_items[i]),
sizeof(gr_complex)*d_fft_size);
d_sample_counter += d_fft_size;
d_sample_counter_buffer.push_back(d_sample_counter);
}
if (ninput_items[0] > static_cast<int>(num_dwells))
{
d_sample_counter += d_fft_size * (ninput_items[0] - num_dwells);
}
}
else
{
// We already have d_max_dwells consecutive blocks in the internal buffer,
// just skip input blocks.
d_sample_counter += d_fft_size * ninput_items[0];
}
// We create a new thread to process next block if the following
// conditions are fulfilled:
// 1. There are new blocks in d_in_buffer that have not been processed yet
// (d_well_count < d_in_dwell_count).
// 2. No other acquisition_core thead is working (!d_core_working).
// 3. d_state==1. We need to check again d_state because it can be modified at any
// moment by the external thread (may have changed since checked in the switch()).
// If the external thread has already declared positive (d_state=2) or negative
// (d_state=3) acquisition, we don't have to process next block!!
if ((d_well_count < d_in_dwell_count) && !d_core_working && d_state == 1)
{
d_core_working = true;
if (d_opencl == 0)
{ // Use OpenCL implementation
boost::thread(&pcps_opencl_acquisition_cc::acquisition_core_opencl, this);
}
else
{ // Use Volk implementation
boost::thread(&pcps_opencl_acquisition_cc::acquisition_core_volk, this);
}
}
break;
}
case 2:
{
// Declare positive acquisition using a message port
DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
acquisition_message = 1;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
case 3:
{
// Declare negative acquisition using a message port
DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
acquisition_message = 2;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
}
consume_each(ninput_items[0]); consume_each(ninput_items[0]);
return noutput_items; return noutput_items;

138
src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h
View File

@ -57,13 +57,13 @@
#include <gnuradio/block.h> #include <gnuradio/block.h>
#include <gnuradio/gr_complex.h> #include <gnuradio/gr_complex.h>
#include <gnuradio/fft/fft.h> #include <gnuradio/fft/fft.h>
#include "fft_internal.h" #include "opencl/fft_internal.h"
#include "gnss_synchro.h" #include "gnss_synchro.h"
#ifdef __APPLE__ #ifdef __APPLE__
#include "cl.hpp" #include "opencl/cl.hpp"
#else #else
#include <CL/cl.hpp> #include <CL/cl.hpp>
#endif #endif
class pcps_opencl_acquisition_cc; class pcps_opencl_acquisition_cc;
@ -72,11 +72,11 @@ typedef boost::shared_ptr<pcps_opencl_acquisition_cc> pcps_opencl_acquisition_cc
pcps_opencl_acquisition_cc_sptr pcps_opencl_acquisition_cc_sptr
pcps_make_opencl_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, pcps_make_opencl_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
bool dump, bool dump,
std::string dump_filename); std::string dump_filename);
/*! /*!
* \brief This class implements a Parallel Code Phase Search Acquisition. * \brief This class implements a Parallel Code Phase Search Acquisition.
@ -84,26 +84,26 @@ pcps_make_opencl_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells
* Check \ref Navitec2012 "An Open Source Galileo E1 Software Receiver", * Check \ref Navitec2012 "An Open Source Galileo E1 Software Receiver",
* Algorithm 1, for a pseudocode description of this implementation. * Algorithm 1, for a pseudocode description of this implementation.
*/ */
class pcps_opencl_acquisition_cc: public gr::block class pcps_opencl_acquisition_cc : public gr::block
{ {
private: private:
friend pcps_opencl_acquisition_cc_sptr friend pcps_opencl_acquisition_cc_sptr
pcps_make_opencl_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, pcps_make_opencl_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
bool dump, bool dump,
std::string dump_filename); std::string dump_filename);
pcps_opencl_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, pcps_opencl_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
bool dump, bool dump,
std::string dump_filename); std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift, void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset); int doppler_offset);
int init_opencl_environment(std::string kernel_filename); int init_opencl_environment(std::string kernel_filename);
@ -128,7 +128,7 @@ private:
gr_complex* d_fft_codes; gr_complex* d_fft_codes;
gr::fft::fft_complex* d_fft_if; gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft; gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro; Gnss_Synchro* d_gnss_synchro;
unsigned int d_code_phase; unsigned int d_code_phase;
float d_doppler_freq; float d_doppler_freq;
float d_mag; float d_mag;
@ -168,101 +168,101 @@ public:
/*! /*!
* \brief Default destructor. * \brief Default destructor.
*/ */
~pcps_opencl_acquisition_cc(); ~pcps_opencl_acquisition_cc();
/*! /*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer * \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks. * to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks. * \param p_gnss_synchro Satellite information shared by the processing blocks.
*/ */
inline void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) inline void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{ {
d_gnss_synchro = p_gnss_synchro; d_gnss_synchro = p_gnss_synchro;
} }
/*! /*!
* \brief Returns the maximum peak of grid search. * \brief Returns the maximum peak of grid search.
*/ */
inline unsigned int mag() const inline unsigned int mag() const
{ {
return d_mag; return d_mag;
} }
/*! /*!
* \brief Initializes acquisition algorithm. * \brief Initializes acquisition algorithm.
*/ */
void init(); void init();
/*! /*!
* \brief Sets local code for PCPS acquisition algorithm. * \brief Sets local code for PCPS acquisition algorithm.
* \param code - Pointer to the PRN code. * \param code - Pointer to the PRN code.
*/ */
void set_local_code(std::complex<float> * code); void set_local_code(std::complex<float>* code);
/*! /*!
* \brief Starts acquisition algorithm, turning from standby mode to * \brief Starts acquisition algorithm, turning from standby mode to
* active mode * active mode
* \param active - bool that activates/deactivates the block. * \param active - bool that activates/deactivates the block.
*/ */
inline void set_active(bool active) inline void set_active(bool active)
{ {
d_active = active; d_active = active;
} }
/*! /*!
* \brief If set to 1, ensures that acquisition starts at the * \brief If set to 1, ensures that acquisition starts at the
* first available sample. * first available sample.
* \param state - int=1 forces start of acquisition * \param state - int=1 forces start of acquisition
*/ */
void set_state(int state); void set_state(int state);
/*! /*!
* \brief Set acquisition channel unique ID * \brief Set acquisition channel unique ID
* \param channel - receiver channel. * \param channel - receiver channel.
*/ */
inline void set_channel(unsigned int channel) inline void set_channel(unsigned int channel)
{ {
d_channel = channel; d_channel = channel;
} }
/*! /*!
* \brief Set statistics threshold of PCPS algorithm. * \brief Set statistics threshold of PCPS algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012, * \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold). * Algorithm 1, for a definition of this threshold).
*/ */
inline void set_threshold(float threshold) inline void set_threshold(float threshold)
{ {
d_threshold = threshold; d_threshold = threshold;
} }
/*! /*!
* \brief Set maximum Doppler grid search * \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz]. * \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/ */
inline void set_doppler_max(unsigned int doppler_max) inline void set_doppler_max(unsigned int doppler_max)
{ {
d_doppler_max = doppler_max; d_doppler_max = doppler_max;
} }
/*! /*!
* \brief Set Doppler steps for the grid search * \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz]. * \param doppler_step - Frequency bin of the search grid [Hz].
*/ */
inline void set_doppler_step(unsigned int doppler_step) inline void set_doppler_step(unsigned int doppler_step)
{ {
d_doppler_step = doppler_step; d_doppler_step = doppler_step;
} }
/*! /*!
* \brief Parallel Code Phase Search Acquisition signal processing. * \brief Parallel Code Phase Search Acquisition signal processing.
*/ */
int general_work(int noutput_items, gr_vector_int &ninput_items, int general_work(int noutput_items, gr_vector_int& ninput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star& input_items,
gr_vector_void_star &output_items); gr_vector_void_star& output_items);
void acquisition_core_volk(); void acquisition_core_volk();
void acquisition_core_opencl(); void acquisition_core_opencl();
}; };
#endif #endif

570
src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc
View File

@ -42,38 +42,38 @@
using google::LogMessage; using google::LogMessage;
pcps_quicksync_acquisition_cc_sptr pcps_quicksync_make_acquisition_cc( pcps_quicksync_acquisition_cc_sptr pcps_quicksync_make_acquisition_cc(
unsigned int folding_factor, unsigned int folding_factor,
unsigned int sampled_ms, unsigned int max_dwells, unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
bool dump, bool dump,
std::string dump_filename) std::string dump_filename)
{ {
return pcps_quicksync_acquisition_cc_sptr( return pcps_quicksync_acquisition_cc_sptr(
new pcps_quicksync_acquisition_cc( new pcps_quicksync_acquisition_cc(
folding_factor, folding_factor,
sampled_ms, max_dwells, doppler_max, sampled_ms, max_dwells, doppler_max,
freq, fs_in, samples_per_ms, freq, fs_in, samples_per_ms,
samples_per_code, samples_per_code,
bit_transition_flag, bit_transition_flag,
dump, dump_filename)); dump, dump_filename));
} }
pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc( pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
unsigned int folding_factor, unsigned int folding_factor,
unsigned int sampled_ms, unsigned int max_dwells, unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
bool dump, std::string dump_filename): bool dump,
gr::block("pcps_quicksync_acquisition_cc", std::string dump_filename) : gr::block("pcps_quicksync_acquisition_cc",
gr::io_signature::make(1, 1, (sizeof(gr_complex)*sampled_ms * samples_per_ms )), gr::io_signature::make(1, 1, (sizeof(gr_complex) * sampled_ms * samples_per_ms)),
gr::io_signature::make(0, 0, (sizeof(gr_complex)*sampled_ms * samples_per_ms ))) gr::io_signature::make(0, 0, (sizeof(gr_complex) * sampled_ms * samples_per_ms)))
{ {
this->message_port_register_out(pmt::mp("events")); this->message_port_register_out(pmt::mp("events"));
d_sample_counter = 0; // SAMPLE COUNTER d_sample_counter = 0; // SAMPLE COUNTER
d_active = false; d_active = false;
d_state = 0; d_state = 0;
d_freq = freq; d_freq = freq;
@ -178,16 +178,15 @@ void pcps_quicksync_acquisition_cc::set_local_code(std::complex<float>* code)
in the frequency domain after applying the fftw operation*/ in the frequency domain after applying the fftw operation*/
for (unsigned int i = 0; i < d_folding_factor; i++) for (unsigned int i = 0; i < d_folding_factor; i++)
{ {
std::transform ((code + i * d_fft_size), (code + ((i + 1) * d_fft_size)) , std::transform((code + i * d_fft_size), (code + ((i + 1) * d_fft_size)),
d_fft_if->get_inbuf(), d_fft_if->get_inbuf(), d_fft_if->get_inbuf(), d_fft_if->get_inbuf(),
std::plus<gr_complex>()); std::plus<gr_complex>());
} }
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size); volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
} }
@ -205,13 +204,13 @@ void pcps_quicksync_acquisition_cc::init()
d_mag = 0.0; d_mag = 0.0;
d_input_power = 0.0; d_input_power = 0.0;
if(d_doppler_step == 0) d_doppler_step = 250; if (d_doppler_step == 0) d_doppler_step = 250;
// Count the number of bins // Count the number of bins
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
for (int doppler = static_cast<int>(-d_doppler_max); for (int doppler = static_cast<int>(-d_doppler_max);
doppler <= static_cast<int>(d_doppler_max); doppler <= static_cast<int>(d_doppler_max);
doppler += d_doppler_step) doppler += d_doppler_step)
{ {
d_num_doppler_bins++; d_num_doppler_bins++;
} }
@ -225,37 +224,38 @@ void pcps_quicksync_acquisition_cc::init()
float phase_step_rad = GPS_TWO_PI * (d_freq + doppler) / static_cast<float>(d_fs_in); float phase_step_rad = GPS_TWO_PI * (d_freq + doppler) / static_cast<float>(d_fs_in);
float _phase[1]; float _phase[1];
_phase[0] = 0; _phase[0] = 0;
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], - phase_step_rad, _phase, d_samples_per_code * d_folding_factor); volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase, d_samples_per_code * d_folding_factor);
} }
// DLOG(INFO) << "end init"; // DLOG(INFO) << "end init";
} }
void pcps_quicksync_acquisition_cc::set_state(int state) void pcps_quicksync_acquisition_cc::set_state(int state)
{ {
d_state = state; d_state = state;
if (d_state == 1) if (d_state == 1)
{ {
d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0; d_gnss_synchro->Acq_samplestamp_samples = 0;
d_well_count = 0; d_well_count = 0;
d_mag = 0.0; d_mag = 0.0;
d_input_power = 0.0; d_input_power = 0.0;
d_test_statistics = 0.0; d_test_statistics = 0.0;
d_active = 1; d_active = 1;
} }
else if (d_state == 0) else if (d_state == 0)
{} {
else }
{ else
LOG(ERROR) << "State can only be set to 0 or 1"; {
} LOG(ERROR) << "State can only be set to 0 or 1";
} }
}
int pcps_quicksync_acquisition_cc::general_work(int noutput_items, int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_int& ninput_items, gr_vector_const_void_star& input_items,
gr_vector_void_star &output_items __attribute__((unused))) gr_vector_void_star& output_items __attribute__((unused)))
{ {
/* /*
* By J.Arribas, L.Esteve and M.Molina * By J.Arribas, L.Esteve and M.Molina
@ -268,314 +268,312 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
* 6. Declare positive or negative acquisition using a message queue * 6. Declare positive or negative acquisition using a message queue
*/ */
//DLOG(INFO) << "START GENERAL WORK"; //DLOG(INFO) << "START GENERAL WORK";
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
//std::cout<<"general_work in quicksync gnuradio block"<<std::endl; //std::cout<<"general_work in quicksync gnuradio block"<<std::endl;
switch (d_state) switch (d_state)
{
case 0:
{ {
//DLOG(INFO) << "START CASE 0"; case 0:
if (d_active) {
{ //DLOG(INFO) << "START CASE 0";
//restart acquisition variables if (d_active)
d_gnss_synchro->Acq_delay_samples = 0.0; {
d_gnss_synchro->Acq_doppler_hz = 0.0; //restart acquisition variables
d_gnss_synchro->Acq_samplestamp_samples = 0; d_gnss_synchro->Acq_delay_samples = 0.0;
d_well_count = 0; d_gnss_synchro->Acq_doppler_hz = 0.0;
d_mag = 0.0; d_gnss_synchro->Acq_samplestamp_samples = 0;
d_input_power = 0.0; d_well_count = 0;
d_test_statistics = 0.0; d_mag = 0.0;
d_input_power = 0.0;
d_test_statistics = 0.0;
d_state = 1; d_state = 1;
} }
d_sample_counter += d_sampled_ms * d_samples_per_ms * ninput_items[0]; // sample counter d_sample_counter += d_sampled_ms * d_samples_per_ms * ninput_items[0]; // sample counter
consume_each(ninput_items[0]); consume_each(ninput_items[0]);
//DLOG(INFO) << "END CASE 0"; //DLOG(INFO) << "END CASE 0";
break; break;
} }
case 1: case 1:
{ {
/* initialize acquisition implementing the QuickSync algorithm*/ /* initialize acquisition implementing the QuickSync algorithm*/
//DLOG(INFO) << "START CASE 1"; //DLOG(INFO) << "START CASE 1";
int doppler; int doppler;
uint32_t indext = 0; uint32_t indext = 0;
float magt = 0.0; float magt = 0.0;
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer const gr_complex* in = reinterpret_cast<const gr_complex*>(input_items[0]); //Get the input samples pointer
gr_complex* in_temp = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * d_folding_factor * sizeof(gr_complex), volk_gnsssdr_get_alignment())); gr_complex* in_temp = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * d_folding_factor * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
gr_complex* in_temp_folded = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); gr_complex* in_temp_folded = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
/*Create a signal to store a signal of size 1ms, to perform correlation /*Create a signal to store a signal of size 1ms, to perform correlation
in time. No folding on this data is required*/ in time. No folding on this data is required*/
gr_complex* in_1code = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * sizeof(gr_complex), volk_gnsssdr_get_alignment())); gr_complex* in_1code = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
/*Stores the values of the correlation output between the local code /*Stores the values of the correlation output between the local code
and the signal with doppler shift corrected */ and the signal with doppler shift corrected */
gr_complex* corr_output = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * sizeof(gr_complex), volk_gnsssdr_get_alignment())); gr_complex* corr_output = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
/*Stores a copy of the folded version of the signal.This is used for /*Stores a copy of the folded version of the signal.This is used for
the FFT operations in future steps of excecution*/ the FFT operations in future steps of excecution*/
// gr_complex in_folded[d_fft_size]; // gr_complex in_folded[d_fft_size];
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size); float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_input_power = 0.0; d_input_power = 0.0;
d_mag = 0.0; d_mag = 0.0;
d_test_statistics = 0.0; d_test_statistics = 0.0;
d_noise_floor_power = 0.0; d_noise_floor_power = 0.0;
d_sample_counter += d_sampled_ms * d_samples_per_ms; // sample counter d_sample_counter += d_sampled_ms * d_samples_per_ms; // sample counter
d_well_count++; d_well_count++;
DLOG(INFO) << "Channel: " << d_channel DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << " , doing acquisition of satellite: "
<< d_gnss_synchro->System << " "<< d_gnss_synchro->PRN << d_gnss_synchro->System << " " << d_gnss_synchro->PRN
<< " ,algorithm: pcps_quicksync_acquisition" << " ,algorithm: pcps_quicksync_acquisition"
<< " ,folding factor: " << d_folding_factor << " ,folding factor: " << d_folding_factor
<< " ,sample stamp: " << d_sample_counter << ", threshold: " << " ,sample stamp: " << d_sample_counter << ", threshold: "
<< d_threshold << ", doppler_max: " << d_doppler_max << d_threshold << ", doppler_max: " << d_doppler_max
<< ", doppler_step: " << d_doppler_step << ", Signal Size: " << ", doppler_step: " << d_doppler_step << ", Signal Size: "
<< d_samples_per_code * d_folding_factor; << d_samples_per_code * d_folding_factor;
/* 1- Compute the input signal power estimation. This operation is /* 1- Compute the input signal power estimation. This operation is
being performed in a signal of size nxp */ being performed in a signal of size nxp */
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_samples_per_code * d_folding_factor); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_samples_per_code * d_folding_factor);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_samples_per_code * d_folding_factor); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_samples_per_code * d_folding_factor);
d_input_power /= static_cast<float>(d_samples_per_code * d_folding_factor); d_input_power /= static_cast<float>(d_samples_per_code * d_folding_factor);
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
/*Ensure that the signal is going to start with all samples /*Ensure that the signal is going to start with all samples
at zero. This is done to avoid over acumulation when performing at zero. This is done to avoid over acumulation when performing
the folding process to be stored in d_fft_if->get_inbuf()*/ the folding process to be stored in d_fft_if->get_inbuf()*/
d_signal_folded = new gr_complex[d_fft_size](); d_signal_folded = new gr_complex[d_fft_size]();
memcpy( d_fft_if->get_inbuf(), d_signal_folded, sizeof(gr_complex) * (d_fft_size)); memcpy(d_fft_if->get_inbuf(), d_signal_folded, sizeof(gr_complex) * (d_fft_size));
/*Doppler search steps and then multiplication of the incoming /*Doppler search steps and then multiplication of the incoming
signal with the doppler wipeoffs to eliminate frequency offset signal with the doppler wipeoffs to eliminate frequency offset
*/ */
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index; doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
/*Perform multiplication of the incoming signal with the /*Perform multiplication of the incoming signal with the
complex exponential vector. This removes the frequency doppler complex exponential vector. This removes the frequency doppler
shift offset*/ shift offset*/
volk_32fc_x2_multiply_32fc(in_temp, in, volk_32fc_x2_multiply_32fc(in_temp, in,
d_grid_doppler_wipeoffs[doppler_index], d_grid_doppler_wipeoffs[doppler_index],
d_samples_per_code * d_folding_factor); d_samples_per_code * d_folding_factor);
/*Perform folding of the carrier wiped-off incoming signal. Since /*Perform folding of the carrier wiped-off incoming signal. Since
superlinear method is being used the folding factor in the superlinear method is being used the folding factor in the
incoming raw data signal is of d_folding_factor^2*/ incoming raw data signal is of d_folding_factor^2*/
for ( int i = 0; i < static_cast<int>(d_folding_factor * d_folding_factor); i++) for (int i = 0; i < static_cast<int>(d_folding_factor * d_folding_factor); i++)
{ {
std::transform ((in_temp + i * d_fft_size), std::transform((in_temp + i * d_fft_size),
(in_temp + ((i + 1) * d_fft_size)) , (in_temp + ((i + 1) * d_fft_size)),
d_fft_if->get_inbuf(), d_fft_if->get_inbuf(),
d_fft_if->get_inbuf(), d_fft_if->get_inbuf(),
std::plus<gr_complex>()); std::plus<gr_complex>());
} }
/* 3- Perform the FFT-based convolution (parallel time search) /* 3- Perform the FFT-based convolution (parallel time search)
Compute the FFT of the carrier wiped--off incoming signal*/ Compute the FFT of the carrier wiped--off incoming signal*/
d_fft_if->execute(); d_fft_if->execute();
/*Multiply carrier wiped--off, Fourier transformed incoming /*Multiply carrier wiped--off, Fourier transformed incoming
signal with the local FFT'd code reference using SIMD signal with the local FFT'd code reference using SIMD
operations with VOLK library*/ operations with VOLK library*/
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
/* compute the inverse FFT of the aliased signal*/ /* compute the inverse FFT of the aliased signal*/
d_ifft->execute(); d_ifft->execute();
/* Compute the magnitude and get the maximum value with its /* Compute the magnitude and get the maximum value with its
index position*/ index position*/
volk_32fc_magnitude_squared_32f(d_magnitude_folded, volk_32fc_magnitude_squared_32f(d_magnitude_folded,
d_ifft->get_outbuf(), d_fft_size); d_ifft->get_outbuf(), d_fft_size);
/* Normalize the maximum value to correct the scale factor /* Normalize the maximum value to correct the scale factor
introduced by FFTW*/ introduced by FFTW*/
//volk_32f_s32f_multiply_32f_a(d_magnitude_folded,d_magnitude_folded, //volk_32f_s32f_multiply_32f_a(d_magnitude_folded,d_magnitude_folded,
// (1 / (fft_normalization_factor * fft_normalization_factor)), d_fft_size); // (1 / (fft_normalization_factor * fft_normalization_factor)), d_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude_folded, d_fft_size); volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude_folded, d_fft_size);
magt = d_magnitude_folded[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitude_folded[indext] / (fft_normalization_factor * fft_normalization_factor);
delete[] d_signal_folded; delete[] d_signal_folded;
// 4- record the maximum peak and the associated synchronization parameters // 4- record the maximum peak and the associated synchronization parameters
if (d_mag < magt) if (d_mag < magt)
{ {
d_mag = magt; d_mag = magt;
/* In case that d_bit_transition_flag = true, we compare the potentially /* In case that d_bit_transition_flag = true, we compare the potentially
new maximum test statistics (d_mag/d_input_power) with the value in new maximum test statistics (d_mag/d_input_power) with the value in
d_test_statistics. When the second dwell is being processed, the value d_test_statistics. When the second dwell is being processed, the value
of d_mag/d_input_power could be lower than d_test_statistics (i.e, of d_mag/d_input_power could be lower than d_test_statistics (i.e,
the maximum test statistics in the previous dwell is greater than the maximum test statistics in the previous dwell is greater than
current d_mag/d_input_power). Note that d_test_statistics is not current d_mag/d_input_power). Note that d_test_statistics is not
restarted between consecutive dwells in multidwell operation.*/ restarted between consecutive dwells in multidwell operation.*/
if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag) if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag)
{ {
unsigned int detected_delay_samples_folded = 0; unsigned int detected_delay_samples_folded = 0;
detected_delay_samples_folded = (indext % d_samples_per_code); detected_delay_samples_folded = (indext % d_samples_per_code);
gr_complex complex_acumulator[100]; gr_complex complex_acumulator[100];
//gr_complex complex_acumulator[d_folding_factor]; //gr_complex complex_acumulator[d_folding_factor];
for (int i = 0; i < static_cast<int>(d_folding_factor); i++) for (int i = 0; i < static_cast<int>(d_folding_factor); i++)
{ {
d_possible_delay[i] = detected_delay_samples_folded + (i) * d_fft_size; d_possible_delay[i] = detected_delay_samples_folded + (i)*d_fft_size;
} }
for ( int i = 0; i < static_cast<int>(d_folding_factor); i++) for (int i = 0; i < static_cast<int>(d_folding_factor); i++)
{ {
/*Copy a signal of 1 code length into suggested buffer.
/*Copy a signal of 1 code length into suggested buffer.
The copied signal must have doppler effect corrected*/ The copied signal must have doppler effect corrected*/
memcpy(in_1code,&in_temp[d_possible_delay[i]], memcpy(in_1code, &in_temp[d_possible_delay[i]],
sizeof(gr_complex) * (d_samples_per_code)); sizeof(gr_complex) * (d_samples_per_code));
/*Perform multiplication of the unmodified local /*Perform multiplication of the unmodified local
generated code with the incoming signal with doppler generated code with the incoming signal with doppler
effect corrected and accumulates its value. This effect corrected and accumulates its value. This
is indeed correlation in time for an specific value is indeed correlation in time for an specific value
of a shift*/ of a shift*/
volk_32fc_x2_multiply_32fc(corr_output, in_1code, d_code, d_samples_per_code); volk_32fc_x2_multiply_32fc(corr_output, in_1code, d_code, d_samples_per_code);
for(int j = 0; j < d_samples_per_code; j++) for (int j = 0; j < d_samples_per_code; j++)
{ {
complex_acumulator[i] += (corr_output[j]); complex_acumulator[i] += (corr_output[j]);
} }
}
/*Obtain maximun value of correlation given the possible delay selected */
volk_32fc_magnitude_squared_32f(d_corr_output_f, complex_acumulator, d_folding_factor);
volk_gnsssdr_32f_index_max_32u(&indext, d_corr_output_f, d_folding_factor);
} /*Now save the real code phase in the gnss_syncro block for use in other stages*/
/*Obtain maximun value of correlation given the possible delay selected */ d_gnss_synchro->Acq_delay_samples = static_cast<double>(d_possible_delay[indext]);
volk_32fc_magnitude_squared_32f(d_corr_output_f, complex_acumulator, d_folding_factor); d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
volk_gnsssdr_32f_index_max_32u(&indext, d_corr_output_f, d_folding_factor); d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
/*Now save the real code phase in the gnss_syncro block for use in other stages*/ /* 5- Compute the test statistics and compare to the threshold d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;*/
d_gnss_synchro->Acq_delay_samples = static_cast<double>(d_possible_delay[indext]); d_test_statistics = d_mag / d_input_power;
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler); //delete complex_acumulator;
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; }
}
/* 5- Compute the test statistics and compare to the threshold d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;*/ // Record results to file if required
d_test_statistics = d_mag / d_input_power; if (d_dump)
//delete complex_acumulator; {
} /*Since QuickSYnc performs a folded correlation in frequency by means
}
// Record results to file if required
if (d_dump)
{
/*Since QuickSYnc performs a folded correlation in frequency by means
of the FFT, it is esential to also keep the values obtained from the of the FFT, it is esential to also keep the values obtained from the
possible delay to show how it is maximize*/ possible delay to show how it is maximize*/
std::stringstream filename; std::stringstream filename;
std::streamsize n = sizeof(float) * (d_fft_size); // complex file write std::streamsize n = sizeof(float) * (d_fft_size); // complex file write
filename.str(""); filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System filename << "../data/test_statistics_" << d_gnss_synchro->System
<< "_" << d_gnss_synchro->Signal << "_sat_" << "_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat"; << d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char*>(d_magnitude_folded), n); //write directly |abs(x)|^2 in this Doppler bin? d_dump_file.write(reinterpret_cast<char*>(d_magnitude_folded), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close(); d_dump_file.close();
} }
} }
if (!d_bit_transition_flag) if (!d_bit_transition_flag)
{ {
if (d_test_statistics > d_threshold) if (d_test_statistics > d_threshold)
{ {
d_state = 2; // Positive acquisition d_state = 2; // Positive acquisition
} }
else if (d_well_count == d_max_dwells) else if (d_well_count == d_max_dwells)
{ {
d_state = 3; // Negative acquisition d_state = 3; // Negative acquisition
} }
} }
else else
{ {
if (d_well_count == d_max_dwells) // d_max_dwells = 2 if (d_well_count == d_max_dwells) // d_max_dwells = 2
{ {
if (d_test_statistics > d_threshold) if (d_test_statistics > d_threshold)
{ {
d_state = 2; // Positive acquisition d_state = 2; // Positive acquisition
} }
else else
{ {
d_state = 3; // Negative acquisition d_state = 3; // Negative acquisition
} }
} }
} }
volk_gnsssdr_free(in_temp); volk_gnsssdr_free(in_temp);
volk_gnsssdr_free(in_temp_folded); volk_gnsssdr_free(in_temp_folded);
volk_gnsssdr_free(in_1code); volk_gnsssdr_free(in_1code);
volk_gnsssdr_free(corr_output); volk_gnsssdr_free(corr_output);
consume_each(1); consume_each(1);
break; break;
}
case 2:
{
//DLOG(INFO) << "START CASE 2";
// 6.1- Declare positive acquisition using a message port
DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "folding factor " << d_folding_factor;
DLOG(INFO) << "possible delay correlation output";
for (int i = 0; i < static_cast<int>(d_folding_factor); i++) DLOG(INFO) << d_possible_delay[i] << "\t\t\t" << d_corr_output_f[i];
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude folded " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_sampled_ms * d_samples_per_ms * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 1;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
//DLOG(INFO) << "END CASE 2";
break;
}
case 3:
{
//DLOG(INFO) << "START CASE 3";
// 6.2- Declare negative acquisition using a message port
DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "folding factor " << d_folding_factor;
DLOG(INFO) << "possible delay corr output";
for (int i = 0; i < static_cast<int>(d_folding_factor); i++) DLOG(INFO) << d_possible_delay[i] << "\t\t\t" << d_corr_output_f[i];
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude folded " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_sampled_ms * d_samples_per_ms * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 2;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
//DLOG(INFO) << "END CASE 3";
break;
}
} }
case 2:
{
//DLOG(INFO) << "START CASE 2";
// 6.1- Declare positive acquisition using a message port
DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "folding factor " << d_folding_factor;
DLOG(INFO) << "possible delay correlation output";
for (int i = 0; i < static_cast<int>(d_folding_factor); i++) DLOG(INFO) << d_possible_delay[i] << "\t\t\t" << d_corr_output_f[i];
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude folded " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_sampled_ms * d_samples_per_ms * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 1;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
//DLOG(INFO) << "END CASE 2";
break;
}
case 3:
{
//DLOG(INFO) << "START CASE 3";
// 6.2- Declare negative acquisition using a message port
DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "folding factor "<<d_folding_factor;
DLOG(INFO) << "possible delay corr output";
for (int i = 0; i < static_cast<int>(d_folding_factor); i++) DLOG(INFO) << d_possible_delay[i] << "\t\t\t" << d_corr_output_f[i];
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude folded " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_sampled_ms * d_samples_per_ms * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 2;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
//DLOG(INFO) << "END CASE 3";
break;
}
}
return noutput_items; return noutput_items;
} }

54
src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h
View File

@ -64,16 +64,16 @@
class pcps_quicksync_acquisition_cc; class pcps_quicksync_acquisition_cc;
typedef boost::shared_ptr<pcps_quicksync_acquisition_cc> typedef boost::shared_ptr<pcps_quicksync_acquisition_cc>
pcps_quicksync_acquisition_cc_sptr; pcps_quicksync_acquisition_cc_sptr;
pcps_quicksync_acquisition_cc_sptr pcps_quicksync_acquisition_cc_sptr
pcps_quicksync_make_acquisition_cc(unsigned int folding_factor, pcps_quicksync_make_acquisition_cc(unsigned int folding_factor,
unsigned int sampled_ms, unsigned int max_dwells, unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
bool dump, bool dump,
std::string dump_filename); std::string dump_filename);
/*! /*!
* \brief This class implements a Parallel Code Phase Search Acquisition with * \brief This class implements a Parallel Code Phase Search Acquisition with
@ -82,31 +82,31 @@ pcps_quicksync_make_acquisition_cc(unsigned int folding_factor,
* Check \ref Navitec2012 "Faster GPS via the Sparse Fourier Transform", * Check \ref Navitec2012 "Faster GPS via the Sparse Fourier Transform",
* for details of its implementation and functionality. * for details of its implementation and functionality.
*/ */
class pcps_quicksync_acquisition_cc: public gr::block class pcps_quicksync_acquisition_cc : public gr::block
{ {
private: private:
friend pcps_quicksync_acquisition_cc_sptr friend pcps_quicksync_acquisition_cc_sptr
pcps_quicksync_make_acquisition_cc(unsigned int folding_factor, pcps_quicksync_make_acquisition_cc(unsigned int folding_factor,
unsigned int sampled_ms, unsigned int max_dwells, unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
bool dump, bool dump,
std::string dump_filename); std::string dump_filename);
pcps_quicksync_acquisition_cc(unsigned int folding_factor, pcps_quicksync_acquisition_cc(unsigned int folding_factor,
unsigned int sampled_ms, unsigned int max_dwells, unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
bool dump, bool dump,
std::string dump_filename); std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift, void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset); int doppler_offset);
gr_complex* d_code; gr_complex* d_code;
unsigned int d_folding_factor; // also referred in the paper as 'p' unsigned int d_folding_factor; // also referred in the paper as 'p'
float* d_corr_acumulator; float* d_corr_acumulator;
unsigned int* d_possible_delay; unsigned int* d_possible_delay;
float* d_corr_output_f; float* d_corr_output_f;
@ -135,7 +135,7 @@ private:
gr::fft::fft_complex* d_fft_if; gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_fft_if2; gr::fft::fft_complex* d_fft_if2;
gr::fft::fft_complex* d_ifft; gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro; Gnss_Synchro* d_gnss_synchro;
unsigned int d_code_phase; unsigned int d_code_phase;
float d_doppler_freq; float d_doppler_freq;
float d_mag; float d_mag;
@ -183,7 +183,7 @@ public:
* \brief Sets local code for PCPS acquisition algorithm. * \brief Sets local code for PCPS acquisition algorithm.
* \param code - Pointer to the PRN code. * \param code - Pointer to the PRN code.
*/ */
void set_local_code(std::complex<float> * code); void set_local_code(std::complex<float>* code);
/*! /*!
* \brief Starts acquisition algorithm, turning from standby mode to * \brief Starts acquisition algorithm, turning from standby mode to
@ -242,9 +242,9 @@ public:
/*! /*!
* \brief Parallel Code Phase Search Acquisition signal processing. * \brief Parallel Code Phase Search Acquisition signal processing.
*/ */
int general_work(int noutput_items, gr_vector_int &ninput_items, int general_work(int noutput_items, gr_vector_int& ninput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star& input_items,
gr_vector_void_star &output_items); gr_vector_void_star& output_items);
}; };
#endif /* GNSS_SDR_PCPS_ACQUISITION_CC_H_*/ #endif /* GNSS_SDR_PCPS_ACQUISITION_CC_H_*/

455
src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc
View File

@ -50,7 +50,7 @@
#include "pcps_tong_acquisition_cc.h" #include "pcps_tong_acquisition_cc.h"
#include "control_message_factory.h" #include "control_message_factory.h"
#include "GPS_L1_CA.h" // for GPS_TWO_PI #include "GPS_L1_CA.h" // for GPS_TWO_PI
#include <glog/logging.h> #include <glog/logging.h>
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <volk/volk.h> #include <volk/volk.h>
@ -60,29 +60,29 @@
using google::LogMessage; using google::LogMessage;
pcps_tong_acquisition_cc_sptr pcps_tong_make_acquisition_cc( pcps_tong_acquisition_cc_sptr pcps_tong_make_acquisition_cc(
unsigned int sampled_ms, unsigned int doppler_max, unsigned int sampled_ms, unsigned int doppler_max,
long freq, long fs_in, int samples_per_ms, long freq, long fs_in, int samples_per_ms,
int samples_per_code, unsigned int tong_init_val, int samples_per_code, unsigned int tong_init_val,
unsigned int tong_max_val, unsigned int tong_max_dwells, unsigned int tong_max_val, unsigned int tong_max_dwells,
bool dump, std::string dump_filename) bool dump, std::string dump_filename)
{ {
return pcps_tong_acquisition_cc_sptr( return pcps_tong_acquisition_cc_sptr(
new pcps_tong_acquisition_cc(sampled_ms, doppler_max, freq, fs_in, samples_per_ms, samples_per_code, new pcps_tong_acquisition_cc(sampled_ms, doppler_max, freq, fs_in, samples_per_ms, samples_per_code,
tong_init_val, tong_max_val, tong_max_dwells, dump, dump_filename)); tong_init_val, tong_max_val, tong_max_dwells, dump, dump_filename));
} }
pcps_tong_acquisition_cc::pcps_tong_acquisition_cc( pcps_tong_acquisition_cc::pcps_tong_acquisition_cc(
unsigned int sampled_ms, unsigned int doppler_max, unsigned int sampled_ms, unsigned int doppler_max,
long freq, long fs_in, int samples_per_ms, long freq, long fs_in, int samples_per_ms,
int samples_per_code, unsigned int tong_init_val, int samples_per_code, unsigned int tong_init_val,
unsigned int tong_max_val, unsigned int tong_max_dwells, unsigned int tong_max_val, unsigned int tong_max_dwells,
bool dump, std::string dump_filename) : bool dump,
gr::block("pcps_tong_acquisition_cc", std::string dump_filename) : gr::block("pcps_tong_acquisition_cc",
gr::io_signature::make(1, 1, sizeof(gr_complex) * sampled_ms * samples_per_ms), gr::io_signature::make(1, 1, sizeof(gr_complex) * sampled_ms * samples_per_ms),
gr::io_signature::make(0, 0, sizeof(gr_complex) * sampled_ms * samples_per_ms)) gr::io_signature::make(0, 0, sizeof(gr_complex) * sampled_ms * samples_per_ms))
{ {
this->message_port_register_out(pmt::mp("events")); this->message_port_register_out(pmt::mp("events"));
d_sample_counter = 0; // SAMPLE COUNTER d_sample_counter = 0; // SAMPLE COUNTER
d_active = false; d_active = false;
d_state = 0; d_state = 0;
d_freq = freq; d_freq = freq;
@ -101,8 +101,8 @@ pcps_tong_acquisition_cc::pcps_tong_acquisition_cc(
d_input_power = 0.0; d_input_power = 0.0;
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
d_fft_codes = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_fft_codes = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitude = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); d_magnitude = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@ -151,11 +151,11 @@ pcps_tong_acquisition_cc::~pcps_tong_acquisition_cc()
} }
} }
void pcps_tong_acquisition_cc::set_local_code(std::complex<float> * code) void pcps_tong_acquisition_cc::set_local_code(std::complex<float> *code)
{ {
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size); volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
@ -179,24 +179,24 @@ void pcps_tong_acquisition_cc::init()
for (int doppler = static_cast<int>(-d_doppler_max); for (int doppler = static_cast<int>(-d_doppler_max);
doppler <= static_cast<int>(d_doppler_max); doppler <= static_cast<int>(d_doppler_max);
doppler += d_doppler_step) doppler += d_doppler_step)
{ {
d_num_doppler_bins++; d_num_doppler_bins++;
} }
// Create the carrier Doppler wipeoff signals and allocate data grid. // Create the carrier Doppler wipeoff signals and allocate data grid.
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_bins];
d_grid_data = new float*[d_num_doppler_bins]; d_grid_data = new float *[d_num_doppler_bins];
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index; int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = GPS_TWO_PI * (d_freq + doppler) / static_cast<float>(d_fs_in); float phase_step_rad = GPS_TWO_PI * (d_freq + doppler) / static_cast<float>(d_fs_in);
float _phase[1]; float _phase[1];
_phase[0] = 0; _phase[0] = 0;
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], - phase_step_rad, _phase, d_fft_size); volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase, d_fft_size);
d_grid_data[doppler_index] = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); d_grid_data[doppler_index] = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
for (unsigned int i = 0; i < d_fft_size; i++) for (unsigned int i = 0; i < d_fft_size; i++)
{ {
@ -228,7 +228,8 @@ void pcps_tong_acquisition_cc::set_state(int state)
} }
} }
else if (d_state == 0) else if (d_state == 0)
{} {
}
else else
{ {
LOG(ERROR) << "State can only be set to 0 or 1"; LOG(ERROR) << "State can only be set to 0 or 1";
@ -236,211 +237,211 @@ void pcps_tong_acquisition_cc::set_state(int state)
} }
int pcps_tong_acquisition_cc::general_work(int noutput_items, int pcps_tong_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused))) gr_vector_void_star &output_items __attribute__((unused)))
{ {
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
switch (d_state) switch (d_state)
{
case 0:
{ {
if (d_active) case 0:
{ {
//restart acquisition variables if (d_active)
d_gnss_synchro->Acq_delay_samples = 0.0; {
d_gnss_synchro->Acq_doppler_hz = 0.0; //restart acquisition variables
d_gnss_synchro->Acq_samplestamp_samples = 0; d_gnss_synchro->Acq_delay_samples = 0.0;
d_dwell_count = 0; d_gnss_synchro->Acq_doppler_hz = 0.0;
d_tong_count = d_tong_init_val; d_gnss_synchro->Acq_samplestamp_samples = 0;
d_mag = 0.0; d_dwell_count = 0;
d_input_power = 0.0; d_tong_count = d_tong_init_val;
d_test_statistics = 0.0; d_mag = 0.0;
d_input_power = 0.0;
d_test_statistics = 0.0;
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
for (unsigned int i = 0; i < d_fft_size; i++) for (unsigned int i = 0; i < d_fft_size; i++)
{ {
d_grid_data[doppler_index][i] = 0; d_grid_data[doppler_index][i] = 0;
} }
} }
d_state = 1; d_state = 1;
} }
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]); consume_each(ninput_items[0]);
break; break;
}
case 1:
{
// initialize acquisition algorithm
int doppler;
uint32_t indext = 0;
float magt = 0.0;
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_input_power = 0.0;
d_mag = 0.0;
d_sample_counter += d_fft_size; // sample counter
d_dwell_count++;
DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN
<< " ,sample stamp: " << d_sample_counter << ", threshold: "
<< d_threshold << ", doppler_max: " << d_doppler_max
<< ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
// doppler search steps
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal
d_fft_if->execute();
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// Compute magnitude
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
// Compute vector of test statistics corresponding to current doppler index.
volk_32f_s32f_multiply_32f(d_magnitude, d_magnitude,
1 / (fft_normalization_factor * fft_normalization_factor * d_input_power),
d_fft_size);
// Accumulate test statistics in d_grid_data.
volk_32f_x2_add_32f(d_grid_data[doppler_index], d_magnitude, d_grid_data[doppler_index], d_fft_size);
// Search maximum
volk_gnsssdr_32f_index_max_32u(&indext, d_grid_data[doppler_index], d_fft_size);
magt = d_grid_data[doppler_index][indext];
// 4- record the maximum peak and the associated synchronization parameters
if (d_mag < magt)
{
d_mag = magt;
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
}
// Record results to file if required
if (d_dump)
{
std::stringstream filename;
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<< "_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char *>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
}
}
// 5- Compute the test statistics and compare to the threshold
d_test_statistics = d_mag;
if (d_test_statistics > d_threshold * d_dwell_count)
{
d_tong_count++;
if (d_tong_count == d_tong_max_val)
{
d_state = 2; // Positive acquisition
}
}
else
{
d_tong_count--;
if (d_tong_count == 0)
{
d_state = 3; // Negative acquisition
}
}
if (d_dwell_count >= d_tong_max_dwells)
{
d_state = 3; // Negative acquisition
}
consume_each(1);
break;
}
case 2:
{
// 6.1- Declare positive acquisition using a message port
DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 1;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
case 3:
{
// 6.2- Declare negative acquisition using a message port
DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 2;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
} }
case 1:
{
// initialize acquisition algorithm
int doppler;
uint32_t indext = 0;
float magt = 0.0;
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_input_power = 0.0;
d_mag = 0.0;
d_sample_counter += d_fft_size; // sample counter
d_dwell_count++;
DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " "<< d_gnss_synchro->PRN
<< " ,sample stamp: " << d_sample_counter << ", threshold: "
<< d_threshold << ", doppler_max: " << d_doppler_max
<< ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
// doppler search steps
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal
d_fft_if->execute();
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
// compute the inverse FFT
d_ifft->execute();
// Compute magnitude
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
// Compute vector of test statistics corresponding to current doppler index.
volk_32f_s32f_multiply_32f(d_magnitude, d_magnitude,
1/(fft_normalization_factor*fft_normalization_factor*d_input_power),
d_fft_size);
// Accumulate test statistics in d_grid_data.
volk_32f_x2_add_32f(d_grid_data[doppler_index], d_magnitude, d_grid_data[doppler_index], d_fft_size);
// Search maximum
volk_gnsssdr_32f_index_max_32u(&indext, d_grid_data[doppler_index], d_fft_size);
magt = d_grid_data[doppler_index][indext];
// 4- record the maximum peak and the associated synchronization parameters
if (d_mag < magt)
{
d_mag = magt;
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
}
// Record results to file if required
if (d_dump)
{
std::stringstream filename;
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<<"_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char*>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
}
}
// 5- Compute the test statistics and compare to the threshold
d_test_statistics = d_mag;
if (d_test_statistics > d_threshold * d_dwell_count)
{
d_tong_count++;
if (d_tong_count == d_tong_max_val)
{
d_state = 2; // Positive acquisition
}
}
else
{
d_tong_count--;
if (d_tong_count == 0)
{
d_state = 3; // Negative acquisition
}
}
if(d_dwell_count >= d_tong_max_dwells)
{
d_state = 3; // Negative acquisition
}
consume_each(1);
break;
}
case 2:
{
// 6.1- Declare positive acquisition using a message port
DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 1;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
case 3:
{
// 6.2- Declare negative acquisition using a message port
DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 2;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
break;
}
}
return noutput_items; return noutput_items;
} }

122
src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h
View File

@ -65,33 +65,33 @@ typedef boost::shared_ptr<pcps_tong_acquisition_cc> pcps_tong_acquisition_cc_spt
pcps_tong_acquisition_cc_sptr pcps_tong_acquisition_cc_sptr
pcps_tong_make_acquisition_cc(unsigned int sampled_ms, unsigned int doppler_max, pcps_tong_make_acquisition_cc(unsigned int sampled_ms, unsigned int doppler_max,
long freq, long fs_in, int samples_per_ms, long freq, long fs_in, int samples_per_ms,
int samples_per_code, unsigned int tong_init_val, int samples_per_code, unsigned int tong_init_val,
unsigned int tong_max_val, unsigned int tong_max_dwells, unsigned int tong_max_val, unsigned int tong_max_dwells,
bool dump, std::string dump_filename); bool dump, std::string dump_filename);
/*! /*!
* \brief This class implements a Parallel Code Phase Search Acquisition with * \brief This class implements a Parallel Code Phase Search Acquisition with
* Tong algorithm. * Tong algorithm.
*/ */
class pcps_tong_acquisition_cc: public gr::block class pcps_tong_acquisition_cc : public gr::block
{ {
private: private:
friend pcps_tong_acquisition_cc_sptr friend pcps_tong_acquisition_cc_sptr
pcps_tong_make_acquisition_cc(unsigned int sampled_ms, unsigned int doppler_max, pcps_tong_make_acquisition_cc(unsigned int sampled_ms, unsigned int doppler_max,
long freq, long fs_in, int samples_per_ms, long freq, long fs_in, int samples_per_ms,
int samples_per_code, unsigned int tong_init_val, int samples_per_code, unsigned int tong_init_val,
unsigned int tong_max_val, unsigned int tong_max_dwells, unsigned int tong_max_val, unsigned int tong_max_dwells,
bool dump, std::string dump_filename); bool dump, std::string dump_filename);
pcps_tong_acquisition_cc(unsigned int sampled_ms, unsigned int doppler_max, pcps_tong_acquisition_cc(unsigned int sampled_ms, unsigned int doppler_max,
long freq, long fs_in, int samples_per_ms, long freq, long fs_in, int samples_per_ms,
int samples_per_code, unsigned int tong_init_val, int samples_per_code, unsigned int tong_init_val,
unsigned int tong_max_val, unsigned int tong_max_dwells, unsigned int tong_max_val, unsigned int tong_max_dwells,
bool dump, std::string dump_filename); bool dump, std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift, void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset); int doppler_offset);
long d_fs_in; long d_fs_in;
long d_freq; long d_freq;
@ -116,7 +116,7 @@ private:
float** d_grid_data; float** d_grid_data;
gr::fft::fft_complex* d_fft_if; gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft; gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro; Gnss_Synchro* d_gnss_synchro;
unsigned int d_code_phase; unsigned int d_code_phase;
float d_doppler_freq; float d_doppler_freq;
float d_mag; float d_mag;
@ -134,97 +134,97 @@ public:
/*! /*!
* \brief Default destructor. * \brief Default destructor.
*/ */
~pcps_tong_acquisition_cc(); ~pcps_tong_acquisition_cc();
/*! /*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer * \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks. * to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks. * \param p_gnss_synchro Satellite information shared by the processing blocks.
*/ */
inline void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) inline void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{ {
d_gnss_synchro = p_gnss_synchro; d_gnss_synchro = p_gnss_synchro;
} }
/*! /*!
* \brief Returns the maximum peak of grid search. * \brief Returns the maximum peak of grid search.
*/ */
inline unsigned int mag() const inline unsigned int mag() const
{ {
return d_mag; return d_mag;
} }
/*! /*!
* \brief Initializes acquisition algorithm. * \brief Initializes acquisition algorithm.
*/ */
void init(); void init();
/*! /*!
* \brief Sets local code for TONG acquisition algorithm. * \brief Sets local code for TONG acquisition algorithm.
* \param code - Pointer to the PRN code. * \param code - Pointer to the PRN code.
*/ */
void set_local_code(std::complex<float> * code); void set_local_code(std::complex<float>* code);
/*! /*!
* \brief Starts acquisition algorithm, turning from standby mode to * \brief Starts acquisition algorithm, turning from standby mode to
* active mode * active mode
* \param active - bool that activates/deactivates the block. * \param active - bool that activates/deactivates the block.
*/ */
inline void set_active(bool active) inline void set_active(bool active)
{ {
d_active = active; d_active = active;
} }
/*! /*!
* \brief If set to 1, ensures that acquisition starts at the * \brief If set to 1, ensures that acquisition starts at the
* first available sample. * first available sample.
* \param state - int=1 forces start of acquisition * \param state - int=1 forces start of acquisition
*/ */
void set_state(int state); void set_state(int state);
/*! /*!
* \brief Set acquisition channel unique ID * \brief Set acquisition channel unique ID
* \param channel - receiver channel. * \param channel - receiver channel.
*/ */
inline void set_channel(unsigned int channel) inline void set_channel(unsigned int channel)
{ {
d_channel = channel; d_channel = channel;
} }
/*! /*!
* \brief Set statistics threshold of TONG algorithm. * \brief Set statistics threshold of TONG algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012, * \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold). * Algorithm 1, for a definition of this threshold).
*/ */
inline void set_threshold(float threshold) inline void set_threshold(float threshold)
{ {
d_threshold = threshold; d_threshold = threshold;
} }
/*! /*!
* \brief Set maximum Doppler grid search * \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz]. * \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/ */
inline void set_doppler_max(unsigned int doppler_max) inline void set_doppler_max(unsigned int doppler_max)
{ {
d_doppler_max = doppler_max; d_doppler_max = doppler_max;
} }
/*! /*!
* \brief Set Doppler steps for the grid search * \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz]. * \param doppler_step - Frequency bin of the search grid [Hz].
*/ */
inline void set_doppler_step(unsigned int doppler_step) inline void set_doppler_step(unsigned int doppler_step)
{ {
d_doppler_step = doppler_step; d_doppler_step = doppler_step;
} }
/*! /*!
* \brief Parallel Code Phase Search Acquisition signal processing. * \brief Parallel Code Phase Search Acquisition signal processing.
*/ */
int general_work(int noutput_items, gr_vector_int &ninput_items, int general_work(int noutput_items, gr_vector_int& ninput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star& input_items,
gr_vector_void_star &output_items); gr_vector_void_star& output_items);
}; };
#endif /* GNSS_SDR_PCPS_TONG_ACQUISITION_CC_H_ */ #endif /* GNSS_SDR_PCPS_TONG_ACQUISITION_CC_H_ */

31
src/algorithms/channel/adapters/channel.cc
View File

@ -39,10 +39,10 @@
using google::LogMessage; using google::LogMessage;
// Constructor // Constructor
Channel::Channel(ConfigurationInterface *configuration, unsigned int channel, Channel::Channel(ConfigurationInterface* configuration, unsigned int channel,
std::shared_ptr<GNSSBlockInterface> pass_through, std::shared_ptr<AcquisitionInterface> acq, std::shared_ptr<GNSSBlockInterface> pass_through, std::shared_ptr<AcquisitionInterface> acq,
std::shared_ptr<TrackingInterface> trk, std::shared_ptr<TelemetryDecoderInterface> nav, std::shared_ptr<TrackingInterface> trk, std::shared_ptr<TelemetryDecoderInterface> nav,
std::string role, std::string implementation, gr::msg_queue::sptr queue) std::string role, std::string implementation, gr::msg_queue::sptr queue)
{ {
pass_through_ = pass_through; pass_through_ = pass_through;
acq_ = acq; acq_ = acq;
@ -64,10 +64,10 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel,
trk_->set_gnss_synchro(&gnss_synchro_); trk_->set_gnss_synchro(&gnss_synchro_);
// Provide a warning to the user about the change of parameter name // Provide a warning to the user about the change of parameter name
if(channel_ == 0) if (channel_ == 0)
{ {
long int deprecation_warning = configuration->property("GNSS-SDR.internal_fs_hz", 0); long int deprecation_warning = configuration->property("GNSS-SDR.internal_fs_hz", 0);
if(deprecation_warning != 0) if (deprecation_warning != 0)
{ {
std::cout << "WARNING: The global parameter name GNSS-SDR.internal_fs_hz has been DEPRECATED." << std::endl; std::cout << "WARNING: The global parameter name GNSS-SDR.internal_fs_hz has been DEPRECATED." << std::endl;
std::cout << "WARNING: Please replace it by GNSS-SDR.internal_fs_sps in your configuration file." << std::endl; std::cout << "WARNING: Please replace it by GNSS-SDR.internal_fs_sps in your configuration file." << std::endl;
@ -77,14 +77,14 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel,
// IMPORTANT: Do not change the order between set_doppler_step and set_threshold // IMPORTANT: Do not change the order between set_doppler_step and set_threshold
unsigned int doppler_step = configuration->property("Acquisition_" + implementation_ + boost::lexical_cast<std::string>(channel_) + ".doppler_step", 0); unsigned int doppler_step = configuration->property("Acquisition_" + implementation_ + boost::lexical_cast<std::string>(channel_) + ".doppler_step", 0);
if(doppler_step == 0) doppler_step = configuration->property("Acquisition_" + implementation_ + ".doppler_step", 500); if (doppler_step == 0) doppler_step = configuration->property("Acquisition_" + implementation_ + ".doppler_step", 500);
if(FLAGS_doppler_step != 0) doppler_step = static_cast<unsigned int>(FLAGS_doppler_step); if (FLAGS_doppler_step != 0) doppler_step = static_cast<unsigned int>(FLAGS_doppler_step);
DLOG(INFO) << "Channel "<< channel_ << " Doppler_step = " << doppler_step; DLOG(INFO) << "Channel " << channel_ << " Doppler_step = " << doppler_step;
acq_->set_doppler_step(doppler_step); acq_->set_doppler_step(doppler_step);
float threshold = configuration->property("Acquisition_" + implementation_ + boost::lexical_cast<std::string>(channel_) + ".threshold", 0.0); float threshold = configuration->property("Acquisition_" + implementation_ + boost::lexical_cast<std::string>(channel_) + ".threshold", 0.0);
if(threshold == 0.0) threshold = configuration->property("Acquisition_" + implementation_ + ".threshold", 0.0); if (threshold == 0.0) threshold = configuration->property("Acquisition_" + implementation_ + ".threshold", 0.0);
acq_->set_threshold(threshold); acq_->set_threshold(threshold);
@ -107,7 +107,7 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel,
// Destructor // Destructor
Channel::~Channel(){} Channel::~Channel() {}
void Channel::connect(gr::top_block_sptr top_block) void Channel::connect(gr::top_block_sptr top_block)
@ -190,9 +190,9 @@ void Channel::set_signal(const Gnss_Signal& gnss_signal)
std::lock_guard<std::mutex> lk(mx); std::lock_guard<std::mutex> lk(mx);
gnss_signal_ = gnss_signal; gnss_signal_ = gnss_signal;
std::string str_aux = gnss_signal_.get_signal_str(); std::string str_aux = gnss_signal_.get_signal_str();
const char * str = str_aux.c_str(); // get a C style null terminated string const char* str = str_aux.c_str(); // get a C style null terminated string
std::memcpy(static_cast<void*>(gnss_synchro_.Signal), str, 3); // copy string into synchro char array: 2 char + null std::memcpy(static_cast<void*>(gnss_synchro_.Signal), str, 3); // copy string into synchro char array: 2 char + null
gnss_synchro_.Signal[2] = 0; // make sure that string length is only two characters gnss_synchro_.Signal[2] = 0; // make sure that string length is only two characters
gnss_synchro_.PRN = gnss_signal_.get_satellite().get_PRN(); gnss_synchro_.PRN = gnss_signal_.get_satellite().get_PRN();
gnss_synchro_.System = gnss_signal_.get_satellite().get_system_short().c_str()[0]; gnss_synchro_.System = gnss_signal_.get_satellite().get_system_short().c_str()[0];
acq_->set_local_code(); acq_->set_local_code();
@ -205,11 +205,10 @@ void Channel::start_acquisition()
std::lock_guard<std::mutex> lk(mx); std::lock_guard<std::mutex> lk(mx);
bool result = false; bool result = false;
result = channel_fsm_->Event_start_acquisition(); result = channel_fsm_->Event_start_acquisition();
if(!result) if (!result)
{ {
LOG(WARNING) << "Invalid channel event"; LOG(WARNING) << "Invalid channel event";
return; return;
} }
DLOG(INFO) << "Channel start_acquisition()"; DLOG(INFO) << "Channel start_acquisition()";
} }

19
src/algorithms/channel/adapters/channel.h
View File

@ -56,15 +56,14 @@ class TelemetryDecoderInterface;
* their interaction through a Finite State Machine * their interaction through a Finite State Machine
* *
*/ */
class Channel: public ChannelInterface class Channel : public ChannelInterface
{ {
public: public:
//! Constructor //! Constructor
Channel(ConfigurationInterface *configuration, unsigned int channel, Channel(ConfigurationInterface* configuration, unsigned int channel,
std::shared_ptr<GNSSBlockInterface> pass_through, std::shared_ptr<AcquisitionInterface> acq, std::shared_ptr<GNSSBlockInterface> pass_through, std::shared_ptr<AcquisitionInterface> acq,
std::shared_ptr<TrackingInterface> trk, std::shared_ptr<TelemetryDecoderInterface> nav, std::shared_ptr<TrackingInterface> trk, std::shared_ptr<TelemetryDecoderInterface> nav,
std::string role, std::string implementation, gr::msg_queue::sptr queue); std::string role, std::string implementation, gr::msg_queue::sptr queue);
//! Virtual destructor //! Virtual destructor
virtual ~Channel(); virtual ~Channel();
@ -82,12 +81,12 @@ public:
inline Gnss_Signal get_signal() const override { return gnss_signal_; } inline Gnss_Signal get_signal() const override { return gnss_signal_; }
void start_acquisition() override; //!< Start the State Machine void start_acquisition() override; //!< Start the State Machine
void set_signal(const Gnss_Signal& gnss_signal_) override; //!< Sets the channel GNSS signal void set_signal(const Gnss_Signal& gnss_signal_) override; //!< Sets the channel GNSS signal
inline std::shared_ptr<AcquisitionInterface> acquisition(){ return acq_; } inline std::shared_ptr<AcquisitionInterface> acquisition() { return acq_; }
inline std::shared_ptr<TrackingInterface> tracking(){ return trk_; } inline std::shared_ptr<TrackingInterface> tracking() { return trk_; }
inline std::shared_ptr<TelemetryDecoderInterface> telemetry(){ return nav_; } inline std::shared_ptr<TelemetryDecoderInterface> telemetry() { return nav_; }
void msg_handler_events(pmt::pmt_t msg); void msg_handler_events(pmt::pmt_t msg);

15
src/algorithms/channel/libs/channel_fsm.cc
View File

@ -34,7 +34,6 @@
#include <glog/logging.h> #include <glog/logging.h>
ChannelFsm::ChannelFsm() ChannelFsm::ChannelFsm()
{ {
acq_ = nullptr; acq_ = nullptr;
@ -44,9 +43,7 @@ ChannelFsm::ChannelFsm()
} }
ChannelFsm::ChannelFsm(std::shared_ptr<AcquisitionInterface> acquisition) : acq_(acquisition)
ChannelFsm::ChannelFsm(std::shared_ptr<AcquisitionInterface> acquisition) :
acq_(acquisition)
{ {
trk_ = nullptr; trk_ = nullptr;
channel_ = 0; channel_ = 0;
@ -57,7 +54,7 @@ ChannelFsm::ChannelFsm(std::shared_ptr<AcquisitionInterface> acquisition) :
bool ChannelFsm::Event_start_acquisition() bool ChannelFsm::Event_start_acquisition()
{ {
std::lock_guard<std::mutex> lk(mx); std::lock_guard<std::mutex> lk(mx);
if((d_state == 1) || (d_state == 2)) if ((d_state == 1) || (d_state == 2))
{ {
return false; return false;
} }
@ -74,7 +71,7 @@ bool ChannelFsm::Event_start_acquisition()
bool ChannelFsm::Event_valid_acquisition() bool ChannelFsm::Event_valid_acquisition()
{ {
std::lock_guard<std::mutex> lk(mx); std::lock_guard<std::mutex> lk(mx);
if(d_state != 1) if (d_state != 1)
{ {
return false; return false;
} }
@ -91,7 +88,7 @@ bool ChannelFsm::Event_valid_acquisition()
bool ChannelFsm::Event_failed_acquisition_repeat() bool ChannelFsm::Event_failed_acquisition_repeat()
{ {
std::lock_guard<std::mutex> lk(mx); std::lock_guard<std::mutex> lk(mx);
if(d_state != 1) if (d_state != 1)
{ {
return false; return false;
} }
@ -108,7 +105,7 @@ bool ChannelFsm::Event_failed_acquisition_repeat()
bool ChannelFsm::Event_failed_acquisition_no_repeat() bool ChannelFsm::Event_failed_acquisition_no_repeat()
{ {
std::lock_guard<std::mutex> lk(mx); std::lock_guard<std::mutex> lk(mx);
if(d_state != 1) if (d_state != 1)
{ {
return false; return false;
} }
@ -125,7 +122,7 @@ bool ChannelFsm::Event_failed_acquisition_no_repeat()
bool ChannelFsm::Event_failed_tracking_standby() bool ChannelFsm::Event_failed_tracking_standby()
{ {
std::lock_guard<std::mutex> lk(mx); std::lock_guard<std::mutex> lk(mx);
if(d_state != 2) if (d_state != 2)
{ {
return false; return false;
} }

1
src/algorithms/channel/libs/channel_fsm.h
View File

@ -62,7 +62,6 @@ public:
bool Event_failed_tracking_standby(); bool Event_failed_tracking_standby();
private: private:
void start_acquisition(); void start_acquisition();
void start_tracking(); void start_tracking();
void request_satellite(); void request_satellite();

62
src/algorithms/channel/libs/channel_msg_receiver_cc.cc
View File

@ -37,7 +37,7 @@
using google::LogMessage; using google::LogMessage;
channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr<ChannelFsm> channel_fsm, bool repeat) channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr<ChannelFsm> channel_fsm, bool repeat)
{ {
return channel_msg_receiver_cc_sptr(new channel_msg_receiver_cc(channel_fsm, repeat)); return channel_msg_receiver_cc_sptr(new channel_msg_receiver_cc(channel_fsm, repeat));
} }
@ -46,44 +46,43 @@ void channel_msg_receiver_cc::msg_handler_events(pmt::pmt_t msg)
{ {
bool result = false; bool result = false;
try try
{ {
long int message = pmt::to_long(msg); long int message = pmt::to_long(msg);
switch (message) switch (message)
{ {
case 1: //positive acquisition case 1: //positive acquisition
result = d_channel_fsm->Event_valid_acquisition(); result = d_channel_fsm->Event_valid_acquisition();
break; break;
case 2: //negative acquisition case 2: //negative acquisition
if (d_repeat == true) if (d_repeat == true)
{ {
result = d_channel_fsm->Event_failed_acquisition_repeat(); result = d_channel_fsm->Event_failed_acquisition_repeat();
} }
else else
{ {
result = d_channel_fsm->Event_failed_acquisition_no_repeat(); result = d_channel_fsm->Event_failed_acquisition_no_repeat();
} }
break; break;
case 3: // tracking loss of lock event case 3: // tracking loss of lock event
result = d_channel_fsm->Event_failed_tracking_standby(); result = d_channel_fsm->Event_failed_tracking_standby();
break; break;
default: default:
LOG(WARNING) << "Default case, invalid message."; LOG(WARNING) << "Default case, invalid message.";
break; break;
} }
} }
catch(boost::bad_any_cast& e) catch (boost::bad_any_cast& e)
{ {
LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; LOG(WARNING) << "msg_handler_telemetry Bad any cast!";
} }
if(!result) if (!result)
{ {
LOG(WARNING) << "msg_handler_telemetry invalid event"; LOG(WARNING) << "msg_handler_telemetry invalid event";
} }
} }
channel_msg_receiver_cc::channel_msg_receiver_cc(std::shared_ptr<ChannelFsm> channel_fsm, bool repeat) : channel_msg_receiver_cc::channel_msg_receiver_cc(std::shared_ptr<ChannelFsm> channel_fsm, bool repeat) : gr::block("channel_msg_receiver_cc", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0))
gr::block("channel_msg_receiver_cc", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0))
{ {
this->message_port_register_in(pmt::mp("events")); this->message_port_register_in(pmt::mp("events"));
this->set_msg_handler(pmt::mp("events"), boost::bind(&channel_msg_receiver_cc::msg_handler_events, this, _1)); this->set_msg_handler(pmt::mp("events"), boost::bind(&channel_msg_receiver_cc::msg_handler_events, this, _1));
@ -93,5 +92,4 @@ channel_msg_receiver_cc::channel_msg_receiver_cc(std::shared_ptr<ChannelFsm> cha
} }
channel_msg_receiver_cc::~channel_msg_receiver_cc() channel_msg_receiver_cc::~channel_msg_receiver_cc() {}
{}

5
src/algorithms/channel/libs/channel_msg_receiver_cc.h
View File

@ -47,14 +47,13 @@ class channel_msg_receiver_cc : public gr::block
{ {
private: private:
std::shared_ptr<ChannelFsm> d_channel_fsm; std::shared_ptr<ChannelFsm> d_channel_fsm;
bool d_repeat; // todo: change FSM to include repeat value bool d_repeat; // todo: change FSM to include repeat value
friend channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr<ChannelFsm> channel_fsm, bool repeat); friend channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr<ChannelFsm> channel_fsm, bool repeat);
void msg_handler_events(pmt::pmt_t msg); void msg_handler_events(pmt::pmt_t msg);
channel_msg_receiver_cc(std::shared_ptr<ChannelFsm> channel_fsm, bool repeat); channel_msg_receiver_cc(std::shared_ptr<ChannelFsm> channel_fsm, bool repeat);
public: public:
~channel_msg_receiver_cc (); //!< Default destructor ~channel_msg_receiver_cc(); //!< Default destructor
}; };
#endif #endif

24
src/algorithms/conditioner/adapters/array_signal_conditioner.cc
View File

@ -37,19 +37,22 @@ using google::LogMessage;
// Constructor // Constructor
ArraySignalConditioner::ArraySignalConditioner(ConfigurationInterface *configuration, ArraySignalConditioner::ArraySignalConditioner(ConfigurationInterface *configuration,
std::shared_ptr<GNSSBlockInterface> data_type_adapt, std::shared_ptr<GNSSBlockInterface> in_filt, std::shared_ptr<GNSSBlockInterface> data_type_adapt, std::shared_ptr<GNSSBlockInterface> in_filt,
std::shared_ptr<GNSSBlockInterface> res, std::string role, std::string implementation) : std::shared_ptr<GNSSBlockInterface> res, std::string role, std::string implementation) : data_type_adapt_(data_type_adapt),
data_type_adapt_(data_type_adapt), in_filt_(in_filt),
in_filt_(in_filt), res_(res), role_(role), implementation_(implementation) res_(res),
role_(role),
implementation_(implementation)
{ {
connected_ = false; connected_ = false;
if(configuration){ }; if (configuration)
{
};
} }
// Destructor // Destructor
ArraySignalConditioner::~ArraySignalConditioner() ArraySignalConditioner::~ArraySignalConditioner() {}
{}
void ArraySignalConditioner::connect(gr::top_block_sptr top_block) void ArraySignalConditioner::connect(gr::top_block_sptr top_block)
@ -68,7 +71,7 @@ void ArraySignalConditioner::connect(gr::top_block_sptr top_block)
//DLOG(INFO) << "data_type_adapter -> input_filter"; //DLOG(INFO) << "data_type_adapter -> input_filter";
top_block->connect(in_filt_->get_right_block(), 0, top_block->connect(in_filt_->get_right_block(), 0,
res_->get_left_block(), 0); res_->get_left_block(), 0);
DLOG(INFO) << "Array input_filter -> resampler"; DLOG(INFO) << "Array input_filter -> resampler";
@ -76,7 +79,6 @@ void ArraySignalConditioner::connect(gr::top_block_sptr top_block)
} }
void ArraySignalConditioner::disconnect(gr::top_block_sptr top_block) void ArraySignalConditioner::disconnect(gr::top_block_sptr top_block)
{ {
if (!connected_) if (!connected_)
@ -88,7 +90,7 @@ void ArraySignalConditioner::disconnect(gr::top_block_sptr top_block)
//top_block->disconnect(data_type_adapt_->get_right_block(), 0, //top_block->disconnect(data_type_adapt_->get_right_block(), 0,
// in_filt_->get_left_block(), 0); // in_filt_->get_left_block(), 0);
top_block->disconnect(in_filt_->get_right_block(), 0, top_block->disconnect(in_filt_->get_right_block(), 0,
res_->get_left_block(), 0); res_->get_left_block(), 0);
//data_type_adapt_->disconnect(top_block); //data_type_adapt_->disconnect(top_block);
in_filt_->disconnect(top_block); in_filt_->disconnect(top_block);
@ -105,9 +107,7 @@ gr::basic_block_sptr ArraySignalConditioner::get_left_block()
} }
gr::basic_block_sptr ArraySignalConditioner::get_right_block() gr::basic_block_sptr ArraySignalConditioner::get_right_block()
{ {
return res_->get_right_block(); return res_->get_right_block();
} }

12
src/algorithms/conditioner/adapters/array_signal_conditioner.h
View File

@ -47,13 +47,13 @@ class TelemetryDecoderInterface;
* \brief This class wraps blocks to change data_type_adapter, input_filter and resampler * \brief This class wraps blocks to change data_type_adapter, input_filter and resampler
* to be applied to the input flow of sampled signal. * to be applied to the input flow of sampled signal.
*/ */
class ArraySignalConditioner: public GNSSBlockInterface class ArraySignalConditioner : public GNSSBlockInterface
{ {
public: public:
//! Constructor //! Constructor
ArraySignalConditioner(ConfigurationInterface *configuration, ArraySignalConditioner(ConfigurationInterface *configuration,
std::shared_ptr<GNSSBlockInterface> data_type_adapt, std::shared_ptr<GNSSBlockInterface> in_filt, std::shared_ptr<GNSSBlockInterface> data_type_adapt, std::shared_ptr<GNSSBlockInterface> in_filt,
std::shared_ptr<GNSSBlockInterface> res, std::string role, std::string implementation); std::shared_ptr<GNSSBlockInterface> res, std::string role, std::string implementation);
//! Virtual destructor //! Virtual destructor
virtual ~ArraySignalConditioner(); virtual ~ArraySignalConditioner();
@ -68,9 +68,9 @@ public:
inline std::string implementation() override { return "Array_Signal_Conditioner"; } inline std::string implementation() override { return "Array_Signal_Conditioner"; }
inline size_t item_size() override { return 0; } inline size_t item_size() override { return 0; }
inline std::shared_ptr<GNSSBlockInterface> data_type_adapter(){ return data_type_adapt_; } inline std::shared_ptr<GNSSBlockInterface> data_type_adapter() { return data_type_adapt_; }
inline std::shared_ptr<GNSSBlockInterface> input_filter(){ return in_filt_; } inline std::shared_ptr<GNSSBlockInterface> input_filter() { return in_filt_; }
inline std::shared_ptr<GNSSBlockInterface> resampler(){ return res_; } inline std::shared_ptr<GNSSBlockInterface> resampler() { return res_; }
private: private:
std::shared_ptr<GNSSBlockInterface> data_type_adapt_; std::shared_ptr<GNSSBlockInterface> data_type_adapt_;

22
src/algorithms/conditioner/adapters/signal_conditioner.cc
View File

@ -37,19 +37,22 @@ using google::LogMessage;
// Constructor // Constructor
SignalConditioner::SignalConditioner(ConfigurationInterface *configuration, SignalConditioner::SignalConditioner(ConfigurationInterface *configuration,
std::shared_ptr<GNSSBlockInterface> data_type_adapt, std::shared_ptr<GNSSBlockInterface> in_filt, std::shared_ptr<GNSSBlockInterface> data_type_adapt, std::shared_ptr<GNSSBlockInterface> in_filt,
std::shared_ptr<GNSSBlockInterface> res, std::string role, std::string implementation) : std::shared_ptr<GNSSBlockInterface> res, std::string role, std::string implementation) : data_type_adapt_(data_type_adapt),
data_type_adapt_(data_type_adapt), in_filt_(in_filt),
in_filt_(in_filt), res_(res), role_(role), implementation_(implementation) res_(res),
role_(role),
implementation_(implementation)
{ {
connected_ = false; connected_ = false;
if(configuration){ }; if (configuration)
{
};
} }
// Destructor // Destructor
SignalConditioner::~SignalConditioner() SignalConditioner::~SignalConditioner() {}
{}
void SignalConditioner::connect(gr::top_block_sptr top_block) void SignalConditioner::connect(gr::top_block_sptr top_block)
@ -81,9 +84,9 @@ void SignalConditioner::disconnect(gr::top_block_sptr top_block)
} }
top_block->disconnect(data_type_adapt_->get_right_block(), 0, top_block->disconnect(data_type_adapt_->get_right_block(), 0,
in_filt_->get_left_block(), 0); in_filt_->get_left_block(), 0);
top_block->disconnect(in_filt_->get_right_block(), 0, top_block->disconnect(in_filt_->get_right_block(), 0,
res_->get_left_block(), 0); res_->get_left_block(), 0);
data_type_adapt_->disconnect(top_block); data_type_adapt_->disconnect(top_block);
in_filt_->disconnect(top_block); in_filt_->disconnect(top_block);
@ -102,4 +105,3 @@ gr::basic_block_sptr SignalConditioner::get_right_block()
{ {
return res_->get_right_block(); return res_->get_right_block();
} }

12
src/algorithms/conditioner/adapters/signal_conditioner.h
View File

@ -44,13 +44,13 @@ class TelemetryDecoderInterface;
* \brief This class wraps blocks to change data_type_adapter, input_filter and resampler * \brief This class wraps blocks to change data_type_adapter, input_filter and resampler
* to be applied to the input flow of sampled signal. * to be applied to the input flow of sampled signal.
*/ */
class SignalConditioner: public GNSSBlockInterface class SignalConditioner : public GNSSBlockInterface
{ {
public: public:
//! Constructor //! Constructor
SignalConditioner(ConfigurationInterface *configuration, SignalConditioner(ConfigurationInterface *configuration,
std::shared_ptr<GNSSBlockInterface> data_type_adapt, std::shared_ptr<GNSSBlockInterface> in_filt, std::shared_ptr<GNSSBlockInterface> data_type_adapt, std::shared_ptr<GNSSBlockInterface> in_filt,
std::shared_ptr<GNSSBlockInterface> res, std::string role, std::string implementation); std::shared_ptr<GNSSBlockInterface> res, std::string role, std::string implementation);
//! Virtual destructor //! Virtual destructor
virtual ~SignalConditioner(); virtual ~SignalConditioner();
@ -66,9 +66,9 @@ public:
inline size_t item_size() override { return 0; } inline size_t item_size() override { return 0; }
inline std::shared_ptr<GNSSBlockInterface> data_type_adapter(){ return data_type_adapt_; } inline std::shared_ptr<GNSSBlockInterface> data_type_adapter() { return data_type_adapt_; }
inline std::shared_ptr<GNSSBlockInterface> input_filter(){ return in_filt_; } inline std::shared_ptr<GNSSBlockInterface> input_filter() { return in_filt_; }
inline std::shared_ptr<GNSSBlockInterface> resampler(){ return res_; } inline std::shared_ptr<GNSSBlockInterface> resampler() { return res_; }
private: private:
std::shared_ptr<GNSSBlockInterface> data_type_adapt_; std::shared_ptr<GNSSBlockInterface> data_type_adapt_;

10
src/algorithms/data_type_adapter/adapters/byte_to_short.cc
View File

@ -36,9 +36,7 @@
using google::LogMessage; using google::LogMessage;
ByteToShort::ByteToShort(ConfigurationInterface* configuration, std::string role, ByteToShort::ByteToShort(ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : config_(configuration), role_(role), in_streams_(in_streams), out_streams_(out_streams)
config_(configuration), role_(role), in_streams_(in_streams),
out_streams_(out_streams)
{ {
std::string default_input_item_type = "byte"; std::string default_input_item_type = "byte";
std::string default_output_item_type = "short"; std::string default_output_item_type = "short";
@ -66,7 +64,8 @@ ByteToShort::ByteToShort(ConfigurationInterface* configuration, std::string role
ByteToShort::~ByteToShort() ByteToShort::~ByteToShort()
{} {
}
void ByteToShort::connect(gr::top_block_sptr top_block) void ByteToShort::connect(gr::top_block_sptr top_block)
@ -91,16 +90,13 @@ void ByteToShort::disconnect(gr::top_block_sptr top_block)
} }
gr::basic_block_sptr ByteToShort::get_left_block() gr::basic_block_sptr ByteToShort::get_left_block()
{ {
return gr_char_to_short_; return gr_char_to_short_;
} }
gr::basic_block_sptr ByteToShort::get_right_block() gr::basic_block_sptr ByteToShort::get_right_block()
{ {
return gr_char_to_short_; return gr_char_to_short_;
} }

6
src/algorithms/data_type_adapter/adapters/byte_to_short.h
View File

@ -42,12 +42,12 @@ class ConfigurationInterface;
* \brief Adapts an 8-bits sample stream (IF) to a short int stream (IF) * \brief Adapts an 8-bits sample stream (IF) to a short int stream (IF)
* *
*/ */
class ByteToShort: public GNSSBlockInterface class ByteToShort : public GNSSBlockInterface
{ {
public: public:
ByteToShort(ConfigurationInterface* configuration, ByteToShort(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~ByteToShort(); virtual ~ByteToShort();

19
src/algorithms/data_type_adapter/adapters/ibyte_to_cbyte.cc
View File

@ -37,9 +37,7 @@
using google::LogMessage; using google::LogMessage;
IbyteToCbyte::IbyteToCbyte(ConfigurationInterface* configuration, std::string role, IbyteToCbyte::IbyteToCbyte(ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : config_(configuration), role_(role), in_streams_(in_streams), out_streams_(out_streams)
config_(configuration), role_(role), in_streams_(in_streams),
out_streams_(out_streams)
{ {
std::string default_input_item_type = "byte"; std::string default_input_item_type = "byte";
std::string default_output_item_type = "lv_8sc_t"; std::string default_output_item_type = "lv_8sc_t";
@ -64,7 +62,7 @@ IbyteToCbyte::IbyteToCbyte(ConfigurationInterface* configuration, std::string ro
DLOG(INFO) << "Dumping output into file " << dump_filename_; DLOG(INFO) << "Dumping output into file " << dump_filename_;
file_sink_ = gr::blocks::file_sink::make(item_size, dump_filename_.c_str()); file_sink_ = gr::blocks::file_sink::make(item_size, dump_filename_.c_str());
} }
if(inverted_spectrum) if (inverted_spectrum)
{ {
conjugate_ic_ = make_conjugate_ic(); conjugate_ic_ = make_conjugate_ic();
} }
@ -72,14 +70,15 @@ IbyteToCbyte::IbyteToCbyte(ConfigurationInterface* configuration, std::string ro
IbyteToCbyte::~IbyteToCbyte() IbyteToCbyte::~IbyteToCbyte()
{} {
}
void IbyteToCbyte::connect(gr::top_block_sptr top_block) void IbyteToCbyte::connect(gr::top_block_sptr top_block)
{ {
if (dump_) if (dump_)
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->connect(ibyte_to_cbyte_, 0, conjugate_ic_, 0); top_block->connect(ibyte_to_cbyte_, 0, conjugate_ic_, 0);
top_block->connect(conjugate_ic_, 0, file_sink_, 0); top_block->connect(conjugate_ic_, 0, file_sink_, 0);
@ -91,7 +90,7 @@ void IbyteToCbyte::connect(gr::top_block_sptr top_block)
} }
else else
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->connect(ibyte_to_cbyte_, 0, conjugate_ic_, 0); top_block->connect(ibyte_to_cbyte_, 0, conjugate_ic_, 0);
} }
@ -107,7 +106,7 @@ void IbyteToCbyte::disconnect(gr::top_block_sptr top_block)
{ {
if (dump_) if (dump_)
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->disconnect(ibyte_to_cbyte_, 0, conjugate_ic_, 0); top_block->disconnect(ibyte_to_cbyte_, 0, conjugate_ic_, 0);
top_block->disconnect(conjugate_ic_, 0, file_sink_, 0); top_block->disconnect(conjugate_ic_, 0, file_sink_, 0);
@ -119,7 +118,7 @@ void IbyteToCbyte::disconnect(gr::top_block_sptr top_block)
} }
else else
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->disconnect(ibyte_to_cbyte_, 0, conjugate_ic_, 0); top_block->disconnect(ibyte_to_cbyte_, 0, conjugate_ic_, 0);
} }
@ -135,7 +134,7 @@ gr::basic_block_sptr IbyteToCbyte::get_left_block()
gr::basic_block_sptr IbyteToCbyte::get_right_block() gr::basic_block_sptr IbyteToCbyte::get_right_block()
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
return conjugate_ic_; return conjugate_ic_;
} }

4
src/algorithms/data_type_adapter/adapters/ibyte_to_cbyte.h
View File

@ -48,8 +48,8 @@ class IbyteToCbyte : public GNSSBlockInterface
{ {
public: public:
IbyteToCbyte(ConfigurationInterface* configuration, IbyteToCbyte(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~IbyteToCbyte(); virtual ~IbyteToCbyte();

17
src/algorithms/data_type_adapter/adapters/ibyte_to_complex.cc
View File

@ -35,9 +35,7 @@
using google::LogMessage; using google::LogMessage;
IbyteToComplex::IbyteToComplex(ConfigurationInterface* configuration, std::string role, IbyteToComplex::IbyteToComplex(ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : config_(configuration), role_(role), in_streams_(in_streams), out_streams_(out_streams)
config_(configuration), role_(role), in_streams_(in_streams),
out_streams_(out_streams)
{ {
std::string default_input_item_type = "byte"; std::string default_input_item_type = "byte";
std::string default_output_item_type = "gr_complex"; std::string default_output_item_type = "gr_complex";
@ -70,14 +68,15 @@ IbyteToComplex::IbyteToComplex(ConfigurationInterface* configuration, std::strin
IbyteToComplex::~IbyteToComplex() IbyteToComplex::~IbyteToComplex()
{} {
}
void IbyteToComplex::connect(gr::top_block_sptr top_block) void IbyteToComplex::connect(gr::top_block_sptr top_block)
{ {
if (dump_) if (dump_)
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->connect(gr_interleaved_char_to_complex_, 0, conjugate_cc_, 0); top_block->connect(gr_interleaved_char_to_complex_, 0, conjugate_cc_, 0);
top_block->connect(conjugate_cc_, 0, file_sink_, 0); top_block->connect(conjugate_cc_, 0, file_sink_, 0);
@ -89,7 +88,7 @@ void IbyteToComplex::connect(gr::top_block_sptr top_block)
} }
else else
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->connect(gr_interleaved_char_to_complex_, 0, conjugate_cc_, 0); top_block->connect(gr_interleaved_char_to_complex_, 0, conjugate_cc_, 0);
} }
@ -105,7 +104,7 @@ void IbyteToComplex::disconnect(gr::top_block_sptr top_block)
{ {
if (dump_) if (dump_)
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->disconnect(gr_interleaved_char_to_complex_, 0, conjugate_cc_, 0); top_block->disconnect(gr_interleaved_char_to_complex_, 0, conjugate_cc_, 0);
top_block->disconnect(conjugate_cc_, 0, file_sink_, 0); top_block->disconnect(conjugate_cc_, 0, file_sink_, 0);
@ -117,7 +116,7 @@ void IbyteToComplex::disconnect(gr::top_block_sptr top_block)
} }
else else
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->disconnect(gr_interleaved_char_to_complex_, 0, conjugate_cc_, 0); top_block->disconnect(gr_interleaved_char_to_complex_, 0, conjugate_cc_, 0);
} }
@ -133,7 +132,7 @@ gr::basic_block_sptr IbyteToComplex::get_left_block()
gr::basic_block_sptr IbyteToComplex::get_right_block() gr::basic_block_sptr IbyteToComplex::get_right_block()
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
return conjugate_cc_; return conjugate_cc_;
} }

6
src/algorithms/data_type_adapter/adapters/ibyte_to_complex.h
View File

@ -44,12 +44,12 @@ class ConfigurationInterface;
* \brief Adapts an I/Q interleaved byte integer sample stream to a gr_complex (float) stream * \brief Adapts an I/Q interleaved byte integer sample stream to a gr_complex (float) stream
* *
*/ */
class IbyteToComplex: public GNSSBlockInterface class IbyteToComplex : public GNSSBlockInterface
{ {
public: public:
IbyteToComplex(ConfigurationInterface* configuration, IbyteToComplex(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~IbyteToComplex(); virtual ~IbyteToComplex();

21
src/algorithms/data_type_adapter/adapters/ibyte_to_cshort.cc
View File

@ -38,9 +38,7 @@
using google::LogMessage; using google::LogMessage;
IbyteToCshort::IbyteToCshort(ConfigurationInterface* configuration, std::string role, IbyteToCshort::IbyteToCshort(ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : config_(configuration), role_(role), in_streams_(in_streams), out_streams_(out_streams)
config_(configuration), role_(role), in_streams_(in_streams),
out_streams_(out_streams)
{ {
std::string default_input_item_type = "byte"; std::string default_input_item_type = "byte";
std::string default_output_item_type = "cshort"; std::string default_output_item_type = "cshort";
@ -58,14 +56,14 @@ IbyteToCshort::IbyteToCshort(ConfigurationInterface* configuration, std::string
interleaved_byte_to_complex_short_ = make_interleaved_byte_to_complex_short(); interleaved_byte_to_complex_short_ = make_interleaved_byte_to_complex_short();
DLOG(INFO) << "data_type_adapter_(" << interleaved_byte_to_complex_short_->unique_id()<<")"; DLOG(INFO) << "data_type_adapter_(" << interleaved_byte_to_complex_short_->unique_id() << ")";
if (dump_) if (dump_)
{ {
DLOG(INFO) << "Dumping output into file " << dump_filename_; DLOG(INFO) << "Dumping output into file " << dump_filename_;
file_sink_ = gr::blocks::file_sink::make(item_size, dump_filename_.c_str()); file_sink_ = gr::blocks::file_sink::make(item_size, dump_filename_.c_str());
} }
if(inverted_spectrum) if (inverted_spectrum)
{ {
conjugate_sc_ = make_conjugate_sc(); conjugate_sc_ = make_conjugate_sc();
} }
@ -73,14 +71,15 @@ IbyteToCshort::IbyteToCshort(ConfigurationInterface* configuration, std::string
IbyteToCshort::~IbyteToCshort() IbyteToCshort::~IbyteToCshort()
{} {
}
void IbyteToCshort::connect(gr::top_block_sptr top_block) void IbyteToCshort::connect(gr::top_block_sptr top_block)
{ {
if (dump_) if (dump_)
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->connect(interleaved_byte_to_complex_short_, 0, conjugate_sc_, 0); top_block->connect(interleaved_byte_to_complex_short_, 0, conjugate_sc_, 0);
top_block->connect(conjugate_sc_, 0, file_sink_, 0); top_block->connect(conjugate_sc_, 0, file_sink_, 0);
@ -92,7 +91,7 @@ void IbyteToCshort::connect(gr::top_block_sptr top_block)
} }
else else
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->connect(interleaved_byte_to_complex_short_, 0, conjugate_sc_, 0); top_block->connect(interleaved_byte_to_complex_short_, 0, conjugate_sc_, 0);
} }
@ -104,7 +103,7 @@ void IbyteToCshort::disconnect(gr::top_block_sptr top_block)
{ {
if (dump_) if (dump_)
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->disconnect(interleaved_byte_to_complex_short_, 0, conjugate_sc_, 0); top_block->disconnect(interleaved_byte_to_complex_short_, 0, conjugate_sc_, 0);
top_block->disconnect(conjugate_sc_, 0, file_sink_, 0); top_block->disconnect(conjugate_sc_, 0, file_sink_, 0);
@ -116,7 +115,7 @@ void IbyteToCshort::disconnect(gr::top_block_sptr top_block)
} }
else else
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->disconnect(interleaved_byte_to_complex_short_, 0, conjugate_sc_, 0); top_block->disconnect(interleaved_byte_to_complex_short_, 0, conjugate_sc_, 0);
} }
@ -132,7 +131,7 @@ gr::basic_block_sptr IbyteToCshort::get_left_block()
gr::basic_block_sptr IbyteToCshort::get_right_block() gr::basic_block_sptr IbyteToCshort::get_right_block()
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
return conjugate_sc_; return conjugate_sc_;
} }

6
src/algorithms/data_type_adapter/adapters/ibyte_to_cshort.h
View File

@ -44,12 +44,12 @@ class ConfigurationInterface;
* \brief Adapts a short integer (16 bits) interleaved sample stream into a std::complex<short> stream * \brief Adapts a short integer (16 bits) interleaved sample stream into a std::complex<short> stream
* *
*/ */
class IbyteToCshort: public GNSSBlockInterface class IbyteToCshort : public GNSSBlockInterface
{ {
public: public:
IbyteToCshort(ConfigurationInterface* configuration, IbyteToCshort(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~IbyteToCshort(); virtual ~IbyteToCshort();

17
src/algorithms/data_type_adapter/adapters/ishort_to_complex.cc
View File

@ -35,9 +35,7 @@
using google::LogMessage; using google::LogMessage;
IshortToComplex::IshortToComplex(ConfigurationInterface* configuration, std::string role, IshortToComplex::IshortToComplex(ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : config_(configuration), role_(role), in_streams_(in_streams), out_streams_(out_streams)
config_(configuration), role_(role), in_streams_(in_streams),
out_streams_(out_streams)
{ {
std::string default_input_item_type = "short"; std::string default_input_item_type = "short";
std::string default_output_item_type = "gr_complex"; std::string default_output_item_type = "gr_complex";
@ -70,14 +68,15 @@ IshortToComplex::IshortToComplex(ConfigurationInterface* configuration, std::str
IshortToComplex::~IshortToComplex() IshortToComplex::~IshortToComplex()
{} {
}
void IshortToComplex::connect(gr::top_block_sptr top_block) void IshortToComplex::connect(gr::top_block_sptr top_block)
{ {
if (dump_) if (dump_)
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->connect(gr_interleaved_short_to_complex_, 0, conjugate_cc_, 0); top_block->connect(gr_interleaved_short_to_complex_, 0, conjugate_cc_, 0);
top_block->connect(conjugate_cc_, 0, file_sink_, 0); top_block->connect(conjugate_cc_, 0, file_sink_, 0);
@ -89,7 +88,7 @@ void IshortToComplex::connect(gr::top_block_sptr top_block)
} }
else else
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->connect(gr_interleaved_short_to_complex_, 0, conjugate_cc_, 0); top_block->connect(gr_interleaved_short_to_complex_, 0, conjugate_cc_, 0);
} }
@ -105,7 +104,7 @@ void IshortToComplex::disconnect(gr::top_block_sptr top_block)
{ {
if (dump_) if (dump_)
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->disconnect(gr_interleaved_short_to_complex_, 0, conjugate_cc_, 0); top_block->disconnect(gr_interleaved_short_to_complex_, 0, conjugate_cc_, 0);
top_block->disconnect(conjugate_cc_, 0, file_sink_, 0); top_block->disconnect(conjugate_cc_, 0, file_sink_, 0);
@ -117,7 +116,7 @@ void IshortToComplex::disconnect(gr::top_block_sptr top_block)
} }
else else
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->disconnect(gr_interleaved_short_to_complex_, 0, conjugate_cc_, 0); top_block->disconnect(gr_interleaved_short_to_complex_, 0, conjugate_cc_, 0);
} }
@ -133,7 +132,7 @@ gr::basic_block_sptr IshortToComplex::get_left_block()
gr::basic_block_sptr IshortToComplex::get_right_block() gr::basic_block_sptr IshortToComplex::get_right_block()
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
return conjugate_cc_; return conjugate_cc_;
} }

6
src/algorithms/data_type_adapter/adapters/ishort_to_complex.h
View File

@ -43,12 +43,12 @@ class ConfigurationInterface;
* \brief Adapts an I/Q interleaved short integer sample stream to a gr_complex (float) stream * \brief Adapts an I/Q interleaved short integer sample stream to a gr_complex (float) stream
* *
*/ */
class IshortToComplex: public GNSSBlockInterface class IshortToComplex : public GNSSBlockInterface
{ {
public: public:
IshortToComplex(ConfigurationInterface* configuration, IshortToComplex(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~IshortToComplex(); virtual ~IshortToComplex();

19
src/algorithms/data_type_adapter/adapters/ishort_to_cshort.cc
View File

@ -37,9 +37,7 @@
using google::LogMessage; using google::LogMessage;
IshortToCshort::IshortToCshort(ConfigurationInterface* configuration, std::string role, IshortToCshort::IshortToCshort(ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : unsigned int in_streams, unsigned int out_streams) : config_(configuration), role_(role), in_streams_(in_streams), out_streams_(out_streams)
config_(configuration), role_(role), in_streams_(in_streams),
out_streams_(out_streams)
{ {
std::string default_input_item_type = "short"; std::string default_input_item_type = "short";
std::string default_output_item_type = "cshort"; std::string default_output_item_type = "cshort";
@ -64,7 +62,7 @@ IshortToCshort::IshortToCshort(ConfigurationInterface* configuration, std::strin
DLOG(INFO) << "Dumping output into file " << dump_filename_; DLOG(INFO) << "Dumping output into file " << dump_filename_;
file_sink_ = gr::blocks::file_sink::make(item_size, dump_filename_.c_str()); file_sink_ = gr::blocks::file_sink::make(item_size, dump_filename_.c_str());
} }
if(inverted_spectrum) if (inverted_spectrum)
{ {
conjugate_sc_ = make_conjugate_sc(); conjugate_sc_ = make_conjugate_sc();
} }
@ -72,14 +70,15 @@ IshortToCshort::IshortToCshort(ConfigurationInterface* configuration, std::strin
IshortToCshort::~IshortToCshort() IshortToCshort::~IshortToCshort()
{} {
}
void IshortToCshort::connect(gr::top_block_sptr top_block) void IshortToCshort::connect(gr::top_block_sptr top_block)
{ {
if (dump_) if (dump_)
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->connect(interleaved_short_to_complex_short_, 0, conjugate_sc_, 0); top_block->connect(interleaved_short_to_complex_short_, 0, conjugate_sc_, 0);
top_block->connect(conjugate_sc_, 0, file_sink_, 0); top_block->connect(conjugate_sc_, 0, file_sink_, 0);
@ -91,7 +90,7 @@ void IshortToCshort::connect(gr::top_block_sptr top_block)
} }
else else
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->connect(interleaved_short_to_complex_short_, 0, conjugate_sc_, 0); top_block->connect(interleaved_short_to_complex_short_, 0, conjugate_sc_, 0);
} }
@ -107,7 +106,7 @@ void IshortToCshort::disconnect(gr::top_block_sptr top_block)
{ {
if (dump_) if (dump_)
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->disconnect(interleaved_short_to_complex_short_, 0, conjugate_sc_, 0); top_block->disconnect(interleaved_short_to_complex_short_, 0, conjugate_sc_, 0);
top_block->disconnect(conjugate_sc_, 0, file_sink_, 0); top_block->disconnect(conjugate_sc_, 0, file_sink_, 0);
@ -119,7 +118,7 @@ void IshortToCshort::disconnect(gr::top_block_sptr top_block)
} }
else else
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
top_block->disconnect(interleaved_short_to_complex_short_, 0, conjugate_sc_, 0); top_block->disconnect(interleaved_short_to_complex_short_, 0, conjugate_sc_, 0);
} }
@ -135,7 +134,7 @@ gr::basic_block_sptr IshortToCshort::get_left_block()
gr::basic_block_sptr IshortToCshort::get_right_block() gr::basic_block_sptr IshortToCshort::get_right_block()
{ {
if(inverted_spectrum) if (inverted_spectrum)
{ {
return conjugate_sc_; return conjugate_sc_;
} }

6
src/algorithms/data_type_adapter/adapters/ishort_to_cshort.h
View File

@ -44,12 +44,12 @@ class ConfigurationInterface;
* \brief Adapts a short integer (16 bits) interleaved sample stream into a std::complex<short> stream * \brief Adapts a short integer (16 bits) interleaved sample stream into a std::complex<short> stream
* *
*/ */
class IshortToCshort: public GNSSBlockInterface class IshortToCshort : public GNSSBlockInterface
{ {
public: public:
IshortToCshort(ConfigurationInterface* configuration, IshortToCshort(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~IshortToCshort(); virtual ~IshortToCshort();

13
src/algorithms/data_type_adapter/gnuradio_blocks/interleaved_byte_to_complex_byte.cc
View File

@ -40,11 +40,10 @@ interleaved_byte_to_complex_byte_sptr make_interleaved_byte_to_complex_byte()
} }
interleaved_byte_to_complex_byte::interleaved_byte_to_complex_byte() : sync_decimator("interleaved_byte_to_complex_byte", interleaved_byte_to_complex_byte::interleaved_byte_to_complex_byte() : sync_decimator("interleaved_byte_to_complex_byte",
gr::io_signature::make (1, 1, sizeof(int8_t)), gr::io_signature::make(1, 1, sizeof(int8_t)),
gr::io_signature::make (1, 1, sizeof(lv_8sc_t)), // lv_8sc_t is a Volk's typedef for std::complex<signed char> gr::io_signature::make(1, 1, sizeof(lv_8sc_t)), // lv_8sc_t is a Volk's typedef for std::complex<signed char>
2) 2)
{ {
const int alignment_multiple = volk_get_alignment() / sizeof(lv_8sc_t); const int alignment_multiple = volk_get_alignment() / sizeof(lv_8sc_t);
set_alignment(std::max(1, alignment_multiple)); set_alignment(std::max(1, alignment_multiple));
@ -52,15 +51,15 @@ interleaved_byte_to_complex_byte::interleaved_byte_to_complex_byte() : sync_deci
int interleaved_byte_to_complex_byte::work(int noutput_items, int interleaved_byte_to_complex_byte::work(int noutput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items) gr_vector_void_star &output_items)
{ {
const int8_t *in = reinterpret_cast<const int8_t *>(input_items[0]); const int8_t *in = reinterpret_cast<const int8_t *>(input_items[0]);
lv_8sc_t *out = reinterpret_cast<lv_8sc_t *>(output_items[0]); lv_8sc_t *out = reinterpret_cast<lv_8sc_t *>(output_items[0]);
// This could be put into a Volk kernel // This could be put into a Volk kernel
int8_t real_part; int8_t real_part;
int8_t imag_part; int8_t imag_part;
for(int number = 0; number < noutput_items; number++) for (int number = 0; number < noutput_items; number++)
{ {
// lv_cmake(r, i) defined at volk/volk_complex.h // lv_cmake(r, i) defined at volk/volk_complex.h
real_part = *in++; real_part = *in++;

Some files were not shown because too many files have changed in this diff Show More