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

Merge branch 'next' of https://github.com/carlesfernandez/gnss-sdr into next

Browse Source
This commit is contained in:
Carles Fernandez 2019-02-11 21:30:31 +01:00
commit 632f16cf1d
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
147 changed files with 8209 additions and 2980 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
.clang-tidy
View File

@ -1,4 +1,62 @@
--- ---
Checks: '-*,boost-use-to-string,cert-dcl21-cpp,cert-dcl58-cpp,cert-env33-c,cert-err52-cpp,cert-err60-cpp,cert-flp30-c,clang-analyzer-cplusplus*,cppcoreguidelines-pro-type-static-cast-downcast,cppcoreguidelines-slicing,google-build-namespaces,google-runtime-int,google-runtime-references,misc-misplaced-const,misc-new-delete-overloads,misc-non-copyable-objects,misc-static-assert,misc-throw-by-value-catch-by-reference,misc-uniqueptr-reset-release,modernize-loop-convert,modernize-pass-by-value,modernize-raw-string-literal,modernize-use-auto,modernize-use-bool-literals,modernize-use-equals-default,modernize-use-equals-delete,modernize-use-noexcept,modernize-use-nullptr,performance-faster-string-find,performance-inefficient-algorithm,performance-move-const-arg,performance-type-promotion-in-math-fn,performance-unnecessary-copy-initialization,performance-unnecessary-value-param,readability-container-size-empty,readability-named-parameter,readability-non-const-parameter,readability-string-compare' Checks: '-*,boost-use-to-string,cert-dcl21-cpp,cert-dcl58-cpp,cert-env33-c,cert-err52-cpp,cert-err60-cpp,cert-flp30-c,clang-analyzer-cplusplus*,cppcoreguidelines-pro-type-static-cast-downcast,cppcoreguidelines-slicing,google-build-namespaces,google-runtime-int,google-runtime-references,misc-misplaced-const,misc-new-delete-overloads,misc-non-copyable-objects,misc-static-assert,misc-throw-by-value-catch-by-reference,misc-uniqueptr-reset-release,modernize-loop-convert,modernize-pass-by-value,modernize-raw-string-literal,modernize-use-auto,modernize-use-bool-literals,modernize-use-equals-default,modernize-use-equals-delete,modernize-use-noexcept,modernize-use-nullptr,performance-faster-string-find,performance-inefficient-algorithm,performance-move-const-arg,performance-type-promotion-in-math-fn,performance-unnecessary-copy-initialization,performance-unnecessary-value-param,readability-container-size-empty,readability-named-parameter,readability-non-const-parameter,readability-string-compare'
HeaderFilterRegex: '.*' WarningsAsErrors: ''
HeaderFilterRegex: ''
AnalyzeTemporaryDtors: false
FormatStyle: 'file' FormatStyle: 'file'
CheckOptions:
- key: google-build-namespaces.HeaderFileExtensions
value: ',h'
- key: google-readability-braces-around-statements.ShortStatementLines
value: '1'
- key: google-readability-function-size.StatementThreshold
value: '800'
- key: google-readability-namespace-comments.ShortNamespaceLines
value: '10'
- key: google-readability-namespace-comments.SpacesBeforeComments
value: '2'
- key: google-runtime-int.SignedTypePrefix
value: int
- key: google-runtime-int.TypeSuffix
value: '_t'
- key: google-runtime-int.UnsignedTypePrefix
value: uint
- key: google-runtime-references.WhiteListTypes
value: ''
- key: misc-throw-by-value-catch-by-reference.CheckThrowTemporaries
value: '1'
- key: modernize-loop-convert.MaxCopySize
value: '16'
- key: modernize-loop-convert.MinConfidence
value: reasonable
- key: modernize-loop-convert.NamingStyle
value: CamelCase
- key: modernize-pass-by-value.IncludeStyle
value: llvm
- key: modernize-pass-by-value.ValuesOnly
value: '0'
- key: modernize-raw-string-literal.ReplaceShorterLiterals
value: '0'
- key: modernize-replace-auto-ptr.IncludeStyle
value: llvm
- key: modernize-use-auto.MinTypeNameLength
value: '5'
- key: modernize-use-auto.RemoveStars
value: '0'
- key: modernize-use-equals-default.IgnoreMacros
value: '1'
- key: modernize-use-noexcept.ReplacementString
value: ''
- key: modernize-use-noexcept.UseNoexceptFalse
value: '1'
- key: modernize-use-nullptr.NullMacros
value: 'NULL'
- key: performance-faster-string-find.StringLikeClasses
value: 'std::basic_string'
- key: performance-move-const-arg.CheckTriviallyCopyableMove
value: '1'
- key: performance-type-promotion-in-math-fn.IncludeStyle
value: llvm
- key: performance-unnecessary-value-param.IncludeStyle
value: llvm
...

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

@ -190,58 +190,174 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
int glo_2G_count = configuration->property("Channels_2G.count", 0); int glo_2G_count = configuration->property("Channels_2G.count", 0);
int bds_B1_count = configuration->property("Channels_B1.count", 0); int bds_B1_count = configuration->property("Channels_B1.count", 0);
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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 1; // L1 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_2G_count == 0) && (bds_B1_count == 0))
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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 3; // L5 pvt_output_parameters.type_of_receiver = 1; // L1
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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 4; // E1 }
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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 5; // E5a 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_2G_count == 0) && (bds_B1_count == 0))
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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 6; {
pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 8; // L1+L5 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_2G_count == 0) && (bds_B1_count == 0))
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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 9; // L1+E1 {
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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 10; pvt_output_parameters.type_of_receiver = 3; // L5
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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0))
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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 13; // L5+E5a {
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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 14; pvt_output_parameters.type_of_receiver = 4; // E1
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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.type_of_receiver = 5; // E5a
}
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_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.type_of_receiver = 8; // L1+L5
}
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_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.type_of_receiver = 9; // L1+E1
}
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_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.type_of_receiver = 13; // L5+E5a
}
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_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.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)) pvt_output_parameters.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)) pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0))
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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 18; {
pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.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)) pvt_output_parameters.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)) pvt_output_parameters.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)) pvt_output_parameters.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)) pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.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)) pvt_output_parameters.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)) pvt_output_parameters.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) && (bds_B1_count == 0)) pvt_output_parameters.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) && (bds_B1_count == 0))
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_2G_count != 0) && (bds_B1_count == 0)) pvt_output_parameters.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_2G_count != 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 25; pvt_output_parameters.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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.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) && (glo_2G_count != 0) && (bds_B1_count == 0))
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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.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) && (glo_2G_count != 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 29; pvt_output_parameters.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_2G_count != 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 30; }
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_2G_count != 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 31; 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_2G_count != 0) && (bds_B1_count == 0))
{
pvt_output_parameters.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_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.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) && (glo_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.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) && (glo_2G_count != 0) && (bds_B1_count == 0))
{
pvt_output_parameters.type_of_receiver = 29;
}
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_2G_count != 0) && (bds_B1_count == 0))
{
pvt_output_parameters.type_of_receiver = 30;
}
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_2G_count != 0) && (bds_B1_count == 0))
{
pvt_output_parameters.type_of_receiver = 31;
}
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_2G_count == 0) && (bds_B1_count == 0)) pvt_output_parameters.type_of_receiver = 32; // L1+E1+L5+E5a 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_2G_count == 0) && (bds_B1_count == 0))
{
pvt_output_parameters.type_of_receiver = 32; // L1+E1+L5+E5a
}
// BeiDou B1I Receiver // BeiDou B1I Receiver
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_2G_count == 0) && (bds_B1_count != 0)) pvt_output_parameters.type_of_receiver = 50; 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_2G_count == 0) && (bds_B1_count != 0))
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_2G_count == 0) && (bds_B1_count != 0)) pvt_output_parameters.type_of_receiver = 51; {
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_2G_count == 0) && (bds_B1_count != 0)) pvt_output_parameters.type_of_receiver = 52; pvt_output_parameters.type_of_receiver = 50;
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_2G_count == 0) && (bds_B1_count != 0)) pvt_output_parameters.type_of_receiver = 53; }
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_2G_count == 0) && (bds_B1_count != 0)) pvt_output_parameters.type_of_receiver = 54; 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_2G_count == 0) && (bds_B1_count != 0))
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_2G_count == 0) && (bds_B1_count != 0)) pvt_output_parameters.type_of_receiver = 55; {
pvt_output_parameters.type_of_receiver = 51;
}
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_2G_count == 0) && (bds_B1_count != 0))
{
pvt_output_parameters.type_of_receiver = 52;
}
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_2G_count == 0) && (bds_B1_count != 0))
{
pvt_output_parameters.type_of_receiver = 53;
}
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_2G_count == 0) && (bds_B1_count != 0))
{
pvt_output_parameters.type_of_receiver = 54;
}
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_2G_count == 0) && (bds_B1_count != 0))
{
pvt_output_parameters.type_of_receiver = 55;
}
// 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 == "Single") positioning_mode = PMODE_SINGLE; if (positioning_mode_str == "Single")
if (positioning_mode_str == "Static") positioning_mode = PMODE_STATIC; {
if (positioning_mode_str == "Kinematic") positioning_mode = PMODE_KINEMA; positioning_mode = PMODE_SINGLE;
if (positioning_mode_str == "PPP_Static") positioning_mode = PMODE_PPP_STATIC; }
if (positioning_mode_str == "PPP_Kinematic") positioning_mode = PMODE_PPP_KINEMA; if (positioning_mode_str == "Static")
{
positioning_mode = PMODE_STATIC;
}
if (positioning_mode_str == "Kinematic")
{
positioning_mode = PMODE_KINEMA;
}
if (positioning_mode_str == "PPP_Static")
{
positioning_mode = PMODE_PPP_STATIC;
}
if (positioning_mode_str == "PPP_Kinematic")
{
positioning_mode = PMODE_PPP_KINEMA;
}
if (positioning_mode == -1) if (positioning_mode == -1)
{ {
@ -255,10 +371,22 @@ 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) || (bds_B1_count > 0)) num_bands = 1; if ((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0) || (bds_B1_count > 0))
if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) && ((gps_2S_count > 0) || (glo_2G_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; num_bands = 1;
if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) && ((gps_2S_count > 0) || (glo_2G_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) || (glo_2G_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) || (glo_2G_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))
@ -286,12 +414,30 @@ 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 == "OFF") iono_model = IONOOPT_OFF; if (iono_model_str == "OFF")
if (iono_model_str == "Broadcast") iono_model = IONOOPT_BRDC; {
if (iono_model_str == "SBAS") iono_model = IONOOPT_SBAS; iono_model = IONOOPT_OFF;
if (iono_model_str == "Iono-Free-LC") iono_model = IONOOPT_IFLC; }
if (iono_model_str == "Estimate_STEC") iono_model = IONOOPT_EST; if (iono_model_str == "Broadcast")
if (iono_model_str == "IONEX") iono_model = IONOOPT_TEC; {
iono_model = IONOOPT_BRDC;
}
if (iono_model_str == "SBAS")
{
iono_model = IONOOPT_SBAS;
}
if (iono_model_str == "Iono-Free-LC")
{
iono_model = IONOOPT_IFLC;
}
if (iono_model_str == "Estimate_STEC")
{
iono_model = IONOOPT_EST;
}
if (iono_model_str == "IONEX")
{
iono_model = IONOOPT_TEC;
}
if (iono_model == -1) if (iono_model == -1)
{ {
//warn user and set the default //warn user and set the default
@ -305,11 +451,26 @@ 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 == "OFF") trop_model = TROPOPT_OFF; if (trop_model_str == "OFF")
if (trop_model_str == "Saastamoinen") trop_model = TROPOPT_SAAS; {
if (trop_model_str == "SBAS") trop_model = TROPOPT_SBAS; trop_model = TROPOPT_OFF;
if (trop_model_str == "Estimate_ZTD") trop_model = TROPOPT_EST; }
if (trop_model_str == "Estimate_ZTD_Grad") trop_model = TROPOPT_ESTG; if (trop_model_str == "Saastamoinen")
{
trop_model = TROPOPT_SAAS;
}
if (trop_model_str == "SBAS")
{
trop_model = TROPOPT_SBAS;
}
if (trop_model_str == "Estimate_ZTD")
{
trop_model = TROPOPT_EST;
}
if (trop_model_str == "Estimate_ZTD_Grad")
{
trop_model = TROPOPT_ESTG;
}
if (trop_model == -1) if (trop_model == -1)
{ {
//warn user and set the default //warn user and set the default
@ -339,10 +500,22 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
int earth_tide = configuration->property(role + ".earth_tide", 0); int earth_tide = configuration->property(role + ".earth_tide", 0);
int nsys = 0; int nsys = 0;
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))
if ((gal_1B_count > 0) || (gal_E5a_count > 0) || (gal_E5b_count > 0)) nsys += SYS_GAL; {
if ((glo_1G_count > 0) || (glo_2G_count > 0)) nsys += SYS_GLO; nsys += SYS_GPS;
if ((bds_B1_count > 0)) nsys += SYS_BDS; }
if ((gal_1B_count > 0) || (gal_E5a_count > 0) || (gal_E5b_count > 0))
{
nsys += SYS_GAL;
}
if ((glo_1G_count > 0) || (glo_2G_count > 0))
{
nsys += SYS_GLO;
}
if ((bds_B1_count > 0))
{
nsys += SYS_BDS;
}
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 */
@ -356,11 +529,26 @@ 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 == "OFF") integer_ambiguity_resolution_gps = ARMODE_OFF; if (integer_ambiguity_resolution_gps_str == "OFF")
if (integer_ambiguity_resolution_gps_str == "Continuous") integer_ambiguity_resolution_gps = ARMODE_CONT; {
if (integer_ambiguity_resolution_gps_str == "Instantaneous") integer_ambiguity_resolution_gps = ARMODE_INST; integer_ambiguity_resolution_gps = ARMODE_OFF;
if (integer_ambiguity_resolution_gps_str == "Fix-and-Hold") integer_ambiguity_resolution_gps = ARMODE_FIXHOLD; }
if (integer_ambiguity_resolution_gps_str == "PPP-AR") integer_ambiguity_resolution_gps = ARMODE_PPPAR; if (integer_ambiguity_resolution_gps_str == "Continuous")
{
integer_ambiguity_resolution_gps = ARMODE_CONT;
}
if (integer_ambiguity_resolution_gps_str == "Instantaneous")
{
integer_ambiguity_resolution_gps = ARMODE_INST;
}
if (integer_ambiguity_resolution_gps_str == "Fix-and-Hold")
{
integer_ambiguity_resolution_gps = ARMODE_FIXHOLD;
}
if (integer_ambiguity_resolution_gps_str == "PPP-AR")
{
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

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

@ -176,9 +176,18 @@ void rtklib_pvt_cc::msg_handler_telemetry(const pmt::pmt_t& msg)
Galileo_Almanac sv2 = galileo_almanac_helper->get_almanac(2); Galileo_Almanac sv2 = galileo_almanac_helper->get_almanac(2);
Galileo_Almanac sv3 = galileo_almanac_helper->get_almanac(3); Galileo_Almanac sv3 = galileo_almanac_helper->get_almanac(3);
if (sv1.i_satellite_PRN != 0) d_pvt_solver->galileo_almanac_map[sv1.i_satellite_PRN] = sv1; if (sv1.i_satellite_PRN != 0)
if (sv2.i_satellite_PRN != 0) d_pvt_solver->galileo_almanac_map[sv2.i_satellite_PRN] = sv2; {
if (sv3.i_satellite_PRN != 0) d_pvt_solver->galileo_almanac_map[sv3.i_satellite_PRN] = sv3; d_pvt_solver->galileo_almanac_map[sv1.i_satellite_PRN] = sv1;
}
if (sv2.i_satellite_PRN != 0)
{
d_pvt_solver->galileo_almanac_map[sv2.i_satellite_PRN] = sv2;
}
if (sv3.i_satellite_PRN != 0)
{
d_pvt_solver->galileo_almanac_map[sv3.i_satellite_PRN] = sv3;
}
DLOG(INFO) << "New Galileo Almanac data have arrived "; DLOG(INFO) << "New Galileo Almanac data have arrived ";
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Almanac>)) else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Almanac>))
@ -1421,10 +1430,22 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
send_sys_v_ttff_msg(ttff); send_sys_v_ttff_msg(ttff);
first_fix = false; first_fix = false;
} }
if (d_kml_output_enabled) d_kml_dump->print_position(d_pvt_solver, false); if (d_kml_output_enabled)
if (d_gpx_output_enabled) d_gpx_dump->print_position(d_pvt_solver, false); {
if (d_geojson_output_enabled) d_geojson_printer->print_position(d_pvt_solver, false); d_kml_dump->print_position(d_pvt_solver, false);
if (d_nmea_output_file_enabled) d_nmea_printer->Print_Nmea_Line(d_pvt_solver, false); }
if (d_gpx_output_enabled)
{
d_gpx_dump->print_position(d_pvt_solver, false);
}
if (d_geojson_output_enabled)
{
d_geojson_printer->print_position(d_pvt_solver, false);
}
if (d_nmea_output_file_enabled)
{
d_nmea_printer->Print_Nmea_Line(d_pvt_solver, false);
}
/* /*
* TYPE | RECEIVER * TYPE | RECEIVER
@ -1638,7 +1659,9 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
std::string glo_signal("1G"); std::string glo_signal("1G");
rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal);
if (d_rinex_version == 3) if (d_rinex_version == 3)
{
rp->rinex_nav_header(rp->navMixFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model, d_pvt_solver->glonass_gnav_utc_model, d_pvt_solver->glonass_gnav_almanac); rp->rinex_nav_header(rp->navMixFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model, d_pvt_solver->glonass_gnav_utc_model, d_pvt_solver->glonass_gnav_almanac);
}
if (d_rinex_version == 2) if (d_rinex_version == 2)
{ {
rp->rinex_nav_header(rp->navFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model); rp->rinex_nav_header(rp->navFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model);
@ -1672,7 +1695,9 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
std::string glo_signal("2G"); std::string glo_signal("2G");
rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal);
if (d_rinex_version == 3) if (d_rinex_version == 3)
{
rp->rinex_nav_header(rp->navMixFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model, d_pvt_solver->glonass_gnav_utc_model, d_pvt_solver->glonass_gnav_almanac); rp->rinex_nav_header(rp->navMixFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model, d_pvt_solver->glonass_gnav_utc_model, d_pvt_solver->glonass_gnav_almanac);
}
if (d_rinex_version == 2) if (d_rinex_version == 2)
{ {
rp->rinex_nav_header(rp->navFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model); rp->rinex_nav_header(rp->navFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model);
@ -1780,7 +1805,9 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
break; break;
case 26: // GPS L1 C/A + GLONASS L1 C/A case 26: // GPS L1 C/A + GLONASS L1 C/A
if (d_rinex_version == 3) if (d_rinex_version == 3)
{
rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map); rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map);
}
if (d_rinex_version == 2) if (d_rinex_version == 2)
{ {
rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_ephemeris_map); rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_ephemeris_map);
@ -1795,7 +1822,9 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
break; break;
case 29: // GPS L1 C/A + GLONASS L2 C/A case 29: // GPS L1 C/A + GLONASS L2 C/A
if (d_rinex_version == 3) if (d_rinex_version == 3)
{
rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map); rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map);
}
if (d_rinex_version == 2) if (d_rinex_version == 2)
{ {
rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_ephemeris_map); rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_ephemeris_map);

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

@ -221,7 +221,10 @@ 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;
} }

10
src/algorithms/PVT/libs/gpx_printer.cc
View File

@ -171,7 +171,10 @@ bool Gpx_Printer::print_position(const std::shared_ptr<rtklib_solver>& position,
double vdop = position_->get_vdop(); double vdop = position_->get_vdop();
double pdop = position_->get_pdop(); double pdop = position_->get_pdop();
std::string utc_time = to_iso_extended_string(position_->get_position_UTC_time()); std::string utc_time = to_iso_extended_string(position_->get_position_UTC_time());
if (utc_time.length() < 23) utc_time += "."; if (utc_time.length() < 23)
{
utc_time += ".";
}
utc_time.resize(23, '0'); // time up to ms utc_time.resize(23, '0'); // time up to ms
utc_time.append("Z"); // UTC time zone utc_time.append("Z"); // UTC time zone
@ -229,6 +232,9 @@ Gpx_Printer::~Gpx_Printer()
} }
if (!positions_printed) if (!positions_printed)
{ {
if (remove(gpx_filename.c_str()) != 0) LOG(INFO) << "Error deleting temporary GPX file"; if (remove(gpx_filename.c_str()) != 0)
{
LOG(INFO) << "Error deleting temporary GPX file";
}
} }
} }

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

@ -229,7 +229,10 @@ bool Kml_Printer::print_position(const std::shared_ptr<rtklib_solver>& position,
double vdop = position_->get_vdop(); double vdop = position_->get_vdop();
double pdop = position_->get_pdop(); double pdop = position_->get_pdop();
std::string utc_time = to_iso_extended_string(position_->get_position_UTC_time()); std::string utc_time = to_iso_extended_string(position_->get_position_UTC_time());
if (utc_time.length() < 23) utc_time += "."; if (utc_time.length() < 23)
{
utc_time += ".";
}
utc_time.resize(23, '0'); // time up to ms utc_time.resize(23, '0'); // time up to ms
utc_time.append("Z"); // UTC time zone utc_time.append("Z"); // UTC time zone
@ -330,6 +333,9 @@ Kml_Printer::~Kml_Printer()
} }
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";
}
} }
} }

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

@ -253,7 +253,10 @@ 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(*elev * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); Ls_Pvt::tropo(&trop, sin(*elev * 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
}
} }
} }
} }

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

@ -161,9 +161,15 @@ int Nmea_Printer::init_serial(const std::string& serial_device)
int64_t PARITY; int64_t PARITY;
fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_CLOEXEC); fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_CLOEXEC);
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;
@ -367,13 +373,22 @@ std::string Nmea_Printer::get_UTC_NMEA_time(boost::posix_time::ptime d_position_
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)

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

@ -230,31 +230,52 @@ Rinex_Printer::~Rinex_Printer()
// If nothing written, erase the files. // If nothing written, erase the files.
if (posn == 0) if (posn == 0)
{ {
if (remove(navfilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; if (remove(navfilename.c_str()) != 0)
{
LOG(INFO) << "Error deleting temporary file";
}
} }
if (poso == 0) if (poso == 0)
{ {
if (remove(obsfilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; if (remove(obsfilename.c_str()) != 0)
{
LOG(INFO) << "Error deleting temporary file";
}
} }
if (poss == 0) if (poss == 0)
{ {
if (remove(sbsfilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; if (remove(sbsfilename.c_str()) != 0)
{
LOG(INFO) << "Error deleting temporary file";
}
} }
if (posng == 0) if (posng == 0)
{ {
if (remove(navGalfilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; if (remove(navGalfilename.c_str()) != 0)
{
LOG(INFO) << "Error deleting temporary file";
}
} }
if (posmn == 0) if (posmn == 0)
{ {
if (remove(navMixfilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; if (remove(navMixfilename.c_str()) != 0)
{
LOG(INFO) << "Error deleting temporary file";
}
} }
if (posnr == 0) if (posnr == 0)
{ {
if (remove(navGlofilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; if (remove(navGlofilename.c_str()) != 0)
{
LOG(INFO) << "Error deleting temporary file";
}
} }
if (posnc == 0) if (posnc == 0)
{ {
if (remove(navBdsfilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; if (remove(navBdsfilename.c_str()) != 0)
{
LOG(INFO) << "Error deleting temporary file";
}
} }
} }
@ -277,8 +298,14 @@ std::string Rinex_Printer::createFilename(const std::string& type)
boost::gregorian::date today = boost::gregorian::day_clock::local_day(); boost::gregorian::date today = boost::gregorian::day_clock::local_day();
const int32_t dayOfTheYear = today.day_of_year(); const int32_t dayOfTheYear = today.day_of_year();
std::stringstream strm0; std::stringstream strm0;
if (dayOfTheYear < 100) strm0 << "0"; // three digits for day of the year if (dayOfTheYear < 100)
if (dayOfTheYear < 10) strm0 << "0"; // three digits for day of the year {
strm0 << "0"; // three digits for day of the year
}
if (dayOfTheYear < 10)
{
strm0 << "0"; // three digits for day of the year
}
strm0 << dayOfTheYear; strm0 << dayOfTheYear;
std::string dayOfTheYearTag = strm0.str(); std::string dayOfTheYearTag = strm0.str();
@ -332,7 +359,10 @@ std::string Rinex_Printer::createFilename(const std::string& type)
int32_t local_minute = pt_tm.tm_min; int32_t local_minute = pt_tm.tm_min;
std::stringstream strm2; std::stringstream strm2;
if (local_minute < 10) strm2 << "0"; // at least two digits for minutes if (local_minute < 10)
{
strm2 << "0"; // at least two digits for minutes
}
strm2 << local_minute; strm2 << local_minute;
std::string minTag = strm2.str(); std::string minTag = strm2.str();
@ -374,12 +404,18 @@ std::string Rinex_Printer::getLocalTime()
std::stringstream strmHour; std::stringstream strmHour;
int32_t utc_hour = pt_tm.tm_hour; int32_t utc_hour = pt_tm.tm_hour;
if (utc_hour < 10) strmHour << "0"; // two digits for hours if (utc_hour < 10)
{
strmHour << "0"; // two digits for hours
}
strmHour << utc_hour; strmHour << utc_hour;
std::stringstream strmMin; std::stringstream strmMin;
int32_t utc_minute = pt_tm.tm_min; int32_t utc_minute = pt_tm.tm_min;
if (utc_minute < 10) strmMin << "0"; // two digits for minutes if (utc_minute < 10)
{
strmMin << "0"; // two digits for minutes
}
strmMin << utc_minute; strmMin << utc_minute;
if (version == 2) if (version == 2)
@ -421,7 +457,10 @@ std::string Rinex_Printer::getLocalTime()
std::stringstream strm2; std::stringstream strm2;
int32_t utc_seconds = pt_tm.tm_sec; int32_t utc_seconds = pt_tm.tm_sec;
if (utc_seconds < 10) strm2 << "0"; // two digits for seconds if (utc_seconds < 10)
{
strm2 << "0"; // two digits for seconds
}
strm2 << utc_seconds; strm2 << utc_seconds;
line += strm2.str(); line += strm2.str();
line += std::string(1, ' '); line += std::string(1, ' ');
@ -1716,19 +1755,31 @@ void Rinex_Printer::rinex_sbs_header(std::fstream& out)
strYear << utc_year; strYear << utc_year;
std::stringstream strMonth; std::stringstream strMonth;
int32_t utc_month = pt.date().month().as_number(); int32_t utc_month = pt.date().month().as_number();
if (utc_month < 10) strMonth << "0"; // two digits for months if (utc_month < 10)
{
strMonth << "0"; // two digits for months
}
strMonth << utc_month; strMonth << utc_month;
std::stringstream strmDay; std::stringstream strmDay;
int32_t utc_day = pt.date().day().as_number(); int32_t utc_day = pt.date().day().as_number();
if (utc_day < 10) strmDay << "0"; // two digits for days if (utc_day < 10)
{
strmDay << "0"; // two digits for days
}
strmDay << utc_day; strmDay << utc_day;
std::stringstream strmHour; std::stringstream strmHour;
int32_t utc_hour = pt_tm.tm_hour; int32_t utc_hour = pt_tm.tm_hour;
if (utc_hour < 10) strmHour << "0"; // two digits for hours if (utc_hour < 10)
{
strmHour << "0"; // two digits for hours
}
strmHour << utc_hour; strmHour << utc_hour;
std::stringstream strmMin; std::stringstream strmMin;
int32_t utc_minute = pt_tm.tm_min; int32_t utc_minute = pt_tm.tm_min;
if (utc_minute < 10) strmMin << "0"; // two digits for minutes if (utc_minute < 10)
{
strmMin << "0"; // two digits for minutes
}
strmMin << utc_minute; strmMin << utc_minute;
std::string time_str; std::string time_str;
time_str += strmDay.str(); time_str += strmDay.str();
@ -2959,7 +3010,10 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map<int32_t, Gps
if (version == 3) if (version == 3)
{ {
line += satelliteSystem["GPS"]; line += satelliteSystem["GPS"];
if (gps_ephemeris_iter->second.i_satellite_PRN < 10) line += std::string("0"); if (gps_ephemeris_iter->second.i_satellite_PRN < 10)
{
line += std::string("0");
}
line += std::to_string(gps_ephemeris_iter->second.i_satellite_PRN); line += std::to_string(gps_ephemeris_iter->second.i_satellite_PRN);
std::string year(timestring, 0, 4); std::string year(timestring, 0, 4);
line += std::string(1, ' '); line += std::string(1, ' ');
@ -3156,12 +3210,30 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map<int32_t, Gps
if (gps_ephemeris_iter->second.satelliteBlock.at(gps_ephemeris_iter->second.i_satellite_PRN) == "IIA") if (gps_ephemeris_iter->second.satelliteBlock.at(gps_ephemeris_iter->second.i_satellite_PRN) == "IIA")
{ {
// Block II/IIA (Table 20-XI IS-GPS-200E ) // Block II/IIA (Table 20-XI IS-GPS-200E )
if ((gps_ephemeris_iter->second.d_IODC > 239) && (gps_ephemeris_iter->second.d_IODC < 248)) curve_fit_interval = 8; if ((gps_ephemeris_iter->second.d_IODC > 239) && (gps_ephemeris_iter->second.d_IODC < 248))
if (((gps_ephemeris_iter->second.d_IODC > 247) && (gps_ephemeris_iter->second.d_IODC < 256)) || (gps_ephemeris_iter->second.d_IODC == 496)) curve_fit_interval = 14; {
if ((gps_ephemeris_iter->second.d_IODC > 496) && (gps_ephemeris_iter->second.d_IODC < 504)) curve_fit_interval = 26; curve_fit_interval = 8;
if ((gps_ephemeris_iter->second.d_IODC > 503) && (gps_ephemeris_iter->second.d_IODC < 511)) curve_fit_interval = 50; }
if (((gps_ephemeris_iter->second.d_IODC > 751) && (gps_ephemeris_iter->second.d_IODC < 757)) || (gps_ephemeris_iter->second.d_IODC == 511)) curve_fit_interval = 74; if (((gps_ephemeris_iter->second.d_IODC > 247) && (gps_ephemeris_iter->second.d_IODC < 256)) || (gps_ephemeris_iter->second.d_IODC == 496))
if (gps_ephemeris_iter->second.d_IODC == 757) curve_fit_interval = 98; {
curve_fit_interval = 14;
}
if ((gps_ephemeris_iter->second.d_IODC > 496) && (gps_ephemeris_iter->second.d_IODC < 504))
{
curve_fit_interval = 26;
}
if ((gps_ephemeris_iter->second.d_IODC > 503) && (gps_ephemeris_iter->second.d_IODC < 511))
{
curve_fit_interval = 50;
}
if (((gps_ephemeris_iter->second.d_IODC > 751) && (gps_ephemeris_iter->second.d_IODC < 757)) || (gps_ephemeris_iter->second.d_IODC == 511))
{
curve_fit_interval = 74;
}
if (gps_ephemeris_iter->second.d_IODC == 757)
{
curve_fit_interval = 98;
}
} }
if ((gps_ephemeris_iter->second.satelliteBlock.at(gps_ephemeris_iter->second.i_satellite_PRN) == "IIR") || if ((gps_ephemeris_iter->second.satelliteBlock.at(gps_ephemeris_iter->second.i_satellite_PRN) == "IIR") ||
@ -3170,9 +3242,18 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map<int32_t, Gps
(gps_ephemeris_iter->second.satelliteBlock.at(gps_ephemeris_iter->second.i_satellite_PRN) == "IIIA")) (gps_ephemeris_iter->second.satelliteBlock.at(gps_ephemeris_iter->second.i_satellite_PRN) == "IIIA"))
{ {
// Block IIR/IIR-M/IIF/IIIA (Table 20-XII IS-GPS-200E ) // Block IIR/IIR-M/IIF/IIIA (Table 20-XII IS-GPS-200E )
if ((gps_ephemeris_iter->second.d_IODC > 239) && (gps_ephemeris_iter->second.d_IODC < 248)) curve_fit_interval = 8; if ((gps_ephemeris_iter->second.d_IODC > 239) && (gps_ephemeris_iter->second.d_IODC < 248))
if (((gps_ephemeris_iter->second.d_IODC > 247) && (gps_ephemeris_iter->second.d_IODC < 256)) || (gps_ephemeris_iter->second.d_IODC == 496)) curve_fit_interval = 14; {
if (((gps_ephemeris_iter->second.d_IODC > 496) && (gps_ephemeris_iter->second.d_IODC < 504)) || ((gps_ephemeris_iter->second.d_IODC > 1020) && (gps_ephemeris_iter->second.d_IODC < 1024))) curve_fit_interval = 26; curve_fit_interval = 8;
}
if (((gps_ephemeris_iter->second.d_IODC > 247) && (gps_ephemeris_iter->second.d_IODC < 256)) || (gps_ephemeris_iter->second.d_IODC == 496))
{
curve_fit_interval = 14;
}
if (((gps_ephemeris_iter->second.d_IODC > 496) && (gps_ephemeris_iter->second.d_IODC < 504)) || ((gps_ephemeris_iter->second.d_IODC > 1020) && (gps_ephemeris_iter->second.d_IODC < 1024)))
{
curve_fit_interval = 26;
}
} }
line += Rinex_Printer::doub2for(curve_fit_interval, 18, 2); line += Rinex_Printer::doub2for(curve_fit_interval, 18, 2);
line += std::string(1, ' '); line += std::string(1, ' ');
@ -3208,7 +3289,10 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map<int32_t, Gps
std::string minutes(timestring, 11, 2); std::string minutes(timestring, 11, 2);
std::string seconds(timestring, 13, 2); std::string seconds(timestring, 13, 2);
line += satelliteSystem["GPS"]; line += satelliteSystem["GPS"];
if (gps_ephemeris_iter->second.i_satellite_PRN < 10) line += std::string("0"); if (gps_ephemeris_iter->second.i_satellite_PRN < 10)
{
line += std::string("0");
}
line += std::to_string(gps_ephemeris_iter->second.i_satellite_PRN); line += std::to_string(gps_ephemeris_iter->second.i_satellite_PRN);
std::string year(timestring, 0, 4); std::string year(timestring, 0, 4);
line += std::string(1, ' '); line += std::string(1, ' ');
@ -3242,7 +3326,10 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map<int32_t, Gps
{ {
// Toe1: Toe in message type 10, Toe2: Toe in message type 11, // Toe1: Toe in message type 10, Toe2: Toe in message type 11,
fake_cnav_iode = fake_cnav_iode + 1; fake_cnav_iode = fake_cnav_iode + 1;
if (fake_cnav_iode == 240) fake_cnav_iode = 1; if (fake_cnav_iode == 240)
{
fake_cnav_iode = 1;
}
} }
line += Rinex_Printer::doub2for(fake_cnav_iode, 18, 2); line += Rinex_Printer::doub2for(fake_cnav_iode, 18, 2);
@ -3365,7 +3452,10 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map<int32_t, Gal
std::string seconds(timestring, 13, 2); std::string seconds(timestring, 13, 2);
line += satelliteSystem["Galileo"]; line += satelliteSystem["Galileo"];
if (galileo_ephemeris_iter->second.i_satellite_PRN < 10) line += std::string("0"); if (galileo_ephemeris_iter->second.i_satellite_PRN < 10)
{
line += std::string("0");
}
line += std::to_string(galileo_ephemeris_iter->second.i_satellite_PRN); line += std::to_string(galileo_ephemeris_iter->second.i_satellite_PRN);
std::string year(timestring, 0, 4); std::string year(timestring, 0, 4);
line += std::string(1, ' '); line += std::string(1, ' ');
@ -3470,22 +3560,58 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map<int32_t, Gal
line += std::string(1, ' '); line += std::string(1, ' ');
std::string E1B_HS; std::string E1B_HS;
std::string E5B_HS; std::string E5B_HS;
if (galileo_ephemeris_iter->second.E1B_HS_5 == 0) E1B_HS = "00"; if (galileo_ephemeris_iter->second.E1B_HS_5 == 0)
if (galileo_ephemeris_iter->second.E1B_HS_5 == 1) E1B_HS = "01"; {
if (galileo_ephemeris_iter->second.E1B_HS_5 == 2) E1B_HS = "10"; E1B_HS = "00";
if (galileo_ephemeris_iter->second.E1B_HS_5 == 3) E1B_HS = "11"; }
if (galileo_ephemeris_iter->second.E5b_HS_5 == 0) E5B_HS = "00"; if (galileo_ephemeris_iter->second.E1B_HS_5 == 1)
if (galileo_ephemeris_iter->second.E5b_HS_5 == 1) E5B_HS = "01"; {
if (galileo_ephemeris_iter->second.E5b_HS_5 == 2) E5B_HS = "10"; E1B_HS = "01";
if (galileo_ephemeris_iter->second.E5b_HS_5 == 3) E5B_HS = "11"; }
if (galileo_ephemeris_iter->second.E1B_HS_5 == 2)
{
E1B_HS = "10";
}
if (galileo_ephemeris_iter->second.E1B_HS_5 == 3)
{
E1B_HS = "11";
}
if (galileo_ephemeris_iter->second.E5b_HS_5 == 0)
{
E5B_HS = "00";
}
if (galileo_ephemeris_iter->second.E5b_HS_5 == 1)
{
E5B_HS = "01";
}
if (galileo_ephemeris_iter->second.E5b_HS_5 == 2)
{
E5B_HS = "10";
}
if (galileo_ephemeris_iter->second.E5b_HS_5 == 3)
{
E5B_HS = "11";
}
if (E1B_HS == "11") LOG(WARNING) << "Signal Component currently in Test"; if (E1B_HS == "11")
if (E1B_HS == "10") LOG(WARNING) << "Signal will be out of service"; {
if (E1B_HS == "01") LOG(WARNING) << "Signal out of service"; LOG(WARNING) << "Signal Component currently in Test";
}
if (E1B_HS == "10")
{
LOG(WARNING) << "Signal will be out of service";
}
if (E1B_HS == "01")
{
LOG(WARNING) << "Signal out of service";
}
E1B_HS = "00"; // *************** CHANGE THIS WHEN GALILEO SIGNAL IS VALID E1B_HS = "00"; // *************** CHANGE THIS WHEN GALILEO SIGNAL IS VALID
std::string E1B_DVS = std::to_string(galileo_ephemeris_iter->second.E1B_DVS_5); std::string E1B_DVS = std::to_string(galileo_ephemeris_iter->second.E1B_DVS_5);
if (E1B_DVS == "1") LOG(WARNING) << "Navigation data without guarantee"; if (E1B_DVS == "1")
{
LOG(WARNING) << "Navigation data without guarantee";
}
E1B_DVS = "0"; // *************** CHANGE THIS WHEN GALILEO SIGNAL IS VALID E1B_DVS = "0"; // *************** CHANGE THIS WHEN GALILEO SIGNAL IS VALID
std::string SVhealth_str = E5B_HS + std::to_string(galileo_ephemeris_iter->second.E5b_DVS_5) + "11" + "1" + E1B_DVS + E1B_HS + std::to_string(galileo_ephemeris_iter->second.E1B_DVS_5); std::string SVhealth_str = E5B_HS + std::to_string(galileo_ephemeris_iter->second.E5b_DVS_5) + "11" + "1" + E1B_DVS + E1B_HS + std::to_string(galileo_ephemeris_iter->second.E1B_DVS_5);
@ -3607,7 +3733,10 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map<int32_t, Glo
if (version == 3) if (version == 3)
{ {
line += satelliteSystem["GLONASS"]; line += satelliteSystem["GLONASS"];
if (glonass_gnav_ephemeris_iter->second.i_satellite_PRN < 10) line += std::string("0"); if (glonass_gnav_ephemeris_iter->second.i_satellite_PRN < 10)
{
line += std::string("0");
}
line += std::to_string(glonass_gnav_ephemeris_iter->second.i_satellite_PRN); line += std::to_string(glonass_gnav_ephemeris_iter->second.i_satellite_PRN);
std::string year(timestring, 0, 4); std::string year(timestring, 0, 4);
line += std::string(1, ' '); line += std::string(1, ' ');
@ -3773,7 +3902,10 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map<int32_t, Bei
std::string seconds(timestring, 13, 2); std::string seconds(timestring, 13, 2);
line += satelliteSystem["Beidou"]; line += satelliteSystem["Beidou"];
if (bds_ephemeris_iter->second.i_satellite_PRN < 10) line += std::string("0"); if (bds_ephemeris_iter->second.i_satellite_PRN < 10)
{
line += std::string("0");
}
line += std::to_string(bds_ephemeris_iter->second.i_satellite_PRN); line += std::to_string(bds_ephemeris_iter->second.i_satellite_PRN);
std::string year(timestring, 0, 4); std::string year(timestring, 0, 4);
line += std::string(1, ' '); line += std::string(1, ' ');
@ -4009,7 +4141,10 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Glonass_Gnav_Ephem
line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property
line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property
//line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property
if (gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); if (gnss_sdr_version.length() > 20)
{
gnss_sdr_version.resize(9, ' ');
}
line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); line += Rinex_Printer::leftJustify(gnss_sdr_version, 20);
line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20);
lengthCheck(line); lengthCheck(line);
@ -4332,7 +4467,10 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps
line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property
line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property
//line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property
if (gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); if (gnss_sdr_version.length() > 20)
{
gnss_sdr_version.resize(9, ' ');
}
line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); line += Rinex_Printer::leftJustify(gnss_sdr_version, 20);
line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20);
lengthCheck(line); lengthCheck(line);
@ -4684,7 +4822,10 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_CNAV_Ephemeris
line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property
line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property
//line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property
if (gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); if (gnss_sdr_version.length() > 20)
{
gnss_sdr_version.resize(9, ' ');
}
line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); line += Rinex_Printer::leftJustify(gnss_sdr_version, 20);
line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20);
lengthCheck(line); lengthCheck(line);
@ -4990,7 +5131,10 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Galileo_Ephemeris&
line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property
line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property
//line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property
if (gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); if (gnss_sdr_version.length() > 20)
{
gnss_sdr_version.resize(9, ' ');
}
line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); line += Rinex_Printer::leftJustify(gnss_sdr_version, 20);
line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20);
lengthCheck(line); lengthCheck(line);
@ -5310,7 +5454,10 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& eph
line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property
line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property
//line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property
if (gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); if (gnss_sdr_version.length() > 20)
{
gnss_sdr_version.resize(9, ' ');
}
line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); line += Rinex_Printer::leftJustify(gnss_sdr_version, 20);
line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20);
lengthCheck(line); lengthCheck(line);
@ -5564,7 +5711,10 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_CNAV_Ephemeris
line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property
line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property
//line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property
if (gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); if (gnss_sdr_version.length() > 20)
{
gnss_sdr_version.resize(9, ' ');
}
line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); line += Rinex_Printer::leftJustify(gnss_sdr_version, 20);
line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20);
lengthCheck(line); lengthCheck(line);
@ -5812,7 +5962,10 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& eph
line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property
line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property
//line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property
if (gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); if (gnss_sdr_version.length() > 20)
{
gnss_sdr_version.resize(9, ' ');
}
line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); line += Rinex_Printer::leftJustify(gnss_sdr_version, 20);
line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20);
lengthCheck(line); lengthCheck(line);
@ -6090,7 +6243,10 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps
line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property
line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property
//line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property
if (gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); if (gnss_sdr_version.length() > 20)
{
gnss_sdr_version.resize(9, ' ');
}
line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); line += Rinex_Printer::leftJustify(gnss_sdr_version, 20);
line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20);
lengthCheck(line); lengthCheck(line);
@ -6433,7 +6589,10 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_CNAV_Ephemeris
line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property
line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property
//line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property
if (gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); if (gnss_sdr_version.length() > 20)
{
gnss_sdr_version.resize(9, ' ');
}
line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); line += Rinex_Printer::leftJustify(gnss_sdr_version, 20);
line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20);
lengthCheck(line); lengthCheck(line);
@ -6748,7 +6907,10 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Galileo_Ephemeris&
line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property
line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property
//line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property
if (gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); if (gnss_sdr_version.length() > 20)
{
gnss_sdr_version.resize(9, ' ');
}
line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); line += Rinex_Printer::leftJustify(gnss_sdr_version, 20);
line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20);
lengthCheck(line); lengthCheck(line);
@ -7012,7 +7174,10 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps
line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property
line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property
//line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property
if (gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); if (gnss_sdr_version.length() > 20)
{
gnss_sdr_version.resize(9, ' ');
}
line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); line += Rinex_Printer::leftJustify(gnss_sdr_version, 20);
line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20);
lengthCheck(line); lengthCheck(line);
@ -7291,7 +7456,10 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Beidou_Dnav_Epheme
line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property
line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property
//line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property
if (gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); if (gnss_sdr_version.length() > 20)
{
gnss_sdr_version.resize(9, ' ');
}
line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); line += Rinex_Printer::leftJustify(gnss_sdr_version, 20);
line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20);
lengthCheck(line); lengthCheck(line);
@ -7779,7 +7947,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
observables_iter++) observables_iter++)
{ {
line += satelliteSystem["GLONASS"]; line += satelliteSystem["GLONASS"];
if (static_cast<int32_t>(observables_iter->second.PRN) < 10) line += std::string(1, '0'); if (static_cast<int32_t>(observables_iter->second.PRN) < 10)
{
line += std::string(1, '0');
}
line += std::to_string(static_cast<int32_t>(observables_iter->second.PRN)); line += std::to_string(static_cast<int32_t>(observables_iter->second.PRN));
} }
// Receiver clock offset (optional) // Receiver clock offset (optional)
@ -7837,7 +8008,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1); lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
//GLONASS L1 SIGNAL STRENGTH //GLONASS L1 SIGNAL STRENGTH
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14);
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
} }
@ -7893,7 +8067,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
std::string lineObs; std::string lineObs;
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["GLONASS"]; lineObs += satelliteSystem["GLONASS"];
if (static_cast<int32_t>(observables_iter->second.PRN) < 10) lineObs += std::string(1, '0'); if (static_cast<int32_t>(observables_iter->second.PRN) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(observables_iter->second.PRN)); lineObs += std::to_string(static_cast<int32_t>(observables_iter->second.PRN));
//lineObs += std::string(2, ' '); //lineObs += std::string(2, ' ');
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Pseudorange_m, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Pseudorange_m, 3), 14);
@ -7941,7 +8118,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
//GLONASS L1 SIGNAL STRENGTH //GLONASS L1 SIGNAL STRENGTH
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14);
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
} }
@ -8104,7 +8284,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
observables_iter++) observables_iter++)
{ {
line += satelliteSystem["GPS"]; line += satelliteSystem["GPS"];
if (static_cast<int32_t>(observables_iter->second.PRN) < 10) line += std::string(1, '0'); if (static_cast<int32_t>(observables_iter->second.PRN) < 10)
{
line += std::string(1, '0');
}
line += std::to_string(static_cast<int32_t>(observables_iter->second.PRN)); line += std::to_string(static_cast<int32_t>(observables_iter->second.PRN));
} }
// Add list of GLONASS L1 satellites // Add list of GLONASS L1 satellites
@ -8113,7 +8296,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
observables_iter++) observables_iter++)
{ {
line += satelliteSystem["GLONASS"]; line += satelliteSystem["GLONASS"];
if (static_cast<int32_t>(observables_iter->second.PRN) < 10) line += std::string(1, '0'); if (static_cast<int32_t>(observables_iter->second.PRN) < 10)
{
line += std::string(1, '0');
}
line += std::to_string(static_cast<int32_t>(observables_iter->second.PRN)); line += std::to_string(static_cast<int32_t>(observables_iter->second.PRN));
} }
// Add list of GLONASS L2 satellites // Add list of GLONASS L2 satellites
@ -8122,7 +8308,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
observables_iter++) observables_iter++)
{ {
line += satelliteSystem["GLONASS"]; line += satelliteSystem["GLONASS"];
if (static_cast<int32_t>(observables_iter->second.PRN) < 10) line += std::string(1, '0'); if (static_cast<int32_t>(observables_iter->second.PRN) < 10)
{
line += std::string(1, '0');
}
line += std::to_string(static_cast<int32_t>(observables_iter->second.PRN)); line += std::to_string(static_cast<int32_t>(observables_iter->second.PRN));
} }
} }
@ -8143,9 +8332,18 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
if (version == 3) if (version == 3)
{ {
// Specify system only if in version 3 // Specify system only if in version 3
if (s == "G") lineObs += satelliteSystem["GPS"]; if (s == "G")
if (s == "R") lineObs += satelliteSystem["GLONASS"]; // should not happen {
if (static_cast<int32_t>(observables_iter->second.PRN) < 10) lineObs += std::string(1, '0'); lineObs += satelliteSystem["GPS"];
}
if (s == "R")
{
lineObs += satelliteSystem["GLONASS"]; // should not happen
}
if (static_cast<int32_t>(observables_iter->second.PRN) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(observables_iter->second.PRN)); lineObs += std::to_string(static_cast<int32_t>(observables_iter->second.PRN));
} }
@ -8194,7 +8392,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
// SIGNAL STRENGTH // SIGNAL STRENGTH
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14);
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
@ -8207,7 +8408,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
if (version == 3) if (version == 3)
{ {
lineObs += satelliteSystem["GLONASS"]; lineObs += satelliteSystem["GLONASS"];
if (static_cast<int32_t>(*it) < 10) lineObs += std::string(1, '0'); if (static_cast<int32_t>(*it) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(*it)); lineObs += std::to_string(static_cast<int32_t>(*it));
} }
ret = total_glo_map.equal_range(*it); ret = total_glo_map.equal_range(*it);
@ -8260,7 +8464,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14);
} }
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
} }
@ -8386,9 +8593,18 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g
s.assign(1, observables_iter->second.System); s.assign(1, observables_iter->second.System);
// Specify system only if in version 3 // Specify system only if in version 3
if (s == "G") lineObs += satelliteSystem["GPS"]; if (s == "G")
if (s == "R") lineObs += satelliteSystem["GLONASS"]; // should not happen {
if (static_cast<int32_t>(observables_iter->second.PRN) < 10) lineObs += std::string(1, '0'); lineObs += satelliteSystem["GPS"];
}
if (s == "R")
{
lineObs += satelliteSystem["GLONASS"]; // should not happen
}
if (static_cast<int32_t>(observables_iter->second.PRN) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(observables_iter->second.PRN)); lineObs += std::to_string(static_cast<int32_t>(observables_iter->second.PRN));
// Pseudorange Measurements // Pseudorange Measurements
@ -8436,7 +8652,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g
// SIGNAL STRENGTH // SIGNAL STRENGTH
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14);
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
@ -8447,7 +8666,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g
{ {
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["GLONASS"]; lineObs += satelliteSystem["GLONASS"];
if (static_cast<int32_t>(*it) < 10) lineObs += std::string(1, '0'); if (static_cast<int32_t>(*it) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(*it)); lineObs += std::to_string(static_cast<int32_t>(*it));
ret = total_glo_map.equal_range(*it); ret = total_glo_map.equal_range(*it);
@ -8500,7 +8722,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g
lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14);
} }
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
} }
@ -8626,9 +8851,18 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga
lineObs.clear(); lineObs.clear();
s.assign(1, observables_iter->second.System); s.assign(1, observables_iter->second.System);
if (s == "E") lineObs += satelliteSystem["Galileo"]; if (s == "E")
if (s == "R") lineObs += satelliteSystem["GLONASS"]; // should not happen {
if (static_cast<int32_t>(observables_iter->second.PRN) < 10) lineObs += std::string(1, '0'); lineObs += satelliteSystem["Galileo"];
}
if (s == "R")
{
lineObs += satelliteSystem["GLONASS"]; // should not happen
}
if (static_cast<int32_t>(observables_iter->second.PRN) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(observables_iter->second.PRN)); lineObs += std::to_string(static_cast<int32_t>(observables_iter->second.PRN));
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Pseudorange_m, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Pseudorange_m, 3), 14);
@ -8674,7 +8908,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga
// SIGNAL STRENGTH // SIGNAL STRENGTH
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14);
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
@ -8685,7 +8922,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga
{ {
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["Galileo"]; lineObs += satelliteSystem["Galileo"];
if (static_cast<int32_t>(*it) < 10) lineObs += std::string(1, '0'); if (static_cast<int32_t>(*it) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(*it)); lineObs += std::to_string(static_cast<int32_t>(*it));
ret = total_glo_map.equal_range(*it); ret = total_glo_map.equal_range(*it);
for (auto iter = ret.first; iter != ret.second; ++iter) for (auto iter = ret.first; iter != ret.second; ++iter)
@ -8735,7 +8975,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga
lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14);
} }
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
} }
@ -8812,7 +9055,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, c
observables_iter++) observables_iter++)
{ {
line += satelliteSystem["GPS"]; line += satelliteSystem["GPS"];
if (static_cast<int32_t>(observables_iter->second.PRN) < 10) line += std::string(1, '0'); if (static_cast<int32_t>(observables_iter->second.PRN) < 10)
{
line += std::string(1, '0');
}
line += std::to_string(static_cast<int32_t>(observables_iter->second.PRN)); line += std::to_string(static_cast<int32_t>(observables_iter->second.PRN));
} }
// Receiver clock offset (optional) // Receiver clock offset (optional)
@ -8870,7 +9116,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, c
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1); lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
//GPS L1 SIGNAL STRENGTH //GPS L1 SIGNAL STRENGTH
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14);
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
} }
@ -8927,7 +9176,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, c
std::string lineObs; std::string lineObs;
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["GPS"]; lineObs += satelliteSystem["GPS"];
if (static_cast<int32_t>(observables_iter->second.PRN) < 10) lineObs += std::string(1, '0'); if (static_cast<int32_t>(observables_iter->second.PRN) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(observables_iter->second.PRN)); lineObs += std::to_string(static_cast<int32_t>(observables_iter->second.PRN));
//lineObs += std::string(2, ' '); //lineObs += std::string(2, ' ');
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Pseudorange_m, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Pseudorange_m, 3), 14);
@ -8975,7 +9227,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, c
//GPS L1 SIGNAL STRENGTH //GPS L1 SIGNAL STRENGTH
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14);
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
} }
@ -9048,7 +9303,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& e
std::string lineObs; std::string lineObs;
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["GPS"]; lineObs += satelliteSystem["GPS"];
if (static_cast<int32_t>(observables_iter->second.PRN) < 10) lineObs += std::string(1, '0'); if (static_cast<int32_t>(observables_iter->second.PRN) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(observables_iter->second.PRN)); lineObs += std::to_string(static_cast<int32_t>(observables_iter->second.PRN));
//lineObs += std::string(2, ' '); //lineObs += std::string(2, ' ');
//GPS L2 PSEUDORANGE //GPS L2 PSEUDORANGE
@ -9097,7 +9355,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& e
//GPS L2 SIGNAL STRENGTH //GPS L2 SIGNAL STRENGTH
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14);
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
} }
@ -9266,7 +9527,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, c
{ {
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["GPS"]; lineObs += satelliteSystem["GPS"];
if (static_cast<int32_t>(*it) < 10) lineObs += std::string(1, '0'); if (static_cast<int32_t>(*it) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(*it)); lineObs += std::to_string(static_cast<int32_t>(*it));
ret = total_mmap.equal_range(*it); ret = total_mmap.equal_range(*it);
for (auto iter = ret.first; iter != ret.second; ++iter) for (auto iter = ret.first; iter != ret.second; ++iter)
@ -9316,7 +9580,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, c
lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14);
} }
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
} }
@ -9506,7 +9773,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ep
{ {
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["Galileo"]; lineObs += satelliteSystem["Galileo"];
if (static_cast<int32_t>(*it) < 10) lineObs += std::string(1, '0'); if (static_cast<int32_t>(*it) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(*it)); lineObs += std::to_string(static_cast<int32_t>(*it));
ret = total_map.equal_range(*it); ret = total_map.equal_range(*it);
for (auto iter = ret.first; iter != ret.second; ++iter) for (auto iter = ret.first; iter != ret.second; ++iter)
@ -9556,7 +9826,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ep
lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14);
} }
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
} }
@ -9701,9 +9974,18 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
lineObs.clear(); lineObs.clear();
s.assign(1, observables_iter->second.System); s.assign(1, observables_iter->second.System);
if (s == "G") lineObs += satelliteSystem["GPS"]; if (s == "G")
if (s == "E") lineObs += satelliteSystem["Galileo"]; // should not happen {
if (static_cast<int32_t>(observables_iter->second.PRN) < 10) lineObs += std::string(1, '0'); lineObs += satelliteSystem["GPS"];
}
if (s == "E")
{
lineObs += satelliteSystem["Galileo"]; // should not happen
}
if (static_cast<int32_t>(observables_iter->second.PRN) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(observables_iter->second.PRN)); lineObs += std::to_string(static_cast<int32_t>(observables_iter->second.PRN));
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Pseudorange_m, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Pseudorange_m, 3), 14);
@ -9749,7 +10031,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
// SIGNAL STRENGTH // SIGNAL STRENGTH
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.CN0_dB_hz, 3), 14);
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
@ -9760,7 +10045,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
{ {
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["Galileo"]; lineObs += satelliteSystem["Galileo"];
if (static_cast<int32_t>(*it) < 10) lineObs += std::string(1, '0'); if (static_cast<int32_t>(*it) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(*it)); lineObs += std::to_string(static_cast<int32_t>(*it));
ret = total_gal_map.equal_range(*it); ret = total_gal_map.equal_range(*it);
for (auto iter = ret.first; iter != ret.second; ++iter) for (auto iter = ret.first; iter != ret.second; ++iter)
@ -9810,7 +10098,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14);
} }
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
} }
@ -9989,7 +10280,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& e
{ {
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["GPS"]; lineObs += satelliteSystem["GPS"];
if (static_cast<int32_t>(*it) < 10) lineObs += std::string(1, '0'); if (static_cast<int32_t>(*it) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(*it)); lineObs += std::to_string(static_cast<int32_t>(*it));
ret = total_gps_map.equal_range(*it); ret = total_gps_map.equal_range(*it);
for (auto iter = ret.first; iter != ret.second; ++iter) for (auto iter = ret.first; iter != ret.second; ++iter)
@ -10048,7 +10342,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& e
{ {
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["Galileo"]; lineObs += satelliteSystem["Galileo"];
if (static_cast<int32_t>(*it) < 10) lineObs += std::string(1, '0'); if (static_cast<int32_t>(*it) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(*it)); lineObs += std::to_string(static_cast<int32_t>(*it));
ret = total_gal_map.equal_range(*it); ret = total_gal_map.equal_range(*it);
for (auto iter = ret.first; iter != ret.second; ++iter) for (auto iter = ret.first; iter != ret.second; ++iter)
@ -10298,7 +10595,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
{ {
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["GPS"]; lineObs += satelliteSystem["GPS"];
if (static_cast<int32_t>(*it) < 10) lineObs += std::string(1, '0'); if (static_cast<int32_t>(*it) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(*it)); lineObs += std::to_string(static_cast<int32_t>(*it));
ret = total_gps_map.equal_range(*it); ret = total_gps_map.equal_range(*it);
for (auto iter = ret.first; iter != ret.second; ++iter) for (auto iter = ret.first; iter != ret.second; ++iter)
@ -10357,7 +10657,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
{ {
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["Galileo"]; lineObs += satelliteSystem["Galileo"];
if (static_cast<int32_t>(*it) < 10) lineObs += std::string(1, '0'); if (static_cast<int32_t>(*it) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(*it)); lineObs += std::to_string(static_cast<int32_t>(*it));
ret = total_gal_map.equal_range(*it); ret = total_gal_map.equal_range(*it);
for (auto iter = ret.first; iter != ret.second; ++iter) for (auto iter = ret.first; iter != ret.second; ++iter)
@ -10539,7 +10842,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Beidou_Dnav_Ephemeris
{ {
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["Beidou"]; lineObs += satelliteSystem["Beidou"];
if (static_cast<int32_t>(*it) < 10) lineObs += std::string(1, '0'); if (static_cast<int32_t>(*it) < 10)
{
lineObs += std::string(1, '0');
}
lineObs += std::to_string(static_cast<int32_t>(*it)); lineObs += std::to_string(static_cast<int32_t>(*it));
ret = total_map.equal_range(*it); ret = total_map.equal_range(*it);
for (auto iter = ret.first; iter != ret.second; ++iter) for (auto iter = ret.first; iter != ret.second; ++iter)
@ -10577,7 +10883,10 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Beidou_Dnav_Ephemeris
lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14);
} }
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80)
{
lineObs += std::string(80 - lineObs.size(), ' ');
}
out << lineObs << std::endl; out << lineObs << std::endl;
} }
} }

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

@ -676,8 +676,15 @@ inline std::string Rinex_Printer::doub2for(const double& d,
short exponentLength = expLen; short exponentLength = expLen;
/* Validate the assumptions regarding the input arguments */ /* Validate the assumptions regarding the input arguments */
if (exponentLength < 0) exponentLength = 1; if (exponentLength < 0)
if (exponentLength > 3 && checkSwitch) exponentLength = 3; {
exponentLength = 1;
}
if (exponentLength > 3 && checkSwitch)
{
exponentLength = 3;
}
std::string toReturn = doub2sci(d, length, exponentLength, true, checkSwitch); std::string toReturn = doub2sci(d, length, exponentLength, true, checkSwitch);
sci2for(toReturn, 0, length, exponentLength, checkSwitch); sci2for(toReturn, 0, length, exponentLength, checkSwitch);
@ -696,8 +703,15 @@ inline std::string Rinex_Printer::doub2sci(const double& d,
short exponentLength = expLen; short exponentLength = expLen;
/* Validate the assumptions regarding the input arguments */ /* Validate the assumptions regarding the input arguments */
if (exponentLength < 0) exponentLength = 1; if (exponentLength < 0)
if (exponentLength > 3 && checkSwitch) exponentLength = 3; {
exponentLength = 1;
}
if (exponentLength > 3 && checkSwitch)
{
exponentLength = 3;
}
std::stringstream c; std::stringstream c;
c.setf(std::ios::scientific, std::ios::floatfield); c.setf(std::ios::scientific, std::ios::floatfield);
@ -709,7 +723,10 @@ inline std::string Rinex_Printer::doub2sci(const double& d,
// an extra -1 for '-' or ' ' if it's positive or negative // an extra -1 for '-' or ' ' if it's positive or negative
int expSize = 0; int expSize = 0;
if (showSign) if (showSign)
{
expSize = 1; expSize = 1;
}
c.precision(length - 3 - exponentLength - 1 - expSize); c.precision(length - 3 - exponentLength - 1 - expSize);
c << d; c << d;
c >> toReturn; c >> toReturn;
@ -751,8 +768,10 @@ inline std::string& Rinex_Printer::sci2for(std::string& aStr,
aStr[idx - 1] = '.'; aStr[idx - 1] = '.';
// Only add one to the exponent if the number is non-zero // Only add one to the exponent if the number is non-zero
if (asDouble(aStr.substr(startPos, length)) != 0.0) if (asDouble(aStr.substr(startPos, length)) != 0.0)
{
expAdd = 1; expAdd = 1;
} }
}
idx = aStr.find('e', startPos); idx = aStr.find('e', startPos);
if (idx == std::string::npos) if (idx == std::string::npos)
@ -766,9 +785,13 @@ inline std::string& Rinex_Printer::sci2for(std::string& aStr,
// Change the exponent character to D normally, or E of checkSwitch is false. // Change the exponent character to D normally, or E of checkSwitch is false.
if (checkSwitch) if (checkSwitch)
{
aStr[idx] = 'D'; aStr[idx] = 'D';
}
else else
{
aStr[idx] = 'E'; aStr[idx] = 'E';
}
// Change the exponent itself // Change the exponent itself
if (redoexp) if (redoexp)
@ -784,7 +807,10 @@ inline std::string& Rinex_Printer::sci2for(std::string& 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');
} }

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

@ -201,7 +201,10 @@ Rtcm_Printer::~Rtcm_Printer()
} }
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";
}
} }
} }
try try
@ -368,9 +371,15 @@ int Rtcm_Printer::init_serial(const std::string& serial_device)
int64_t PARITY; int64_t PARITY;
fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_CLOEXEC); fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_CLOEXEC);
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;

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

@ -76,7 +76,10 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
count_valid_position = 0; count_valid_position = 0;
this->set_averaging_flag(false); this->set_averaging_flag(false);
rtk_ = rtk; rtk_ = rtk;
for (double &i : dop_) i = 0.0; for (double &i : dop_)
{
i = 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}; 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};
ssat_t ssat0 = {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}}}, {{}, {}}}; ssat_t ssat0 = {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}}}, {{}, {}}};
for (auto &i : pvt_ssat) for (auto &i : pvt_ssat)
@ -505,7 +508,10 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
} }
} }
} }
if (band1 == true and band2 == true) gps_dual_band = true; if (band1 == true and band2 == true)
{
gps_dual_band = true;
}
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();
@ -864,7 +870,10 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
} }
} }
if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_.data()); if (index_aux > 0)
{
dops(index_aux, azel.data(), 0.0, dop_.data());
}
this->set_valid_position(true); this->set_valid_position(true);
arma::vec rx_position_and_time(4); arma::vec rx_position_and_time(4);
rx_position_and_time(0) = pvt_sol.rr[0]; // [m] rx_position_and_time(0) = pvt_sol.rr[0]; // [m]
@ -893,7 +902,10 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
if (ground_speed_ms >= 1.0) if (ground_speed_ms >= 1.0)
{ {
new_cog = atan2(enuv[0], enuv[1]) * R2D; new_cog = atan2(enuv[0], enuv[1]) * R2D;
if (new_cog < 0.0) new_cog += 360.0; if (new_cog < 0.0)
{
new_cog += 360.0;
}
this->set_course_over_ground(new_cog); this->set_course_over_ground(new_cog);
} }

5
src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc
View File

@ -68,7 +68,10 @@ BeidouB1iPcpsAcquisition::BeidouB1iPcpsAcquisition(
blocking_ = configuration_->property(role + ".blocking", true); blocking_ = configuration_->property(role + ".blocking", true);
acq_parameters.blocking = blocking_; acq_parameters.blocking = blocking_;
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;
}
acq_parameters.doppler_max = doppler_max_; acq_parameters.doppler_max = doppler_max_;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1);
acq_parameters.sampled_ms = sampled_ms_; acq_parameters.sampled_ms = sampled_ms_;

2
src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h
View File

@ -41,8 +41,8 @@
#include <gnuradio/blocks/float_to_complex.h> #include <gnuradio/blocks/float_to_complex.h>
#include <gnuradio/blocks/stream_to_vector.h> #include <gnuradio/blocks/stream_to_vector.h>
#include <volk_gnsssdr/volk_gnsssdr.h> #include <volk_gnsssdr/volk_gnsssdr.h>
#include <string>
#include <cstdint> #include <cstdint>
#include <string>
class ConfigurationInterface; class ConfigurationInterface;

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

@ -61,7 +61,10 @@ GalileoE1Pcps8msAmbiguousAcquisition::GalileoE1Pcps8msAmbiguousAcquisition(
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
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)
@ -145,7 +148,10 @@ void GalileoE1Pcps8msAmbiguousAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + std::to_string(channel_) + ".pfa", 0.0); float pfa = configuration_->property(role_ + std::to_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)
{ {

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

@ -62,7 +62,10 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition(
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_parameters_.fs_in = fs_in_; acq_parameters_.fs_in = fs_in_;
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;
}
acq_parameters_.doppler_max = doppler_max_; acq_parameters_.doppler_max = doppler_max_;
acq_parameters_.ms_per_code = 4; acq_parameters_.ms_per_code = 4;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", acq_parameters_.ms_per_code); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", acq_parameters_.ms_per_code);
@ -187,7 +190,10 @@ void GalileoE1PcpsAmbiguousAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + std::to_string(channel_) + ".pfa", 0.0); float pfa = configuration_->property(role_ + std::to_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)
{ {

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

@ -60,7 +60,10 @@ GalileoE1PcpsCccwsrAmbiguousAcquisition::GalileoE1PcpsCccwsrAmbiguousAcquisition
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
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)

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

@ -61,7 +61,10 @@ GalileoE1PcpsQuickSyncAmbiguousAcquisition::GalileoE1PcpsQuickSyncAmbiguousAcqui
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
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) -----------------*/
@ -179,7 +182,10 @@ void GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + std::to_string(channel_) + ".pfa", 0.0); float pfa = configuration_->property(role_ + std::to_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)
{ {

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

@ -61,7 +61,10 @@ GalileoE1PcpsTongAmbiguousAcquisition::GalileoE1PcpsTongAmbiguousAcquisition(
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
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)
@ -149,7 +152,10 @@ void GalileoE1PcpsTongAmbiguousAcquisition::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + std::to_string(channel_) + ".pfa", 0.0); float pfa = configuration_->property(role_ + std::to_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)
{ {

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

@ -66,7 +66,10 @@ GalileoE5aNoncoherentIQAcquisitionCaf::GalileoE5aNoncoherentIQAcquisitionCaf(
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
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);
@ -155,7 +158,10 @@ void GalileoE5aNoncoherentIQAcquisitionCaf::set_threshold(float threshold)
{ {
float pfa = configuration_->property(role_ + std::to_string(channel_) + ".pfa", 0.0); float pfa = configuration_->property(role_ + std::to_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)
{ {

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

@ -70,7 +70,10 @@ GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* con
acq_parameters_.dump = dump_; acq_parameters_.dump = dump_;
acq_parameters_.dump_channel = configuration_->property(role + ".dump_channel", 0); acq_parameters_.dump_channel = configuration_->property(role + ".dump_channel", 0);
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;
}
acq_parameters_.doppler_max = doppler_max_; acq_parameters_.doppler_max = doppler_max_;
sampled_ms_ = 1; sampled_ms_ = 1;
max_dwells_ = configuration_->property(role + ".max_dwells", 1); max_dwells_ = configuration_->property(role + ".max_dwells", 1);

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

@ -71,7 +71,10 @@ GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition(
blocking_ = configuration_->property(role + ".blocking", true); blocking_ = configuration_->property(role + ".blocking", true);
acq_parameters.blocking = blocking_; acq_parameters.blocking = blocking_;
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;
}
acq_parameters.doppler_max = doppler_max_; acq_parameters.doppler_max = doppler_max_;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1);
acq_parameters.sampled_ms = sampled_ms_; acq_parameters.sampled_ms = sampled_ms_;

5
src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc
View File

@ -70,7 +70,10 @@ GlonassL2CaPcpsAcquisition::GlonassL2CaPcpsAcquisition(
blocking_ = configuration_->property(role + ".blocking", true); blocking_ = configuration_->property(role + ".blocking", true);
acq_parameters.blocking = blocking_; acq_parameters.blocking = blocking_;
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;
}
acq_parameters.doppler_max = doppler_max_; acq_parameters.doppler_max = 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);

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

@ -70,7 +70,10 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition(
blocking_ = configuration_->property(role + ".blocking", true); blocking_ = configuration_->property(role + ".blocking", true);
acq_parameters_.blocking = blocking_; acq_parameters_.blocking = blocking_;
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;
}
acq_parameters_.doppler_max = doppler_max_; acq_parameters_.doppler_max = doppler_max_;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1);
acq_parameters_.sampled_ms = sampled_ms_; acq_parameters_.sampled_ms = sampled_ms_;

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

@ -67,7 +67,10 @@ GpsL1CaPcpsAcquisitionFineDoppler::GpsL1CaPcpsAcquisitionFineDoppler(
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
acq_parameters.dump_filename = dump_filename_; acq_parameters.dump_filename = 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;
}
acq_parameters.doppler_max = doppler_max_; acq_parameters.doppler_max = doppler_max_;
sampled_ms_ = configuration->property(role + ".coherent_integration_time_ms", 1); sampled_ms_ = configuration->property(role + ".coherent_integration_time_ms", 1);
acq_parameters.sampled_ms = sampled_ms_; acq_parameters.sampled_ms = sampled_ms_;

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

@ -61,7 +61,10 @@ GpsL1CaPcpsAssistedAcquisition::GpsL1CaPcpsAssistedAcquisition(
fs_in_ = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in_ = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
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);

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

@ -61,7 +61,10 @@ GpsL1CaPcpsQuickSyncAcquisition::GpsL1CaPcpsQuickSyncAcquisition(
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
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 -------------------------

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

@ -61,7 +61,10 @@ GpsL1CaPcpsTongAcquisition::GpsL1CaPcpsTongAcquisition(
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
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);

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

@ -68,7 +68,10 @@ GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition(
blocking_ = configuration_->property(role + ".blocking", true); blocking_ = configuration_->property(role + ".blocking", true);
acq_parameters_.blocking = blocking_; acq_parameters_.blocking = blocking_;
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;
}
acq_parameters_.doppler_max = doppler_max_; acq_parameters_.doppler_max = doppler_max_;
bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false);
acq_parameters_.bit_transition_flag = bit_transition_flag_; acq_parameters_.bit_transition_flag = bit_transition_flag_;

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

@ -69,7 +69,10 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
blocking_ = configuration_->property(role + ".blocking", true); blocking_ = configuration_->property(role + ".blocking", true);
acq_parameters_.blocking = blocking_; acq_parameters_.blocking = blocking_;
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;
}
acq_parameters_.doppler_max = doppler_max_; acq_parameters_.doppler_max = doppler_max_;
bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false);
acq_parameters_.bit_transition_flag = bit_transition_flag_; acq_parameters_.bit_transition_flag = bit_transition_flag_;

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

@ -134,7 +134,7 @@ galileo_pcps_8ms_acquisition_cc::~galileo_pcps_8ms_acquisition_cc()
d_dump_file.close(); d_dump_file.close();
} }
} }
catch (const std::ifstream::failure &e) catch (const std::ofstream::failure &e)
{ {
std::cerr << "Problem closing Acquisition dump file: " << d_dump_filename << '\n'; std::cerr << "Problem closing Acquisition dump file: " << d_dump_filename << '\n';
} }

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

@ -317,7 +317,10 @@ void pcps_acquisition::init()
d_num_doppler_bins = static_cast<uint32_t>(std::ceil(static_cast<double>(static_cast<int32_t>(acq_parameters.doppler_max) - static_cast<int32_t>(-acq_parameters.doppler_max)) / static_cast<double>(d_doppler_step))); d_num_doppler_bins = static_cast<uint32_t>(std::ceil(static_cast<double>(static_cast<int32_t>(acq_parameters.doppler_max) - static_cast<int32_t>(-acq_parameters.doppler_max)) / static_cast<double>(d_doppler_step)));
// Create the carrier Doppler wipeoff signals // Create the carrier Doppler wipeoff signals
if (d_grid_doppler_wipeoffs == nullptr) d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; if (d_grid_doppler_wipeoffs == nullptr)
{
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
}
if (acq_parameters.make_2_steps && (d_grid_doppler_wipeoffs_step_two == nullptr)) if (acq_parameters.make_2_steps && (d_grid_doppler_wipeoffs_step_two == nullptr))
{ {
d_grid_doppler_wipeoffs_step_two = new gr_complex*[d_num_doppler_bins_step2]; d_grid_doppler_wipeoffs_step_two = new gr_complex*[d_num_doppler_bins_step2];
@ -629,7 +632,10 @@ float pcps_acquisition::first_vs_second_peak_statistic(uint32_t& indext, int32_t
{ {
d_tmp_buffer[idx] = 0.0; d_tmp_buffer[idx] = 0.0;
idx++; idx++;
if (idx == static_cast<int32_t>(d_fft_size)) idx = 0; if (idx == static_cast<int32_t>(d_fft_size))
{
idx = 0;
}
} }
while (idx != excludeRangeIndex2); while (idx != excludeRangeIndex2);
@ -842,7 +848,10 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count)
if (d_num_noncoherent_integrations_counter == acq_parameters.max_dwells) if (d_num_noncoherent_integrations_counter == acq_parameters.max_dwells)
{ {
if (d_state != 0) send_negative_acquisition(); if (d_state != 0)
{
send_negative_acquisition();
}
d_state = 0; d_state = 0;
d_active = false; d_active = false;
d_step_two = false; d_step_two = false;

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

@ -313,7 +313,10 @@ double pcps_acquisition_fine_doppler_cc::compute_CAF()
{ {
d_grid_data[index_doppler][idx] = 0.0; d_grid_data[index_doppler][idx] = 0.0;
idx++; idx++;
if (idx == static_cast<int>(d_fft_size)) idx = 0; if (idx == static_cast<int>(d_fft_size))
{
idx = 0;
}
} }
while (idx != excludeRangeIndex2); while (idx != excludeRangeIndex2);

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

@ -392,7 +392,10 @@ 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 += static_cast<uint64_t>(ninput_items[0]); // sample counter d_sample_counter += static_cast<uint64_t>(ninput_items[0]); // sample counter
consume_each(ninput_items[0]); consume_each(ninput_items[0]);
break; break;

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

@ -218,7 +218,10 @@ 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;
@ -545,7 +548,10 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
DLOG(INFO) << "test statistics threshold " << d_threshold; DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "folding factor " << d_folding_factor; DLOG(INFO) << "folding factor " << d_folding_factor;
DLOG(INFO) << "possible delay correlation output"; DLOG(INFO) << "possible delay correlation output";
for (int32_t i = 0; i < static_cast<int32_t>(d_folding_factor); i++) DLOG(INFO) << d_possible_delay[i] << "\t\t\t" << d_corr_output_f[i]; for (int32_t i = 0; i < static_cast<int32_t>(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) << "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) << "magnitude folded " << d_mag; DLOG(INFO) << "magnitude folded " << d_mag;
@ -574,7 +580,10 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
DLOG(INFO) << "test statistics threshold " << d_threshold; DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "folding factor " << d_folding_factor; DLOG(INFO) << "folding factor " << d_folding_factor;
DLOG(INFO) << "possible delay corr output"; DLOG(INFO) << "possible delay corr output";
for (int32_t i = 0; i < static_cast<int32_t>(d_folding_factor); i++) DLOG(INFO) << d_possible_delay[i] << "\t\t\t" << d_corr_output_f[i]; for (int32_t i = 0; i < static_cast<int32_t>(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) << "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) << "magnitude folded " << d_mag; DLOG(INFO) << "magnitude folded " << d_mag;

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

@ -75,14 +75,23 @@ Channel::Channel(ConfigurationInterface* configuration, uint32_t channel, std::s
// 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
uint32_t doppler_step = configuration->property("Acquisition_" + implementation_ + std::to_string(channel_) + ".doppler_step", 0); uint32_t doppler_step = configuration->property("Acquisition_" + implementation_ + std::to_string(channel_) + ".doppler_step", 0);
if (doppler_step == 0) doppler_step = configuration->property("Acquisition_" + implementation_ + ".doppler_step", 500); if (doppler_step == 0)
if (FLAGS_doppler_step != 0) doppler_step = static_cast<uint32_t>(FLAGS_doppler_step); {
doppler_step = configuration->property("Acquisition_" + implementation_ + ".doppler_step", 500);
}
if (FLAGS_doppler_step != 0)
{
doppler_step = static_cast<uint32_t>(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_ + std::to_string(channel_) + ".threshold", 0.0); float threshold = configuration->property("Acquisition_" + implementation_ + std::to_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);

2
src/algorithms/libs/beidou_b1i_signal_processing.cc
View File

@ -58,7 +58,9 @@ void beidou_b1i_code_gen_int(int32_t* _dest, int32_t _prn, uint32_t _chip_shift)
/* A simple error check */ /* A simple error check */
if ((prn_idx < 0) || (prn_idx > 51)) if ((prn_idx < 0) || (prn_idx > 51))
{
return; return;
}
/*for (lcv = 0; lcv < 11; lcv++) /*for (lcv = 0; lcv < 11; lcv++)
{ {

2
src/algorithms/libs/beidou_b1i_signal_processing.h
View File

@ -34,8 +34,8 @@
#define GNSS_SDR_BEIDOU_B1I_SDR_SIGNAL_PROCESSING_H_ #define GNSS_SDR_BEIDOU_B1I_SDR_SIGNAL_PROCESSING_H_
#include <complex> #include <complex>
#include <iostream>
#include <cstdint> #include <cstdint>
#include <iostream>
//! Generates int32_t GPS L1 C/A code for the desired SV ID and code shift //! Generates int32_t GPS L1 C/A code for the desired SV ID and code shift
void beidou_b1i_code_gen_int(int32_t* _dest, int32_t _prn, uint32_t _chip_shift); void beidou_b1i_code_gen_int(int32_t* _dest, int32_t _prn, uint32_t _chip_shift);

21
src/algorithms/libs/geofunctions.cc
View File

@ -304,8 +304,14 @@ arma::vec CTM_to_Euler(const arma::mat &C)
arma::mat CTM(C); arma::mat CTM(C);
arma::vec eul = arma::zeros(3, 1); arma::vec eul = arma::zeros(3, 1);
eul(0) = atan2(CTM(1, 2), CTM(2, 2)); // roll eul(0) = atan2(CTM(1, 2), CTM(2, 2)); // roll
if (CTM(0, 2) < -1.0) CTM(0, 2) = -1.0; if (CTM(0, 2) < -1.0)
if (CTM(0, 2) > 1.0) CTM(0, 2) = 1.0; {
CTM(0, 2) = -1.0;
}
if (CTM(0, 2) > 1.0)
{
CTM(0, 2) = 1.0;
}
eul(1) = -asin(CTM(0, 2)); // pitch eul(1) = -asin(CTM(0, 2)); // pitch
eul(2) = atan2(CTM(0, 1), CTM(0, 0)); // yaw eul(2) = atan2(CTM(0, 1), CTM(0, 0)); // yaw
return eul; return eul;
@ -614,7 +620,10 @@ void cart2utm(const arma::vec &r_eb_e, int zone, arma::vec &r_enu)
// Ellipsoidal latitude, longitude to spherical latitude, longitude // Ellipsoidal latitude, longitude to spherical latitude, longitude
bool neg_geo = false; bool neg_geo = false;
if (B < 0.0) neg_geo = true; if (B < 0.0)
{
neg_geo = true;
}
double Bg_r = fabs(B); double Bg_r = fabs(B);
double res_clensin = clsin(bg, 4, 2.0 * Bg_r); double res_clensin = clsin(bg, 4, 2.0 * Bg_r);
@ -732,10 +741,14 @@ int findUtmZone(double latitude_deg, double longitude_deg)
// Check value bounds // Check value bounds
if ((longitude_deg > 180.0) || (longitude_deg < -180.0)) if ((longitude_deg > 180.0) || (longitude_deg < -180.0))
{
std::cout << "Longitude value exceeds limits (-180:180).\n"; std::cout << "Longitude value exceeds limits (-180:180).\n";
}
if ((latitude_deg > 84.0) || (latitude_deg < -80.0)) if ((latitude_deg > 84.0) || (latitude_deg < -80.0))
{
std::cout << "Latitude value exceeds limits (-80:84).\n"; std::cout << "Latitude value exceeds limits (-80:84).\n";
}
// //
// Find zone // Find zone
@ -769,7 +782,9 @@ int findUtmZone(double latitude_deg, double longitude_deg)
{ {
// Correction for zone 32 // Correction for zone 32
if ((longitude_deg >= 3.0) && (longitude_deg < 12.0)) if ((longitude_deg >= 3.0) && (longitude_deg < 12.0))
{
utmZone = 32; utmZone = 32;
} }
}
return utmZone; return utmZone;
} }

48
src/algorithms/libs/gnss_sdr_flags.cc
View File

@ -69,8 +69,10 @@ DEFINE_double(pll_bw_hz, 0.0, "If defined, bandwidth of the PLL low pass filter,
static bool ValidateC(const char* flagname, const std::string& value) static bool ValidateC(const char* flagname, const std::string& value)
{ {
if (boost::filesystem::exists(value) or value == "-") // value is ok if (boost::filesystem::exists(value) or value == "-")
{ // value is ok
return true; return true;
}
std::cout << "Invalid value for flag -" << flagname << ". The file '" << value << "' does not exist." << std::endl; std::cout << "Invalid value for flag -" << flagname << ". The file '" << value << "' does not exist." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl; std::cout << "GNSS-SDR program ended." << std::endl;
return false; return false;
@ -78,8 +80,10 @@ static bool ValidateC(const char* flagname, const std::string& value)
static bool ValidateConfigFile(const char* flagname, const std::string& value) static bool ValidateConfigFile(const char* flagname, const std::string& value)
{ {
if (boost::filesystem::exists(value) or value == std::string(GNSSSDR_INSTALL_DIR "/share/gnss-sdr/conf/default.conf")) // value is ok if (boost::filesystem::exists(value) or value == std::string(GNSSSDR_INSTALL_DIR "/share/gnss-sdr/conf/default.conf"))
{ // value is ok
return true; return true;
}
std::cout << "Invalid value for flag -" << flagname << ". The file '" << value << "' does not exist." << std::endl; std::cout << "Invalid value for flag -" << flagname << ". The file '" << value << "' does not exist." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl; std::cout << "GNSS-SDR program ended." << std::endl;
return false; return false;
@ -87,8 +91,10 @@ static bool ValidateConfigFile(const char* flagname, const std::string& value)
static bool ValidateS(const char* flagname, const std::string& value) static bool ValidateS(const char* flagname, const std::string& value)
{ {
if (boost::filesystem::exists(value) or value == "-") // value is ok if (boost::filesystem::exists(value) or value == "-")
{ // value is ok
return true; return true;
}
std::cout << "Invalid value for flag -" << flagname << ". The file '" << value << "' does not exist." << std::endl; std::cout << "Invalid value for flag -" << flagname << ". The file '" << value << "' does not exist." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl; std::cout << "GNSS-SDR program ended." << std::endl;
return false; return false;
@ -96,8 +102,10 @@ static bool ValidateS(const char* flagname, const std::string& value)
static bool ValidateSignalSource(const char* flagname, const std::string& value) static bool ValidateSignalSource(const char* flagname, const std::string& value)
{ {
if (boost::filesystem::exists(value) or value == "-") // value is ok if (boost::filesystem::exists(value) or value == "-")
{ // value is ok
return true; return true;
}
std::cout << "Invalid value for flag -" << flagname << ". The file '" << value << "' does not exist." << std::endl; std::cout << "Invalid value for flag -" << flagname << ". The file '" << value << "' does not exist." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl; std::cout << "GNSS-SDR program ended." << std::endl;
return false; return false;
@ -106,8 +114,10 @@ static bool ValidateSignalSource(const char* flagname, const std::string& value)
static bool ValidateDopplerMax(const char* flagname, int32_t value) static bool ValidateDopplerMax(const char* flagname, int32_t value)
{ {
const int32_t max_value = 1000000; const int32_t max_value = 1000000;
if (value >= 0 && value < max_value) // value is ok if (value >= 0 && value < max_value)
{ // value is ok
return true; return true;
}
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " Hz." << std::endl; std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " Hz." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl; std::cout << "GNSS-SDR program ended." << std::endl;
return false; return false;
@ -116,8 +126,10 @@ static bool ValidateDopplerMax(const char* flagname, int32_t value)
static bool ValidateDopplerStep(const char* flagname, int32_t value) static bool ValidateDopplerStep(const char* flagname, int32_t value)
{ {
const int32_t max_value = 10000; const int32_t max_value = 10000;
if (value >= 0 && value < max_value) // value is ok if (value >= 0 && value < max_value)
{ // value is ok
return true; return true;
}
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " Hz." << std::endl; std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " Hz." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl; std::cout << "GNSS-SDR program ended." << std::endl;
return false; return false;
@ -126,8 +138,10 @@ static bool ValidateDopplerStep(const char* flagname, int32_t value)
static bool ValidateCn0Samples(const char* flagname, int32_t value) static bool ValidateCn0Samples(const char* flagname, int32_t value)
{ {
const int32_t max_value = 10000; const int32_t max_value = 10000;
if (value > 0 && value < max_value) // value is ok if (value > 0 && value < max_value)
{ // value is ok
return true; return true;
}
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " samples." << std::endl; std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " samples." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl; std::cout << "GNSS-SDR program ended." << std::endl;
return false; return false;
@ -136,8 +150,10 @@ static bool ValidateCn0Samples(const char* flagname, int32_t value)
static bool ValidateCn0Min(const char* flagname, int32_t value) static bool ValidateCn0Min(const char* flagname, int32_t value)
{ {
const int32_t max_value = 100; const int32_t max_value = 100;
if (value > 0 && value < max_value) // value is ok if (value > 0 && value < max_value)
{ // value is ok
return true; return true;
}
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " dB-Hz." << std::endl; std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " dB-Hz." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl; std::cout << "GNSS-SDR program ended." << std::endl;
return false; return false;
@ -146,8 +162,10 @@ static bool ValidateCn0Min(const char* flagname, int32_t value)
static bool ValidateMaxLockFail(const char* flagname, int32_t value) static bool ValidateMaxLockFail(const char* flagname, int32_t value)
{ {
const int32_t max_value = 10000; const int32_t max_value = 10000;
if (value > 0 && value < max_value) // value is ok if (value > 0 && value < max_value)
{ // value is ok
return true; return true;
}
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " fails." << std::endl; std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " fails." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl; std::cout << "GNSS-SDR program ended." << std::endl;
return false; return false;
@ -156,8 +174,10 @@ static bool ValidateMaxLockFail(const char* flagname, int32_t value)
static bool ValidateCarrierLockTh(const char* flagname, double value) static bool ValidateCarrierLockTh(const char* flagname, double value)
{ {
const double max_value = 1.508; const double max_value = 1.508;
if (value > 0.0 && value < max_value) // value is ok if (value > 0.0 && value < max_value)
{ // value is ok
return true; return true;
}
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " rad." << std::endl; std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " rad." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl; std::cout << "GNSS-SDR program ended." << std::endl;
return false; return false;
@ -166,8 +186,10 @@ static bool ValidateCarrierLockTh(const char* flagname, double value)
static bool ValidateDllBw(const char* flagname, double value) static bool ValidateDllBw(const char* flagname, double value)
{ {
const double max_value = 10000.0; const double max_value = 10000.0;
if (value >= 0.0 && value < max_value) // value is ok if (value >= 0.0 && value < max_value)
{ // value is ok
return true; return true;
}
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " Hz." << std::endl; std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " Hz." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl; std::cout << "GNSS-SDR program ended." << std::endl;
return false; return false;
@ -176,8 +198,10 @@ static bool ValidateDllBw(const char* flagname, double value)
static bool ValidatePllBw(const char* flagname, double value) static bool ValidatePllBw(const char* flagname, double value)
{ {
const double max_value = 10000.0; const double max_value = 10000.0;
if (value >= 0.0 && value < max_value) // value is ok if (value >= 0.0 && value < max_value)
{ // value is ok
return true; return true;
}
std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " Hz." << std::endl; std::cout << "Invalid value for flag -" << flagname << ": " << value << ". Allowed range is 0 < " << flagname << " < " << max_value << " Hz." << std::endl;
std::cout << "GNSS-SDR program ended." << std::endl; std::cout << "GNSS-SDR program ended." << std::endl;
return false; return false;

2
src/algorithms/libs/gps_sdr_signal_processing.cc
View File

@ -64,7 +64,9 @@ void gps_l1_ca_code_gen_int(int32_t* _dest, int32_t _prn, uint32_t _chip_shift)
/* A simple error check */ /* A simple error check */
if ((prn_idx < 0) || (prn_idx > 51)) if ((prn_idx < 0) || (prn_idx > 51))
{
return; return;
}
for (lcv = 0; lcv < 10; lcv++) for (lcv = 0; lcv < 10; lcv++)
{ {

272
src/algorithms/libs/rtcm.cc
View File

@ -344,8 +344,14 @@ int32_t Rtcm::bin_to_sint(const std::string& s) const
// Find the sign for glonass data fields (neg = 1, pos = 0) // Find the sign for glonass data fields (neg = 1, pos = 0)
static inline uint64_t glo_sgn(double val) static inline uint64_t glo_sgn(double val)
{ {
if (val < 0) return 1; // If value is negative return 1 if (val < 0)
if (val == 0) return 0; // Positive or equal to zero return 0 {
return 1; // If value is negative return 1
}
if (val == 0)
{
return 0; // Positive or equal to zero return 0
}
return 0; return 0;
} }
@ -2283,10 +2289,22 @@ std::string Rtcm::print_MSM_1(const Gps_Ephemeris& gps_eph,
bool more_messages) bool more_messages)
{ {
uint32_t msg_number = 0; uint32_t msg_number = 0;
if (gps_eph.i_satellite_PRN != 0) msg_number = 1071; if (gps_eph.i_satellite_PRN != 0)
if (gps_cnav_eph.i_satellite_PRN != 0) msg_number = 1071; {
if (glo_gnav_eph.i_satellite_PRN != 0) msg_number = 1081; msg_number = 1071;
if (gal_eph.i_satellite_PRN != 0) msg_number = 1091; }
if (gps_cnav_eph.i_satellite_PRN != 0)
{
msg_number = 1071;
}
if (glo_gnav_eph.i_satellite_PRN != 0)
{
msg_number = 1081;
}
if (gal_eph.i_satellite_PRN != 0)
{
msg_number = 1091;
}
if (((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0)) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0)) if (((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0)) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0))
{ {
LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages? LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages?
@ -2466,10 +2484,22 @@ std::string Rtcm::print_MSM_2(const Gps_Ephemeris& gps_eph,
bool more_messages) bool more_messages)
{ {
uint32_t msg_number = 0; uint32_t msg_number = 0;
if (gps_eph.i_satellite_PRN != 0) msg_number = 1072; if (gps_eph.i_satellite_PRN != 0)
if (gps_cnav_eph.i_satellite_PRN != 0) msg_number = 1072; {
if (glo_gnav_eph.i_satellite_PRN != 0) msg_number = 1082; msg_number = 1072;
if (gal_eph.i_satellite_PRN != 0) msg_number = 1092; }
if (gps_cnav_eph.i_satellite_PRN != 0)
{
msg_number = 1072;
}
if (glo_gnav_eph.i_satellite_PRN != 0)
{
msg_number = 1082;
}
if (gal_eph.i_satellite_PRN != 0)
{
msg_number = 1092;
}
if (((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0)) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0)) if (((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0)) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0))
{ {
LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages? LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages?
@ -2567,10 +2597,22 @@ std::string Rtcm::print_MSM_3(const Gps_Ephemeris& gps_eph,
bool more_messages) bool more_messages)
{ {
uint32_t msg_number = 0; uint32_t msg_number = 0;
if (gps_eph.i_satellite_PRN != 0) msg_number = 1073; if (gps_eph.i_satellite_PRN != 0)
if (gps_cnav_eph.i_satellite_PRN != 0) msg_number = 1073; {
if (glo_gnav_eph.i_satellite_PRN != 0) msg_number = 1083; msg_number = 1073;
if (gal_eph.i_satellite_PRN != 0) msg_number = 1093; }
if (gps_cnav_eph.i_satellite_PRN != 0)
{
msg_number = 1073;
}
if (glo_gnav_eph.i_satellite_PRN != 0)
{
msg_number = 1083;
}
if (gal_eph.i_satellite_PRN != 0)
{
msg_number = 1093;
}
if (((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0)) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0)) if (((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0)) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0))
{ {
LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages? LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages?
@ -2671,10 +2713,22 @@ std::string Rtcm::print_MSM_4(const Gps_Ephemeris& gps_eph,
bool more_messages) bool more_messages)
{ {
uint32_t msg_number = 0; uint32_t msg_number = 0;
if (gps_eph.i_satellite_PRN != 0) msg_number = 1074; if (gps_eph.i_satellite_PRN != 0)
if (gps_cnav_eph.i_satellite_PRN != 0) msg_number = 1074; {
if (glo_gnav_eph.i_satellite_PRN != 0) msg_number = 1084; msg_number = 1074;
if (gal_eph.i_satellite_PRN != 0) msg_number = 1094; }
if (gps_cnav_eph.i_satellite_PRN != 0)
{
msg_number = 1074;
}
if (glo_gnav_eph.i_satellite_PRN != 0)
{
msg_number = 1084;
}
if (gal_eph.i_satellite_PRN != 0)
{
msg_number = 1094;
}
if (((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0)) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0)) if (((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0)) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0))
{ {
LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages? LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages?
@ -2818,10 +2872,22 @@ std::string Rtcm::print_MSM_5(const Gps_Ephemeris& gps_eph,
bool more_messages) bool more_messages)
{ {
uint32_t msg_number = 0; uint32_t msg_number = 0;
if (gps_eph.i_satellite_PRN != 0) msg_number = 1075; if (gps_eph.i_satellite_PRN != 0)
if (gps_cnav_eph.i_satellite_PRN != 0) msg_number = 1075; {
if (glo_gnav_eph.i_satellite_PRN != 0) msg_number = 1085; msg_number = 1075;
if (gal_eph.i_satellite_PRN != 0) msg_number = 1095; }
if (gps_cnav_eph.i_satellite_PRN != 0)
{
msg_number = 1075;
}
if (glo_gnav_eph.i_satellite_PRN != 0)
{
msg_number = 1085;
}
if (gal_eph.i_satellite_PRN != 0)
{
msg_number = 1095;
}
if (((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0)) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0)) if (((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0)) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0))
{ {
LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages? LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages?
@ -2974,10 +3040,22 @@ std::string Rtcm::print_MSM_6(const Gps_Ephemeris& gps_eph,
bool more_messages) bool more_messages)
{ {
uint32_t msg_number = 0; uint32_t msg_number = 0;
if (gps_eph.i_satellite_PRN != 0) msg_number = 1076; if (gps_eph.i_satellite_PRN != 0)
if (gps_cnav_eph.i_satellite_PRN != 0) msg_number = 1076; {
if (glo_gnav_eph.i_satellite_PRN != 0) msg_number = 1086; msg_number = 1076;
if (gal_eph.i_satellite_PRN != 0) msg_number = 1096; }
if (gps_cnav_eph.i_satellite_PRN != 0)
{
msg_number = 1076;
}
if (glo_gnav_eph.i_satellite_PRN != 0)
{
msg_number = 1086;
}
if (gal_eph.i_satellite_PRN != 0)
{
msg_number = 1096;
}
if (((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0)) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0)) if (((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0)) && (gal_eph.i_satellite_PRN != 0) && (glo_gnav_eph.i_satellite_PRN != 0))
{ {
LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages? LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages?
@ -3081,10 +3159,22 @@ std::string Rtcm::print_MSM_7(const Gps_Ephemeris& gps_eph,
bool more_messages) bool more_messages)
{ {
uint32_t msg_number = 0; uint32_t msg_number = 0;
if (gps_eph.i_satellite_PRN != 0) msg_number = 1077; if (gps_eph.i_satellite_PRN != 0)
if (gps_cnav_eph.i_satellite_PRN != 0) msg_number = 1077; {
if (glo_gnav_eph.i_satellite_PRN != 0) msg_number = 1087; msg_number = 1077;
if (gal_eph.i_satellite_PRN != 0) msg_number = 1097; }
if (gps_cnav_eph.i_satellite_PRN != 0)
{
msg_number = 1077;
}
if (glo_gnav_eph.i_satellite_PRN != 0)
{
msg_number = 1087;
}
if (gal_eph.i_satellite_PRN != 0)
{
msg_number = 1097;
}
if (((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0)) && (glo_gnav_eph.i_satellite_PRN != 0) && (gal_eph.i_satellite_PRN != 0)) if (((gps_eph.i_satellite_PRN != 0) || (gps_cnav_eph.i_satellite_PRN != 0)) && (glo_gnav_eph.i_satellite_PRN != 0) && (gal_eph.i_satellite_PRN != 0))
{ {
LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages? LOG(WARNING) << "MSM messages for observables from different systems are not defined"; //print two messages?
@ -3461,13 +3551,34 @@ uint32_t Rtcm::lock_time(const Glonass_Gnav_Ephemeris& eph, double obs_time, con
uint32_t Rtcm::lock_time_indicator(uint32_t lock_time_period_s) uint32_t Rtcm::lock_time_indicator(uint32_t lock_time_period_s)
{ {
// Table 3.4-2 // Table 3.4-2
if (lock_time_period_s <= 0) return 0; if (lock_time_period_s <= 0)
if (lock_time_period_s < 24) return lock_time_period_s; {
if (lock_time_period_s < 72) return (lock_time_period_s + 24) / 2; return 0;
if (lock_time_period_s < 168) return (lock_time_period_s + 120) / 4; }
if (lock_time_period_s < 360) return (lock_time_period_s + 408) / 8; if (lock_time_period_s < 24)
if (lock_time_period_s < 744) return (lock_time_period_s + 1176) / 16; {
if (lock_time_period_s < 937) return (lock_time_period_s + 3096) / 32; return lock_time_period_s;
}
if (lock_time_period_s < 72)
{
return (lock_time_period_s + 24) / 2;
}
if (lock_time_period_s < 168)
{
return (lock_time_period_s + 120) / 4;
}
if (lock_time_period_s < 360)
{
return (lock_time_period_s + 408) / 8;
}
if (lock_time_period_s < 744)
{
return (lock_time_period_s + 1176) / 16;
}
if (lock_time_period_s < 937)
{
return (lock_time_period_s + 3096) / 32;
}
return 127; return 127;
} }
@ -3475,21 +3586,66 @@ uint32_t Rtcm::lock_time_indicator(uint32_t lock_time_period_s)
uint32_t Rtcm::msm_lock_time_indicator(uint32_t lock_time_period_s) uint32_t Rtcm::msm_lock_time_indicator(uint32_t lock_time_period_s)
{ {
// Table 3.5-74 // Table 3.5-74
if (lock_time_period_s < 32) return 0; if (lock_time_period_s < 32)
if (lock_time_period_s < 64) return 1; {
if (lock_time_period_s < 128) return 2; return 0;
if (lock_time_period_s < 256) return 3; }
if (lock_time_period_s < 512) return 4; if (lock_time_period_s < 64)
if (lock_time_period_s < 1024) return 5; {
if (lock_time_period_s < 2048) return 6; return 1;
if (lock_time_period_s < 4096) return 7; }
if (lock_time_period_s < 8192) return 8; if (lock_time_period_s < 128)
if (lock_time_period_s < 16384) return 9; {
if (lock_time_period_s < 32768) return 10; return 2;
if (lock_time_period_s < 65536) return 11; }
if (lock_time_period_s < 131072) return 12; if (lock_time_period_s < 256)
if (lock_time_period_s < 262144) return 13; {
if (lock_time_period_s < 524288) return 14; return 3;
}
if (lock_time_period_s < 512)
{
return 4;
}
if (lock_time_period_s < 1024)
{
return 5;
}
if (lock_time_period_s < 2048)
{
return 6;
}
if (lock_time_period_s < 4096)
{
return 7;
}
if (lock_time_period_s < 8192)
{
return 8;
}
if (lock_time_period_s < 16384)
{
return 9;
}
if (lock_time_period_s < 32768)
{
return 10;
}
if (lock_time_period_s < 65536)
{
return 11;
}
if (lock_time_period_s < 131072)
{
return 12;
}
if (lock_time_period_s < 262144)
{
return 13;
}
if (lock_time_period_s < 524288)
{
return 14;
}
return 15; return 15;
} }
@ -3521,7 +3677,7 @@ uint32_t Rtcm::msm_extended_lock_time_indicator(uint32_t lock_time_period_s)
if( 33554432 <= lock_time_period_s && lock_time_period_s < 67108864 ) return (672 + (lock_time_period_s - 33554432) / 1048576); if( 33554432 <= lock_time_period_s && lock_time_period_s < 67108864 ) return (672 + (lock_time_period_s - 33554432) / 1048576);
if( 67108864 <= lock_time_period_s ) return (704 ); if( 67108864 <= lock_time_period_s ) return (704 );
return 1023; // will never happen return 1023; // will never happen
} }
// clang-format on // clang-format on
// ***************************************************************************************************** // *****************************************************************************************************
@ -5182,8 +5338,14 @@ int32_t Rtcm::set_DF399(const Gnss_Synchro& gnss_synchro)
} }
double rough_phase_range_rate_ms = std::round(-gnss_synchro.Carrier_Doppler_hz * lambda); double rough_phase_range_rate_ms = std::round(-gnss_synchro.Carrier_Doppler_hz * lambda);
if (rough_phase_range_rate_ms < -8191) rough_phase_range_rate_ms = -8192; if (rough_phase_range_rate_ms < -8191)
if (rough_phase_range_rate_ms > 8191) rough_phase_range_rate_ms = -8192; {
rough_phase_range_rate_ms = -8192;
}
if (rough_phase_range_rate_ms > 8191)
{
rough_phase_range_rate_ms = -8192;
}
DF399 = std::bitset<14>(static_cast<int32_t>(rough_phase_range_rate_ms)); DF399 = std::bitset<14>(static_cast<int32_t>(rough_phase_range_rate_ms));
return 0; return 0;

19
src/algorithms/libs/rtcm.h
View File

@ -553,8 +553,10 @@ private:
{ {
body_length_ = new_length; body_length_ = new_length;
if (body_length_ > max_body_length) if (body_length_ > max_body_length)
{
body_length_ = max_body_length; body_length_ = max_body_length;
} }
}
inline bool decode_header() inline bool decode_header()
{ {
@ -609,8 +611,10 @@ private:
{ {
participants_.insert(participant); participants_.insert(participant);
for (auto msg : recent_msgs_) for (auto msg : recent_msgs_)
{
participant->deliver(msg); participant->deliver(msg);
} }
}
inline void leave(const std::shared_ptr<Rtcm_Listener>& participant) inline void leave(const std::shared_ptr<Rtcm_Listener>& participant)
{ {
@ -621,11 +625,15 @@ private:
{ {
recent_msgs_.push_back(msg); recent_msgs_.push_back(msg);
while (recent_msgs_.size() > max_recent_msgs) while (recent_msgs_.size() > max_recent_msgs)
{
recent_msgs_.pop_front(); recent_msgs_.pop_front();
}
for (auto participant : participants_) for (auto participant : participants_)
{
participant->deliver(msg); participant->deliver(msg);
} }
}
private: private:
std::set<std::shared_ptr<Rtcm_Listener> > participants_; std::set<std::shared_ptr<Rtcm_Listener> > participants_;
@ -854,7 +862,11 @@ private:
std::string message; std::string message;
Rtcm_Message msg; Rtcm_Message msg;
queue_->wait_and_pop(message); //message += '\n'; queue_->wait_and_pop(message); //message += '\n';
if (message == "Goodbye") break; if (message == "Goodbye")
{
break;
}
const char* char_msg = message.c_str(); const char* char_msg = message.c_str();
msg.body_length(message.length()); msg.body_length(message.length());
std::memcpy(msg.body(), char_msg, msg.body_length()); std::memcpy(msg.body(), char_msg, msg.body_length());
@ -918,7 +930,10 @@ private:
LOG(INFO) << "Serving client from " << remote_addr; LOG(INFO) << "Serving client from " << remote_addr;
} }
} }
if (start_session) std::make_shared<Rtcm_Session>(std::move(socket_), room_)->start(); if (start_session)
{
std::make_shared<Rtcm_Session>(std::move(socket_), room_)->start();
}
} }
else else
{ {

2
src/algorithms/libs/rtklib/rtklib.h
View File

@ -343,7 +343,7 @@ const int POSOPT_RINEX = 3; //!< pos option: rinex header pos
const int MAXSTRPATH = 1024; //!< max length of stream path const int MAXSTRPATH = 1024; //!< max length of stream path
const int MAXSTRMSG = 1024; //!< max length of stream message const int MAXSTRMSG = 1024; //!< max length of stream message
using fatalfunc_t = void (const char *); //!< fatal callback function type using fatalfunc_t = void(const char *); //!< fatal callback function type
#define STR_MODE_R 0x1 /* stream mode: read */ #define STR_MODE_R 0x1 /* stream mode: read */
#define STR_MODE_W 0x2 /* stream mode: write */ #define STR_MODE_W 0x2 /* stream mode: write */

10
src/algorithms/libs/rtklib/rtklib_conversions.cc
View File

@ -49,8 +49,14 @@ obsd_t insert_obs_to_rtklib(obsd_t& rtklib_obs, const Gnss_Synchro& gnss_synchro
break; break;
} }
double CN0_dB_Hz_est = gnss_synchro.CN0_dB_hz; double CN0_dB_Hz_est = gnss_synchro.CN0_dB_hz;
if (CN0_dB_Hz_est > 63.75) CN0_dB_Hz_est = 63.75; if (CN0_dB_Hz_est > 63.75)
if (CN0_dB_Hz_est < 0.0) CN0_dB_Hz_est = 0.0; {
CN0_dB_Hz_est = 63.75;
}
if (CN0_dB_Hz_est < 0.0)
{
CN0_dB_Hz_est = 0.0;
}
auto CN0_dB_Hz = static_cast<unsigned char>(std::round(CN0_dB_Hz_est / 0.25)); auto CN0_dB_Hz = static_cast<unsigned char>(std::round(CN0_dB_Hz_est / 0.25));
rtklib_obs.SNR[band] = CN0_dB_Hz; rtklib_obs.SNR[band] = CN0_dB_Hz;
//Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris //Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris

200
src/algorithms/libs/rtklib/rtklib_ephemeris.cc
View File

@ -99,8 +99,14 @@ double var_uraeph(int ura)
double var_urassr(int ura) double var_urassr(int ura)
{ {
double std_; double std_;
if (ura <= 0) return std::pow(DEFURASSR, 2.0); if (ura <= 0)
if (ura >= 63) return std::pow(5.4665, 2.0); {
return std::pow(DEFURASSR, 2.0);
}
if (ura >= 63)
{
return std::pow(5.4665, 2.0);
}
std_ = (std::pow((ura >> 3) & 7, 2.0) * (1.0 + (ura & 7) / 4.0) - 1.0) * 1e-3; std_ = (std::pow((ura >> 3) & 7, 2.0) * (1.0 + (ura & 7) / 4.0) - 1.0) * 1e-3;
return std::pow(std_, 2.0); return std::pow(std_, 2.0);
} }
@ -323,13 +329,25 @@ void glorbit(double t, double *x, const double *acc)
int i; int i;
deq(x, k1, acc); deq(x, k1, acc);
for (i = 0; i < 6; i++) w[i] = x[i] + k1[i] * t / 2.0; for (i = 0; i < 6; i++)
{
w[i] = x[i] + k1[i] * t / 2.0;
}
deq(w, k2, acc); deq(w, k2, acc);
for (i = 0; i < 6; i++) w[i] = x[i] + k2[i] * t / 2.0; for (i = 0; i < 6; i++)
{
w[i] = x[i] + k2[i] * t / 2.0;
}
deq(w, k3, acc); deq(w, k3, acc);
for (i = 0; i < 6; i++) w[i] = x[i] + k3[i] * t; for (i = 0; i < 6; i++)
{
w[i] = x[i] + k3[i] * t;
}
deq(w, k4, acc); deq(w, k4, acc);
for (i = 0; i < 6; i++) x[i] += (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]) * t / 6.0; for (i = 0; i < 6; i++)
{
x[i] += (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]) * t / 6.0;
}
} }
@ -386,10 +404,16 @@ void geph2pos(gtime_t time, const geph_t *geph, double *rs, double *dts,
} }
for (tt = t < 0.0 ? -TSTEP : TSTEP; fabs(t) > 1e-9; t -= tt) for (tt = t < 0.0 ? -TSTEP : TSTEP; fabs(t) > 1e-9; t -= tt)
{ {
if (fabs(t) < TSTEP) tt = t; if (fabs(t) < TSTEP)
{
tt = t;
}
glorbit(tt, x, geph->acc); glorbit(tt, x, geph->acc);
} }
for (i = 0; i < 3; i++) rs[i] = x[i]; for (i = 0; i < 3; i++)
{
rs[i] = x[i];
}
*var = std::pow(ERREPH_GLO, 2.0); *var = std::pow(ERREPH_GLO, 2.0);
} }
@ -476,10 +500,22 @@ eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav)
for (i = 0; i < nav->n; i++) for (i = 0; i < nav->n; i++)
{ {
if (nav->eph[i].sat != sat) continue; if (nav->eph[i].sat != sat)
if (iode >= 0 && nav->eph[i].iode != iode) continue; {
if ((t = fabs(timediff(nav->eph[i].toe, time))) > tmax) continue; continue;
if (iode >= 0) return nav->eph + i; }
if (iode >= 0 && nav->eph[i].iode != iode)
{
continue;
}
if ((t = fabs(timediff(nav->eph[i].toe, time))) > tmax)
{
continue;
}
if (iode >= 0)
{
return nav->eph + i;
}
if (t <= tmin) if (t <= tmin)
{ {
j = i; j = i;
@ -506,10 +542,22 @@ geph_t *selgeph(gtime_t time, int sat, int iode, const nav_t *nav)
for (i = 0; i < nav->ng; i++) for (i = 0; i < nav->ng; i++)
{ {
if (nav->geph[i].sat != sat) continue; if (nav->geph[i].sat != sat)
if (iode >= 0 && nav->geph[i].iode != iode) continue; {
if ((t = fabs(timediff(nav->geph[i].toe, time))) > tmax) continue; continue;
if (iode >= 0) return nav->geph + i; }
if (iode >= 0 && nav->geph[i].iode != iode)
{
continue;
}
if ((t = fabs(timediff(nav->geph[i].toe, time))) > tmax)
{
continue;
}
if (iode >= 0)
{
return nav->geph + i;
}
if (t <= tmin) if (t <= tmin)
{ {
j = i; j = i;
@ -536,8 +584,14 @@ seph_t *selseph(gtime_t time, int sat, const nav_t *nav)
for (i = 0; i < nav->ns; i++) for (i = 0; i < nav->ns; i++)
{ {
if (nav->seph[i].sat != sat) continue; if (nav->seph[i].sat != sat)
if ((t = fabs(timediff(nav->seph[i].t0, time))) > tmax) continue; {
continue;
}
if ((t = fabs(timediff(nav->seph[i].t0, time))) > tmax)
{
continue;
}
if (t <= tmin) if (t <= tmin)
{ {
j = i; j = i;
@ -568,21 +622,32 @@ int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS) if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS)
{ {
if (!(eph = seleph(teph, sat, -1, nav))) return 0; if (!(eph = seleph(teph, sat, -1, nav)))
{
return 0;
}
*dts = eph2clk(time, eph); *dts = eph2clk(time, eph);
} }
else if (sys == SYS_GLO) else if (sys == SYS_GLO)
{ {
if (!(geph = selgeph(teph, sat, -1, nav))) return 0; if (!(geph = selgeph(teph, sat, -1, nav)))
{
return 0;
}
*dts = geph2clk(time, geph); *dts = geph2clk(time, geph);
} }
else if (sys == SYS_SBS) else if (sys == SYS_SBS)
{ {
if (!(seph = selseph(teph, sat, nav))) return 0; if (!(seph = selseph(teph, sat, nav)))
{
return 0;
}
*dts = seph2clk(time, seph); *dts = seph2clk(time, seph);
} }
else else
{
return 0; return 0;
}
return 1; return 1;
} }
@ -606,7 +671,10 @@ int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS) if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS)
{ {
if (!(eph = seleph(teph, sat, iode, nav))) return 0; if (!(eph = seleph(teph, sat, iode, nav)))
{
return 0;
}
eph2pos(time, eph, rs, dts, var); eph2pos(time, eph, rs, dts, var);
time = timeadd(time, tt); time = timeadd(time, tt);
@ -615,7 +683,10 @@ int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
} }
else if (sys == SYS_GLO) else if (sys == SYS_GLO)
{ {
if (!(geph = selgeph(teph, sat, iode, nav))) return 0; if (!(geph = selgeph(teph, sat, iode, nav)))
{
return 0;
}
geph2pos(time, geph, rs, dts, var); geph2pos(time, geph, rs, dts, var);
time = timeadd(time, tt); time = timeadd(time, tt);
geph2pos(time, geph, rst, dtst, var); geph2pos(time, geph, rst, dtst, var);
@ -623,7 +694,10 @@ int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
} }
else if (sys == SYS_SBS) else if (sys == SYS_SBS)
{ {
if (!(seph = selseph(teph, sat, nav))) return 0; if (!(seph = selseph(teph, sat, nav)))
{
return 0;
}
seph2pos(time, seph, rs, dts, var); seph2pos(time, seph, rs, dts, var);
time = timeadd(time, tt); time = timeadd(time, tt);
@ -631,10 +705,15 @@ int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
*svh = seph->svh; *svh = seph->svh;
} }
else else
{
return 0; return 0;
}
/* satellite velocity and clock drift by differential approx */ /* satellite velocity and clock drift by differential approx */
for (i = 0; i < 3; i++) rs[i + 3] = (rst[i] - rs[i]) / tt; for (i = 0; i < 3; i++)
{
rs[i + 3] = (rst[i] - rs[i]) / tt;
}
dts[1] = (dtst[0] - dts[0]) / tt; dts[1] = (dtst[0] - dts[0]) / tt;
return 1; return 1;
@ -654,7 +733,10 @@ int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
for (i = 0; i < nav->sbssat.nsat; i++) for (i = 0; i < nav->sbssat.nsat; i++)
{ {
sbs = nav->sbssat.sat + i; sbs = nav->sbssat.sat + i;
if (sbs->sat == sat) break; if (sbs->sat == sat)
{
break;
}
} }
if (i >= nav->sbssat.nsat) if (i >= nav->sbssat.nsat)
{ {
@ -664,10 +746,16 @@ int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
return 0; return 0;
} }
/* satellite position and clock by broadcast ephemeris */ /* satellite position and clock by broadcast ephemeris */
if (!ephpos(time, teph, sat, nav, sbs->lcorr.iode, rs, dts, var, svh)) return 0; if (!ephpos(time, teph, sat, nav, sbs->lcorr.iode, rs, dts, var, svh))
{
return 0;
}
/* sbas satellite correction (long term and fast) */ /* sbas satellite correction (long term and fast) */
if (sbssatcorr(time, sat, nav, rs, dts, var)) return 1; if (sbssatcorr(time, sat, nav, rs, dts, var))
{
return 1;
}
*svh = -1; *svh = -1;
return 0; return 0;
} }
@ -716,10 +804,19 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
*svh = -1; *svh = -1;
return 0; return 0;
} }
if (ssr->udi[0] >= 1.0) t1 -= ssr->udi[0] / 2.0; if (ssr->udi[0] >= 1.0)
if (ssr->udi[1] >= 1.0) t2 -= ssr->udi[0] / 2.0; {
t1 -= ssr->udi[0] / 2.0;
}
if (ssr->udi[1] >= 1.0)
{
t2 -= ssr->udi[0] / 2.0;
}
for (i = 0; i < 3; i++) deph[i] = ssr->deph[i] + ssr->ddeph[i] * t1; for (i = 0; i < 3; i++)
{
deph[i] = ssr->deph[i] + ssr->ddeph[i] * t1;
}
dclk = ssr->dclk[0] + ssr->dclk[1] * t2 + ssr->dclk[2] * t2 * t2; dclk = ssr->dclk[0] + ssr->dclk[1] * t2 + ssr->dclk[2] * t2 * t2;
/* ssr highrate clock correction (ref [4]) */ /* ssr highrate clock correction (ref [4]) */
@ -735,13 +832,19 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
return 0; return 0;
} }
/* satellite position and clock by broadcast ephemeris */ /* satellite position and clock by broadcast ephemeris */
if (!ephpos(time, teph, sat, nav, ssr->iode, rs, dts, var, svh)) return 0; if (!ephpos(time, teph, sat, nav, ssr->iode, rs, dts, var, svh))
{
return 0;
}
/* satellite clock for gps, galileo and qzss */ /* satellite clock for gps, galileo and qzss */
sys = satsys(sat, nullptr); sys = satsys(sat, nullptr);
if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS) if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS)
{ {
if (!(eph = seleph(teph, sat, ssr->iode, nav))) return 0; if (!(eph = seleph(teph, sat, ssr->iode, nav)))
{
return 0;
}
/* satellite clock by clock parameters */ /* satellite clock by clock parameters */
tk = timediff(time, eph->toc); tk = timediff(time, eph->toc);
@ -752,7 +855,10 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
dts[0] -= 2.0 * dot(rs, rs + 3, 3) / SPEED_OF_LIGHT / SPEED_OF_LIGHT; dts[0] -= 2.0 * dot(rs, rs + 3, 3) / SPEED_OF_LIGHT / SPEED_OF_LIGHT;
} }
/* radial-along-cross directions in ecef */ /* radial-along-cross directions in ecef */
if (!normv3(rs + 3, ea)) return 0; if (!normv3(rs + 3, ea))
{
return 0;
}
cross3(rs, rs + 3, rc); cross3(rs, rs + 3, rc);
if (!normv3(rc, ec)) if (!normv3(rc, ec))
{ {
@ -819,9 +925,13 @@ int satpos(gtime_t time, gtime_t teph, int sat, int ephopt,
return satpos_ssr(time, teph, sat, nav, 1, rs, dts, var, svh); return satpos_ssr(time, teph, sat, nav, 1, rs, dts, var, svh);
case EPHOPT_PREC: case EPHOPT_PREC:
if (!peph2pos(time, sat, nav, 1, rs, dts, var)) if (!peph2pos(time, sat, nav, 1, rs, dts, var))
{
break; break;
}
else else
{
return 1; return 1;
}
//TODO: enable lex //TODO: enable lex
//case EPHOPT_LEX : //case EPHOPT_LEX :
// if (!lexeph2pos(time, sat, nav, rs, dts, var)) break; else return 1; // if (!lexeph2pos(time, sat, nav, rs, dts, var)) break; else return 1;
@ -866,14 +976,25 @@ void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
for (i = 0; i < n && i < MAXOBS; i++) for (i = 0; i < n && i < MAXOBS; i++)
{ {
for (j = 0; j < 6; j++) rs[j + i * 6] = 0.0; for (j = 0; j < 6; j++)
for (j = 0; j < 2; j++) dts[j + i * 2] = 0.0; {
rs[j + i * 6] = 0.0;
}
for (j = 0; j < 2; j++)
{
dts[j + i * 2] = 0.0;
}
var[i] = 0.0; var[i] = 0.0;
svh[i] = 0; svh[i] = 0;
/* search any pseudorange */ /* search any pseudorange */
for (j = 0, pr = 0.0; j < NFREQ; j++) for (j = 0, pr = 0.0; j < NFREQ; j++)
if ((pr = obs[i].P[j]) != 0.0) break; {
if ((pr = obs[i].P[j]) != 0.0)
{
break;
}
}
if (j >= NFREQ) if (j >= NFREQ)
{ {
@ -901,7 +1022,10 @@ void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
/* if no precise clock available, use broadcast clock instead */ /* if no precise clock available, use broadcast clock instead */
if (dts[i * 2] == 0.0) if (dts[i * 2] == 0.0)
{ {
if (!ephclk(time[i], teph, obs[i].sat, nav, dts + i * 2)) continue; if (!ephclk(time[i], teph, obs[i].sat, nav, dts + i * 2))
{
continue;
}
dts[1 + i * 2] = 0.0; dts[1 + i * 2] = 0.0;
*var = std::pow(STD_BRDCCLK, 2.0); *var = std::pow(STD_BRDCCLK, 2.0);
} }

129
src/algorithms/libs/rtklib/rtklib_ionex.cc
View File

@ -64,9 +64,18 @@
/* get index -----------------------------------------------------------------*/ /* get index -----------------------------------------------------------------*/
int getindex(double value, const double *range) int getindex(double value, const double *range)
{ {
if (range[2] == 0.0) return 0; if (range[2] == 0.0)
if (range[1] > 0.0 && (value < range[0] || range[1] < value)) return -1; {
if (range[1] < 0.0 && (value < range[1] || range[0] < value)) return -1; return 0;
}
if (range[1] > 0.0 && (value < range[0] || range[1] < value))
{
return -1;
}
if (range[1] < 0.0 && (value < range[1] || range[0] < value))
{
return -1;
}
return static_cast<int>(floor((value - range[0]) / range[2] + 0.5)); return static_cast<int>(floor((value - range[0]) / range[2] + 0.5));
} }
@ -81,7 +90,10 @@ int nitem(const double *range)
/* data index (i:lat,j:lon,k:hgt) --------------------------------------------*/ /* data index (i:lat,j:lon,k:hgt) --------------------------------------------*/
int dataindex(int i, int j, int k, const int *ndata) int dataindex(int i, int j, int k, const int *ndata)
{ {
if (i < 0 || ndata[0] <= i || j < 0 || ndata[1] <= j || k < 0 || ndata[2] <= k) return -1; if (i < 0 || ndata[0] <= i || j < 0 || ndata[1] <= j || k < 0 || ndata[2] <= k)
{
return -1;
}
return i + ndata[0] * (j + ndata[1] * k); return i + ndata[0] * (j + ndata[1] * k);
} }
@ -99,7 +111,10 @@ tec_t *addtec(const double *lats, const double *lons, const double *hgts,
ndata[0] = nitem(lats); ndata[0] = nitem(lats);
ndata[1] = nitem(lons); ndata[1] = nitem(lons);
ndata[2] = nitem(hgts); ndata[2] = nitem(hgts);
if (ndata[0] <= 1 || ndata[1] <= 1 || ndata[2] <= 0) return nullptr; if (ndata[0] <= 1 || ndata[1] <= 1 || ndata[2] <= 0)
{
return nullptr;
}
if (nav->nt >= nav->ntmax) if (nav->nt >= nav->ntmax)
{ {
@ -149,11 +164,17 @@ void readionexdcb(FILE *fp, double *dcb, double *rms)
trace(3, "readionexdcb:\n"); trace(3, "readionexdcb:\n");
for (i = 0; i < MAXSAT; i++) dcb[i] = rms[i] = 0.0; for (i = 0; i < MAXSAT; i++)
{
dcb[i] = rms[i] = 0.0;
}
while (fgets(buff, sizeof(buff), fp)) while (fgets(buff, sizeof(buff), fp))
{ {
if (strlen(buff) < 60) continue; if (strlen(buff) < 60)
{
continue;
}
label = buff + 60; label = buff + 60;
if (strstr(label, "PRN / BIAS / RMS") == label) if (strstr(label, "PRN / BIAS / RMS") == label)
@ -170,8 +191,10 @@ void readionexdcb(FILE *fp, double *dcb, double *rms)
rms[sat - 1] = str2num(buff, 16, 10); rms[sat - 1] = str2num(buff, 16, 10);
} }
else if (strstr(label, "END OF AUX DATA") == label) else if (strstr(label, "END OF AUX DATA") == label)
{
break; break;
} }
}
} }
@ -186,12 +209,18 @@ double readionexh(FILE *fp, double *lats, double *lons, double *hgts,
while (fgets(buff, sizeof(buff), fp)) while (fgets(buff, sizeof(buff), fp))
{ {
if (strlen(buff) < 60) continue; if (strlen(buff) < 60)
{
continue;
}
label = buff + 60; label = buff + 60;
if (strstr(label, "IONEX VERSION / TYPE") == label) if (strstr(label, "IONEX VERSION / TYPE") == label)
{ {
if (buff[20] == 'I') ver = str2num(buff, 0, 8); if (buff[20] == 'I')
{
ver = str2num(buff, 0, 8);
}
} }
else if (strstr(label, "BASE RADIUS") == label) else if (strstr(label, "BASE RADIUS") == label)
{ {
@ -247,11 +276,17 @@ int readionexb(FILE *fp, const double *lats, const double *lons,
while (fgets(buff, sizeof(buff), fp)) while (fgets(buff, sizeof(buff), fp))
{ {
if (strlen(buff) < 60) continue; if (strlen(buff) < 60)
{
continue;
}
if (strstr(label, "START OF TEC MAP") == label) if (strstr(label, "START OF TEC MAP") == label)
{ {
if ((p = addtec(lats, lons, hgts, rb, nav))) type = 1; if ((p = addtec(lats, lons, hgts, rb, nav)))
{
type = 1;
}
} }
else if (strstr(label, "END OF TEC MAP") == label) else if (strstr(label, "END OF TEC MAP") == label)
{ {
@ -279,14 +314,19 @@ int readionexb(FILE *fp, const double *lats, const double *lons,
{ {
for (i = nav->nt - 1; i >= 0; i--) for (i = nav->nt - 1; i >= 0; i--)
{ {
if (fabs(timediff(time, nav->tec[i].time)) >= 1.0) continue; if (fabs(timediff(time, nav->tec[i].time)) >= 1.0)
{
continue;
}
p = nav->tec + i; p = nav->tec + i;
break; break;
} }
} }
else if (p) else if (p)
{
p->time = time; p->time = time;
} }
}
else if (strstr(label, "LAT/LON1/LON2/DLON/H") == label && p) else if (strstr(label, "LAT/LON1/LON2/DLON/H") == label && p)
{ {
lat = str2num(buff, 2, 6); lat = str2num(buff, 2, 6);
@ -301,20 +341,33 @@ int readionexb(FILE *fp, const double *lats, const double *lons,
for (m = 0; m < n; m++) for (m = 0; m < n; m++)
{ {
if (m % 16 == 0 && !fgets(buff, sizeof(buff), fp)) break; if (m % 16 == 0 && !fgets(buff, sizeof(buff), fp))
{
break;
}
j = getindex(lon[0] + lon[2] * m, p->lons); j = getindex(lon[0] + lon[2] * m, p->lons);
if ((index = dataindex(i, j, k, p->ndata)) < 0) continue; if ((index = dataindex(i, j, k, p->ndata)) < 0)
{
continue;
}
if ((x = str2num(buff, m % 16 * 5, 5)) == 9999.0) continue; if ((x = str2num(buff, m % 16 * 5, 5)) == 9999.0)
{
continue;
}
if (type == 1) if (type == 1)
{
p->data[index] = x * std::pow(10.0, nexp); p->data[index] = x * std::pow(10.0, nexp);
}
else else
{
p->rms[index] = static_cast<float>(x * std::pow(10.0, nexp)); p->rms[index] = static_cast<float>(x * std::pow(10.0, nexp));
} }
} }
} }
}
return 1; return 1;
} }
@ -387,7 +440,10 @@ void readtec(const char *file, nav_t *nav, int opt)
{ {
if (!(efiles[i] = static_cast<char *>(malloc(1024)))) if (!(efiles[i] = static_cast<char *>(malloc(1024))))
{ {
for (i--; i >= 0; i--) free(efiles[i]); for (i--; i >= 0; i--)
{
free(efiles[i]);
}
return; return;
} }
} }
@ -414,10 +470,16 @@ void readtec(const char *file, nav_t *nav, int opt)
readionexb(fp, lats, lons, hgts, rb, nexp, nav); readionexb(fp, lats, lons, hgts, rb, nexp, nav);
fclose(fp); fclose(fp);
} }
for (i = 0; i < MAXEXFILE; i++) free(efiles[i]); for (i = 0; i < MAXEXFILE; i++)
{
free(efiles[i]);
}
/* combine tec grid data */ /* combine tec grid data */
if (nav->nt > 0) combtec(nav); if (nav->nt > 0)
{
combtec(nav);
}
/* P1-P2 dcb */ /* P1-P2 dcb */
for (i = 0; i < MAXSAT; i++) for (i = 0; i < MAXSAT; i++)
@ -437,14 +499,21 @@ int interptec(const tec_t *tec, int k, const double *posp, double *value,
trace(3, "interptec: k=%d posp=%.2f %.2f\n", k, posp[0] * R2D, posp[1] * R2D); trace(3, "interptec: k=%d posp=%.2f %.2f\n", k, posp[0] * R2D, posp[1] * R2D);
*value = *rms = 0.0; *value = *rms = 0.0;
if (tec->lats[2] == 0.0 || tec->lons[2] == 0.0) return 0; if (tec->lats[2] == 0.0 || tec->lons[2] == 0.0)
{
return 0;
}
dlat = posp[0] * R2D - tec->lats[0]; dlat = posp[0] * R2D - tec->lats[0];
dlon = posp[1] * R2D - tec->lons[0]; dlon = posp[1] * R2D - tec->lons[0];
if (tec->lons[2] > 0.0) if (tec->lons[2] > 0.0)
{
dlon -= floor(dlon / 360) * 360.0; /* 0 <= dlon<360 */ dlon -= floor(dlon / 360) * 360.0; /* 0 <= dlon<360 */
}
else else
{
dlon += floor(-dlon / 360) * 360.0; /* -360<dlon <= 0 */ dlon += floor(-dlon / 360) * 360.0; /* -360<dlon <= 0 */
}
a = dlat / tec->lats[2]; a = dlat / tec->lats[2];
b = dlon / tec->lons[2]; b = dlon / tec->lons[2];
@ -456,7 +525,10 @@ int interptec(const tec_t *tec, int k, const double *posp, double *value,
/* get gridded tec data */ /* get gridded tec data */
for (n = 0; n < 4; n++) for (n = 0; n < 4; n++)
{ {
if ((index = dataindex(i + (n % 2), j + (n < 2 ? 0 : 1), k, tec->ndata)) < 0) continue; if ((index = dataindex(i + (n % 2), j + (n < 2 ? 0 : 1), k, tec->ndata)) < 0)
{
continue;
}
d[n] = tec->data[index]; d[n] = tec->data[index];
r[n] = tec->rms[index]; r[n] = tec->rms[index];
} }
@ -491,13 +563,18 @@ int interptec(const tec_t *tec, int k, const double *posp, double *value,
{ {
i = 0; i = 0;
for (n = 0; n < 4; n++) for (n = 0; n < 4; n++)
{
if (d[n] > 0.0) if (d[n] > 0.0)
{ {
i++; i++;
*value += d[n]; *value += d[n];
*rms += r[n]; *rms += r[n];
} }
if (i == 0) return 0; }
if (i == 0)
{
return 0;
}
*value /= i; *value /= i;
*rms /= i; *rms /= i;
} }
@ -537,7 +614,10 @@ int iondelay(gtime_t time, const tec_t *tec, const double *pos,
posp[1] += 2.0 * PI * timediff(time, tec->time) / 86400.0; posp[1] += 2.0 * PI * timediff(time, tec->time) / 86400.0;
} }
/* interpolate tec grid data */ /* interpolate tec grid data */
if (!interptec(tec, i, posp, &vtec, &rms)) return 0; if (!interptec(tec, i, posp, &vtec, &rms))
{
return 0;
}
*delay += fact * fs * vtec; *delay += fact * fs * vtec;
*var += fact * fact * fs * fs * rms * rms; *var += fact * fact * fs * fs * rms * rms;
@ -580,7 +660,10 @@ int iontec(gtime_t time, const nav_t *nav, const double *pos,
} }
for (i = 0; i < nav->nt; i++) for (i = 0; i < nav->nt; i++)
{ {
if (timediff(nav->tec[i].time, time) > 0.0) break; if (timediff(nav->tec[i].time, time) > 0.0)
{
break;
}
} }
if (i == 0 || i >= nav->nt) if (i == 0 || i >= nav->nt)
{ {

104
src/algorithms/libs/rtklib/rtklib_lambda.cc
View File

@ -69,13 +69,27 @@ int LD(int n, const double *Q, double *L, double *D)
break; break;
} }
a = sqrt(D[i]); a = sqrt(D[i]);
for (j = 0; j <= i; j++) L[i + j * n] = A[i + j * n] / a; for (j = 0; j <= i; j++)
{
L[i + j * n] = A[i + j * n] / a;
}
for (j = 0; j <= i - 1; j++) for (j = 0; j <= i - 1; j++)
for (k = 0; k <= j; k++) A[j + k * n] -= L[i + k * n] * L[i + j * n]; {
for (j = 0; j <= i; j++) L[i + j * n] /= L[i + i * n]; for (k = 0; k <= j; k++)
{
A[j + k * n] -= L[i + k * n] * L[i + j * n];
}
}
for (j = 0; j <= i; j++)
{
L[i + j * n] /= L[i + i * n];
}
} }
free(A); free(A);
if (info) fprintf(stderr, "%s : LD factorization error\n", __FILE__); if (info)
{
fprintf(stderr, "%s : LD factorization error\n", __FILE__);
}
return info; return info;
} }
@ -87,8 +101,14 @@ void gauss(int n, double *L, double *Z, int i, int j)
if ((mu = static_cast<int> ROUND_LAMBDA(L[i + j * n])) != 0) if ((mu = static_cast<int> ROUND_LAMBDA(L[i + j * n])) != 0)
{ {
for (k = i; k < n; k++) L[k + n * j] -= static_cast<double>(mu) * L[k + i * n]; for (k = i; k < n; k++)
for (k = 0; k < n; k++) Z[k + n * j] -= static_cast<double>(mu) * Z[k + i * n]; {
L[k + n * j] -= static_cast<double>(mu) * L[k + i * n];
}
for (k = 0; k < n; k++)
{
Z[k + n * j] -= static_cast<double>(mu) * Z[k + i * n];
}
} }
} }
@ -111,8 +131,14 @@ void perm(int n, double *L, double *D, int j, double del, double *Z)
L[j + 1 + k * n] = eta * a0 + lam * a1; L[j + 1 + k * n] = eta * a0 + lam * a1;
} }
L[j + 1 + j * n] = lam; L[j + 1 + j * n] = lam;
for (k = j + 2; k < n; k++) SWAP_LAMBDA(L[k + j * n], L[k + (j + 1) * n]); for (k = j + 2; k < n; k++)
for (k = 0; k < n; k++) SWAP_LAMBDA(Z[k + j * n], Z[k + (j + 1) * n]); {
SWAP_LAMBDA(L[k + j * n], L[k + (j + 1) * n]);
}
for (k = 0; k < n; k++)
{
SWAP_LAMBDA(Z[k + j * n], Z[k + (j + 1) * n]);
}
} }
@ -127,7 +153,12 @@ void reduction(int n, double *L, double *D, double *Z)
while (j >= 0) while (j >= 0)
{ {
if (j <= k) if (j <= k)
for (i = j + 1; i < n; i++) gauss(n, L, Z, i, j); {
for (i = j + 1; i < n; i++)
{
gauss(n, L, Z, i, j);
}
}
del = D[j] + L[j + 1 + j * n] * L[j + 1 + j * n] * D[j + 1]; del = D[j] + L[j + 1 + j * n] * L[j + 1 + j * n] * D[j + 1];
if (del + 1E-6 < D[j + 1]) if (del + 1E-6 < D[j + 1])
{ /* compared considering numerical error */ { /* compared considering numerical error */
@ -136,8 +167,10 @@ void reduction(int n, double *L, double *D, double *Z)
j = n - 2; j = n - 2;
} }
else else
{
j--; j--;
} }
}
} }
@ -164,7 +197,9 @@ int search(int n, int m, const double *L, const double *D,
{ {
dist[--k] = newdist; dist[--k] = newdist;
for (i = 0; i <= k; i++) for (i = 0; i <= k; i++)
{
S[k + i * n] = S[k + 1 + i * n] + (z[k + 1] - zb[k + 1]) * L[k + 1 + i * n]; S[k + i * n] = S[k + 1 + i * n] + (z[k + 1] - zb[k + 1]) * L[k + 1 + i * n];
}
zb[k] = zs[k] + S[k + k * n]; zb[k] = zs[k] + S[k + k * n];
z[k] = ROUND_LAMBDA(zb[k]); z[k] = ROUND_LAMBDA(zb[k]);
y = zb[k] - z[k]; y = zb[k] - z[k];
@ -174,18 +209,32 @@ int search(int n, int m, const double *L, const double *D,
{ {
if (nn < m) if (nn < m)
{ {
if (nn == 0 || newdist > s[imax]) imax = nn; if (nn == 0 || newdist > s[imax])
for (i = 0; i < n; i++) zn[i + nn * n] = z[i]; {
imax = nn;
}
for (i = 0; i < n; i++)
{
zn[i + nn * n] = z[i];
}
s[nn++] = newdist; s[nn++] = newdist;
} }
else else
{ {
if (newdist < s[imax]) if (newdist < s[imax])
{ {
for (i = 0; i < n; i++) zn[i + imax * n] = z[i]; for (i = 0; i < n; i++)
{
zn[i + imax * n] = z[i];
}
s[imax] = newdist; s[imax] = newdist;
for (i = imax = 0; i < m; i++) for (i = imax = 0; i < m; i++)
if (s[imax] < s[i]) imax = i; {
if (s[imax] < s[i])
{
imax = i;
}
}
} }
maxdist = s[imax]; maxdist = s[imax];
} }
@ -197,7 +246,9 @@ int search(int n, int m, const double *L, const double *D,
else else
{ {
if (k == n - 1) if (k == n - 1)
{
break; break;
}
k++; k++;
z[k] += step[k]; z[k] += step[k];
@ -209,9 +260,15 @@ int search(int n, int m, const double *L, const double *D,
{ /* sort by s */ { /* sort by s */
for (j = i + 1; j < m; j++) for (j = i + 1; j < m; j++)
{ {
if (s[i] < s[j]) continue; if (s[i] < s[j])
{
continue;
}
SWAP_LAMBDA(s[i], s[j]); SWAP_LAMBDA(s[i], s[j]);
for (k = 0; k < n; k++) SWAP_LAMBDA(zn[k + i * n], zn[k + j * n]); for (k = 0; k < n; k++)
{
SWAP_LAMBDA(zn[k + i * n], zn[k + j * n]);
}
} }
} }
free(S); free(S);
@ -247,7 +304,10 @@ int lambda(int n, int m, const double *a, const double *Q, double *F,
int info; int info;
double *L, *D, *Z, *z, *E; double *L, *D, *Z, *z, *E;
if (n <= 0 || m <= 0) return -1; if (n <= 0 || m <= 0)
{
return -1;
}
L = zeros(n, n); L = zeros(n, n);
D = mat(n, 1); D = mat(n, 1);
Z = eye(n); Z = eye(n);
@ -288,16 +348,21 @@ int lambda_reduction(int n, const double *Q, double *Z)
double *L, *D; double *L, *D;
int i, j, info; int i, j, info;
if (n <= 0) return -1; if (n <= 0)
{
return -1;
}
L = zeros(n, n); L = zeros(n, n);
D = mat(n, 1); D = mat(n, 1);
for (i = 0; i < n; i++) for (i = 0; i < n; i++)
{
for (j = 0; j < n; j++) for (j = 0; j < n; j++)
{ {
Z[i + j * n] = i == j ? 1.0 : 0.0; Z[i + j * n] = i == j ? 1.0 : 0.0;
} }
}
/* LD factorization */ /* LD factorization */
if ((info = LD(n, Q, L, D))) if ((info = LD(n, Q, L, D)))
{ {
@ -330,7 +395,10 @@ int lambda_search(int n, int m, const double *a, const double *Q,
double *L, *D; double *L, *D;
int info; int info;
if (n <= 0 || m <= 0) return -1; if (n <= 0 || m <= 0)
{
return -1;
}
L = zeros(n, n); L = zeros(n, n);
D = mat(n, 1); D = mat(n, 1);

172
src/algorithms/libs/rtklib/rtklib_pntpos.cc
View File

@ -62,7 +62,10 @@ double varerr(const prcopt_t *opt, double el, int sys)
double fact, varr; double fact, varr;
fact = sys == SYS_GLO ? EFACT_GLO : (sys == SYS_SBS ? EFACT_SBS : EFACT_GPS); fact = sys == SYS_GLO ? EFACT_GLO : (sys == SYS_SBS ? EFACT_SBS : EFACT_GPS);
varr = std::pow(opt->err[0], 2.0) * (std::pow(opt->err[1], 2.0) + std::pow(opt->err[2], 2.0) / sin(el)); varr = std::pow(opt->err[0], 2.0) * (std::pow(opt->err[1], 2.0) + std::pow(opt->err[2], 2.0) / sin(el));
if (opt->ionoopt == IONOOPT_IFLC) varr *= std::pow(2, 3.0); /* iono-free */ if (opt->ionoopt == IONOOPT_IFLC)
{
varr *= std::pow(2, 3.0); /* iono-free */
}
return std::pow(fact, 2.0) * varr; return std::pow(fact, 2.0) * varr;
} }
@ -73,7 +76,10 @@ double gettgd(int sat, const nav_t *nav)
int i; int i;
for (i = 0; i < nav->n; i++) for (i = 0; i < nav->n; i++)
{ {
if (nav->eph[i].sat != sat) continue; if (nav->eph[i].sat != sat)
{
continue;
}
return SPEED_OF_LIGHT * nav->eph[i].tgd[0]; return SPEED_OF_LIGHT * nav->eph[i].tgd[0];
} }
return 0.0; return 0.0;
@ -84,7 +90,10 @@ double getiscl1(int sat, const nav_t *nav)
{ {
for (int i = 0; i < nav->n; i++) for (int i = 0; i < nav->n; i++)
{ {
if (nav->eph[i].sat != sat) continue; if (nav->eph[i].sat != sat)
{
continue;
}
return SPEED_OF_LIGHT * nav->eph[i].isc[0]; return SPEED_OF_LIGHT * nav->eph[i].isc[0];
} }
return 0.0; return 0.0;
@ -94,7 +103,10 @@ double getiscl2(int sat, const nav_t *nav)
{ {
for (int i = 0; i < nav->n; i++) for (int i = 0; i < nav->n; i++)
{ {
if (nav->eph[i].sat != sat) continue; if (nav->eph[i].sat != sat)
{
continue;
}
return SPEED_OF_LIGHT * nav->eph[i].isc[1]; return SPEED_OF_LIGHT * nav->eph[i].isc[1];
} }
return 0.0; return 0.0;
@ -104,7 +116,10 @@ double getiscl5i(int sat, const nav_t *nav)
{ {
for (int i = 0; i < nav->n; i++) for (int i = 0; i < nav->n; i++)
{ {
if (nav->eph[i].sat != sat) continue; if (nav->eph[i].sat != sat)
{
continue;
}
return SPEED_OF_LIGHT * nav->eph[i].isc[2]; return SPEED_OF_LIGHT * nav->eph[i].isc[2];
} }
return 0.0; return 0.0;
@ -114,7 +129,10 @@ double getiscl5q(int sat, const nav_t *nav)
{ {
for (int i = 0; i < nav->n; i++) for (int i = 0; i < nav->n; i++)
{ {
if (nav->eph[i].sat != sat) continue; if (nav->eph[i].sat != sat)
{
continue;
}
return SPEED_OF_LIGHT * nav->eph[i].isc[3]; return SPEED_OF_LIGHT * nav->eph[i].isc[3];
} }
return 0.0; return 0.0;
@ -407,7 +425,10 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
trace(3, "resprng : n=%d\n", n); trace(3, "resprng : n=%d\n", n);
for (i = 0; i < 3; i++) rr[i] = x[i]; for (i = 0; i < 3; i++)
{
rr[i] = x[i];
}
dtr = x[3]; dtr = x[3];
ecef2pos(rr, pos); ecef2pos(rr, pos);
@ -417,7 +438,10 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
vsat[i] = 0; vsat[i] = 0;
azel[i * 2] = azel[1 + i * 2] = resp[i] = 0.0; azel[i * 2] = azel[1 + i * 2] = resp[i] = 0.0;
if (!(sys = satsys(obs[i].sat, nullptr))) continue; if (!(sys = satsys(obs[i].sat, nullptr)))
{
continue;
}
/* reject duplicated observation data */ /* reject duplicated observation data */
if (i < n - 1 && i < MAXOBS - 1 && obs[i].sat == obs[i + 1].sat) if (i < n - 1 && i < MAXOBS - 1 && obs[i].sat == obs[i + 1].sat)
@ -477,7 +501,10 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
v[nv] = P - (r + dtr - SPEED_OF_LIGHT * dts[i * 2] + dion + dtrp); v[nv] = P - (r + dtr - SPEED_OF_LIGHT * dts[i * 2] + dion + dtrp);
/* design matrix */ /* design matrix */
for (j = 0; j < NX; j++) H[j + nv * NX] = j < 3 ? -e[j] : (j == 3 ? 1.0 : 0.0); for (j = 0; j < NX; j++)
{
H[j + nv * NX] = j < 3 ? -e[j] : (j == 3 ? 1.0 : 0.0);
}
/* time system and receiver bias offset correction */ /* time system and receiver bias offset correction */
if (sys == SYS_GLO) if (sys == SYS_GLO)
@ -499,7 +526,9 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
mask[3] = 1; mask[3] = 1;
} }
else else
{
mask[0] = 1; mask[0] = 1;
}
vsat[i] = 1; vsat[i] = 1;
resp[i] = v[nv]; resp[i] = v[nv];
@ -514,9 +543,15 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
/* constraint to avoid rank-deficient */ /* constraint to avoid rank-deficient */
for (i = 0; i < 4; i++) for (i = 0; i < 4; i++)
{ {
if (mask[i]) continue; if (mask[i])
{
continue;
}
v[nv] = 0.0; v[nv] = 0.0;
for (j = 0; j < NX; j++) H[j + nv * NX] = j == i + 3 ? 1.0 : 0.0; for (j = 0; j < NX; j++)
{
H[j + nv * NX] = j == i + 3 ? 1.0 : 0.0;
}
var[nv++] = 0.01; var[nv++] = 0.01;
} }
return nv; return nv;
@ -544,7 +579,10 @@ int valsol(const double *azel, const int *vsat, int n,
/* large gdop check */ /* large gdop check */
for (i = ns = 0; i < n; i++) for (i = ns = 0; i < n; i++)
{ {
if (!vsat[i]) continue; if (!vsat[i])
{
continue;
}
azels[ns * 2] = azel[i * 2]; azels[ns * 2] = azel[i * 2];
azels[1 + ns * 2] = azel[1 + i * 2]; azels[1 + ns * 2] = azel[1 + i * 2];
ns++; ns++;
@ -574,7 +612,10 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
H = mat(NX, n + 4); H = mat(NX, n + 4);
var = mat(n + 4, 1); var = mat(n + 4, 1);
for (i = 0; i < 3; i++) x[i] = sol->rr[i]; for (i = 0; i < 3; i++)
{
x[i] = sol->rr[i];
}
for (i = 0; i < MAXITR; i++) for (i = 0; i < MAXITR; i++)
{ {
@ -591,7 +632,10 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
{ {
sig = sqrt(var[j]); sig = sqrt(var[j]);
v[j] /= sig; v[j] /= sig;
for (k = 0; k < NX; k++) H[k + j * NX] /= sig; for (k = 0; k < NX; k++)
{
H[k + j * NX] /= sig;
}
} }
/* least square estimation */ /* least square estimation */
if ((info = lsq(H, v, NX, nv, dx, Q))) if ((info = lsq(H, v, NX, nv, dx, Q)))
@ -599,7 +643,10 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
sprintf(msg, "lsq error info=%d", info); sprintf(msg, "lsq error info=%d", info);
break; break;
} }
for (j = 0; j < NX; j++) x[j] += dx[j]; for (j = 0; j < NX; j++)
{
x[j] += dx[j];
}
if (norm_rtk(dx, NX) < 1e-4) if (norm_rtk(dx, NX) < 1e-4)
{ {
@ -609,8 +656,14 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
sol->dtr[1] = x[4] / SPEED_OF_LIGHT; /* glo-gps time offset (s) */ sol->dtr[1] = x[4] / SPEED_OF_LIGHT; /* glo-gps time offset (s) */
sol->dtr[2] = x[5] / SPEED_OF_LIGHT; /* gal-gps time offset (s) */ sol->dtr[2] = x[5] / SPEED_OF_LIGHT; /* gal-gps time offset (s) */
sol->dtr[3] = x[6] / SPEED_OF_LIGHT; /* bds-gps time offset (s) */ sol->dtr[3] = x[6] / SPEED_OF_LIGHT; /* bds-gps time offset (s) */
for (j = 0; j < 6; j++) sol->rr[j] = j < 3 ? x[j] : 0.0; for (j = 0; j < 6; j++)
for (j = 0; j < 3; j++) sol->qr[j] = static_cast<float>(Q[j + j * NX]); {
sol->rr[j] = j < 3 ? x[j] : 0.0;
}
for (j = 0; j < 3; j++)
{
sol->qr[j] = static_cast<float>(Q[j + j * NX]);
}
sol->qr[3] = static_cast<float>(Q[1]); /* cov xy */ sol->qr[3] = static_cast<float>(Q[1]); /* cov xy */
sol->qr[4] = static_cast<float>(Q[2 + NX]); /* cov yz */ sol->qr[4] = static_cast<float>(Q[2 + NX]); /* cov yz */
sol->qr[5] = static_cast<float>(Q[2]); /* cov zx */ sol->qr[5] = static_cast<float>(Q[2]); /* cov zx */
@ -629,7 +682,10 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
return stat; return stat;
} }
} }
if (i >= MAXITR) sprintf(msg, "iteration divergent i=%d", i); if (i >= MAXITR)
{
sprintf(msg, "iteration divergent i=%d", i);
}
free(v); free(v);
free(H); free(H);
@ -653,7 +709,10 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
trace(3, "raim_fde: %s n=%2d\n", time_str(obs[0].time, 0), n); trace(3, "raim_fde: %s n=%2d\n", time_str(obs[0].time, 0), n);
if (!(obs_e = static_cast<obsd_t *>(malloc(sizeof(obsd_t) * n)))) return 0; if (!(obs_e = static_cast<obsd_t *>(malloc(sizeof(obsd_t) * n))))
{
return 0;
}
rs_e = mat(6, n); rs_e = mat(6, n);
dts_e = mat(2, n); dts_e = mat(2, n);
vare_e = mat(1, n); vare_e = mat(1, n);
@ -667,7 +726,10 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
/* satellite exclution */ /* satellite exclution */
for (j = k = 0; j < n; j++) for (j = k = 0; j < n; j++)
{ {
if (j == i) continue; if (j == i)
{
continue;
}
obs_e[k] = obs[j]; obs_e[k] = obs[j];
matcpy(rs_e + 6 * k, rs + 6 * j, 6, 1); matcpy(rs_e + 6 * k, rs + 6 * j, 6, 1);
matcpy(dts_e + 2 * k, dts + 2 * j, 2, 1); matcpy(dts_e + 2 * k, dts + 2 * j, 2, 1);
@ -683,7 +745,10 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
} }
for (j = nvsat = 0, rms_e = 0.0; j < n - 1; j++) for (j = nvsat = 0, rms_e = 0.0; j < n - 1; j++)
{ {
if (!vsat_e[j]) continue; if (!vsat_e[j])
{
continue;
}
rms_e += std::pow(resp_e[j], 2.0); rms_e += std::pow(resp_e[j], 2.0);
nvsat++; nvsat++;
} }
@ -697,12 +762,18 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
trace(3, "raim_fde: exsat=%2d rms=%8.3f\n", obs[i].sat, rms_e); trace(3, "raim_fde: exsat=%2d rms=%8.3f\n", obs[i].sat, rms_e);
if (rms_e > rms) continue; if (rms_e > rms)
{
continue;
}
/* save result */ /* save result */
for (j = k = 0; j < n; j++) for (j = k = 0; j < n; j++)
{ {
if (j == i) continue; if (j == i)
{
continue;
}
matcpy(azel + 2 * j, azel_e + 2 * k, 2, 1); matcpy(azel + 2 * j, azel_e + 2 * k, 2, 1);
vsat[j] = vsat_e[k]; vsat[j] = vsat_e[k];
resp[j] = resp_e[k++]; resp[j] = resp_e[k++];
@ -749,9 +820,18 @@ int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
for (i = 0; i < n && i < MAXOBS; i++) for (i = 0; i < n && i < MAXOBS; i++)
{ {
if (obs[i].code[0] != CODE_NONE) band = 0; if (obs[i].code[0] != CODE_NONE)
if (obs[i].code[1] != CODE_NONE) band = 1; {
if (obs[i].code[2] != CODE_NONE) band = 2; band = 0;
}
if (obs[i].code[1] != CODE_NONE)
{
band = 1;
}
if (obs[i].code[2] != CODE_NONE)
{
band = 2;
}
lam = nav->lam[obs[i].sat - 1][band]; lam = nav->lam[obs[i].sat - 1][band];
if (obs[i].D[band] == 0.0 || lam == 0.0 || !vsat[i] || norm_rtk(rs + 3 + i * 6, 3) <= 0.0) if (obs[i].D[band] == 0.0 || lam == 0.0 || !vsat[i] || norm_rtk(rs + 3 + i * 6, 3) <= 0.0)
@ -766,7 +846,10 @@ int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
matmul("TN", 3, 1, 3, 1.0, E, a, 0.0, e); matmul("TN", 3, 1, 3, 1.0, E, a, 0.0, e);
/* satellite velocity relative to receiver in ecef */ /* satellite velocity relative to receiver in ecef */
for (j = 0; j < 3; j++) vs[j] = rs[j + 3 + i * 6] - x[j]; for (j = 0; j < 3; j++)
{
vs[j] = rs[j + 3 + i * 6] - x[j];
}
/* range rate with earth rotation correction */ /* range rate with earth rotation correction */
rate = dot(vs, e, 3) + DEFAULT_OMEGA_EARTH_DOT / SPEED_OF_LIGHT * (rs[4 + i * 6] * rr[0] + rs[1 + i * 6] * x[0] - rs[3 + i * 6] * rr[1] - rs[i * 6] * x[1]); rate = dot(vs, e, 3) + DEFAULT_OMEGA_EARTH_DOT / SPEED_OF_LIGHT * (rs[4 + i * 6] * rr[0] + rs[1 + i * 6] * x[0] - rs[3 + i * 6] * rr[1] - rs[i * 6] * x[1]);
@ -775,7 +858,10 @@ int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
v[nv] = -lam * obs[i].D[band] - (rate + x[3] - SPEED_OF_LIGHT * dts[1 + i * 2]); v[nv] = -lam * obs[i].D[band] - (rate + x[3] - SPEED_OF_LIGHT * dts[1 + i * 2]);
/* design matrix */ /* design matrix */
for (j = 0; j < 4; j++) H[j + nv * 4] = j < 3 ? -e[j] : 1.0; for (j = 0; j < 4; j++)
{
H[j + nv * 4] = j < 3 ? -e[j] : 1.0;
}
nv++; nv++;
} }
@ -804,13 +890,22 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
break; break;
} }
/* least square estimation */ /* least square estimation */
if (lsq(H, v, 4, nv, dx, Q)) break; if (lsq(H, v, 4, nv, dx, Q))
{
break;
}
for (j = 0; j < 4; j++) x[j] += dx[j]; for (j = 0; j < 4; j++)
{
x[j] += dx[j];
}
if (norm_rtk(dx, 4) < 1e-6) if (norm_rtk(dx, 4) < 1e-6)
{ {
for (i = 0; i < 3; i++) sol->rr[i + 3] = x[i]; for (i = 0; i < 3; i++)
{
sol->rr[i + 3] = x[i];
}
break; break;
} }
} }
@ -882,11 +977,17 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav,
stat = raim_fde(obs, n, rs, dts, var, svh, nav, &opt_, sol, azel_, vsat, resp, msg); stat = raim_fde(obs, n, rs, dts, var, svh, nav, &opt_, sol, azel_, vsat, resp, msg);
} }
/* estimate receiver velocity with doppler */ /* estimate receiver velocity with doppler */
if (stat) estvel(obs, n, rs, dts, nav, &opt_, sol, azel_, vsat); if (stat)
{
estvel(obs, n, rs, dts, nav, &opt_, sol, azel_, vsat);
}
if (azel) if (azel)
{ {
for (i = 0; i < n * 2; i++) azel[i] = azel_[i]; for (i = 0; i < n * 2; i++)
{
azel[i] = azel_[i];
}
} }
if (ssat) if (ssat)
{ {
@ -902,7 +1003,10 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav,
ssat[obs[i].sat - 1].azel[0] = azel_[i * 2]; ssat[obs[i].sat - 1].azel[0] = azel_[i * 2];
ssat[obs[i].sat - 1].azel[1] = azel_[1 + i * 2]; ssat[obs[i].sat - 1].azel[1] = azel_[1 + i * 2];
ssat[obs[i].sat - 1].snr[0] = obs[i].SNR[0]; ssat[obs[i].sat - 1].snr[0] = obs[i].SNR[0];
if (!vsat[i]) continue; if (!vsat[i])
{
continue;
}
ssat[obs[i].sat - 1].vs = 1; ssat[obs[i].sat - 1].vs = 1;
ssat[obs[i].sat - 1].resp[0] = resp[i]; ssat[obs[i].sat - 1].resp[0] = resp[i];
} }

385
src/algorithms/libs/rtklib/rtklib_ppp.cc
View File

@ -117,8 +117,14 @@ double p_gamma(double a, double x, double log_gamma_a)
double y, w; double y, w;
int i; int i;
if (x == 0.0) return 0.0; if (x == 0.0)
if (x >= a + 1.0) return 1.0 - q_gamma(a, x, log_gamma_a); {
return 0.0;
}
if (x >= a + 1.0)
{
return 1.0 - q_gamma(a, x, log_gamma_a);
}
y = w = exp(a * log(x) - x - log_gamma_a) / a; y = w = exp(a * log(x) - x - log_gamma_a) / a;
@ -126,7 +132,10 @@ double p_gamma(double a, double x, double log_gamma_a)
{ {
w *= x / (a + i); w *= x / (a + i);
y += w; y += w;
if (fabs(w) < 1E-15) break; if (fabs(w) < 1E-15)
{
break;
}
} }
return y; return y;
} }
@ -137,7 +146,10 @@ double q_gamma(double a, double x, double log_gamma_a)
double y, w, la = 1.0, lb = x + 1.0 - a, lc; double y, w, la = 1.0, lb = x + 1.0 - a, lc;
int i; int i;
if (x < a + 1.0) return 1.0 - p_gamma(a, x, log_gamma_a); if (x < a + 1.0)
{
return 1.0 - p_gamma(a, x, log_gamma_a);
}
w = exp(-x + a * log(x) - log_gamma_a); w = exp(-x + a * log(x) - log_gamma_a);
y = w / lb; y = w / lb;
for (i = 2; i < 100; i++) for (i = 2; i < 100; i++)
@ -147,7 +159,10 @@ double q_gamma(double a, double x, double log_gamma_a)
lb = lc; lb = lc;
w *= (i - 1 - a) / i; w *= (i - 1 - a) / i;
y += w / la / lb; y += w / la / lb;
if (fabs(w / la / lb) < 1E-15) break; if (fabs(w / la / lb) < 1E-15)
{
break;
}
} }
return y; return y;
} }
@ -186,9 +201,15 @@ void average_LC(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav __attribu
{ {
sat = obs[i].sat; sat = obs[i].sat;
if (azel[1 + 2 * i] < rtk->opt.elmin) continue; if (azel[1 + 2 * i] < rtk->opt.elmin)
{
continue;
}
if (satsys(sat, nullptr) != SYS_GPS) continue; if (satsys(sat, nullptr) != SYS_GPS)
{
continue;
}
/* triple-freq carrier and code LC (m) */ /* triple-freq carrier and code LC (m) */
LC1 = L_LC(1, -1, 0, obs[i].L) - P_LC(1, 1, 0, obs[i].P); LC1 = L_LC(1, -1, 0, obs[i].L) - P_LC(1, 1, 0, obs[i].P);
@ -212,7 +233,10 @@ void average_LC(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav __attribu
amb->LC[0] = amb->LC[1] = amb->LC[2] = 0.0; amb->LC[0] = amb->LC[1] = amb->LC[2] = 0.0;
amb->LCv[0] = amb->LCv[1] = amb->LCv[2] = 0.0; amb->LCv[0] = amb->LCv[1] = amb->LCv[2] = 0.0;
amb->fixcnt = 0; amb->fixcnt = 0;
for (j = 0; j < MAXSAT; j++) amb->flags[j] = 0; for (j = 0; j < MAXSAT; j++)
{
amb->flags[j] = 0;
}
} }
/* averaging */ /* averaging */
if (LC1) if (LC1)
@ -246,7 +270,10 @@ int fix_amb_WL(rtk_t *rtk, const nav_t *nav, int sat1, int sat2, int *NW)
amb1 = rtk->ambc + sat1 - 1; amb1 = rtk->ambc + sat1 - 1;
amb2 = rtk->ambc + sat2 - 1; amb2 = rtk->ambc + sat2 - 1;
if (!amb1->n[0] || !amb2->n[0]) return 0; if (!amb1->n[0] || !amb2->n[0])
{
return 0;
}
/* wide-lane ambiguity */ /* wide-lane ambiguity */
#ifndef REV_WL_FCB #ifndef REV_WL_FCB
@ -285,15 +312,27 @@ int is_depend(int sat1, int sat2, int *flgs, int *max_flg)
else if (flgs[sat1 - 1] > flgs[sat2 - 1]) else if (flgs[sat1 - 1] > flgs[sat2 - 1])
{ {
for (i = 0; i < MAXSAT; i++) for (i = 0; i < MAXSAT; i++)
if (flgs[i] == flgs[sat2 - 1]) flgs[i] = flgs[sat1 - 1]; {
if (flgs[i] == flgs[sat2 - 1])
{
flgs[i] = flgs[sat1 - 1];
}
}
} }
else if (flgs[sat1 - 1] < flgs[sat2 - 1]) else if (flgs[sat1 - 1] < flgs[sat2 - 1])
{ {
for (i = 0; i < MAXSAT; i++) for (i = 0; i < MAXSAT; i++)
if (flgs[i] == flgs[sat1 - 1]) flgs[i] = flgs[sat2 - 1]; {
if (flgs[i] == flgs[sat1 - 1])
{
flgs[i] = flgs[sat2 - 1];
}
}
} }
else else
{
return 0; /* linear depenent */ return 0; /* linear depenent */
}
return 1; return 1;
} }
@ -305,18 +344,26 @@ int sel_amb(int *sat1, int *sat2, double *N, double *var, int n)
/* sort by variance */ /* sort by variance */
for (i = 0; i < n; i++) for (i = 0; i < n; i++)
{
for (j = 1; j < n - i; j++) for (j = 1; j < n - i; j++)
{ {
if (var[j] >= var[j - 1]) continue; if (var[j] >= var[j - 1])
{
continue;
}
SWAP_I(sat1[j], sat1[j - 1]); SWAP_I(sat1[j], sat1[j - 1]);
SWAP_I(sat2[j], sat2[j - 1]); SWAP_I(sat2[j], sat2[j - 1]);
SWAP_D(N[j], N[j - 1]); SWAP_D(N[j], N[j - 1]);
SWAP_D(var[j], var[j - 1]); SWAP_D(var[j], var[j - 1]);
} }
}
/* select linearly independent satellite pair */ /* select linearly independent satellite pair */
for (i = j = 0; i < n; i++) for (i = j = 0; i < n; i++)
{ {
if (!is_depend(sat1[i], sat2[i], flgs, &max_flg)) continue; if (!is_depend(sat1[i], sat2[i], flgs, &max_flg))
{
continue;
}
sat1[j] = sat1[i]; sat1[j] = sat1[i];
sat2[j] = sat2[i]; sat2[j] = sat2[i];
N[j] = N[i]; N[j] = N[i];
@ -333,7 +380,10 @@ int fix_sol(rtk_t *rtk, const int *sat1, const int *sat2,
double *v, *H, *R; double *v, *H, *R;
int i, j, k, info; int i, j, k, info;
if (n <= 0) return 0; if (n <= 0)
{
return 0;
}
v = zeros(n, 1); v = zeros(n, 1);
H = zeros(rtk->nx, n); H = zeros(rtk->nx, n);
@ -420,7 +470,10 @@ int fix_amb_ROUND(rtk_t *rtk, int *sat1, int *sat2, const int *NW, int n)
/* check residuals */ /* check residuals */
v = rtk->ssat[sat1[i] - 1].resc[0] - rtk->ssat[sat2[i] - 1].resc[0]; v = rtk->ssat[sat1[i] - 1].resc[0] - rtk->ssat[sat2[i] - 1].resc[0];
vc = v + (BC - (rtk->x[j] - rtk->x[k])); vc = v + (BC - (rtk->x[j] - rtk->x[k]));
if (fabs(vc) > THRES_RES) continue; if (fabs(vc) > THRES_RES)
{
continue;
}
sat1[m] = sat1[i]; sat1[m] = sat1[i];
sat2[m] = sat2[i]; sat2[m] = sat2[i];
@ -461,7 +514,10 @@ int fix_amb_ILS(rtk_t *rtk, int *sat1, int *sat2, int *NW, int n)
for (i = 0; i < n; i++) for (i = 0; i < n; i++)
{ {
/* check linear independency */ /* check linear independency */
if (!is_depend(sat1[i], sat2[i], flgs, &max_flg)) continue; if (!is_depend(sat1[i], sat2[i], flgs, &max_flg))
{
continue;
}
j = IB_PPP(sat1[i], &rtk->opt); j = IB_PPP(sat1[i], &rtk->opt);
k = IB_PPP(sat2[i], &rtk->opt); k = IB_PPP(sat2[i], &rtk->opt);
@ -471,7 +527,10 @@ int fix_amb_ILS(rtk_t *rtk, int *sat1, int *sat2, int *NW, int n)
N1[m] = ROUND_PPP(B1[m]); N1[m] = ROUND_PPP(B1[m]);
/* validation of narrow-lane ambiguity */ /* validation of narrow-lane ambiguity */
if (fabs(N1[m] - B1[m]) > rtk->opt.thresar[2]) continue; if (fabs(N1[m] - B1[m]) > rtk->opt.thresar[2])
{
continue;
}
/* narrow-lane ambiguity transformation matrix */ /* narrow-lane ambiguity transformation matrix */
D[j + m * rtk->nx] = 1.0 / lam_NL; D[j + m * rtk->nx] = 1.0 / lam_NL;
@ -562,7 +621,10 @@ int pppamb(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav,
double elmask; double elmask;
int i, j, m = 0, stat = 0, *NW, *sat1, *sat2; int i, j, m = 0, stat = 0, *NW, *sat1, *sat2;
if (n <= 0 || rtk->opt.ionoopt != IONOOPT_IFLC || rtk->opt.nf < 2) return 0; if (n <= 0 || rtk->opt.ionoopt != IONOOPT_IFLC || rtk->opt.nf < 2)
{
return 0;
}
trace(3, "pppamb: time=%s n=%d\n", time_str(obs[0].time, 0), n); trace(3, "pppamb: time=%s n=%d\n", time_str(obs[0].time, 0), n);
@ -577,11 +639,15 @@ int pppamb(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav,
/* fix wide-lane ambiguity */ /* fix wide-lane ambiguity */
for (i = 0; i < n - 1; i++) for (i = 0; i < n - 1; i++)
{
for (j = i + 1; j < n; j++) for (j = i + 1; j < n; j++)
{ {
if (!rtk->ssat[obs[i].sat - 1].vsat[0] || if (!rtk->ssat[obs[i].sat - 1].vsat[0] ||
!rtk->ssat[obs[j].sat - 1].vsat[0] || !rtk->ssat[obs[j].sat - 1].vsat[0] ||
azel[1 + i * 2] < elmask || azel[1 + j * 2] < elmask) continue; azel[1 + i * 2] < elmask || azel[1 + j * 2] < elmask)
{
continue;
}
#if 0 #if 0
/* test already fixed */ /* test already fixed */
if (rtk->ambc[obs[i].sat-1].flags[obs[j].sat-1] && if (rtk->ambc[obs[i].sat-1].flags[obs[j].sat-1] &&
@ -589,7 +655,11 @@ int pppamb(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav,
#endif #endif
sat1[m] = obs[i].sat; sat1[m] = obs[i].sat;
sat2[m] = obs[j].sat; sat2[m] = obs[j].sat;
if (fix_amb_WL(rtk, nav, sat1[m], sat2[m], NW + m)) m++; if (fix_amb_WL(rtk, nav, sat1[m], sat2[m], NW + m))
{
m++;
}
}
} }
/* fix narrow-lane ambiguity */ /* fix narrow-lane ambiguity */
if (rtk->opt.modear == ARMODE_PPPAR) if (rtk->opt.modear == ARMODE_PPPAR)
@ -615,7 +685,10 @@ void pppoutsolstat(rtk_t *rtk, int level, FILE *fp)
int i, j, week, nfreq = 1; int i, j, week, nfreq = 1;
char id[32]; char id[32];
if (level <= 0 || !fp) return; if (level <= 0 || !fp)
{
return;
}
trace(3, "pppoutsolstat:\n"); trace(3, "pppoutsolstat:\n");
@ -654,13 +727,19 @@ void pppoutsolstat(rtk_t *rtk, int level, FILE *fp)
fprintf(fp, "$TRPG,%d,%.3f,%d,%d,%.5f,%.5f,%.5f,%.5f\n", week, tow, fprintf(fp, "$TRPG,%d,%.3f,%d,%d,%.5f,%.5f,%.5f,%.5f\n", week, tow,
rtk->sol.stat, 1, rtk->x[i + 1], rtk->x[i + 2], 0.0, 0.0); rtk->sol.stat, 1, rtk->x[i + 1], rtk->x[i + 2], 0.0, 0.0);
} }
if (rtk->sol.stat == SOLQ_NONE || level <= 1) return; if (rtk->sol.stat == SOLQ_NONE || level <= 1)
{
return;
}
/* residuals and status */ /* residuals and status */
for (i = 0; i < MAXSAT; i++) for (i = 0; i < MAXSAT; i++)
{ {
ssat = rtk->ssat + i; ssat = rtk->ssat + i;
if (!ssat->vs) continue; if (!ssat->vs)
{
continue;
}
satno2id(i + 1, id); satno2id(i + 1, id);
for (j = 0; j < nfreq; j++) for (j = 0; j < nfreq; j++)
{ {
@ -685,16 +764,25 @@ void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs)
/* unit vector of sun direction (ecef) */ /* unit vector of sun direction (ecef) */
sunmoonpos(gpst2utc(obs[0].time), erpv, rsun, nullptr, nullptr); sunmoonpos(gpst2utc(obs[0].time), erpv, rsun, nullptr, nullptr);
if (normv3(rsun, esun) == 0) trace(1, "Error computing the norm"); if (normv3(rsun, esun) == 0)
{
trace(1, "Error computing the norm");
}
for (i = 0; i < n; i++) for (i = 0; i < n; i++)
{ {
type = nav->pcvs[obs[i].sat - 1].type; type = nav->pcvs[obs[i].sat - 1].type;
if ((r = norm_rtk(rs + i * 6, 3)) <= 0.0) continue; if ((r = norm_rtk(rs + i * 6, 3)) <= 0.0)
{
continue;
}
#if 1 #if 1
/* only block IIA */ /* only block IIA */
if (*type && !strstr(type, "BLOCK IIA")) continue; if (*type && !strstr(type, "BLOCK IIA"))
{
continue;
}
#endif #endif
/* sun-earth-satellite angle */ /* sun-earth-satellite angle */
cosa = dot(rs + i * 6, esun, 3) / r; cosa = dot(rs + i * 6, esun, 3) / r;
@ -702,12 +790,18 @@ void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs)
ang = acos(cosa); ang = acos(cosa);
/* test eclipse */ /* test eclipse */
if (ang < PI / 2.0 || r * sin(ang) > RE_WGS84) continue; if (ang < PI / 2.0 || r * sin(ang) > RE_WGS84)
{
continue;
}
trace(2, "eclipsing sat excluded %s sat=%2d\n", time_str(obs[0].time, 0), trace(2, "eclipsing sat excluded %s sat=%2d\n", time_str(obs[0].time, 0),
obs[i].sat); obs[i].sat);
for (j = 0; j < 3; j++) rs[j + i * 6] = 0.0; for (j = 0; j < 3; j++)
{
rs[j + i * 6] = 0.0;
}
} }
} }
@ -746,9 +840,15 @@ double varerr(int sat __attribute__((unused)), int sys, double el, int type, con
} }
else else
{ /* normal error model */ { /* normal error model */
if (type == 1) fact *= opt->eratio[0]; if (type == 1)
{
fact *= opt->eratio[0];
}
fact *= sys == SYS_GLO ? EFACT_GLO : (sys == SYS_SBS ? EFACT_SBS : EFACT_GPS); fact *= sys == SYS_GLO ? EFACT_GLO : (sys == SYS_SBS ? EFACT_SBS : EFACT_GPS);
if (opt->ionoopt == IONOOPT_IFLC) fact *= 3.0; if (opt->ionoopt == IONOOPT_IFLC)
{
fact *= 3.0;
}
a = fact * opt->err[1]; a = fact * opt->err[1];
b = fact * opt->err[2]; b = fact * opt->err[2];
} }
@ -780,9 +880,15 @@ int ifmeas(const obsd_t *obs, const nav_t *nav, const double *azel,
trace(4, "ifmeas :\n"); trace(4, "ifmeas :\n");
/* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */
if (NFREQ >= 3 && (satsys(obs->sat, nullptr) & (SYS_GAL | SYS_SBS))) j = 2; if (NFREQ >= 3 && (satsys(obs->sat, nullptr) & (SYS_GAL | SYS_SBS)))
{
j = 2;
}
if (NFREQ < 2 || lam[i] == 0.0 || lam[j] == 0.0) return 0; if (NFREQ < 2 || lam[i] == 0.0 || lam[j] == 0.0)
{
return 0;
}
/* test snr mask */ /* test snr mask */
if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask) || if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask) ||
@ -804,18 +910,30 @@ int ifmeas(const obsd_t *obs, const nav_t *nav, const double *azel,
{ {
P1_C1 = nav->lexeph[obs->sat - 1].isc[0] * SPEED_OF_LIGHT; /* ISC_L1C/A */ P1_C1 = nav->lexeph[obs->sat - 1].isc[0] * SPEED_OF_LIGHT; /* ISC_L1C/A */
} }
if (L1 == 0.0 || L2 == 0.0 || P1 == 0.0 || P2 == 0.0) return 0; if (L1 == 0.0 || L2 == 0.0 || P1 == 0.0 || P2 == 0.0)
{
return 0;
}
/* iono-free phase with windup correction */ /* iono-free phase with windup correction */
meas[0] = c1 * L1 + c2 * L2 - (c1 * lam[i] + c2 * lam[j]) * phw; meas[0] = c1 * L1 + c2 * L2 - (c1 * lam[i] + c2 * lam[j]) * phw;
/* iono-free code with dcb correction */ /* iono-free code with dcb correction */
if (obs->code[i] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ if (obs->code[i] == CODE_L1C)
if (obs->code[j] == CODE_L2C) P2 += P2_C2; /* C2->P2 */ {
P1 += P1_C1; /* C1->P1 */
}
if (obs->code[j] == CODE_L2C)
{
P2 += P2_C2; /* C2->P2 */
}
meas[1] = c1 * P1 + c2 * P2; meas[1] = c1 * P1 + c2 * P2;
var[1] = std::pow(ERR_CBIAS, 2.0); var[1] = std::pow(ERR_CBIAS, 2.0);
if (opt->sateph == EPHOPT_SBAS) meas[1] -= P1_C1; /* sbas clock based C1 */ if (opt->sateph == EPHOPT_SBAS)
{
meas[1] -= P1_C1; /* sbas clock based C1 */
}
/* gps-glonass h/w bias correction for code */ /* gps-glonass h/w bias correction for code */
if (opt->exterr.ena[3] && satsys(obs->sat, nullptr) == SYS_GLO) if (opt->exterr.ena[3] && satsys(obs->sat, nullptr) == SYS_GLO)
@ -825,8 +943,14 @@ int ifmeas(const obsd_t *obs, const nav_t *nav, const double *azel,
/* antenna phase center variation correction */ /* antenna phase center variation correction */
for (k = 0; k < 2; k++) for (k = 0; k < 2; k++)
{ {
if (dants) meas[k] -= c1 * dants[i] + c2 * dants[j]; if (dants)
if (dantr) meas[k] -= c1 * dantr[i] + c2 * dantr[j]; {
meas[k] -= c1 * dants[i] + c2 * dants[j];
}
if (dantr)
{
meas[k] -= c1 * dantr[i] + c2 * dantr[j];
}
} }
return 1; return 1;
} }
@ -838,7 +962,10 @@ double gettgd_ppp(int sat, const nav_t *nav)
int i; int i;
for (i = 0; i < nav->n; i++) for (i = 0; i < nav->n; i++)
{ {
if (nav->eph[i].sat != sat) continue; if (nav->eph[i].sat != sat)
{
continue;
}
return SPEED_OF_LIGHT * nav->eph[i].tgd[0]; return SPEED_OF_LIGHT * nav->eph[i].tgd[0];
} }
return 0.0; return 0.0;
@ -903,9 +1030,15 @@ int corrmeas(const obsd_t *obs, const nav_t *nav, const double *pos,
{ {
return ifmeas(obs, nav, azel, opt, dantr, dants, phw, meas, var); return ifmeas(obs, nav, azel, opt, dantr, dants, phw, meas, var);
} }
if (lam[0] == 0.0 || obs->L[0] == 0.0 || obs->P[0] == 0.0) return 0; if (lam[0] == 0.0 || obs->L[0] == 0.0 || obs->P[0] == 0.0)
{
return 0;
}
if (testsnr(0, 0, azel[1], obs->SNR[0] * 0.25, &opt->snrmask)) return 0; if (testsnr(0, 0, azel[1], obs->SNR[0] * 0.25, &opt->snrmask))
{
return 0;
}
L1 = obs->L[0] * lam[0]; L1 = obs->L[0] * lam[0];
P1 = obs->P[0]; P1 = obs->P[0];
@ -918,7 +1051,10 @@ int corrmeas(const obsd_t *obs, const nav_t *nav, const double *pos,
{ {
P1_P2 = (1.0 - gamma) * gettgd_ppp(obs->sat, nav); P1_P2 = (1.0 - gamma) * gettgd_ppp(obs->sat, nav);
} }
if (obs->code[0] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ if (obs->code[0] == CODE_L1C)
{
P1 += P1_C1; /* C1->P1 */
}
PC = P1 - P1_P2 / (1.0 - gamma); /* P1->PC */ PC = P1 - P1_P2 / (1.0 - gamma); /* P1->PC */
/* slant ionospheric delay L1 (m) */ /* slant ionospheric delay L1 (m) */
@ -938,8 +1074,14 @@ int corrmeas(const obsd_t *obs, const nav_t *nav, const double *pos,
/* antenna phase center variation correction */ /* antenna phase center variation correction */
for (i = 0; i < 2; i++) for (i = 0; i < 2; i++)
{ {
if (dants) meas[i] -= dants[0]; if (dants)
if (dantr) meas[i] -= dantr[0]; {
meas[i] -= dants[0];
}
if (dantr)
{
meas[i] -= dantr[0];
}
} }
return 1; return 1;
} }
@ -950,7 +1092,10 @@ double gfmeas(const obsd_t *obs, const nav_t *nav)
{ {
const double *lam = nav->lam[obs->sat - 1]; const double *lam = nav->lam[obs->sat - 1];
if (lam[0] == 0.0 || lam[1] == 0.0 || obs->L[0] == 0.0 || obs->L[1] == 0.0) return 0.0; if (lam[0] == 0.0 || lam[1] == 0.0 || obs->L[0] == 0.0 || obs->L[1] == 0.0)
{
return 0.0;
}
return lam[0] * obs->L[0] - lam[1] * obs->L[1]; return lam[0] * obs->L[0] - lam[1] * obs->L[1];
} }
@ -966,16 +1111,25 @@ void udpos_ppp(rtk_t *rtk)
/* fixed mode */ /* fixed mode */
if (rtk->opt.mode == PMODE_PPP_FIXED) if (rtk->opt.mode == PMODE_PPP_FIXED)
{ {
for (i = 0; i < 3; i++) initx(rtk, rtk->opt.ru[i], 1E-8, i); for (i = 0; i < 3; i++)
{
initx(rtk, rtk->opt.ru[i], 1E-8, i);
}
return; return;
} }
/* initialize position for first epoch */ /* initialize position for first epoch */
if (norm_rtk(rtk->x, 3) <= 0.0) if (norm_rtk(rtk->x, 3) <= 0.0)
{ {
for (i = 0; i < 3; i++) initx(rtk, rtk->sol.rr[i], VAR_POS_PPP, i); for (i = 0; i < 3; i++)
{
initx(rtk, rtk->sol.rr[i], VAR_POS_PPP, i);
}
} }
/* static ppp mode */ /* static ppp mode */
if (rtk->opt.mode == PMODE_PPP_STATIC) return; if (rtk->opt.mode == PMODE_PPP_STATIC)
{
return;
}
/* kinmatic mode without dynamics */ /* kinmatic mode without dynamics */
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
@ -1027,7 +1181,10 @@ void udtrop_ppp(rtk_t *rtk)
if (rtk->opt.tropopt >= TROPOPT_ESTG) if (rtk->opt.tropopt >= TROPOPT_ESTG)
{ {
for (j = 0; j < 2; j++) initx(rtk, 1E-6, VAR_GRA_PPP, ++i); for (j = 0; j < 2; j++)
{
initx(rtk, 1E-6, VAR_GRA_PPP, ++i);
}
} }
} }
else else
@ -1053,14 +1210,19 @@ void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n)
trace(3, "detslp_ll: n=%d\n", n); trace(3, "detslp_ll: n=%d\n", n);
for (i = 0; i < n && i < MAXOBS; i++) for (i = 0; i < n && i < MAXOBS; i++)
{
for (j = 0; j < rtk->opt.nf; j++) for (j = 0; j < rtk->opt.nf; j++)
{ {
if (obs[i].L[j] == 0.0 || !(obs[i].LLI[j] & 3)) continue; if (obs[i].L[j] == 0.0 || !(obs[i].LLI[j] & 3))
{
continue;
}
trace(3, "detslp_ll: slip detected sat=%2d f=%d\n", obs[i].sat, j + 1); trace(3, "detslp_ll: slip detected sat=%2d f=%d\n", obs[i].sat, j + 1);
rtk->ssat[obs[i].sat - 1].slip[j] = 1; rtk->ssat[obs[i].sat - 1].slip[j] = 1;
} }
}
} }
@ -1074,7 +1236,10 @@ void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
for (i = 0; i < n && i < MAXOBS; i++) for (i = 0; i < n && i < MAXOBS; i++)
{ {
if ((g1 = gfmeas(obs + i, nav)) == 0.0) continue; if ((g1 = gfmeas(obs + i, nav)) == 0.0)
{
continue;
}
g0 = rtk->ssat[obs[i].sat - 1].gf; g0 = rtk->ssat[obs[i].sat - 1].gf;
rtk->ssat[obs[i].sat - 1].gf = g1; rtk->ssat[obs[i].sat - 1].gf = g1;
@ -1086,7 +1251,10 @@ void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
trace(3, "detslip_gf: slip detected sat=%2d gf=%8.3f->%8.3f\n", trace(3, "detslip_gf: slip detected sat=%2d gf=%8.3f->%8.3f\n",
obs[i].sat, g0, g1); obs[i].sat, g0, g1);
for (j = 0; j < rtk->opt.nf; j++) rtk->ssat[obs[i].sat - 1].slip[j] |= 1; for (j = 0; j < rtk->opt.nf; j++)
{
rtk->ssat[obs[i].sat - 1].slip[j] |= 1;
}
} }
} }
} }
@ -1101,10 +1269,12 @@ void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
trace(3, "udbias : n=%d\n", n); trace(3, "udbias : n=%d\n", n);
for (i = 0; i < MAXSAT; i++) for (i = 0; i < MAXSAT; i++)
{
for (j = 0; j < rtk->opt.nf; j++) for (j = 0; j < rtk->opt.nf; j++)
{ {
rtk->ssat[i].slip[j] = 0; rtk->ssat[i].slip[j] = 0;
} }
}
/* detect cycle slip by LLI */ /* detect cycle slip by LLI */
detslp_ll(rtk, obs, n); detslp_ll(rtk, obs, n);
@ -1126,7 +1296,10 @@ void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
sat = obs[i].sat; sat = obs[i].sat;
j = IB_PPP(sat, &rtk->opt); j = IB_PPP(sat, &rtk->opt);
if (!corrmeas(obs + i, nav, pos, rtk->ssat[sat - 1].azel, &rtk->opt, nullptr, nullptr, if (!corrmeas(obs + i, nav, pos, rtk->ssat[sat - 1].azel, &rtk->opt, nullptr, nullptr,
0.0, meas, var, &brk)) continue; 0.0, meas, var, &brk))
{
continue;
}
if (brk) if (brk)
{ {
@ -1135,7 +1308,10 @@ void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
} }
bias[i] = meas[0] - meas[1]; bias[i] = meas[0] - meas[1];
if (rtk->x[j] == 0.0 || if (rtk->x[j] == 0.0 ||
rtk->ssat[sat - 1].slip[0] || rtk->ssat[sat - 1].slip[1]) continue; rtk->ssat[sat - 1].slip[0] || rtk->ssat[sat - 1].slip[1])
{
continue;
}
offset += bias[i] - rtk->x[j]; offset += bias[i] - rtk->x[j];
k++; k++;
} }
@ -1145,7 +1321,10 @@ void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
for (i = 0; i < MAXSAT; i++) for (i = 0; i < MAXSAT; i++)
{ {
j = IB_PPP(i + 1, &rtk->opt); j = IB_PPP(i + 1, &rtk->opt);
if (rtk->x[j] != 0.0) rtk->x[j] += offset / k; if (rtk->x[j] != 0.0)
{
rtk->x[j] += offset / k;
}
} }
trace(2, "phase-code jump corrected: %s n=%2d dt=%12.9fs\n", trace(2, "phase-code jump corrected: %s n=%2d dt=%12.9fs\n",
time_str(rtk->sol.time, 0), k, offset / k / SPEED_OF_LIGHT); time_str(rtk->sol.time, 0), k, offset / k / SPEED_OF_LIGHT);
@ -1158,9 +1337,15 @@ void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
rtk->P[j + j * rtk->nx] += std::pow(rtk->opt.prn[0], 2.0) * fabs(rtk->tt); rtk->P[j + j * rtk->nx] += std::pow(rtk->opt.prn[0], 2.0) * fabs(rtk->tt);
if (rtk->x[j] != 0.0 && if (rtk->x[j] != 0.0 &&
!rtk->ssat[sat - 1].slip[0] && !rtk->ssat[sat - 1].slip[1]) continue; !rtk->ssat[sat - 1].slip[0] && !rtk->ssat[sat - 1].slip[1])
{
continue;
}
if (bias[i] == 0.0) continue; if (bias[i] == 0.0)
{
continue;
}
/* reinitialize phase-bias if detecting cycle slip */ /* reinitialize phase-bias if detecting cycle slip */
initx(rtk, bias[i], VAR_BIAS, IB_PPP(sat, &rtk->opt)); initx(rtk, bias[i], VAR_BIAS, IB_PPP(sat, &rtk->opt));
@ -1203,7 +1388,10 @@ void satantpcv(const double *rs, const double *rr, const pcv_t *pcv,
ru[i] = rr[i] - rs[i]; ru[i] = rr[i] - rs[i];
rz[i] = -rs[i]; rz[i] = -rs[i];
} }
if (!normv3(ru, eu) || !normv3(rz, ez)) return; if (!normv3(ru, eu) || !normv3(rz, ez))
{
return;
}
cosa = dot(eu, ez, 3); cosa = dot(eu, ez, 3);
cosa = cosa < -1.0 ? -1.0 : (cosa > 1.0 ? 1.0 : cosa); cosa = cosa < -1.0 ? -1.0 : (cosa > 1.0 ? 1.0 : cosa);
@ -1256,9 +1444,15 @@ int res_ppp(int iter __attribute__((unused)), const obsd_t *obs, int n, const do
trace(3, "res_ppp : n=%d nx=%d\n", n, nx); trace(3, "res_ppp : n=%d nx=%d\n", n, nx);
for (i = 0; i < MAXSAT; i++) rtk->ssat[i].vsat[0] = 0; for (i = 0; i < MAXSAT; i++)
{
rtk->ssat[i].vsat[0] = 0;
}
for (i = 0; i < 3; i++) rr[i] = x[i]; for (i = 0; i < 3; i++)
{
rr[i] = x[i];
}
/* earth tides correction */ /* earth tides correction */
if (opt->tidecorr) if (opt->tidecorr)
@ -1267,21 +1461,33 @@ int res_ppp(int iter __attribute__((unused)), const obsd_t *obs, int n, const do
tidedisp(gpst2utc(obs[0].time), rr, tideopt, &nav->erp, opt->odisp[0], tidedisp(gpst2utc(obs[0].time), rr, tideopt, &nav->erp, opt->odisp[0],
disp); disp);
for (i = 0; i < 3; i++) rr[i] += disp[i]; for (i = 0; i < 3; i++)
{
rr[i] += disp[i];
}
} }
ecef2pos(rr, pos); ecef2pos(rr, pos);
for (i = 0; i < n && i < MAXOBS; i++) for (i = 0; i < n && i < MAXOBS; i++)
{ {
sat = obs[i].sat; sat = obs[i].sat;
if (!(sys = satsys(sat, nullptr)) || !rtk->ssat[sat - 1].vs) continue; if (!(sys = satsys(sat, nullptr)) || !rtk->ssat[sat - 1].vs)
{
continue;
}
/* geometric distance/azimuth/elevation angle */ /* geometric distance/azimuth/elevation angle */
if ((r = geodist(rs + i * 6, rr, e)) <= 0.0 || if ((r = geodist(rs + i * 6, rr, e)) <= 0.0 ||
satazel(pos, e, azel + i * 2) < opt->elmin) continue; satazel(pos, e, azel + i * 2) < opt->elmin)
{
continue;
}
/* excluded satellite? */ /* excluded satellite? */
if (satexclude(obs[i].sat, svh[i], opt)) continue; if (satexclude(obs[i].sat, svh[i], opt))
{
continue;
}
/* tropospheric delay correction */ /* tropospheric delay correction */
if (opt->tropopt == TROPOPT_SAAS) if (opt->tropopt == TROPOPT_SAAS)
@ -1330,13 +1536,22 @@ int res_ppp(int iter __attribute__((unused)), const obsd_t *obs, int n, const do
for (j = 0; j < 2; j++) for (j = 0; j < 2; j++)
{ /* for phase and code */ { /* for phase and code */
if (meas[j] == 0.0) continue; if (meas[j] == 0.0)
{
continue;
}
for (k = 0; k < nx; k++) H[k + nx * nv] = 0.0; for (k = 0; k < nx; k++)
{
H[k + nx * nv] = 0.0;
}
v[nv] = meas[j] - r; v[nv] = meas[j] - r;
for (k = 0; k < 3; k++) H[k + nx * nv] = -e[k]; for (k = 0; k < 3; k++)
{
H[k + nx * nv] = -e[k];
}
if (sys != SYS_GLO) if (sys != SYS_GLO)
{ {
@ -1363,9 +1578,13 @@ int res_ppp(int iter __attribute__((unused)), const obsd_t *obs, int n, const do
var[nv] = varerr(obs[i].sat, sys, azel[1 + i * 2], j, opt) + varm[j] + vare[i] + vart; var[nv] = varerr(obs[i].sat, sys, azel[1 + i * 2], j, opt) + varm[j] + vare[i] + vart;
if (j == 0) if (j == 0)
{
rtk->ssat[sat - 1].resc[0] = v[nv]; rtk->ssat[sat - 1].resc[0] = v[nv];
}
else else
{
rtk->ssat[sat - 1].resp[0] = v[nv]; rtk->ssat[sat - 1].resp[0] = v[nv];
}
/* test innovation */ /* test innovation */
#if 0 #if 0
@ -1380,15 +1599,20 @@ int res_ppp(int iter __attribute__((unused)), const obsd_t *obs, int n, const do
rtk->ssat[sat - 1].rejc[0]++; rtk->ssat[sat - 1].rejc[0]++;
continue; continue;
} }
if (j == 0) rtk->ssat[sat - 1].vsat[0] = 1; if (j == 0)
{
rtk->ssat[sat - 1].vsat[0] = 1;
}
nv++; nv++;
} }
} }
for (i = 0; i < nv; i++) for (i = 0; i < nv; i++)
{
for (j = 0; j < nv; j++) for (j = 0; j < nv; j++)
{ {
R[i + j * nv] = i == j ? var[i] : 0.0; R[i + j * nv] = i == j ? var[i] : 0.0;
} }
}
trace(5, "x=\n"); trace(5, "x=\n");
tracemat(5, x, 1, nx, 8, 3); tracemat(5, x, 1, nx, 8, 3);
trace(5, "v=\n"); trace(5, "v=\n");
@ -1422,7 +1646,10 @@ void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
var = mat(1, n); var = mat(1, n);
azel = zeros(2, n); azel = zeros(2, n);
for (i = 0; i < MAXSAT; i++) rtk->ssat[i].fix[0] = 0; for (i = 0; i < MAXSAT; i++)
{
rtk->ssat[i].fix[0] = 0;
}
/* temporal update of states */ /* temporal update of states */
udstate_ppp(rtk, obs, n, nav); udstate_ppp(rtk, obs, n, nav);
@ -1449,7 +1676,10 @@ void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
for (i = 0; i < rtk->opt.niter; i++) for (i = 0; i < rtk->opt.niter; i++)
{ {
/* phase and code residuals */ /* phase and code residuals */
if ((nv = res_ppp(i, obs, n, rs, dts, var, svh, nav, xp, rtk, v, H, R, azel)) <= 0) break; if ((nv = res_ppp(i, obs, n, rs, dts, var, svh, nav, xp, rtk, v, H, R, azel)) <= 0)
{
break;
}
/* measurement update */ /* measurement update */
matcpy(Pp, rtk->P, rtk->nx, rtk->nx); matcpy(Pp, rtk->P, rtk->nx, rtk->nx);
@ -1476,13 +1706,19 @@ void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
/* ambiguity resolution in ppp */ /* ambiguity resolution in ppp */
if (opt->modear == ARMODE_PPPAR || opt->modear == ARMODE_PPPAR_ILS) if (opt->modear == ARMODE_PPPAR || opt->modear == ARMODE_PPPAR_ILS)
{ {
if (pppamb(rtk, obs, n, nav, azel)) stat = SOLQ_FIX; if (pppamb(rtk, obs, n, nav, azel))
{
stat = SOLQ_FIX;
}
} }
/* update solution status */ /* update solution status */
rtk->sol.ns = 0; rtk->sol.ns = 0;
for (i = 0; i < n && i < MAXOBS; i++) for (i = 0; i < n && i < MAXOBS; i++)
{ {
if (!rtk->ssat[obs[i].sat - 1].vsat[0]) continue; if (!rtk->ssat[obs[i].sat - 1].vsat[0])
{
continue;
}
rtk->ssat[obs[i].sat - 1].lock[0]++; rtk->ssat[obs[i].sat - 1].lock[0]++;
rtk->ssat[obs[i].sat - 1].outc[0] = 0; rtk->ssat[obs[i].sat - 1].outc[0] = 0;
rtk->ssat[obs[i].sat - 1].fix[0] = 4; rtk->ssat[obs[i].sat - 1].fix[0] = 4;
@ -1506,7 +1742,10 @@ void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
} }
for (i = 0; i < MAXSAT; i++) for (i = 0; i < MAXSAT; i++)
{ {
if (rtk->ssat[i].slip[0] & 3) rtk->ssat[i].slipc[0]++; if (rtk->ssat[i].slip[0] & 3)
{
rtk->ssat[i].slipc[0]++;
}
} }
} }
free(rs); free(rs);

312
src/algorithms/libs/rtklib/rtklib_preceph.cc
View File

@ -67,12 +67,30 @@
/* satellite code to satellite system ----------------------------------------*/ /* satellite code to satellite system ----------------------------------------*/
int code2sys(char code) int code2sys(char code)
{ {
if (code == 'G' || code == ' ') return SYS_GPS; if (code == 'G' || code == ' ')
if (code == 'R') return SYS_GLO; {
if (code == 'E') return SYS_GAL; /* extension to sp3-c */ return SYS_GPS;
if (code == 'J') return SYS_QZS; /* extension to sp3-c */ }
if (code == 'C') return SYS_BDS; /* extension to sp3-c */ if (code == 'R')
if (code == 'L') return SYS_LEO; /* extension to sp3-c */ {
return SYS_GLO;
}
if (code == 'E')
{
return SYS_GAL; /* extension to sp3-c */
}
if (code == 'J')
{
return SYS_QZS; /* extension to sp3-c */
}
if (code == 'C')
{
return SYS_BDS; /* extension to sp3-c */
}
if (code == 'L')
{
return SYS_LEO; /* extension to sp3-c */
}
return SYS_NONE; return SYS_NONE;
} }
@ -88,12 +106,18 @@ int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats,
for (i = 0; i < 22; i++) for (i = 0; i < 22; i++)
{ {
if (!fgets(buff, sizeof(buff), fp)) break; if (!fgets(buff, sizeof(buff), fp))
{
break;
}
if (i == 0) if (i == 0)
{ {
*type = buff[2]; *type = buff[2];
if (str2time(buff, 3, 28, time)) return 0; if (str2time(buff, 3, 28, time))
{
return 0;
}
} }
else if (2 <= i && i <= 6) else if (2 <= i && i <= 6)
{ {
@ -105,7 +129,10 @@ int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats,
{ {
sys = code2sys(buff[9 + 3 * j]); sys = code2sys(buff[9 + 3 * j]);
prn = static_cast<int>(str2num(buff, 10 + 3 * j, 2)); prn = static_cast<int>(str2num(buff, 10 + 3 * j, 2));
if (k < MAXSAT) sats[k++] = satno(sys, prn); if (k < MAXSAT)
{
sats[k++] = satno(sys, prn);
}
} }
} }
else if (i == 12) else if (i == 12)
@ -160,14 +187,20 @@ void readsp3b(FILE *fp, char type, int *sats __attribute__((unused)), int ns, co
while (fgets(buff, sizeof(buff), fp)) while (fgets(buff, sizeof(buff), fp))
{ {
if (!strncmp(buff, "EOF", 3)) break; if (!strncmp(buff, "EOF", 3))
{
break;
}
if (buff[0] != '*' || str2time(buff, 3, 28, &time)) if (buff[0] != '*' || str2time(buff, 3, 28, &time))
{ {
trace(2, "sp3 invalid epoch %31.31s\n", buff); trace(2, "sp3 invalid epoch %31.31s\n", buff);
continue; continue;
} }
if (!strcmp(tsys, "UTC")) time = utc2gpst(time); /* utc->gpst */ if (!strcmp(tsys, "UTC"))
{
time = utc2gpst(time); /* utc->gpst */
}
peph.time = time; peph.time = time;
peph.index = index; peph.index = index;
@ -188,16 +221,26 @@ void readsp3b(FILE *fp, char type, int *sats __attribute__((unused)), int ns, co
} }
for (i = pred_o = pred_c = v = 0; i < n && fgets(buff, sizeof(buff), fp); i++) for (i = pred_o = pred_c = v = 0; i < n && fgets(buff, sizeof(buff), fp); i++)
{ {
if (strlen(buff) < 4 || (buff[0] != 'P' && buff[0] != 'V')) continue; if (strlen(buff) < 4 || (buff[0] != 'P' && buff[0] != 'V'))
{
continue;
}
sys = buff[1] == ' ' ? SYS_GPS : code2sys(buff[1]); sys = buff[1] == ' ' ? SYS_GPS : code2sys(buff[1]);
prn = static_cast<int>(str2num(buff, 2, 2)); prn = static_cast<int>(str2num(buff, 2, 2));
if (sys == SYS_SBS) if (sys == SYS_SBS)
{
prn += 100; prn += 100;
}
else if (sys == SYS_QZS) else if (sys == SYS_QZS)
{
prn += 192; /* extension to sp3-c */ prn += 192; /* extension to sp3-c */
}
if (!(sat = satno(sys, prn))) continue; if (!(sat = satno(sys, prn)))
{
continue;
}
if (buff[0] == 'P') if (buff[0] == 'P')
{ {
@ -207,10 +250,22 @@ void readsp3b(FILE *fp, char type, int *sats __attribute__((unused)), int ns, co
for (j = 0; j < 4; j++) for (j = 0; j < 4; j++)
{ {
/* read option for predicted value */ /* read option for predicted value */
if (j < 3 && (opt & 1) && pred_o) continue; if (j < 3 && (opt & 1) && pred_o)
if (j < 3 && (opt & 2) && !pred_o) continue; {
if (j == 3 && (opt & 1) && pred_c) continue; continue;
if (j == 3 && (opt & 2) && !pred_c) continue; }
if (j < 3 && (opt & 2) && !pred_o)
{
continue;
}
if (j == 3 && (opt & 1) && pred_c)
{
continue;
}
if (j == 3 && (opt & 2) && !pred_c)
{
continue;
}
val = str2num(buff, 4 + j * 14, 14); val = str2num(buff, 4 + j * 14, 14);
std = str2num(buff, 61 + j * 3, j < 3 ? 2 : 3); std = str2num(buff, 61 + j * 3, j < 3 ? 2 : 3);
@ -242,7 +297,10 @@ void readsp3b(FILE *fp, char type, int *sats __attribute__((unused)), int ns, co
} }
if (v) if (v)
{ {
if (!addpeph(nav, &peph)) return; if (!addpeph(nav, &peph))
{
return;
}
} }
} }
} }
@ -266,7 +324,10 @@ void combpeph(nav_t *nav, int opt)
qsort(nav->peph, nav->ne, sizeof(peph_t), cmppeph); qsort(nav->peph, nav->ne, sizeof(peph_t), cmppeph);
if (opt & 4) return; if (opt & 4)
{
return;
}
for (i = 0, j = 1; j < nav->ne; j++) for (i = 0, j = 1; j < nav->ne; j++)
{ {
@ -274,16 +335,33 @@ void combpeph(nav_t *nav, int opt)
{ {
for (k = 0; k < MAXSAT; k++) for (k = 0; k < MAXSAT; k++)
{ {
if (norm_rtk(nav->peph[j].pos[k], 4) <= 0.0) continue; if (norm_rtk(nav->peph[j].pos[k], 4) <= 0.0)
for (m = 0; m < 4; m++) nav->peph[i].pos[k][m] = nav->peph[j].pos[k][m]; {
for (m = 0; m < 4; m++) nav->peph[i].std[k][m] = nav->peph[j].std[k][m]; continue;
for (m = 0; m < 4; m++) nav->peph[i].vel[k][m] = nav->peph[j].vel[k][m]; }
for (m = 0; m < 4; m++) nav->peph[i].vst[k][m] = nav->peph[j].vst[k][m]; for (m = 0; m < 4; m++)
{
nav->peph[i].pos[k][m] = nav->peph[j].pos[k][m];
}
for (m = 0; m < 4; m++)
{
nav->peph[i].std[k][m] = nav->peph[j].std[k][m];
}
for (m = 0; m < 4; m++)
{
nav->peph[i].vel[k][m] = nav->peph[j].vel[k][m];
}
for (m = 0; m < 4; m++)
{
nav->peph[i].vst[k][m] = nav->peph[j].vst[k][m];
}
} }
} }
else if (++i < j) else if (++i < j)
{
nav->peph[i] = nav->peph[j]; nav->peph[i] = nav->peph[j];
} }
}
nav->ne = i + 1; nav->ne = i + 1;
trace(4, "combpeph: ne=%d\n", nav->ne); trace(4, "combpeph: ne=%d\n", nav->ne);
@ -318,7 +396,10 @@ void readsp3(const char *file, nav_t *nav, int opt)
{ {
if (!(efiles[i] = static_cast<char *>(malloc(1024)))) if (!(efiles[i] = static_cast<char *>(malloc(1024))))
{ {
for (i--; i >= 0; i--) free(efiles[i]); for (i--; i >= 0; i--)
{
free(efiles[i]);
}
return; return;
} }
} }
@ -327,10 +408,16 @@ void readsp3(const char *file, nav_t *nav, int opt)
for (i = j = 0; i < n; i++) for (i = j = 0; i < n; i++)
{ {
if (!(ext = strrchr(efiles[i], '.'))) continue; if (!(ext = strrchr(efiles[i], '.')))
{
continue;
}
if (!strstr(ext + 1, "sp3") && !strstr(ext + 1, ".SP3") && if (!strstr(ext + 1, "sp3") && !strstr(ext + 1, ".SP3") &&
!strstr(ext + 1, "eph") && !strstr(ext + 1, ".EPH")) continue; !strstr(ext + 1, "eph") && !strstr(ext + 1, ".EPH"))
{
continue;
}
if (!(fp = fopen(efiles[i], "re"))) if (!(fp = fopen(efiles[i], "re")))
{ {
@ -345,10 +432,16 @@ void readsp3(const char *file, nav_t *nav, int opt)
fclose(fp); fclose(fp);
} }
for (i = 0; i < MAXEXFILE; i++) free(efiles[i]); for (i = 0; i < MAXEXFILE; i++)
{
free(efiles[i]);
}
/* combine precise ephemeris */ /* combine precise ephemeris */
if (nav->ne > 0) combpeph(nav, opt); if (nav->ne > 0)
{
combpeph(nav, opt);
}
} }
@ -368,7 +461,10 @@ int readsap(const char *file, gtime_t time, nav_t *nav)
trace(3, "readsap : file=%s time=%s\n", file, time_str(time, 0)); trace(3, "readsap : file=%s time=%s\n", file, time_str(time, 0));
if (!readpcv(file, &pcvs)) return 0; if (!readpcv(file, &pcvs))
{
return 0;
}
for (i = 0; i < MAXSAT; i++) for (i = 0; i < MAXSAT; i++)
{ {
@ -398,21 +494,36 @@ int readdcbf(const char *file, nav_t *nav, const sta_t *sta)
while (fgets(buff, sizeof(buff), fp)) while (fgets(buff, sizeof(buff), fp))
{ {
if (strstr(buff, "DIFFERENTIAL (P1-P2) CODE BIASES")) if (strstr(buff, "DIFFERENTIAL (P1-P2) CODE BIASES"))
{
type = 1; type = 1;
}
else if (strstr(buff, "DIFFERENTIAL (P1-C1) CODE BIASES")) else if (strstr(buff, "DIFFERENTIAL (P1-C1) CODE BIASES"))
{
type = 2; type = 2;
}
else if (strstr(buff, "DIFFERENTIAL (P2-C2) CODE BIASES")) else if (strstr(buff, "DIFFERENTIAL (P2-C2) CODE BIASES"))
{
type = 3; type = 3;
}
if (!type || sscanf(buff, "%s %s", str1, str2) < 1) continue; if (!type || sscanf(buff, "%s %s", str1, str2) < 1)
{
continue;
}
if ((cbias = str2num(buff, 26, 9)) == 0.0) continue; if ((cbias = str2num(buff, 26, 9)) == 0.0)
{
continue;
}
if (sta && (!strcmp(str1, "G") || !strcmp(str1, "R"))) if (sta && (!strcmp(str1, "G") || !strcmp(str1, "R")))
{ /* receiver dcb */ { /* receiver dcb */
for (i = 0; i < MAXRCV; i++) for (i = 0; i < MAXRCV; i++)
{ {
if (!strcmp(sta[i].name, str2)) break; if (!strcmp(sta[i].name, str2))
{
break;
}
} }
if (i < MAXRCV) if (i < MAXRCV)
{ {
@ -448,15 +559,20 @@ int readdcb(const char *file, nav_t *nav, const sta_t *sta)
trace(3, "readdcb : file=%s\n", file); trace(3, "readdcb : file=%s\n", file);
for (i = 0; i < MAXSAT; i++) for (i = 0; i < MAXSAT; i++)
{
for (j = 0; j < 3; j++) for (j = 0; j < 3; j++)
{ {
nav->cbias[i][j] = 0.0; nav->cbias[i][j] = 0.0;
} }
}
for (i = 0; i < MAXEXFILE; i++) for (i = 0; i < MAXEXFILE; i++)
{ {
if (!(efiles[i] = static_cast<char *>(malloc(1024)))) if (!(efiles[i] = static_cast<char *>(malloc(1024))))
{ {
for (i--; i >= 0; i--) free(efiles[i]); for (i--; i >= 0; i--)
{
free(efiles[i]);
}
return 0; return 0;
} }
} }
@ -466,7 +582,10 @@ int readdcb(const char *file, nav_t *nav, const sta_t *sta)
{ {
readdcbf(efiles[i], nav, sta); readdcbf(efiles[i], nav, sta);
} }
for (i = 0; i < MAXEXFILE; i++) free(efiles[i]); for (i = 0; i < MAXEXFILE; i++)
{
free(efiles[i]);
}
return 1; return 1;
} }
@ -500,10 +619,12 @@ int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat,
nav->fcb = nav_fcb; nav->fcb = nav_fcb;
} }
for (i = 0; i < MAXSAT; i++) for (i = 0; i < MAXSAT; i++)
{
for (j = 0; j < 3; j++) for (j = 0; j < 3; j++)
{ {
nav->fcb[nav->nf].bias[i][j] = nav->fcb[nav->nf].std[i][j] = 0.0; nav->fcb[nav->nf].bias[i][j] = nav->fcb[nav->nf].std[i][j] = 0.0;
} }
}
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
{ {
nav->fcb[nav->nf].bias[sat - 1][i] = bias[i]; nav->fcb[nav->nf].bias[sat - 1][i] = bias[i];
@ -533,14 +654,23 @@ int readfcbf(const char *file, nav_t *nav)
} }
while (fgets(buff, sizeof(buff), fp)) while (fgets(buff, sizeof(buff), fp))
{ {
if ((p = strchr(buff, '#'))) *p = '\0'; if ((p = strchr(buff, '#')))
{
*p = '\0';
}
if (sscanf(buff, if (sscanf(buff,
"%lf/%lf/%lf %lf:%lf:%lf %lf/%lf/%lf %lf:%lf:%lf %s" "%lf/%lf/%lf %lf:%lf:%lf %lf/%lf/%lf %lf:%lf:%lf %s"
"%lf %lf %lf %lf %lf %lf", "%lf %lf %lf %lf %lf %lf",
ep1, ep1 + 1, ep1 + 2, ep1 + 3, ep1 + 4, ep1 + 5, ep1, ep1 + 1, ep1 + 2, ep1 + 3, ep1 + 4, ep1 + 5,
ep2, ep2 + 1, ep2 + 2, ep2 + 3, ep2 + 4, ep2 + 5, str, bias, std, bias + 1, std + 1, ep2, ep2 + 1, ep2 + 2, ep2 + 3, ep2 + 4, ep2 + 5, str, bias, std, bias + 1, std + 1,
bias + 2, std + 2) < 17) continue; bias + 2, std + 2) < 17)
if (!(sat = satid2no(str))) continue; {
continue;
}
if (!(sat = satid2no(str)))
{
continue;
}
ts = epoch2time(ep1); ts = epoch2time(ep1);
te = epoch2time(ep2); te = epoch2time(ep2);
if (!addfcb(nav, ts, te, sat, bias, std)) if (!addfcb(nav, ts, te, sat, bias, std))
@ -581,7 +711,10 @@ int readfcb(const char *file, nav_t *nav)
{ {
if (!(efiles[i] = static_cast<char *>(malloc(1024)))) if (!(efiles[i] = static_cast<char *>(malloc(1024))))
{ {
for (i--; i >= 0; i--) free(efiles[i]); for (i--; i >= 0; i--)
{
free(efiles[i]);
}
return 0; return 0;
} }
} }
@ -591,7 +724,10 @@ int readfcb(const char *file, nav_t *nav)
{ {
readfcbf(efiles[i], nav); readfcbf(efiles[i], nav);
} }
for (i = 0; i < MAXEXFILE; i++) free(efiles[i]); for (i = 0; i < MAXEXFILE; i++)
{
free(efiles[i]);
}
if (nav->nf > 1) if (nav->nf > 1)
{ {
@ -640,18 +776,26 @@ int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
{ {
k = (i + j) / 2; k = (i + j) / 2;
if (timediff(nav->peph[k].time, time) < 0.0) if (timediff(nav->peph[k].time, time) < 0.0)
{
i = k + 1; i = k + 1;
}
else else
{
j = k; j = k;
} }
}
index = i <= 0 ? 0 : i - 1; index = i <= 0 ? 0 : i - 1;
/* polynomial interpolation for orbit */ /* polynomial interpolation for orbit */
i = index - (NMAX + 1) / 2; i = index - (NMAX + 1) / 2;
if (i < 0) if (i < 0)
{
i = 0; i = 0;
}
else if (i + NMAX >= nav->ne) else if (i + NMAX >= nav->ne)
{
i = nav->ne - NMAX - 1; i = nav->ne - NMAX - 1;
}
for (j = 0; j <= NMAX; j++) for (j = 0; j <= NMAX; j++)
{ {
@ -683,14 +827,21 @@ int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
} }
if (vare) if (vare)
{ {
for (i = 0; i < 3; i++) s[i] = nav->peph[index].std[sat - 1][i]; for (i = 0; i < 3; i++)
{
s[i] = nav->peph[index].std[sat - 1][i];
}
std = norm_rtk(s, 3); std = norm_rtk(s, 3);
/* extrapolation error for orbit */ /* extrapolation error for orbit */
if (t[0] > 0.0) if (t[0] > 0.0)
{
std += EXTERR_EPH * std::pow(t[0], 2.0) / 2.0; std += EXTERR_EPH * std::pow(t[0], 2.0) / 2.0;
}
else if (t[NMAX] < 0.0) else if (t[NMAX] < 0.0)
{
std += EXTERR_EPH * std::pow(t[NMAX], 2.0) / 2.0; std += EXTERR_EPH * std::pow(t[NMAX], 2.0) / 2.0;
}
*vare = std::pow(std, 2.0); *vare = std::pow(std, 2.0);
} }
/* linear interpolation for clock */ /* linear interpolation for clock */
@ -723,7 +874,10 @@ int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
{ {
dts[0] = 0.0; dts[0] = 0.0;
} }
if (varc) *varc = std::pow(std, 2.0); if (varc)
{
*varc = std::pow(std, 2.0);
}
return 1; return 1;
} }
@ -749,10 +903,14 @@ int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
{ {
k = (i + j) / 2; k = (i + j) / 2;
if (timediff(nav->pclk[k].time, time) < 0.0) if (timediff(nav->pclk[k].time, time) < 0.0)
{
i = k + 1; i = k + 1;
}
else else
{
j = k; j = k;
} }
}
index = i <= 0 ? 0 : i - 1; index = i <= 0 ? 0 : i - 1;
/* linear interpolation for clock */ /* linear interpolation for clock */
@ -763,12 +921,18 @@ int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
if (t[0] <= 0.0) if (t[0] <= 0.0)
{ {
if ((dts[0] = c[0]) == 0.0) return 0; if ((dts[0] = c[0]) == 0.0)
{
return 0;
}
std = nav->pclk[index].std[sat - 1][0] * SPEED_OF_LIGHT - EXTERR_CLK * t[0]; std = nav->pclk[index].std[sat - 1][0] * SPEED_OF_LIGHT - EXTERR_CLK * t[0];
} }
else if (t[1] >= 0.0) else if (t[1] >= 0.0)
{ {
if ((dts[0] = c[1]) == 0.0) return 0; if ((dts[0] = c[1]) == 0.0)
{
return 0;
}
std = nav->pclk[index + 1].std[sat - 1][0] * SPEED_OF_LIGHT + EXTERR_CLK * t[1]; std = nav->pclk[index + 1].std[sat - 1][0] * SPEED_OF_LIGHT + EXTERR_CLK * t[1];
} }
else if (c[0] != 0.0 && c[1] != 0.0) else if (c[0] != 0.0 && c[1] != 0.0)
@ -782,7 +946,10 @@ int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
trace(3, "prec clock outage %s sat=%2d\n", time_str(time, 0), sat); trace(3, "prec clock outage %s sat=%2d\n", time_str(time, 0), sat);
return 0; return 0;
} }
if (varc) *varc = std::pow(std, 2.0); if (varc)
{
*varc = std::pow(std, 2.0);
}
return 1; return 1;
} }
@ -813,17 +980,38 @@ void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav,
sunmoonpos(gpst2utc(time), erpv, rsun, nullptr, &gmst); sunmoonpos(gpst2utc(time), erpv, rsun, nullptr, &gmst);
/* unit vectors of satellite fixed coordinates */ /* unit vectors of satellite fixed coordinates */
for (i = 0; i < 3; i++) r[i] = -rs[i]; for (i = 0; i < 3; i++)
if (!normv3(r, ez)) return; {
for (i = 0; i < 3; i++) r[i] = rsun[i] - rs[i]; r[i] = -rs[i];
if (!normv3(r, es)) return; }
if (!normv3(r, ez))
{
return;
}
for (i = 0; i < 3; i++)
{
r[i] = rsun[i] - rs[i];
}
if (!normv3(r, es))
{
return;
}
cross3(ez, es, r); cross3(ez, es, r);
if (!normv3(r, ey)) return; if (!normv3(r, ey))
{
return;
}
cross3(ey, ez, ex); cross3(ey, ez, ex);
if (NFREQ >= 3 && (satsys(sat, nullptr) & (SYS_GAL | SYS_SBS))) k = 2; if (NFREQ >= 3 && (satsys(sat, nullptr) & (SYS_GAL | SYS_SBS)))
{
k = 2;
}
if (NFREQ < 2 || lam[j] == 0.0 || lam[k] == 0.0) return; if (NFREQ < 2 || lam[j] == 0.0 || lam[k] == 0.0)
{
return;
}
gamma = std::pow(lam[k], 2.0) / std::pow(lam[j], 2.0); gamma = std::pow(lam[k], 2.0) / std::pow(lam[j], 2.0);
C1 = gamma / (gamma - 1.0); C1 = gamma / (gamma - 1.0);
@ -865,15 +1053,24 @@ int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt,
trace(4, "peph2pos: time=%s sat=%2d opt=%d\n", time_str(time, 3), sat, opt); trace(4, "peph2pos: time=%s sat=%2d opt=%d\n", time_str(time, 3), sat, opt);
if (sat <= 0 || MAXSAT < sat) return 0; if (sat <= 0 || MAXSAT < sat)
{
return 0;
}
/* satellite position and clock bias */ /* satellite position and clock bias */
if (!pephpos(time, sat, nav, rss, dtss, &vare, &varc) || if (!pephpos(time, sat, nav, rss, dtss, &vare, &varc) ||
!pephclk(time, sat, nav, dtss, &varc)) return 0; !pephclk(time, sat, nav, dtss, &varc))
{
return 0;
}
time = timeadd(time, tt); time = timeadd(time, tt);
if (!pephpos(time, sat, nav, rst, dtst, nullptr, nullptr) || if (!pephpos(time, sat, nav, rst, dtst, nullptr, nullptr) ||
!pephclk(time, sat, nav, dtst, nullptr)) return 0; !pephclk(time, sat, nav, dtst, nullptr))
{
return 0;
}
/* satellite antenna offset correction */ /* satellite antenna offset correction */
if (opt) if (opt)
@ -895,7 +1092,10 @@ int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt,
{ /* no precise clock */ { /* no precise clock */
dts[0] = dts[1] = 0.0; dts[0] = dts[1] = 0.0;
} }
if (var) *var = vare + varc; if (var)
{
*var = vare + varc;
}
return 1; return 1;
} }

94
src/algorithms/libs/rtklib/rtklib_rtcm.cc
View File

@ -94,19 +94,30 @@ int init_rtcm(rtcm_t *rtcm)
rtcm->ssr[i] = ssr0; rtcm->ssr[i] = ssr0;
} }
rtcm->msg[0] = rtcm->msgtype[0] = rtcm->opt[0] = '\0'; rtcm->msg[0] = rtcm->msgtype[0] = rtcm->opt[0] = '\0';
for (i = 0; i < 6; i++) rtcm->msmtype[i][0] = '\0'; for (i = 0; i < 6; i++)
{
rtcm->msmtype[i][0] = '\0';
}
rtcm->obsflag = rtcm->ephsat = 0; rtcm->obsflag = rtcm->ephsat = 0;
for (i = 0; i < MAXSAT; i++) for (i = 0; i < MAXSAT; i++)
{
for (j = 0; j < NFREQ + NEXOBS; j++) for (j = 0; j < NFREQ + NEXOBS; j++)
{ {
rtcm->cp[i][j] = 0.0; rtcm->cp[i][j] = 0.0;
rtcm->lock[i][j] = rtcm->loss[i][j] = 0; rtcm->lock[i][j] = rtcm->loss[i][j] = 0;
rtcm->lltime[i][j] = time0; rtcm->lltime[i][j] = time0;
} }
}
rtcm->nbyte = rtcm->nbit = rtcm->len = 0; rtcm->nbyte = rtcm->nbit = rtcm->len = 0;
rtcm->word = 0; rtcm->word = 0;
for (i = 0; i < 100; i++) rtcm->nmsg2[i] = 0; for (i = 0; i < 100; i++)
for (i = 0; i < 300; i++) rtcm->nmsg3[i] = 0; {
rtcm->nmsg2[i] = 0;
}
for (i = 0; i < 300; i++)
{
rtcm->nmsg3[i] = 0;
}
rtcm->obs.data = nullptr; rtcm->obs.data = nullptr;
rtcm->nav.eph = nullptr; rtcm->nav.eph = nullptr;
@ -123,9 +134,18 @@ int init_rtcm(rtcm_t *rtcm)
rtcm->obs.n = 0; rtcm->obs.n = 0;
rtcm->nav.n = MAXSAT; rtcm->nav.n = MAXSAT;
rtcm->nav.ng = MAXPRNGLO; rtcm->nav.ng = MAXPRNGLO;
for (i = 0; i < MAXOBS; i++) rtcm->obs.data[i] = data0; for (i = 0; i < MAXOBS; i++)
for (i = 0; i < MAXSAT; i++) rtcm->nav.eph[i] = eph0; {
for (i = 0; i < MAXPRNGLO; i++) rtcm->nav.geph[i] = geph0; rtcm->obs.data[i] = data0;
}
for (i = 0; i < MAXSAT; i++)
{
rtcm->nav.eph[i] = eph0;
}
for (i = 0; i < MAXPRNGLO; i++)
{
rtcm->nav.geph[i] = geph0;
}
return 1; return 1;
} }
@ -173,7 +193,10 @@ int input_rtcm2(rtcm_t *rtcm, unsigned char data)
trace(5, "input_rtcm2: data=%02x\n", data); trace(5, "input_rtcm2: data=%02x\n", data);
if ((data & 0xC0) != 0x40) return 0; /* ignore if upper 2bit != 01 */ if ((data & 0xC0) != 0x40)
{
return 0; /* ignore if upper 2bit != 01 */
}
for (i = 0; i < 6; i++, data >>= 1) for (i = 0; i < 6; i++, data >>= 1)
{ /* decode 6-of-8 form */ { /* decode 6-of-8 form */
@ -183,17 +206,28 @@ int input_rtcm2(rtcm_t *rtcm, unsigned char data)
if (rtcm->nbyte == 0) if (rtcm->nbyte == 0)
{ {
preamb = static_cast<unsigned char>(rtcm->word >> 22); preamb = static_cast<unsigned char>(rtcm->word >> 22);
if (rtcm->word & 0x40000000) preamb ^= 0xFF; /* decode preamble */ if (rtcm->word & 0x40000000)
if (preamb != RTCM2PREAMB) continue; {
preamb ^= 0xFF; /* decode preamble */
}
if (preamb != RTCM2PREAMB)
{
continue;
}
/* check parity */ /* check parity */
if (!decode_word(rtcm->word, rtcm->buff)) continue; if (!decode_word(rtcm->word, rtcm->buff))
{
continue;
}
rtcm->nbyte = 3; rtcm->nbyte = 3;
rtcm->nbit = 0; rtcm->nbit = 0;
continue; continue;
} }
if (++rtcm->nbit < 30) if (++rtcm->nbit < 30)
{
continue; continue;
}
rtcm->nbit = 0; rtcm->nbit = 0;
@ -206,8 +240,14 @@ int input_rtcm2(rtcm_t *rtcm, unsigned char data)
continue; continue;
} }
rtcm->nbyte += 3; rtcm->nbyte += 3;
if (rtcm->nbyte == 6) rtcm->len = (rtcm->buff[5] >> 3) * 3 + 6; if (rtcm->nbyte == 6)
if (rtcm->nbyte < rtcm->len) continue; {
rtcm->len = (rtcm->buff[5] >> 3) * 3 + 6;
}
if (rtcm->nbyte < rtcm->len)
{
continue;
}
rtcm->nbyte = 0; rtcm->nbyte = 0;
rtcm->word &= 0x3; rtcm->word &= 0x3;
@ -290,7 +330,10 @@ int input_rtcm3(rtcm_t *rtcm, unsigned char data)
/* synchronize frame */ /* synchronize frame */
if (rtcm->nbyte == 0) if (rtcm->nbyte == 0)
{ {
if (data != RTCM3PREAMB) return 0; if (data != RTCM3PREAMB)
{
return 0;
}
rtcm->buff[rtcm->nbyte++] = data; rtcm->buff[rtcm->nbyte++] = data;
return 0; return 0;
} }
@ -300,7 +343,10 @@ int input_rtcm3(rtcm_t *rtcm, unsigned char data)
{ {
rtcm->len = getbitu(rtcm->buff, 14, 10) + 3; /* length without parity */ rtcm->len = getbitu(rtcm->buff, 14, 10) + 3; /* length without parity */
} }
if (rtcm->nbyte < 3 || rtcm->nbyte < rtcm->len + 3) return 0; if (rtcm->nbyte < 3 || rtcm->nbyte < rtcm->len + 3)
{
return 0;
}
rtcm->nbyte = 0; rtcm->nbyte = 0;
/* check parity */ /* check parity */
@ -329,8 +375,14 @@ int input_rtcm2f(rtcm_t *rtcm, FILE *fp)
for (i = 0; i < 4096; i++) for (i = 0; i < 4096; i++)
{ {
if ((data = fgetc(fp)) == EOF) return -2; if ((data = fgetc(fp)) == EOF)
if ((ret = input_rtcm2(rtcm, static_cast<unsigned char>(data)))) return ret; {
return -2;
}
if ((ret = input_rtcm2(rtcm, static_cast<unsigned char>(data))))
{
return ret;
}
} }
return 0; /* return at every 4k bytes */ return 0; /* return at every 4k bytes */
} }
@ -351,8 +403,14 @@ int input_rtcm3f(rtcm_t *rtcm, FILE *fp)
for (i = 0; i < 4096; i++) for (i = 0; i < 4096; i++)
{ {
if ((data = fgetc(fp)) == EOF) return -2; if ((data = fgetc(fp)) == EOF)
if ((ret = input_rtcm3(rtcm, static_cast<unsigned char>(data)))) return ret; {
return -2;
}
if ((ret = input_rtcm3(rtcm, static_cast<unsigned char>(data))))
{
return ret;
}
} }
return 0; /* return at every 4k bytes */ return 0; /* return at every 4k bytes */
} }

58
src/algorithms/libs/rtklib/rtklib_rtcm2.cc
View File

@ -63,14 +63,21 @@ void adjhour(rtcm_t *rtcm, double zcnt)
int week; int week;
/* if no time, get cpu time */ /* if no time, get cpu time */
if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget()); if (rtcm->time.time == 0)
{
rtcm->time = utc2gpst(timeget());
}
tow = time2gpst(rtcm->time, &week); tow = time2gpst(rtcm->time, &week);
hour = floor(tow / 3600.0); hour = floor(tow / 3600.0);
sec = tow - hour * 3600.0; sec = tow - hour * 3600.0;
if (zcnt < sec - 1800.0) if (zcnt < sec - 1800.0)
{
zcnt += 3600.0; zcnt += 3600.0;
}
else if (zcnt > sec + 1800.0) else if (zcnt > sec + 1800.0)
{
zcnt -= 3600.0; zcnt -= 3600.0;
}
rtcm->time = gpst2time(week, hour * 3600 + zcnt); rtcm->time = gpst2time(week, hour * 3600 + zcnt);
} }
@ -82,9 +89,15 @@ int obsindex(obs_t *obs, gtime_t time, int sat)
for (i = 0; i < obs->n; i++) for (i = 0; i < obs->n; i++)
{ {
if (obs->data[i].sat == sat) return i; /* field already exists */ if (obs->data[i].sat == sat)
{
return i; /* field already exists */
}
}
if (i >= MAXOBS)
{
return -1; /* overflow */
} }
if (i >= MAXOBS) return -1; /* overflow */
/* add new field */ /* add new field */
obs->data[i].time = time; obs->data[i].time = time;
@ -122,7 +135,10 @@ int decode_type1(rtcm_t *rtcm)
i += 8; i += 8;
iod = getbits(rtcm->buff, i, 8); iod = getbits(rtcm->buff, i, 8);
i += 8; i += 8;
if (prn == 0) prn = 32; if (prn == 0)
{
prn = 32;
}
if (prc == 0x80000000 || rrc == 0xFFFF8000) if (prc == 0x80000000 || rrc == 0xFFFF8000)
{ {
trace(2, "rtcm2 1 prc/rrc indicates satellite problem: prn=%d\n", prn); trace(2, "rtcm2 1 prc/rrc indicates satellite problem: prn=%d\n", prn);
@ -290,7 +306,10 @@ int decode_type17(rtcm_t *rtcm)
trace(2, "rtcm2 17 length error: len=%d\n", rtcm->len); trace(2, "rtcm2 17 length error: len=%d\n", rtcm->len);
return -1; return -1;
} }
if (prn == 0) prn = 32; if (prn == 0)
{
prn = 32;
}
sat = satno(SYS_GPS, prn); sat = satno(SYS_GPS, prn);
eph.sat = sat; eph.sat = sat;
eph.week = adjgpsweek(week); eph.week = adjgpsweek(week);
@ -346,14 +365,20 @@ int decode_type18(rtcm_t *rtcm)
i += 5; i += 5;
cp = getbits(rtcm->buff, i, 32); cp = getbits(rtcm->buff, i, 32);
i += 32; i += 32;
if (prn == 0) prn = 32; if (prn == 0)
{
prn = 32;
}
if (!(sat = satno(sys ? SYS_GLO : SYS_GPS, prn))) if (!(sat = satno(sys ? SYS_GLO : SYS_GPS, prn)))
{ {
trace(2, "rtcm2 18 satellite number error: sys=%d prn=%d\n", sys, prn); trace(2, "rtcm2 18 satellite number error: sys=%d prn=%d\n", sys, prn);
continue; continue;
} }
time = timeadd(rtcm->time, usec * 1E-6); time = timeadd(rtcm->time, usec * 1E-6);
if (sys) time = utc2gpst(time); /* convert glonass time to gpst */ if (sys)
{
time = utc2gpst(time); /* convert glonass time to gpst */
}
tt = timediff(rtcm->obs.data[0].time, time); tt = timediff(rtcm->obs.data[0].time, time);
if (rtcm->obsflag || fabs(tt) > 1E-9) if (rtcm->obsflag || fabs(tt) > 1E-9)
@ -414,14 +439,20 @@ int decode_type19(rtcm_t *rtcm)
i += 5 + 8; i += 5 + 8;
pr = getbitu(rtcm->buff, i, 32); pr = getbitu(rtcm->buff, i, 32);
i += 32; i += 32;
if (prn == 0) prn = 32; if (prn == 0)
{
prn = 32;
}
if (!(sat = satno(sys ? SYS_GLO : SYS_GPS, prn))) if (!(sat = satno(sys ? SYS_GLO : SYS_GPS, prn)))
{ {
trace(2, "rtcm2 19 satellite number error: sys=%d prn=%d\n", sys, prn); trace(2, "rtcm2 19 satellite number error: sys=%d prn=%d\n", sys, prn);
continue; continue;
} }
time = timeadd(rtcm->time, usec * 1E-6); time = timeadd(rtcm->time, usec * 1E-6);
if (sys) time = utc2gpst(time); /* convert glonass time to gpst */ if (sys)
{
time = utc2gpst(time); /* convert glonass time to gpst */
}
tt = timediff(rtcm->obs.data[0].time, time); tt = timediff(rtcm->obs.data[0].time, time);
if (rtcm->obsflag || fabs(tt) > 1E-9) if (rtcm->obsflag || fabs(tt) > 1E-9)
@ -479,7 +510,10 @@ int decode_type22(rtcm_t *rtcm)
del[1][2] = getbits(rtcm->buff, i, 8) / 1600.0; del[1][2] = getbits(rtcm->buff, i, 8) / 1600.0;
} }
rtcm->sta.deltype = 1; /* xyz */ rtcm->sta.deltype = 1; /* xyz */
for (j = 0; j < 3; j++) rtcm->sta.del[j] = del[0][j]; for (j = 0; j < 3; j++)
{
rtcm->sta.del[j] = del[0][j];
}
rtcm->sta.hgt = hgt; rtcm->sta.hgt = hgt;
return 5; return 5;
} }
@ -640,9 +674,13 @@ int decode_rtcm2(rtcm_t *rtcm)
if (ret >= 0) if (ret >= 0)
{ {
if (1 <= type && type <= 99) if (1 <= type && type <= 99)
{
rtcm->nmsg2[type]++; rtcm->nmsg2[type]++;
}
else else
{
rtcm->nmsg2[0]++; rtcm->nmsg2[0]++;
} }
}
return ret; return ret;
} }

442
src/algorithms/libs/rtklib/rtklib_rtcm3.cc
View File

@ -115,12 +115,19 @@ void adjweek(rtcm_t *rtcm, double tow)
int week; int week;
/* if no time, get cpu time */ /* if no time, get cpu time */
if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget()); if (rtcm->time.time == 0)
{
rtcm->time = utc2gpst(timeget());
}
tow_p = time2gpst(rtcm->time, &week); tow_p = time2gpst(rtcm->time, &week);
if (tow < tow_p - 302400.0) if (tow < tow_p - 302400.0)
{
tow += 604800.0; tow += 604800.0;
}
else if (tow > tow_p + 302400.0) else if (tow > tow_p + 302400.0)
{
tow -= 604800.0; tow -= 604800.0;
}
rtcm->time = gpst2time(week, tow); rtcm->time = gpst2time(week, tow);
} }
@ -130,7 +137,10 @@ int adjbdtweek(int week)
{ {
int w; int w;
(void)time2bdt(gpst2bdt(utc2gpst(timeget())), &w); (void)time2bdt(gpst2bdt(utc2gpst(timeget())), &w);
if (w < 1) w = 1; /* use 2006/1/1 if time is earlier than 2006/1/1 */ if (w < 1)
{
w = 1; /* use 2006/1/1 if time is earlier than 2006/1/1 */
}
return week + (w - week + 512) / 1024 * 1024; return week + (w - week + 512) / 1024 * 1024;
} }
@ -142,15 +152,22 @@ void adjday_glot(rtcm_t *rtcm, double tod)
double tow, tod_p; double tow, tod_p;
int week; int week;
if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget()); if (rtcm->time.time == 0)
{
rtcm->time = utc2gpst(timeget());
}
time = timeadd(gpst2utc(rtcm->time), 10800.0); /* glonass time */ time = timeadd(gpst2utc(rtcm->time), 10800.0); /* glonass time */
tow = time2gpst(time, &week); tow = time2gpst(time, &week);
tod_p = fmod(tow, 86400.0); tod_p = fmod(tow, 86400.0);
tow -= tod_p; tow -= tod_p;
if (tod < tod_p - 43200.0) if (tod < tod_p - 43200.0)
{
tod += 86400.0; tod += 86400.0;
}
else if (tod > tod_p + 43200.0) else if (tod > tod_p + 43200.0)
{
tod -= 86400.0; tod -= 86400.0;
}
time = gpst2time(week, tow + tod); time = gpst2time(week, tow + tod);
rtcm->time = utc2gpst(timeadd(time, -10800.0)); rtcm->time = utc2gpst(timeadd(time, -10800.0));
} }
@ -160,11 +177,17 @@ void adjday_glot(rtcm_t *rtcm, double tod)
double adjcp(rtcm_t *rtcm, int sat, int freq, double cp) double adjcp(rtcm_t *rtcm, int sat, int freq, double cp)
{ {
if (rtcm->cp[sat - 1][freq] == 0.0) if (rtcm->cp[sat - 1][freq] == 0.0)
{
; ;
}
else if (cp < rtcm->cp[sat - 1][freq] - 750.0) else if (cp < rtcm->cp[sat - 1][freq] - 750.0)
{
cp += 1500.0; cp += 1500.0;
}
else if (cp > rtcm->cp[sat - 1][freq] + 750.0) else if (cp > rtcm->cp[sat - 1][freq] + 750.0)
{
cp -= 1500.0; cp -= 1500.0;
}
rtcm->cp[sat - 1][freq] = cp; rtcm->cp[sat - 1][freq] = cp;
return cp; return cp;
} }
@ -193,9 +216,15 @@ int obsindex3(obs_t *obs, gtime_t time, int sat)
for (i = 0; i < obs->n; i++) for (i = 0; i < obs->n; i++)
{ {
if (obs->data[i].sat == sat) return i; /* field already exists */ if (obs->data[i].sat == sat)
{
return i; /* field already exists */
}
}
if (i >= MAXOBS)
{
return -1; /* overflow */
} }
if (i >= MAXOBS) return -1; /* overflow */
/* add new field */ /* add new field */
obs->data[i].time = time; obs->data[i].time = time;
@ -220,7 +249,10 @@ int test_staid(rtcm_t *rtcm, int staid)
/* test station id option */ /* test station id option */
if ((p = strstr(rtcm->opt, "-STA=")) && sscanf(p, "-STA=%d", &id) == 1) if ((p = strstr(rtcm->opt, "-STA=")) && sscanf(p, "-STA=%d", &id) == 1)
{ {
if (staid != id) return 0; if (staid != id)
{
return 0;
}
} }
/* save station id */ /* save station id */
if (rtcm->staid == 0 || rtcm->obsflag) if (rtcm->staid == 0 || rtcm->obsflag)
@ -266,7 +298,10 @@ int decode_head1001(rtcm_t *rtcm, int *sync)
return -1; return -1;
} }
/* test station id */ /* test station id */
if (!test_staid(rtcm, staid)) return -1; if (!test_staid(rtcm, staid))
{
return -1;
}
adjweek(rtcm, tow); adjweek(rtcm, tow);
@ -287,7 +322,10 @@ int decode_head1001(rtcm_t *rtcm, int *sync)
int decode_type1001(rtcm_t *rtcm) int decode_type1001(rtcm_t *rtcm)
{ {
int sync; int sync;
if (decode_head1001(rtcm, &sync) < 0) return -1; if (decode_head1001(rtcm, &sync) < 0)
{
return -1;
}
rtcm->obsflag = !sync; rtcm->obsflag = !sync;
return sync ? 0 : 1; return sync ? 0 : 1;
} }
@ -299,7 +337,10 @@ int decode_type1002(rtcm_t *rtcm)
double pr1, cnr1, tt, cp1; double pr1, cnr1, tt, cp1;
int i = 24 + 64, j, index, nsat, sync, prn, code, sat, ppr1, lock1, amb, sys; int i = 24 + 64, j, index, nsat, sync, prn, code, sat, ppr1, lock1, amb, sys;
if ((nsat = decode_head1001(rtcm, &sync)) < 0) return -1; if ((nsat = decode_head1001(rtcm, &sync)) < 0)
{
return -1;
}
for (j = 0; j < nsat && rtcm->obs.n < MAXOBS && i + 74 <= rtcm->len * 8; j++) for (j = 0; j < nsat && rtcm->obs.n < MAXOBS && i + 74 <= rtcm->len * 8; j++)
{ {
@ -336,7 +377,10 @@ int decode_type1002(rtcm_t *rtcm)
{ {
rtcm->obs.n = rtcm->obsflag = 0; rtcm->obs.n = rtcm->obsflag = 0;
} }
if ((index = obsindex3(&rtcm->obs, rtcm->time, sat)) < 0) continue; if ((index = obsindex3(&rtcm->obs, rtcm->time, sat)) < 0)
{
continue;
}
pr1 = pr1 * 0.02 + amb * PRUNIT_GPS; pr1 = pr1 * 0.02 + amb * PRUNIT_GPS;
if (ppr1 != static_cast<int>(0xFFF80000)) if (ppr1 != static_cast<int>(0xFFF80000))
{ {
@ -356,7 +400,10 @@ int decode_type1002(rtcm_t *rtcm)
int decode_type1003(rtcm_t *rtcm) int decode_type1003(rtcm_t *rtcm)
{ {
int sync; int sync;
if (decode_head1001(rtcm, &sync) < 0) return -1; if (decode_head1001(rtcm, &sync) < 0)
{
return -1;
}
rtcm->obsflag = !sync; rtcm->obsflag = !sync;
return sync ? 0 : 1; return sync ? 0 : 1;
} }
@ -370,7 +417,10 @@ int decode_type1004(rtcm_t *rtcm)
int i = 24 + 64, j, index, nsat, sync, prn, sat, code1, code2, pr21, ppr1, ppr2; int i = 24 + 64, j, index, nsat, sync, prn, sat, code1, code2, pr21, ppr1, ppr2;
int lock1, lock2, amb, sys; int lock1, lock2, amb, sys;
if ((nsat = decode_head1001(rtcm, &sync)) < 0) return -1; if ((nsat = decode_head1001(rtcm, &sync)) < 0)
{
return -1;
}
for (j = 0; j < nsat && rtcm->obs.n < MAXOBS && i + 125 <= rtcm->len * 8; j++) for (j = 0; j < nsat && rtcm->obs.n < MAXOBS && i + 125 <= rtcm->len * 8; j++)
{ {
@ -417,7 +467,10 @@ int decode_type1004(rtcm_t *rtcm)
{ {
rtcm->obs.n = rtcm->obsflag = 0; rtcm->obs.n = rtcm->obsflag = 0;
} }
if ((index = obsindex3(&rtcm->obs, rtcm->time, sat)) < 0) continue; if ((index = obsindex3(&rtcm->obs, rtcm->time, sat)) < 0)
{
continue;
}
pr1 = pr1 * 0.02 + amb * PRUNIT_GPS; pr1 = pr1 * 0.02 + amb * PRUNIT_GPS;
if (ppr1 != static_cast<int>(0xFFF80000)) if (ppr1 != static_cast<int>(0xFFF80000))
{ {
@ -481,13 +534,19 @@ int decode_type1005(rtcm_t *rtcm)
if (rtcm->outtype) if (rtcm->outtype)
{ {
msg = rtcm->msgtype + strlen(rtcm->msgtype); msg = rtcm->msgtype + strlen(rtcm->msgtype);
for (j = 0; j < 3; j++) re[j] = rr[j] * 0.0001; for (j = 0; j < 3; j++)
{
re[j] = rr[j] * 0.0001;
}
ecef2pos(re, pos); ecef2pos(re, pos);
sprintf(msg, " staid=%4d pos=%.8f %.8f %.3f", staid, pos[0] * R2D, pos[1] * R2D, sprintf(msg, " staid=%4d pos=%.8f %.8f %.3f", staid, pos[0] * R2D, pos[1] * R2D,
pos[2]); pos[2]);
} }
/* test station id */ /* test station id */
if (!test_staid(rtcm, staid)) return -1; if (!test_staid(rtcm, staid))
{
return -1;
}
rtcm->sta.deltype = 0; /* xyz */ rtcm->sta.deltype = 0; /* xyz */
for (j = 0; j < 3; j++) for (j = 0; j < 3; j++)
@ -530,13 +589,19 @@ int decode_type1006(rtcm_t *rtcm)
if (rtcm->outtype) if (rtcm->outtype)
{ {
msg = rtcm->msgtype + strlen(rtcm->msgtype); msg = rtcm->msgtype + strlen(rtcm->msgtype);
for (j = 0; j < 3; j++) re[j] = rr[j] * 0.0001; for (j = 0; j < 3; j++)
{
re[j] = rr[j] * 0.0001;
}
ecef2pos(re, pos); ecef2pos(re, pos);
sprintf(msg, " staid=%4d pos=%.8f %.8f %.3f anth=%.3f", staid, pos[0] * R2D, sprintf(msg, " staid=%4d pos=%.8f %.8f %.3f anth=%.3f", staid, pos[0] * R2D,
pos[1] * R2D, pos[2], anth); pos[1] * R2D, pos[2], anth);
} }
/* test station id */ /* test station id */
if (!test_staid(rtcm, staid)) return -1; if (!test_staid(rtcm, staid))
{
return -1;
}
rtcm->sta.deltype = 1; /* xyz */ rtcm->sta.deltype = 1; /* xyz */
for (j = 0; j < 3; j++) for (j = 0; j < 3; j++)
@ -581,7 +646,10 @@ int decode_type1007(rtcm_t *rtcm)
sprintf(msg, " staid=%4d", staid); sprintf(msg, " staid=%4d", staid);
} }
/* test station id */ /* test station id */
if (!test_staid(rtcm, staid)) return -1; if (!test_staid(rtcm, staid))
{
return -1;
}
strncpy(rtcm->sta.antdes, des, n); strncpy(rtcm->sta.antdes, des, n);
rtcm->sta.antdes[n] = '\0'; rtcm->sta.antdes[n] = '\0';
@ -629,7 +697,10 @@ int decode_type1008(rtcm_t *rtcm)
sprintf(msg, " staid=%4d", staid); sprintf(msg, " staid=%4d", staid);
} }
/* test station id */ /* test station id */
if (!test_staid(rtcm, staid)) return -1; if (!test_staid(rtcm, staid))
{
return -1;
}
strncpy(rtcm->sta.antdes, des, n); strncpy(rtcm->sta.antdes, des, n);
rtcm->sta.antdes[n] = '\0'; rtcm->sta.antdes[n] = '\0';
@ -666,7 +737,10 @@ int decode_head1009(rtcm_t *rtcm, int *sync)
return -1; return -1;
} }
/* test station id */ /* test station id */
if (!test_staid(rtcm, staid)) return -1; if (!test_staid(rtcm, staid))
{
return -1;
}
adjday_glot(rtcm, tod); adjday_glot(rtcm, tod);
@ -687,7 +761,10 @@ int decode_head1009(rtcm_t *rtcm, int *sync)
int decode_type1009(rtcm_t *rtcm) int decode_type1009(rtcm_t *rtcm)
{ {
int sync; int sync;
if (decode_head1009(rtcm, &sync) < 0) return -1; if (decode_head1009(rtcm, &sync) < 0)
{
return -1;
}
rtcm->obsflag = !sync; rtcm->obsflag = !sync;
return sync ? 0 : 1; return sync ? 0 : 1;
} }
@ -699,7 +776,10 @@ int decode_type1010(rtcm_t *rtcm)
double pr1, cnr1, tt, cp1, lam1; double pr1, cnr1, tt, cp1, lam1;
int i = 24 + 61, j, index, nsat, sync, prn, sat, code, freq, ppr1, lock1, amb, sys = SYS_GLO; int i = 24 + 61, j, index, nsat, sync, prn, sat, code, freq, ppr1, lock1, amb, sys = SYS_GLO;
if ((nsat = decode_head1009(rtcm, &sync)) < 0) return -1; if ((nsat = decode_head1009(rtcm, &sync)) < 0)
{
return -1;
}
for (j = 0; j < nsat && rtcm->obs.n < MAXOBS && i + 79 <= rtcm->len * 8; j++) for (j = 0; j < nsat && rtcm->obs.n < MAXOBS && i + 79 <= rtcm->len * 8; j++)
{ {
@ -729,7 +809,10 @@ int decode_type1010(rtcm_t *rtcm)
{ {
rtcm->obs.n = rtcm->obsflag = 0; rtcm->obs.n = rtcm->obsflag = 0;
} }
if ((index = obsindex3(&rtcm->obs, rtcm->time, sat)) < 0) continue; if ((index = obsindex3(&rtcm->obs, rtcm->time, sat)) < 0)
{
continue;
}
pr1 = pr1 * 0.02 + amb * PRUNIT_GLO; pr1 = pr1 * 0.02 + amb * PRUNIT_GLO;
if (ppr1 != static_cast<int>(0xFFF80000)) if (ppr1 != static_cast<int>(0xFFF80000))
{ {
@ -750,7 +833,10 @@ int decode_type1010(rtcm_t *rtcm)
int decode_type1011(rtcm_t *rtcm) int decode_type1011(rtcm_t *rtcm)
{ {
int sync; int sync;
if (decode_head1009(rtcm, &sync) < 0) return -1; if (decode_head1009(rtcm, &sync) < 0)
{
return -1;
}
rtcm->obsflag = !sync; rtcm->obsflag = !sync;
return sync ? 0 : 1; return sync ? 0 : 1;
} }
@ -763,7 +849,10 @@ int decode_type1012(rtcm_t *rtcm)
int i = 24 + 61, j, index, nsat, sync, prn, sat, freq, code1, code2, pr21, ppr1, ppr2; int i = 24 + 61, j, index, nsat, sync, prn, sat, freq, code1, code2, pr21, ppr1, ppr2;
int lock1, lock2, amb, sys = SYS_GLO; int lock1, lock2, amb, sys = SYS_GLO;
if ((nsat = decode_head1009(rtcm, &sync)) < 0) return -1; if ((nsat = decode_head1009(rtcm, &sync)) < 0)
{
return -1;
}
for (j = 0; j < nsat && rtcm->obs.n < MAXOBS && i + 130 <= rtcm->len * 8; j++) for (j = 0; j < nsat && rtcm->obs.n < MAXOBS && i + 130 <= rtcm->len * 8; j++)
{ {
@ -803,7 +892,10 @@ int decode_type1012(rtcm_t *rtcm)
{ {
rtcm->obs.n = rtcm->obsflag = 0; rtcm->obs.n = rtcm->obsflag = 0;
} }
if ((index = obsindex3(&rtcm->obs, rtcm->time, sat)) < 0) continue; if ((index = obsindex3(&rtcm->obs, rtcm->time, sat)) < 0)
{
continue;
}
pr1 = pr1 * 0.02 + amb * PRUNIT_GLO; pr1 = pr1 * 0.02 + amb * PRUNIT_GLO;
if (ppr1 != static_cast<int>(0xFFF80000)) if (ppr1 != static_cast<int>(0xFFF80000))
{ {
@ -945,7 +1037,10 @@ int decode_type1019(rtcm_t *rtcm)
eph.A = sqrtA * sqrtA; eph.A = sqrtA * sqrtA;
if (!strstr(rtcm->opt, "-EPHALL")) if (!strstr(rtcm->opt, "-EPHALL"))
{ {
if (eph.iode == rtcm->nav.eph[sat - 1].iode) return 0; /* unchanged */ if (eph.iode == rtcm->nav.eph[sat - 1].iode)
{
return 0; /* unchanged */
}
} }
rtcm->nav.eph[sat - 1] = eph; rtcm->nav.eph[sat - 1] = eph;
rtcm->ephsat = sat; rtcm->ephsat = sat;
@ -1021,27 +1116,41 @@ int decode_type1020(rtcm_t *rtcm)
geph.sat = sat; geph.sat = sat;
geph.svh = bn; geph.svh = bn;
geph.iode = tb & 0x7F; geph.iode = tb & 0x7F;
if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget()); if (rtcm->time.time == 0)
{
rtcm->time = utc2gpst(timeget());
}
tow = time2gpst(gpst2utc(rtcm->time), &week); tow = time2gpst(gpst2utc(rtcm->time), &week);
tod = fmod(tow, 86400.0); tod = fmod(tow, 86400.0);
tow -= tod; tow -= tod;
tof = tk_h * 3600.0 + tk_m * 60.0 + tk_s - 10800.0; /* lt->utc */ tof = tk_h * 3600.0 + tk_m * 60.0 + tk_s - 10800.0; /* lt->utc */
if (tof < tod - 43200.0) if (tof < tod - 43200.0)
{
tof += 86400.0; tof += 86400.0;
}
else if (tof > tod + 43200.0) else if (tof > tod + 43200.0)
{
tof -= 86400.0; tof -= 86400.0;
}
geph.tof = utc2gpst(gpst2time(week, tow + tof)); geph.tof = utc2gpst(gpst2time(week, tow + tof));
toe = tb * 900.0 - 10800.0; /* lt->utc */ toe = tb * 900.0 - 10800.0; /* lt->utc */
if (toe < tod - 43200.0) if (toe < tod - 43200.0)
{
toe += 86400.0; toe += 86400.0;
}
else if (toe > tod + 43200.0) else if (toe > tod + 43200.0)
{
toe -= 86400.0; toe -= 86400.0;
}
geph.toe = utc2gpst(gpst2time(week, tow + toe)); /* utc->gpst */ geph.toe = utc2gpst(gpst2time(week, tow + toe)); /* utc->gpst */
if (!strstr(rtcm->opt, "-EPHALL")) if (!strstr(rtcm->opt, "-EPHALL"))
{ {
if (fabs(timediff(geph.toe, rtcm->nav.geph[prn - 1].toe)) < 1.0 && if (fabs(timediff(geph.toe, rtcm->nav.geph[prn - 1].toe)) < 1.0 &&
geph.svh == rtcm->nav.geph[prn - 1].svh) return 0; /* unchanged */ geph.svh == rtcm->nav.geph[prn - 1].svh)
{
return 0; /* unchanged */
}
} }
rtcm->nav.geph[prn - 1] = geph; rtcm->nav.geph[prn - 1] = geph;
rtcm->ephsat = sat; rtcm->ephsat = sat;
@ -1230,7 +1339,10 @@ int decode_type1033(rtcm_t *rtcm)
sprintf(msg, " staid=%4d", staid); sprintf(msg, " staid=%4d", staid);
} }
/* test station id */ /* test station id */
if (!test_staid(rtcm, staid)) return -1; if (!test_staid(rtcm, staid))
{
return -1;
}
strncpy(rtcm->sta.antdes, des, n); strncpy(rtcm->sta.antdes, des, n);
rtcm->sta.antdes[n] = '\0'; rtcm->sta.antdes[n] = '\0';
@ -1386,7 +1498,10 @@ int decode_type1044(rtcm_t *rtcm)
if (!strstr(rtcm->opt, "-EPHALL")) if (!strstr(rtcm->opt, "-EPHALL"))
{ {
if (eph.iode == rtcm->nav.eph[sat - 1].iode && if (eph.iode == rtcm->nav.eph[sat - 1].iode &&
eph.iodc == rtcm->nav.eph[sat - 1].iodc) return 0; /* unchanged */ eph.iodc == rtcm->nav.eph[sat - 1].iodc)
{
return 0; /* unchanged */
}
} }
rtcm->nav.eph[sat - 1] = eph; rtcm->nav.eph[sat - 1] = eph;
rtcm->ephsat = sat; rtcm->ephsat = sat;
@ -1490,7 +1605,10 @@ int decode_type1045(rtcm_t *rtcm)
eph.code = 2; /* data source = f/nav e5a */ eph.code = 2; /* data source = f/nav e5a */
if (!strstr(rtcm->opt, "-EPHALL")) if (!strstr(rtcm->opt, "-EPHALL"))
{ {
if (eph.iode == rtcm->nav.eph[sat - 1].iode) return 0; /* unchanged */ if (eph.iode == rtcm->nav.eph[sat - 1].iode)
{
return 0; /* unchanged */
}
} }
rtcm->nav.eph[sat - 1] = eph; rtcm->nav.eph[sat - 1] = eph;
rtcm->ephsat = sat; rtcm->ephsat = sat;
@ -1594,7 +1712,10 @@ int decode_type1046(rtcm_t *rtcm)
eph.code = 2; /* data source = f/nav e5a */ eph.code = 2; /* data source = f/nav e5a */
if (!strstr(rtcm->opt, "-EPHALL")) if (!strstr(rtcm->opt, "-EPHALL"))
{ {
if (eph.iode == rtcm->nav.eph[sat - 1].iode) return 0; /* unchanged */ if (eph.iode == rtcm->nav.eph[sat - 1].iode)
{
return 0; /* unchanged */
}
} }
rtcm->nav.eph[sat - 1] = eph; rtcm->nav.eph[sat - 1] = eph;
rtcm->ephsat = sat; rtcm->ephsat = sat;
@ -1704,7 +1825,10 @@ int decode_type1047(rtcm_t *rtcm)
{ {
if (timediff(eph.toe, rtcm->nav.eph[sat - 1].toe) == 0.0 && if (timediff(eph.toe, rtcm->nav.eph[sat - 1].toe) == 0.0 &&
eph.iode == rtcm->nav.eph[sat - 1].iode && eph.iode == rtcm->nav.eph[sat - 1].iode &&
eph.iodc == rtcm->nav.eph[sat - 1].iodc) return 0; /* unchanged */ eph.iodc == rtcm->nav.eph[sat - 1].iodc)
{
return 0; /* unchanged */
}
} }
rtcm->nav.eph[sat - 1] = eph; rtcm->nav.eph[sat - 1] = eph;
rtcm->ephsat = sat; rtcm->ephsat = sat;
@ -1809,7 +1933,10 @@ int decode_type63(rtcm_t *rtcm)
{ {
if (timediff(eph.toe, rtcm->nav.eph[sat - 1].toe) == 0.0 && if (timediff(eph.toe, rtcm->nav.eph[sat - 1].toe) == 0.0 &&
eph.iode == rtcm->nav.eph[sat - 1].iode && eph.iode == rtcm->nav.eph[sat - 1].iode &&
eph.iodc == rtcm->nav.eph[sat - 1].iodc) return 0; /* unchanged */ eph.iodc == rtcm->nav.eph[sat - 1].iodc)
{
return 0; /* unchanged */
}
} }
rtcm->nav.eph[sat - 1] = eph; rtcm->nav.eph[sat - 1] = eph;
rtcm->ephsat = sat; rtcm->ephsat = sat;
@ -1828,7 +1955,10 @@ int decode_ssr1_head(rtcm_t *rtcm, int sys, int *sync, int *iod,
#ifndef SSR_QZSS_DRAFT_V05 #ifndef SSR_QZSS_DRAFT_V05
ns = sys == SYS_QZS ? 4 : 6; ns = sys == SYS_QZS ? 4 : 6;
#endif #endif
if (i + (sys == SYS_GLO ? 53 : 50 + ns) > rtcm->len * 8) return -1; if (i + (sys == SYS_GLO ? 53 : 50 + ns) > rtcm->len * 8)
{
return -1;
}
if (sys == SYS_GLO) if (sys == SYS_GLO)
{ {
@ -1883,7 +2013,10 @@ int decode_ssr2_head(rtcm_t *rtcm, int sys, int *sync, int *iod,
#ifndef SSR_QZSS_DRAFT_V05 #ifndef SSR_QZSS_DRAFT_V05
ns = sys == SYS_QZS ? 4 : 6; ns = sys == SYS_QZS ? 4 : 6;
#endif #endif
if (i + (sys == SYS_GLO ? 52 : 49 + ns) > rtcm->len * 8) return -1; if (i + (sys == SYS_GLO ? 52 : 49 + ns) > rtcm->len * 8)
{
return -1;
}
if (sys == SYS_GLO) if (sys == SYS_GLO)
{ {
@ -1936,7 +2069,10 @@ int decode_ssr7_head(rtcm_t *rtcm, int sys, int *sync, int *iod,
#ifndef SSR_QZSS_DRAFT_V05 #ifndef SSR_QZSS_DRAFT_V05
ns = sys == SYS_QZS ? 4 : 6; ns = sys == SYS_QZS ? 4 : 6;
#endif #endif
if (i + (sys == SYS_GLO ? 54 : 51 + ns) > rtcm->len * 8) return -1; if (i + (sys == SYS_GLO ? 54 : 51 + ns) > rtcm->len * 8)
{
return -1;
}
if (sys == SYS_GLO) if (sys == SYS_GLO)
{ {
@ -2214,7 +2350,10 @@ int decode_ssr3(rtcm_t *rtcm, int sys)
nbias = getbitu(rtcm->buff, i, 5); nbias = getbitu(rtcm->buff, i, 5);
i += 5; i += 5;
for (k = 0; k < MAXCODE; k++) cbias[k] = 0.0; for (k = 0; k < MAXCODE; k++)
{
cbias[k] = 0.0;
}
for (k = 0; k < nbias && i + 19 <= rtcm->len * 8; k++) for (k = 0; k < nbias && i + 19 <= rtcm->len * 8; k++)
{ {
mode = getbitu(rtcm->buff, i, 5); mode = getbitu(rtcm->buff, i, 5);
@ -2544,7 +2683,10 @@ int decode_ssr7(rtcm_t *rtcm, int sys)
yaw_rate = getbits(rtcm->buff, i, 8); yaw_rate = getbits(rtcm->buff, i, 8);
i += 8; i += 8;
for (k = 0; k < MAXCODE; k++) pbias[k] = stdpb[k] = 0.0; for (k = 0; k < MAXCODE; k++)
{
pbias[k] = stdpb[k] = 0.0;
}
for (k = 0; k < nbias && i + 49 <= rtcm->len * 8; k++) for (k = 0; k < nbias && i + 49 <= rtcm->len * 8; k++)
{ {
mode = getbitu(rtcm->buff, i, 5); mode = getbitu(rtcm->buff, i, 5);
@ -2596,7 +2738,10 @@ void sigindex(int sys, const unsigned char *code, const int *freq, int n,
/* test code priority */ /* test code priority */
for (i = 0; i < n; i++) for (i = 0; i < n; i++)
{ {
if (!code[i]) continue; if (!code[i])
{
continue;
}
if (freq[i] > NFREQ) if (freq[i] > NFREQ)
{ /* save as extended signal if freq > NFREQ */ { /* save as extended signal if freq > NFREQ */
@ -2609,20 +2754,29 @@ void sigindex(int sys, const unsigned char *code, const int *freq, int n,
/* select highest priority signal */ /* select highest priority signal */
if (pri > pri_h[freq[i] - 1]) if (pri > pri_h[freq[i] - 1])
{ {
if (index[freq[i] - 1]) ex[index[freq[i] - 1] - 1] = 1; if (index[freq[i] - 1])
{
ex[index[freq[i] - 1] - 1] = 1;
}
pri_h[freq[i] - 1] = pri; pri_h[freq[i] - 1] = pri;
index[freq[i] - 1] = i + 1; index[freq[i] - 1] = i + 1;
} }
else else
{
ex[i] = 1; ex[i] = 1;
} }
}
/* signal index in obs data */ /* signal index in obs data */
for (i = nex = 0; i < n; i++) for (i = nex = 0; i < n; i++)
{ {
if (ex[i] == 0) if (ex[i] == 0)
{
ind[i] = freq[i] - 1; ind[i] = freq[i] - 1;
}
else if (nex < NEXOBS) else if (nex < NEXOBS)
{
ind[i] = NFREQ + nex++; ind[i] = NFREQ + nex++;
}
else else
{ /* no space in obs data */ { /* no space in obs data */
trace(2, "rtcm msm: no space in obs data sys=%d code=%d\n", sys, code[i]); trace(2, "rtcm msm: no space in obs data sys=%d code=%d\n", sys, code[i]);
@ -2704,17 +2858,27 @@ void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r,
if (sys == SYS_BDS) if (sys == SYS_BDS)
{ {
if (freq[i] == 5) if (freq[i] == 5)
{
freq[i] = 2; /* B2 */ freq[i] = 2; /* B2 */
}
else if (freq[i] == 4) else if (freq[i] == 4)
{
freq[i] = 3; /* B3 */ freq[i] = 3; /* B3 */
} }
}
if (code[i] != CODE_NONE) if (code[i] != CODE_NONE)
{ {
if (q) q += sprintf(q, "L%s%s", sig[i], i < h->nsig - 1 ? ", " : ""); if (q)
{
q += sprintf(q, "L%s%s", sig[i], i < h->nsig - 1 ? ", " : "");
}
} }
else else
{ {
if (q) q += sprintf(q, "(%d)%s", h->sigs[i], i < h->nsig - 1 ? ", " : ""); if (q)
{
q += sprintf(q, "(%d)%s", h->sigs[i], i < h->nsig - 1 ? ", " : "");
}
trace(2, "rtcm3 %d: unknown signal id=%2d\n", type, h->sigs[i]); trace(2, "rtcm3 %d: unknown signal id=%2d\n", type, h->sigs[i]);
} }
@ -2728,9 +2892,13 @@ void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r,
{ {
prn = h->sats[i]; prn = h->sats[i];
if (sys == SYS_QZS) if (sys == SYS_QZS)
{
prn += MINPRNQZS - 1; prn += MINPRNQZS - 1;
}
else if (sys == SYS_SBS) else if (sys == SYS_SBS)
{
prn += MINPRNSBS - 1; prn += MINPRNSBS - 1;
}
if ((sat = satno(sys, prn))) if ((sat = satno(sys, prn)))
{ {
@ -2747,7 +2915,10 @@ void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r,
} }
for (k = 0; k < h->nsig; k++) for (k = 0; k < h->nsig; k++)
{ {
if (!h->cellmask[k + i * h->nsig]) continue; if (!h->cellmask[k + i * h->nsig])
{
continue;
}
if (sat && index >= 0 && ind[k] >= 0) if (sat && index >= 0 && ind[k] >= 0)
{ {
@ -2843,13 +3014,19 @@ int decode_msm_head(rtcm_t *rtcm, int sys, int *sync, int *iod,
{ {
mask = getbitu(rtcm->buff, i, 1); mask = getbitu(rtcm->buff, i, 1);
i += 1; i += 1;
if (mask) h->sats[h->nsat++] = j; if (mask)
{
h->sats[h->nsat++] = j;
}
} }
for (j = 1; j <= 32; j++) for (j = 1; j <= 32; j++)
{ {
mask = getbitu(rtcm->buff, i, 1); mask = getbitu(rtcm->buff, i, 1);
i += 1; i += 1;
if (mask) h->sigs[h->nsig++] = j; if (mask)
{
h->sigs[h->nsig++] = j;
}
} }
} }
else else
@ -2858,7 +3035,10 @@ int decode_msm_head(rtcm_t *rtcm, int sys, int *sync, int *iod,
return -1; return -1;
} }
/* test station id */ /* test station id */
if (!test_staid(rtcm, staid)) return -1; if (!test_staid(rtcm, staid))
{
return -1;
}
if (h->nsat * h->nsig > 64) if (h->nsat * h->nsig > 64)
{ {
@ -2876,7 +3056,10 @@ int decode_msm_head(rtcm_t *rtcm, int sys, int *sync, int *iod,
{ {
h->cellmask[j] = getbitu(rtcm->buff, i, 1); h->cellmask[j] = getbitu(rtcm->buff, i, 1);
i += 1; i += 1;
if (h->cellmask[j]) ncell++; if (h->cellmask[j])
{
ncell++;
}
} }
*hsize = i; *hsize = i;
@ -2898,7 +3081,10 @@ int decode_msm0(rtcm_t *rtcm, int sys)
{ {
msm_h_t h = {0, 0, 0, 0, 0, 0, 0, 0, {0}, {0}, {0}}; msm_h_t h = {0, 0, 0, 0, 0, 0, 0, 0, {0}, {0}, {0}};
int i, sync, iod; int i, sync, iod;
if (decode_msm_head(rtcm, sys, &sync, &iod, &h, &i) < 0) return -1; if (decode_msm_head(rtcm, sys, &sync, &iod, &h, &i) < 0)
{
return -1;
}
rtcm->obsflag = !sync; rtcm->obsflag = !sync;
return sync ? 0 : 1; return sync ? 0 : 1;
} }
@ -2914,7 +3100,10 @@ int decode_msm4(rtcm_t *rtcm, int sys)
type = getbitu(rtcm->buff, 24, 12); type = getbitu(rtcm->buff, 24, 12);
/* decode msm header */ /* decode msm header */
if ((ncell = decode_msm_head(rtcm, sys, &sync, &iod, &h, &i)) < 0) return -1; if ((ncell = decode_msm_head(rtcm, sys, &sync, &iod, &h, &i)) < 0)
{
return -1;
}
if (i + h.nsat * 18 + ncell * 48 > rtcm->len * 8) if (i + h.nsat * 18 + ncell * 48 > rtcm->len * 8)
{ {
@ -2922,34 +3111,52 @@ int decode_msm4(rtcm_t *rtcm, int sys)
ncell, rtcm->len); ncell, rtcm->len);
return -1; return -1;
} }
for (j = 0; j < h.nsat; j++) r[j] = 0.0; for (j = 0; j < h.nsat; j++)
for (j = 0; j < ncell; j++) pr[j] = cp[j] = -1E16; {
r[j] = 0.0;
}
for (j = 0; j < ncell; j++)
{
pr[j] = cp[j] = -1E16;
}
/* decode satellite data */ /* decode satellite data */
for (j = 0; j < h.nsat; j++) for (j = 0; j < h.nsat; j++)
{ /* range */ { /* range */
rng = getbitu(rtcm->buff, i, 8); rng = getbitu(rtcm->buff, i, 8);
i += 8; i += 8;
if (rng != 255) r[j] = rng * RANGE_MS; if (rng != 255)
{
r[j] = rng * RANGE_MS;
}
} }
for (j = 0; j < h.nsat; j++) for (j = 0; j < h.nsat; j++)
{ {
rng_m = getbitu(rtcm->buff, i, 10); rng_m = getbitu(rtcm->buff, i, 10);
i += 10; i += 10;
if (r[j] != 0.0) r[j] += rng_m * TWO_N10 * RANGE_MS; if (r[j] != 0.0)
{
r[j] += rng_m * TWO_N10 * RANGE_MS;
}
} }
/* decode signal data */ /* decode signal data */
for (j = 0; j < ncell; j++) for (j = 0; j < ncell; j++)
{ /* pseudorange */ { /* pseudorange */
prv = getbits(rtcm->buff, i, 15); prv = getbits(rtcm->buff, i, 15);
i += 15; i += 15;
if (prv != -16384) pr[j] = prv * TWO_N24 * RANGE_MS; if (prv != -16384)
{
pr[j] = prv * TWO_N24 * RANGE_MS;
}
} }
for (j = 0; j < ncell; j++) for (j = 0; j < ncell; j++)
{ /* phaserange */ { /* phaserange */
cpv = getbits(rtcm->buff, i, 22); cpv = getbits(rtcm->buff, i, 22);
i += 22; i += 22;
if (cpv != -2097152) cp[j] = cpv * TWO_N29 * RANGE_MS; if (cpv != -2097152)
{
cp[j] = cpv * TWO_N29 * RANGE_MS;
}
} }
for (j = 0; j < ncell; j++) for (j = 0; j < ncell; j++)
{ /* lock time */ { /* lock time */
@ -2985,7 +3192,10 @@ int decode_msm5(rtcm_t *rtcm, int sys)
type = getbitu(rtcm->buff, 24, 12); type = getbitu(rtcm->buff, 24, 12);
/* decode msm header */ /* decode msm header */
if ((ncell = decode_msm_head(rtcm, sys, &sync, &iod, &h, &i)) < 0) return -1; if ((ncell = decode_msm_head(rtcm, sys, &sync, &iod, &h, &i)) < 0)
{
return -1;
}
if (i + h.nsat * 36 + ncell * 63 > rtcm->len * 8) if (i + h.nsat * 36 + ncell * 63 > rtcm->len * 8)
{ {
@ -2998,14 +3208,20 @@ int decode_msm5(rtcm_t *rtcm, int sys)
r[j] = rr[j] = 0.0; r[j] = rr[j] = 0.0;
ex[j] = 15; ex[j] = 15;
} }
for (j = 0; j < ncell; j++) pr[j] = cp[j] = rrf[j] = -1E16; for (j = 0; j < ncell; j++)
{
pr[j] = cp[j] = rrf[j] = -1E16;
}
/* decode satellite data */ /* decode satellite data */
for (j = 0; j < h.nsat; j++) for (j = 0; j < h.nsat; j++)
{ /* range */ { /* range */
rng = getbitu(rtcm->buff, i, 8); rng = getbitu(rtcm->buff, i, 8);
i += 8; i += 8;
if (rng != 255) r[j] = rng * RANGE_MS; if (rng != 255)
{
r[j] = rng * RANGE_MS;
}
} }
for (j = 0; j < h.nsat; j++) for (j = 0; j < h.nsat; j++)
{ /* extended info */ { /* extended info */
@ -3016,26 +3232,38 @@ int decode_msm5(rtcm_t *rtcm, int sys)
{ {
rng_m = getbitu(rtcm->buff, i, 10); rng_m = getbitu(rtcm->buff, i, 10);
i += 10; i += 10;
if (r[j] != 0.0) r[j] += rng_m * TWO_N10 * RANGE_MS; if (r[j] != 0.0)
{
r[j] += rng_m * TWO_N10 * RANGE_MS;
}
} }
for (j = 0; j < h.nsat; j++) for (j = 0; j < h.nsat; j++)
{ /* phaserangerate */ { /* phaserangerate */
rate = getbits(rtcm->buff, i, 14); rate = getbits(rtcm->buff, i, 14);
i += 14; i += 14;
if (rate != -8192) rr[j] = rate * 1.0; if (rate != -8192)
{
rr[j] = rate * 1.0;
}
} }
/* decode signal data */ /* decode signal data */
for (j = 0; j < ncell; j++) for (j = 0; j < ncell; j++)
{ /* pseudorange */ { /* pseudorange */
prv = getbits(rtcm->buff, i, 15); prv = getbits(rtcm->buff, i, 15);
i += 15; i += 15;
if (prv != -16384) pr[j] = prv * TWO_N24 * RANGE_MS; if (prv != -16384)
{
pr[j] = prv * TWO_N24 * RANGE_MS;
}
} }
for (j = 0; j < ncell; j++) for (j = 0; j < ncell; j++)
{ /* phaserange */ { /* phaserange */
cpv = getbits(rtcm->buff, i, 22); cpv = getbits(rtcm->buff, i, 22);
i += 22; i += 22;
if (cpv != -2097152) cp[j] = cpv * TWO_N29 * RANGE_MS; if (cpv != -2097152)
{
cp[j] = cpv * TWO_N29 * RANGE_MS;
}
} }
for (j = 0; j < ncell; j++) for (j = 0; j < ncell; j++)
{ /* lock time */ { /* lock time */
@ -3056,7 +3284,10 @@ int decode_msm5(rtcm_t *rtcm, int sys)
{ /* phaserangerate */ { /* phaserangerate */
rrv = getbits(rtcm->buff, i, 15); rrv = getbits(rtcm->buff, i, 15);
i += 15; i += 15;
if (rrv != -16384) rrf[j] = rrv * 0.0001; if (rrv != -16384)
{
rrf[j] = rrv * 0.0001;
}
} }
/* save obs data in msm message */ /* save obs data in msm message */
save_msm_obs(rtcm, sys, &h, r, pr, cp, rr, rrf, cnr, lock, ex, half); save_msm_obs(rtcm, sys, &h, r, pr, cp, rr, rrf, cnr, lock, ex, half);
@ -3076,7 +3307,10 @@ int decode_msm6(rtcm_t *rtcm, int sys)
type = getbitu(rtcm->buff, 24, 12); type = getbitu(rtcm->buff, 24, 12);
/* decode msm header */ /* decode msm header */
if ((ncell = decode_msm_head(rtcm, sys, &sync, &iod, &h, &i)) < 0) return -1; if ((ncell = decode_msm_head(rtcm, sys, &sync, &iod, &h, &i)) < 0)
{
return -1;
}
if (i + h.nsat * 18 + ncell * 65 > rtcm->len * 8) if (i + h.nsat * 18 + ncell * 65 > rtcm->len * 8)
{ {
@ -3084,34 +3318,52 @@ int decode_msm6(rtcm_t *rtcm, int sys)
ncell, rtcm->len); ncell, rtcm->len);
return -1; return -1;
} }
for (j = 0; j < h.nsat; j++) r[j] = 0.0; for (j = 0; j < h.nsat; j++)
for (j = 0; j < ncell; j++) pr[j] = cp[j] = -1E16; {
r[j] = 0.0;
}
for (j = 0; j < ncell; j++)
{
pr[j] = cp[j] = -1E16;
}
/* decode satellite data */ /* decode satellite data */
for (j = 0; j < h.nsat; j++) for (j = 0; j < h.nsat; j++)
{ /* range */ { /* range */
rng = getbitu(rtcm->buff, i, 8); rng = getbitu(rtcm->buff, i, 8);
i += 8; i += 8;
if (rng != 255) r[j] = rng * RANGE_MS; if (rng != 255)
{
r[j] = rng * RANGE_MS;
}
} }
for (j = 0; j < h.nsat; j++) for (j = 0; j < h.nsat; j++)
{ {
rng_m = getbitu(rtcm->buff, i, 10); rng_m = getbitu(rtcm->buff, i, 10);
i += 10; i += 10;
if (r[j] != 0.0) r[j] += rng_m * TWO_N10 * RANGE_MS; if (r[j] != 0.0)
{
r[j] += rng_m * TWO_N10 * RANGE_MS;
}
} }
/* decode signal data */ /* decode signal data */
for (j = 0; j < ncell; j++) for (j = 0; j < ncell; j++)
{ /* pseudorange */ { /* pseudorange */
prv = getbits(rtcm->buff, i, 20); prv = getbits(rtcm->buff, i, 20);
i += 20; i += 20;
if (prv != -524288) pr[j] = prv * TWO_N29 * RANGE_MS; if (prv != -524288)
{
pr[j] = prv * TWO_N29 * RANGE_MS;
}
} }
for (j = 0; j < ncell; j++) for (j = 0; j < ncell; j++)
{ /* phaserange */ { /* phaserange */
cpv = getbits(rtcm->buff, i, 24); cpv = getbits(rtcm->buff, i, 24);
i += 24; i += 24;
if (cpv != -8388608) cp[j] = cpv * TWO_N31 * RANGE_MS; if (cpv != -8388608)
{
cp[j] = cpv * TWO_N31 * RANGE_MS;
}
} }
for (j = 0; j < ncell; j++) for (j = 0; j < ncell; j++)
{ /* lock time */ { /* lock time */
@ -3147,7 +3399,10 @@ int decode_msm7(rtcm_t *rtcm, int sys)
type = getbitu(rtcm->buff, 24, 12); type = getbitu(rtcm->buff, 24, 12);
/* decode msm header */ /* decode msm header */
if ((ncell = decode_msm_head(rtcm, sys, &sync, &iod, &h, &i)) < 0) return -1; if ((ncell = decode_msm_head(rtcm, sys, &sync, &iod, &h, &i)) < 0)
{
return -1;
}
if (i + h.nsat * 36 + ncell * 80 > rtcm->len * 8) if (i + h.nsat * 36 + ncell * 80 > rtcm->len * 8)
{ {
@ -3160,14 +3415,20 @@ int decode_msm7(rtcm_t *rtcm, int sys)
r[j] = rr[j] = 0.0; r[j] = rr[j] = 0.0;
ex[j] = 15; ex[j] = 15;
} }
for (j = 0; j < ncell; j++) pr[j] = cp[j] = rrf[j] = -1E16; for (j = 0; j < ncell; j++)
{
pr[j] = cp[j] = rrf[j] = -1E16;
}
/* decode satellite data */ /* decode satellite data */
for (j = 0; j < h.nsat; j++) for (j = 0; j < h.nsat; j++)
{ /* range */ { /* range */
rng = getbitu(rtcm->buff, i, 8); rng = getbitu(rtcm->buff, i, 8);
i += 8; i += 8;
if (rng != 255) r[j] = rng * RANGE_MS; if (rng != 255)
{
r[j] = rng * RANGE_MS;
}
} }
for (j = 0; j < h.nsat; j++) for (j = 0; j < h.nsat; j++)
{ /* extended info */ { /* extended info */
@ -3178,26 +3439,38 @@ int decode_msm7(rtcm_t *rtcm, int sys)
{ {
rng_m = getbitu(rtcm->buff, i, 10); rng_m = getbitu(rtcm->buff, i, 10);
i += 10; i += 10;
if (r[j] != 0.0) r[j] += rng_m * TWO_N10 * RANGE_MS; if (r[j] != 0.0)
{
r[j] += rng_m * TWO_N10 * RANGE_MS;
}
} }
for (j = 0; j < h.nsat; j++) for (j = 0; j < h.nsat; j++)
{ /* phaserangerate */ { /* phaserangerate */
rate = getbits(rtcm->buff, i, 14); rate = getbits(rtcm->buff, i, 14);
i += 14; i += 14;
if (rate != -8192) rr[j] = rate * 1.0; if (rate != -8192)
{
rr[j] = rate * 1.0;
}
} }
/* decode signal data */ /* decode signal data */
for (j = 0; j < ncell; j++) for (j = 0; j < ncell; j++)
{ /* pseudorange */ { /* pseudorange */
prv = getbits(rtcm->buff, i, 20); prv = getbits(rtcm->buff, i, 20);
i += 20; i += 20;
if (prv != -524288) pr[j] = prv * TWO_N29 * RANGE_MS; if (prv != -524288)
{
pr[j] = prv * TWO_N29 * RANGE_MS;
}
} }
for (j = 0; j < ncell; j++) for (j = 0; j < ncell; j++)
{ /* phaserange */ { /* phaserange */
cpv = getbits(rtcm->buff, i, 24); cpv = getbits(rtcm->buff, i, 24);
i += 24; i += 24;
if (cpv != -8388608) cp[j] = cpv * TWO_N31 * RANGE_MS; if (cpv != -8388608)
{
cp[j] = cpv * TWO_N31 * RANGE_MS;
}
} }
for (j = 0; j < ncell; j++) for (j = 0; j < ncell; j++)
{ /* lock time */ { /* lock time */
@ -3218,7 +3491,10 @@ int decode_msm7(rtcm_t *rtcm, int sys)
{ /* phaserangerate */ { /* phaserangerate */
rrv = getbits(rtcm->buff, i, 15); rrv = getbits(rtcm->buff, i, 15);
i += 15; i += 15;
if (rrv != -16384) rrf[j] = rrv * 0.0001; if (rrv != -16384)
{
rrf[j] = rrv * 0.0001;
}
} }
/* save obs data in msm message */ /* save obs data in msm message */
save_msm_obs(rtcm, sys, &h, r, pr, cp, rr, rrf, cnr, lock, ex, half); save_msm_obs(rtcm, sys, &h, r, pr, cp, rr, rrf, cnr, lock, ex, half);
@ -3624,11 +3900,17 @@ int decode_rtcm3(rtcm_t *rtcm)
{ {
type -= 1000; type -= 1000;
if (1 <= type && type <= 299) if (1 <= type && type <= 299)
{
rtcm->nmsg3[type]++; /* 1001-1299 */ rtcm->nmsg3[type]++; /* 1001-1299 */
}
else if (1000 <= type && type <= 1099) else if (1000 <= type && type <= 1099)
{
rtcm->nmsg3[type - 700]++; /* 2000-2099 */ rtcm->nmsg3[type - 700]++; /* 2000-2099 */
}
else else
{
rtcm->nmsg3[0]++; rtcm->nmsg3[0]++;
} }
}
return ret; return ret;
} }

1118
src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
View File

File diff suppressed because it is too large Load Diff

548
src/algorithms/libs/rtklib/rtklib_rtkpos.cc
View File

File diff suppressed because it is too large Load Diff

279
src/algorithms/libs/rtklib/rtklib_rtksvr.cc
View File

@ -78,10 +78,12 @@ void updatenav(nav_t *nav)
{ {
int i, j; int i, j;
for (i = 0; i < MAXSAT; i++) for (i = 0; i < MAXSAT; i++)
{
for (j = 0; j < NFREQ; j++) for (j = 0; j < NFREQ; j++)
{ {
nav->lam[i][j] = satwavelen(i + 1, j, nav); nav->lam[i][j] = satwavelen(i + 1, j, nav);
} }
}
} }
@ -96,14 +98,23 @@ void updatefcn(rtksvr_t *svr)
for (j = 0, frq = -999; j < 3; j++) for (j = 0, frq = -999; j < 3; j++)
{ {
if (svr->raw[j].nav.geph[i].sat != sat) continue; if (svr->raw[j].nav.geph[i].sat != sat)
{
continue;
}
frq = svr->raw[j].nav.geph[i].frq; frq = svr->raw[j].nav.geph[i].frq;
} }
if (frq < -7 || frq > 6) continue; if (frq < -7 || frq > 6)
{
continue;
}
for (j = 0; j < 3; j++) for (j = 0; j < 3; j++)
{ {
if (svr->raw[j].nav.geph[i].sat == sat) continue; if (svr->raw[j].nav.geph[i].sat == sat)
{
continue;
}
svr->raw[j].nav.geph[i].sat = sat; svr->raw[j].nav.geph[i].sat = sat;
svr->raw[j].nav.geph[i].frq = frq; svr->raw[j].nav.geph[i].frq = frq;
} }
@ -130,7 +141,10 @@ void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
for (i = 0; i < obs->n; i++) for (i = 0; i < obs->n; i++)
{ {
if (svr->rtk.opt.exsats[obs->data[i].sat - 1] == 1 || if (svr->rtk.opt.exsats[obs->data[i].sat - 1] == 1 ||
!(satsys(obs->data[i].sat, nullptr) & svr->rtk.opt.navsys)) continue; !(satsys(obs->data[i].sat, nullptr) & svr->rtk.opt.navsys))
{
continue;
}
svr->obs[index][iobs].data[n] = obs->data[i]; svr->obs[index][iobs].data[n] = obs->data[i];
svr->obs[index][iobs].data[n++].rcv = index + 1; svr->obs[index][iobs].data[n++].rcv = index + 1;
} }
@ -189,7 +203,10 @@ void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
} }
else else
{ {
for (i = 0; i < MAXSBSMSG - 1; i++) svr->sbsmsg[i] = svr->sbsmsg[i + 1]; for (i = 0; i < MAXSBSMSG - 1; i++)
{
svr->sbsmsg[i] = svr->sbsmsg[i + 1];
}
svr->sbsmsg[i] = *sbsmsg; svr->sbsmsg[i] = *sbsmsg;
} }
sbsupdatecorr(sbsmsg, &svr->nav); sbsupdatecorr(sbsmsg, &svr->nav);
@ -200,12 +217,30 @@ void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
{ /* ion/utc parameters */ { /* ion/utc parameters */
if (svr->navsel == index || svr->navsel >= 3) if (svr->navsel == index || svr->navsel >= 3)
{ {
for (i = 0; i < 8; i++) svr->nav.ion_gps[i] = nav->ion_gps[i]; for (i = 0; i < 8; i++)
for (i = 0; i < 4; i++) svr->nav.utc_gps[i] = nav->utc_gps[i]; {
for (i = 0; i < 4; i++) svr->nav.ion_gal[i] = nav->ion_gal[i]; svr->nav.ion_gps[i] = nav->ion_gps[i];
for (i = 0; i < 4; i++) svr->nav.utc_gal[i] = nav->utc_gal[i]; }
for (i = 0; i < 8; i++) svr->nav.ion_qzs[i] = nav->ion_qzs[i]; for (i = 0; i < 4; i++)
for (i = 0; i < 4; i++) svr->nav.utc_qzs[i] = nav->utc_qzs[i]; {
svr->nav.utc_gps[i] = nav->utc_gps[i];
}
for (i = 0; i < 4; i++)
{
svr->nav.ion_gal[i] = nav->ion_gal[i];
}
for (i = 0; i < 4; i++)
{
svr->nav.utc_gal[i] = nav->utc_gal[i];
}
for (i = 0; i < 8; i++)
{
svr->nav.ion_qzs[i] = nav->ion_qzs[i];
}
for (i = 0; i < 4; i++)
{
svr->nav.utc_qzs[i] = nav->utc_qzs[i];
}
svr->nav.leaps = nav->leaps; svr->nav.leaps = nav->leaps;
} }
svr->nmsg[index][2]++; svr->nmsg[index][2]++;
@ -248,7 +283,10 @@ void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
{ /* ssr message */ { /* ssr message */
for (i = 0; i < MAXSAT; i++) for (i = 0; i < MAXSAT; i++)
{ {
if (!svr->rtcm[index].ssr[i].update) continue; if (!svr->rtcm[index].ssr[i].update)
{
continue;
}
svr->rtcm[index].ssr[i].update = 0; svr->rtcm[index].ssr[i].update = 0;
iode = svr->rtcm[index].ssr[i].iode; iode = svr->rtcm[index].ssr[i].iode;
@ -333,17 +371,24 @@ int decoderaw(rtksvr_t *svr, int index)
} }
#endif #endif
/* update rtk server */ /* update rtk server */
if (ret > 0) updatesvr(svr, ret, obs, nav, sat, sbsmsg, index, fobs); if (ret > 0)
{
updatesvr(svr, ret, obs, nav, sat, sbsmsg, index, fobs);
}
/* observation data received */ /* observation data received */
if (ret == 1) if (ret == 1)
{ {
if (fobs < MAXOBSBUF) if (fobs < MAXOBSBUF)
{
fobs++; fobs++;
}
else else
{
svr->prcout++; svr->prcout++;
} }
} }
}
svr->nb[index] = 0; svr->nb[index] = 0;
rtksvrunlock(svr); rtksvrunlock(svr);
@ -360,7 +405,9 @@ void decodefile(rtksvr_t *svr, int index)
// Allocate space for GLONASS frequency channels depending on availability // Allocate space for GLONASS frequency channels depending on availability
for (i = 0; i < MAXPRNGLO + 1; i++) for (i = 0; i < MAXPRNGLO + 1; i++)
{
glo_fcn[i] = '0'; glo_fcn[i] = '0';
}
pcv_t pcvt0[MAXSAT] = {{0, {'0'}, {'0'}, {0, 0.0}, {0, 0.0}, {{0.0}, {0.0}}, {{0.0}, {0.0}}}}; pcv_t pcvt0[MAXSAT] = {{0, {'0'}, {'0'}, {0, 0.0}, {0, 0.0}, {{0.0}, {0.0}}, {{0.0}, {0.0}}}};
sbsfcorr_t sbsfcorr0 = {{0, 0.0}, 0.0, 0.0, 0.0, 0, 0, 0}; sbsfcorr_t sbsfcorr0 = {{0, 0.0}, 0.0, 0.0, 0.0, 0, 0, 0};
sbslcorr_t sbslcorr0 = {{0, 0.0}, 0, {0.0}, {0.0}, 0.0, 0.0}; sbslcorr_t sbslcorr0 = {{0, 0.0}, 0, {0.0}, {0.0}, 0.0, 0.0};
@ -413,7 +460,10 @@ void decodefile(rtksvr_t *svr, int index)
/* update precise ephemeris */ /* update precise ephemeris */
rtksvrlock(svr); rtksvrlock(svr);
if (svr->nav.peph) free(svr->nav.peph); if (svr->nav.peph)
{
free(svr->nav.peph);
}
svr->nav.ne = svr->nav.nemax = nav.ne; svr->nav.ne = svr->nav.nemax = nav.ne;
svr->nav.peph = nav.peph; svr->nav.peph = nav.peph;
svr->ftime[index] = utc2gpst(timeget()); svr->ftime[index] = utc2gpst(timeget());
@ -433,7 +483,10 @@ void decodefile(rtksvr_t *svr, int index)
/* update precise clock */ /* update precise clock */
rtksvrlock(svr); rtksvrlock(svr);
if (svr->nav.pclk) free(svr->nav.pclk); if (svr->nav.pclk)
{
free(svr->nav.pclk);
}
svr->nav.nc = svr->nav.ncmax = nav.nc; svr->nav.nc = svr->nav.ncmax = nav.nc;
svr->nav.pclk = nav.pclk; svr->nav.pclk = nav.pclk;
svr->ftime[index] = utc2gpst(timeget()); svr->ftime[index] = utc2gpst(timeget());
@ -555,12 +608,18 @@ void *rtksvrthread(void *arg)
} }
ticknmea = tick; ticknmea = tick;
} }
if ((cputime = static_cast<int>(tickget() - tick)) > 0) svr->cputime = cputime; if ((cputime = static_cast<int>(tickget() - tick)) > 0)
{
svr->cputime = cputime;
}
/* sleep until next cycle */ /* sleep until next cycle */
sleepms(svr->cycle - cputime); sleepms(svr->cycle - cputime);
} }
for (i = 0; i < MAXSTRRTK; i++) strclose(svr->stream + i); for (i = 0; i < MAXSTRRTK; i++)
{
strclose(svr->stream + i);
}
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
{ {
svr->nb[i] = svr->npb[i] = 0; svr->nb[i] = svr->npb[i] = 0;
@ -602,23 +661,64 @@ int rtksvrinit(rtksvr_t *svr)
tracet(3, "rtksvrinit:\n"); tracet(3, "rtksvrinit:\n");
svr->state = svr->cycle = svr->nmeacycle = svr->nmeareq = 0; svr->state = svr->cycle = svr->nmeacycle = svr->nmeareq = 0;
for (i = 0; i < 3; i++) svr->nmeapos[i] = 0.0; for (i = 0; i < 3; i++)
{
svr->nmeapos[i] = 0.0;
}
svr->buffsize = 0; svr->buffsize = 0;
for (i = 0; i < 3; i++) svr->format[i] = 0; for (i = 0; i < 3; i++)
for (i = 0; i < 2; i++) svr->solopt[i] = solopt_default; {
svr->format[i] = 0;
}
for (i = 0; i < 2; i++)
{
svr->solopt[i] = solopt_default;
}
svr->navsel = svr->nsbs = svr->nsol = 0; svr->navsel = svr->nsbs = svr->nsol = 0;
rtkinit(&svr->rtk, &prcopt_default); rtkinit(&svr->rtk, &prcopt_default);
for (i = 0; i < 3; i++) svr->nb[i] = 0;
for (i = 0; i < 2; i++) svr->nsb[i] = 0;
for (i = 0; i < 3; i++) svr->npb[i] = 0;
for (i = 0; i < 3; i++) svr->buff[i] = nullptr;
for (i = 0; i < 2; i++) svr->sbuf[i] = nullptr;
for (i = 0; i < 3; i++) svr->pbuf[i] = nullptr;
for (i = 0; i < MAXSOLBUF; i++) svr->solbuf[i] = sol0;
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
for (j = 0; j < 10; j++) svr->nmsg[i][j] = 0; {
for (i = 0; i < 3; i++) svr->ftime[i] = time0; svr->nb[i] = 0;
for (i = 0; i < 3; i++) svr->files[i][0] = '\0'; }
for (i = 0; i < 2; i++)
{
svr->nsb[i] = 0;
}
for (i = 0; i < 3; i++)
{
svr->npb[i] = 0;
}
for (i = 0; i < 3; i++)
{
svr->buff[i] = nullptr;
}
for (i = 0; i < 2; i++)
{
svr->sbuf[i] = nullptr;
}
for (i = 0; i < 3; i++)
{
svr->pbuf[i] = nullptr;
}
for (i = 0; i < MAXSOLBUF; i++)
{
svr->solbuf[i] = sol0;
}
for (i = 0; i < 3; i++)
{
for (j = 0; j < 10; j++)
{
svr->nmsg[i][j] = 0;
}
}
for (i = 0; i < 3; i++)
{
svr->ftime[i] = time0;
}
for (i = 0; i < 3; i++)
{
svr->files[i][0] = '\0';
}
svr->moni = nullptr; svr->moni = nullptr;
svr->tick = 0; svr->tick = 0;
svr->thread = 0; // NOLINT svr->thread = 0; // NOLINT
@ -631,14 +731,24 @@ int rtksvrinit(rtksvr_t *svr)
tracet(1, "rtksvrinit: malloc error\n"); tracet(1, "rtksvrinit: malloc error\n");
return 0; return 0;
} }
for (i = 0; i < MAXSAT * 2; i++) svr->nav.eph[i] = eph0; for (i = 0; i < MAXSAT * 2; i++)
for (i = 0; i < NSATGLO * 2; i++) svr->nav.geph[i] = geph0; {
for (i = 0; i < NSATSBS * 2; i++) svr->nav.seph[i] = seph0; svr->nav.eph[i] = eph0;
}
for (i = 0; i < NSATGLO * 2; i++)
{
svr->nav.geph[i] = geph0;
}
for (i = 0; i < NSATSBS * 2; i++)
{
svr->nav.seph[i] = seph0;
}
svr->nav.n = MAXSAT * 2; svr->nav.n = MAXSAT * 2;
svr->nav.ng = NSATGLO * 2; svr->nav.ng = NSATGLO * 2;
svr->nav.ns = NSATSBS * 2; svr->nav.ns = NSATSBS * 2;
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
{
for (j = 0; j < MAXOBSBUF; j++) for (j = 0; j < MAXOBSBUF; j++)
{ {
if (!(svr->obs[i][j].data = static_cast<obsd_t *>(malloc(sizeof(obsd_t) * MAXOBS)))) if (!(svr->obs[i][j].data = static_cast<obsd_t *>(malloc(sizeof(obsd_t) * MAXOBS))))
@ -647,12 +757,16 @@ int rtksvrinit(rtksvr_t *svr)
return 0; return 0;
} }
} }
}
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
{ {
memset(svr->raw + i, 0, sizeof(raw_t)); memset(svr->raw + i, 0, sizeof(raw_t));
memset(svr->rtcm + i, 0, sizeof(rtcm_t)); memset(svr->rtcm + i, 0, sizeof(rtcm_t));
} }
for (i = 0; i < MAXSTRRTK; i++) strinit(svr->stream + i); for (i = 0; i < MAXSTRRTK; i++)
{
strinit(svr->stream + i);
}
initlock(&svr->lock); initlock(&svr->lock);
@ -673,10 +787,12 @@ void rtksvrfree(rtksvr_t *svr)
free(svr->nav.geph); free(svr->nav.geph);
free(svr->nav.seph); free(svr->nav.seph);
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
{
for (j = 0; j < MAXOBSBUF; j++) for (j = 0; j < MAXOBSBUF; j++)
{ {
free(svr->obs[i][j].data); free(svr->obs[i][j].data);
} }
}
} }
@ -742,15 +858,24 @@ int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
tracet(3, "rtksvrstart: cycle=%d buffsize=%d navsel=%d nmeacycle=%d nmeareq=%d\n", tracet(3, "rtksvrstart: cycle=%d buffsize=%d navsel=%d nmeacycle=%d nmeareq=%d\n",
cycle, buffsize, navsel, nmeacycle, nmeareq); cycle, buffsize, navsel, nmeacycle, nmeareq);
if (svr->state) return 0; if (svr->state)
{
return 0;
}
strinitcom(); strinitcom();
svr->cycle = cycle > 1 ? cycle : 1; svr->cycle = cycle > 1 ? cycle : 1;
svr->nmeacycle = nmeacycle > 1000 ? nmeacycle : 1000; svr->nmeacycle = nmeacycle > 1000 ? nmeacycle : 1000;
svr->nmeareq = nmeareq; svr->nmeareq = nmeareq;
for (i = 0; i < 3; i++) svr->nmeapos[i] = nmeapos[i]; for (i = 0; i < 3; i++)
{
svr->nmeapos[i] = nmeapos[i];
}
svr->buffsize = buffsize > 4096 ? buffsize : 4096; svr->buffsize = buffsize > 4096 ? buffsize : 4096;
for (i = 0; i < 3; i++) svr->format[i] = formats[i]; for (i = 0; i < 3; i++)
{
svr->format[i] = formats[i];
}
svr->navsel = navsel; svr->navsel = navsel;
svr->nsbs = 0; svr->nsbs = 0;
svr->nsol = 0; svr->nsol = 0;
@ -767,16 +892,28 @@ int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
tracet(1, "rtksvrstart: malloc error\n"); tracet(1, "rtksvrstart: malloc error\n");
return 0; return 0;
} }
for (j = 0; j < 10; j++) svr->nmsg[i][j] = 0; for (j = 0; j < 10; j++)
for (j = 0; j < MAXOBSBUF; j++) svr->obs[i][j].n = 0; {
svr->nmsg[i][j] = 0;
}
for (j = 0; j < MAXOBSBUF; j++)
{
svr->obs[i][j].n = 0;
}
/* initialize receiver raw and rtcm control */ /* initialize receiver raw and rtcm control */
//init_raw (svr->raw +i); //init_raw (svr->raw +i);
init_rtcm(svr->rtcm + i); init_rtcm(svr->rtcm + i);
/* set receiver and rtcm option */ /* set receiver and rtcm option */
if (strlen(rcvopts[i]) < 256) strcpy(svr->raw[i].opt, rcvopts[i]); if (strlen(rcvopts[i]) < 256)
if (strlen(rcvopts[i]) < 256) strcpy(svr->rtcm[i].opt, rcvopts[i]); {
strcpy(svr->raw[i].opt, rcvopts[i]);
}
if (strlen(rcvopts[i]) < 256)
{
strcpy(svr->rtcm[i].opt, rcvopts[i]);
}
/* connect dgps corrections */ /* connect dgps corrections */
svr->rtcm[i].dgps = svr->nav.dgps; svr->rtcm[i].dgps = svr->nav.dgps;
@ -800,9 +937,18 @@ int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
svr->rtk.rb[i] = i < 3 ? prcopt->rb[i] : 0.0; svr->rtk.rb[i] = i < 3 ? prcopt->rb[i] : 0.0;
} }
/* update navigation data */ /* update navigation data */
for (i = 0; i < MAXSAT * 2; i++) svr->nav.eph[i].ttr = time0; for (i = 0; i < MAXSAT * 2; i++)
for (i = 0; i < NSATGLO * 2; i++) svr->nav.geph[i].tof = time0; {
for (i = 0; i < NSATSBS * 2; i++) svr->nav.seph[i].tof = time0; svr->nav.eph[i].ttr = time0;
}
for (i = 0; i < NSATGLO * 2; i++)
{
svr->nav.geph[i].tof = time0;
}
for (i = 0; i < NSATSBS * 2; i++)
{
svr->nav.seph[i].tof = time0;
}
updatenav(&svr->nav); updatenav(&svr->nav);
/* set monitor stream */ /* set monitor stream */
@ -812,10 +958,16 @@ int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
for (i = 0; i < 8; i++) for (i = 0; i < 8; i++)
{ {
rw = i < 3 ? STR_MODE_R : STR_MODE_W; rw = i < 3 ? STR_MODE_R : STR_MODE_W;
if (strs[i] != STR_FILE) rw |= STR_MODE_W; if (strs[i] != STR_FILE)
{
rw |= STR_MODE_W;
}
if (!stropen(svr->stream + i, strs[i], rw, paths[i])) if (!stropen(svr->stream + i, strs[i], rw, paths[i]))
{ {
for (i--; i >= 0; i--) strclose(svr->stream + i); for (i--; i >= 0; i--)
{
strclose(svr->stream + i);
}
return 0; return 0;
} }
/* set initial time for rtcm and raw */ /* set initial time for rtcm and raw */
@ -833,7 +985,10 @@ int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
/* write start commands to input streams */ /* write start commands to input streams */
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
{ {
if (cmds[i]) strsendcmd(svr->stream + i, cmds[i]); if (cmds[i])
{
strsendcmd(svr->stream + i, cmds[i]);
}
} }
/* write solution header to solution streams */ /* write solution header to solution streams */
for (i = 3; i < 5; i++) for (i = 3; i < 5; i++)
@ -843,7 +998,10 @@ int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs,
/* create rtk server thread */ /* create rtk server thread */
if (pthread_create(&svr->thread, nullptr, rtksvrthread, svr)) if (pthread_create(&svr->thread, nullptr, rtksvrthread, svr))
{ {
for (i = 0; i < MAXSTRRTK; i++) strclose(svr->stream + i); for (i = 0; i < MAXSTRRTK; i++)
{
strclose(svr->stream + i);
}
return 0; return 0;
} }
return 1; return 1;
@ -869,7 +1027,10 @@ void rtksvrstop(rtksvr_t *svr, char **cmds)
rtksvrlock(svr); rtksvrlock(svr);
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
{ {
if (cmds[i]) strsendcmd(svr->stream + i, cmds[i]); if (cmds[i])
{
strsendcmd(svr->stream + i, cmds[i]);
}
} }
rtksvrunlock(svr); rtksvrunlock(svr);
@ -897,7 +1058,10 @@ int rtksvropenstr(rtksvr_t *svr, int index, int str, const char *path,
{ {
tracet(3, "rtksvropenstr: index=%d str=%d path=%s\n", index, str, path); tracet(3, "rtksvropenstr: index=%d str=%d path=%s\n", index, str, path);
if (index < 3 || index > 7 || !svr->state) return 0; if (index < 3 || index > 7 || !svr->state)
{
return 0;
}
rtksvrlock(svr); rtksvrlock(svr);
@ -936,7 +1100,10 @@ void rtksvrclosestr(rtksvr_t *svr, int index)
{ {
tracet(3, "rtksvrclosestr: index=%d\n", index); tracet(3, "rtksvrclosestr: index=%d\n", index);
if (index < 3 || index > 7 || !svr->state) return; if (index < 3 || index > 7 || !svr->state)
{
return;
}
rtksvrlock(svr); rtksvrlock(svr);
@ -966,7 +1133,10 @@ int rtksvrostat(rtksvr_t *svr, int rcv, gtime_t *time, int *sat,
tracet(4, "rtksvrostat: rcv=%d\n", rcv); tracet(4, "rtksvrostat: rcv=%d\n", rcv);
if (!svr->state) return 0; if (!svr->state)
{
return 0;
}
rtksvrlock(svr); rtksvrlock(svr);
ns = svr->obs[rcv][0].n; ns = svr->obs[rcv][0].n;
if (ns > 0) if (ns > 0)
@ -1014,7 +1184,10 @@ void rtksvrsstat(rtksvr_t *svr, int *sstat, char *msg)
for (i = 0; i < MAXSTRRTK; i++) for (i = 0; i < MAXSTRRTK; i++)
{ {
sstat[i] = strstat(svr->stream + i, s); sstat[i] = strstat(svr->stream + i, s);
if (*s) p += sprintf(p, "(%d) %s ", i + 1, s); if (*s)
{
p += sprintf(p, "(%d) %s ", i + 1, s);
}
} }
rtksvrunlock(svr); rtksvrunlock(svr);
} }

338
src/algorithms/libs/rtklib/rtklib_sbas.cc
View File

@ -67,7 +67,12 @@
char *getfield(char *p, int pos) char *getfield(char *p, int pos)
{ {
for (pos--; pos > 0; pos--, p++) for (pos--; pos > 0; pos--, p++)
if (!(p = strchr(p, ','))) return nullptr; {
if (!(p = strchr(p, ',')))
{
return nullptr;
}
}
return p; return p;
} }
@ -114,21 +119,37 @@ int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
if (getbitu(msg->msg, 13 + i, 1)) if (getbitu(msg->msg, 13 + i, 1))
{ {
if (i <= 37) if (i <= 37)
{
sat = satno(SYS_GPS, i); /* 0- 37: gps */ sat = satno(SYS_GPS, i); /* 0- 37: gps */
}
else if (i <= 61) else if (i <= 61)
{
sat = satno(SYS_GLO, i - 37); /* 38- 61: glonass */ sat = satno(SYS_GLO, i - 37); /* 38- 61: glonass */
}
else if (i <= 119) else if (i <= 119)
{
sat = 0; /* 62-119: future gnss */ sat = 0; /* 62-119: future gnss */
}
else if (i <= 138) else if (i <= 138)
{
sat = satno(SYS_SBS, i); /* 120-138: geo/waas */ sat = satno(SYS_SBS, i); /* 120-138: geo/waas */
}
else if (i <= 182) else if (i <= 182)
{
sat = 0; /* 139-182: reserved */ sat = 0; /* 139-182: reserved */
}
else if (i <= 192) else if (i <= 192)
{
sat = satno(SYS_SBS, i + 10); /* 183-192: qzss ref [2] */ sat = satno(SYS_SBS, i + 10); /* 183-192: qzss ref [2] */
}
else if (i <= 202) else if (i <= 202)
{
sat = satno(SYS_QZS, i); /* 193-202: qzss ref [2] */ sat = satno(SYS_QZS, i); /* 193-202: qzss ref [2] */
}
else else
{
sat = 0; /* 203- : reserved */ sat = 0; /* 203- : reserved */
}
sbssat->sat[n++].sat = sat; sbssat->sat[n++].sat = sat;
} }
} }
@ -149,14 +170,20 @@ int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(4, "decode_sbstype2:\n"); trace(4, "decode_sbstype2:\n");
if (sbssat->iodp != static_cast<int>(getbitu(msg->msg, 16, 2))) return 0; if (sbssat->iodp != static_cast<int>(getbitu(msg->msg, 16, 2)))
{
return 0;
}
type = getbitu(msg->msg, 8, 6); type = getbitu(msg->msg, 8, 6);
iodf = getbitu(msg->msg, 14, 2); iodf = getbitu(msg->msg, 14, 2);
for (i = 0; i < 13; i++) for (i = 0; i < 13; i++)
{ {
if ((j = 13 * ((type == 0 ? 2 : type) - 2) + i) >= sbssat->nsat) break; if ((j = 13 * ((type == 0 ? 2 : type) - 2) + i) >= sbssat->nsat)
{
break;
}
udre = getbitu(msg->msg, 174 + 4 * i, 4); udre = getbitu(msg->msg, 174 + 4 * i, 4);
t0 = sbssat->sat[j].fcorr.t0; t0 = sbssat->sat[j].fcorr.t0;
prc = sbssat->sat[j].fcorr.prc; prc = sbssat->sat[j].fcorr.prc;
@ -194,7 +221,10 @@ int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat)
} }
for (i = 0; i < sbssat->nsat && i < MAXSAT; i++) for (i = 0; i < sbssat->nsat && i < MAXSAT; i++)
{ {
if (sbssat->sat[i].fcorr.iodf != iodf[i / 28]) continue; if (sbssat->sat[i].fcorr.iodf != iodf[i / 28])
{
continue;
}
udre = getbitu(msg->msg, 22 + i * 4, 4); udre = getbitu(msg->msg, 22 + i * 4, 4);
sbssat->sat[i].fcorr.udre = udre + 1; sbssat->sat[i].fcorr.udre = udre + 1;
} }
@ -210,7 +240,10 @@ int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(4, "decode_sbstype7\n"); trace(4, "decode_sbstype7\n");
if (sbssat->iodp != static_cast<int>(getbitu(msg->msg, 18, 2))) return 0; if (sbssat->iodp != static_cast<int>(getbitu(msg->msg, 18, 2)))
{
return 0;
}
sbssat->tlat = getbitu(msg->msg, 14, 4); sbssat->tlat = getbitu(msg->msg, 14, 4);
@ -237,9 +270,13 @@ int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav)
} }
t = static_cast<int>(getbitu(msg->msg, 22, 13)) * 16 - msg->tow % 86400; t = static_cast<int>(getbitu(msg->msg, 22, 13)) * 16 - msg->tow % 86400;
if (t <= -43200) if (t <= -43200)
{
t += 86400; t += 86400;
}
else if (t > 43200) else if (t > 43200)
{
t -= 86400; t -= 86400;
}
seph.sat = sat; seph.sat = sat;
seph.t0 = gpst2time(msg->week, msg->tow + t); seph.t0 = gpst2time(msg->week, msg->tow + t);
seph.tof = gpst2time(msg->week, msg->tow); seph.tof = gpst2time(msg->week, msg->tow);
@ -291,16 +328,24 @@ int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion)
m = 5; m = 5;
} }
else else
{
return 0; return 0;
}
sbsion[band].iodi = static_cast<int16_t>(getbitu(msg->msg, 22, 2)); sbsion[band].iodi = static_cast<int16_t>(getbitu(msg->msg, 22, 2));
for (i = 1, n = 0; i <= 201; i++) for (i = 1, n = 0; i <= 201; i++)
{ {
if (!getbitu(msg->msg, 23 + i, 1)) continue; if (!getbitu(msg->msg, 23 + i, 1))
{
continue;
}
for (j = 0; j < m; j++) for (j = 0; j < m; j++)
{ {
if (i < p[j].bits || p[j].bite < i) continue; if (i < p[j].bits || p[j].bite < i)
{
continue;
}
sbsion[band].igp[n].lat = band <= 8 ? p[j].y[i - p[j].bits] : p[j].x; sbsion[band].igp[n].lat = band <= 8 ? p[j].y[i - p[j].bits] : p[j].x;
sbsion[band].igp[n++].lon = band <= 8 ? p[j].x : p[j].y[i - p[j].bits]; sbsion[band].igp[n++].lon = band <= 8 ? p[j].x : p[j].y[i - p[j].bits];
break; break;
@ -320,7 +365,10 @@ int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
trace(4, "decode_longcorr0:\n"); trace(4, "decode_longcorr0:\n");
if (n == 0 || n > MAXSAT) return 0; if (n == 0 || n > MAXSAT)
{
return 0;
}
sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8); sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8);
@ -345,7 +393,10 @@ int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
trace(4, "decode_longcorr1:\n"); trace(4, "decode_longcorr1:\n");
if (n == 0 || n > MAXSAT) return 0; if (n == 0 || n > MAXSAT)
{
return 0;
}
sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8); sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8);
@ -358,9 +409,13 @@ int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
sbssat->sat[n - 1].lcorr.daf1 = getbits(msg->msg, p + 82, 8) * TWO_N39; sbssat->sat[n - 1].lcorr.daf1 = getbits(msg->msg, p + 82, 8) * TWO_N39;
t = static_cast<int>(getbitu(msg->msg, p + 90, 13)) * 16 - msg->tow % 86400; t = static_cast<int>(getbitu(msg->msg, p + 90, 13)) * 16 - msg->tow % 86400;
if (t <= -43200) if (t <= -43200)
{
t += 86400; t += 86400;
}
else if (t > 43200) else if (t > 43200)
{
t -= 86400; t -= 86400;
}
sbssat->sat[n - 1].lcorr.t0 = gpst2time(msg->week, msg->tow + t); sbssat->sat[n - 1].lcorr.t0 = gpst2time(msg->week, msg->tow + t);
trace(5, "decode_longcorr1: sat=%2d\n", sbssat->sat[n - 1].sat); trace(5, "decode_longcorr1: sat=%2d\n", sbssat->sat[n - 1].sat);
@ -396,14 +451,20 @@ int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(4, "decode_sbstype24:\n"); trace(4, "decode_sbstype24:\n");
if (sbssat->iodp != static_cast<int>(getbitu(msg->msg, 110, 2))) return 0; /* check IODP */ if (sbssat->iodp != static_cast<int>(getbitu(msg->msg, 110, 2)))
{
return 0; /* check IODP */
}
blk = getbitu(msg->msg, 112, 2); blk = getbitu(msg->msg, 112, 2);
iodf = getbitu(msg->msg, 114, 2); iodf = getbitu(msg->msg, 114, 2);
for (i = 0; i < 6; i++) for (i = 0; i < 6; i++)
{ {
if ((j = 13 * blk + i) >= sbssat->nsat) break; if ((j = 13 * blk + i) >= sbssat->nsat)
{
break;
}
udre = getbitu(msg->msg, 86 + 4 * i, 4); udre = getbitu(msg->msg, 86 + 4 * i, 4);
sbssat->sat[j].fcorr.t0 = gpst2time(msg->week, msg->tow); sbssat->sat[j].fcorr.t0 = gpst2time(msg->week, msg->tow);
@ -431,13 +492,19 @@ int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion)
trace(4, "decode_sbstype26:\n"); trace(4, "decode_sbstype26:\n");
if (band > MAXBAND || sbsion[band].iodi != static_cast<int>(getbitu(msg->msg, 217, 2))) return 0; if (band > MAXBAND || sbsion[band].iodi != static_cast<int>(getbitu(msg->msg, 217, 2)))
{
return 0;
}
block = getbitu(msg->msg, 18, 4); block = getbitu(msg->msg, 18, 4);
for (i = 0; i < 15; i++) for (i = 0; i < 15; i++)
{ {
if ((j = block * 15 + i) >= sbsion[band].nigp) continue; if ((j = block * 15 + i) >= sbsion[band].nigp)
{
continue;
}
give = getbitu(msg->msg, 22 + i * 13 + 9, 4); give = getbitu(msg->msg, 22 + i * 13 + 9, 4);
delay = getbitu(msg->msg, 22 + i * 13, 9); delay = getbitu(msg->msg, 22 + i * 13, 9);
@ -470,7 +537,10 @@ int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav)
trace(3, "sbsupdatecorr: type=%d\n", type); trace(3, "sbsupdatecorr: type=%d\n", type);
if (msg->week == 0) return -1; if (msg->week == 0)
{
return -1;
}
switch (type) switch (type)
{ {
@ -551,27 +621,62 @@ void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,
} }
else if (!strncmp(buff, "#RAWWAASFRAMEA", 14)) else if (!strncmp(buff, "#RAWWAASFRAMEA", 14))
{ /* NovAtel OEM4/V */ { /* NovAtel OEM4/V */
if (!(p = getfield(buff, 6))) continue; if (!(p = getfield(buff, 6)))
if (sscanf(p, "%d,%lf", &week, &tow) < 2) continue; {
if (!(p = strchr(++p, ';'))) continue; continue;
if (sscanf(++p, "%d,%d", &ch, &prn) < 2) continue; }
if (!(p = getfield(p, 4))) continue; if (sscanf(p, "%d,%lf", &week, &tow) < 2)
{
continue;
}
if (!(p = strchr(++p, ';')))
{
continue;
}
if (sscanf(++p, "%d,%d", &ch, &prn) < 2)
{
continue;
}
if (!(p = getfield(p, 4)))
{
continue;
}
} }
else if (!strncmp(buff, "$FRMA", 5)) else if (!strncmp(buff, "$FRMA", 5))
{ /* NovAtel OEM3 */ { /* NovAtel OEM3 */
if (!(p = getfield(buff, 2))) continue; if (!(p = getfield(buff, 2)))
if (sscanf(p, "%d,%lf,%d", &week, &tow, &prn) < 3) continue; {
if (!(p = getfield(p, 6))) continue; continue;
if (week < WEEKOFFSET) week += WEEKOFFSET; }
if (sscanf(p, "%d,%lf,%d", &week, &tow, &prn) < 3)
{
continue;
}
if (!(p = getfield(p, 6)))
{
continue;
}
if (week < WEEKOFFSET)
{
week += WEEKOFFSET;
}
} }
else else
{
continue; continue;
}
if (sel != 0 && sel != prn) continue; if (sel != 0 && sel != prn)
{
continue;
}
time = gpst2time(week, tow); time = gpst2time(week, tow);
if (!screent(time, ts, te, 0.0)) continue; if (!screent(time, ts, te, 0.0))
{
continue;
}
if (sbs->n >= sbs->nmax) if (sbs->n >= sbs->nmax)
{ {
@ -590,10 +695,16 @@ void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,
sbs->msgs[sbs->n].week = week; sbs->msgs[sbs->n].week = week;
sbs->msgs[sbs->n].tow = static_cast<int>(tow + 0.5); sbs->msgs[sbs->n].tow = static_cast<int>(tow + 0.5);
sbs->msgs[sbs->n].prn = prn; sbs->msgs[sbs->n].prn = prn;
for (i = 0; i < 29; i++) sbs->msgs[sbs->n].msg[i] = 0; for (i = 0; i < 29; i++)
{
sbs->msgs[sbs->n].msg[i] = 0;
}
for (i = 0; *(p - 1) && *p && i < 29; p += 2, i++) for (i = 0; *(p - 1) && *p && i < 29; p += 2, i++)
{ {
if (sscanf(p, "%2X", &b) == 1) sbs->msgs[sbs->n].msg[i] = static_cast<unsigned char>(b); if (sscanf(p, "%2X", &b) == 1)
{
sbs->msgs[sbs->n].msg[i] = static_cast<unsigned char>(b);
}
} }
sbs->msgs[sbs->n++].msg[28] &= 0xC0; sbs->msgs[sbs->n++].msg[28] &= 0xC0;
} }
@ -636,7 +747,10 @@ int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te,
{ {
if (!(efiles[i] = static_cast<char *>(malloc(1024)))) if (!(efiles[i] = static_cast<char *>(malloc(1024))))
{ {
for (i--; i >= 0; i--) free(efiles[i]); for (i--; i >= 0; i--)
{
free(efiles[i]);
}
return 0; return 0;
} }
} }
@ -645,13 +759,22 @@ int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te,
for (i = 0; i < n; i++) for (i = 0; i < n; i++)
{ {
if (!(ext = strrchr(efiles[i], '.'))) continue; if (!(ext = strrchr(efiles[i], '.')))
{
continue;
}
if (strcmp(ext, ".sbs") != 0 && strcmp(ext, ".SBS") != 0 && if (strcmp(ext, ".sbs") != 0 && strcmp(ext, ".SBS") != 0 &&
strcmp(ext, ".ems") != 0 && strcmp(ext, ".EMS") != 0) continue; strcmp(ext, ".ems") != 0 && strcmp(ext, ".EMS") != 0)
{
continue;
}
readmsgs(efiles[i], sel, ts, te, sbs); readmsgs(efiles[i], sel, ts, te, sbs);
} }
for (i = 0; i < MAXEXFILE; i++) free(efiles[i]); for (i = 0; i < MAXEXFILE; i++)
{
free(efiles[i]);
}
/* sort messages */ /* sort messages */
if (sbs->n > 0) if (sbs->n > 0)
@ -685,7 +808,10 @@ void sbsoutmsg(FILE *fp, sbsmsg_t *sbsmsg)
trace(4, "sbsoutmsg:\n"); trace(4, "sbsoutmsg:\n");
fprintf(fp, "%4d %6d %3d %2d : ", sbsmsg->week, sbsmsg->tow, sbsmsg->prn, type); fprintf(fp, "%4d %6d %3d %2d : ", sbsmsg->week, sbsmsg->tow, sbsmsg->prn, type);
for (i = 0; i < 29; i++) fprintf(fp, "%02X", sbsmsg->msg[i]); for (i = 0; i < 29; i++)
{
fprintf(fp, "%02X", sbsmsg->msg[i]);
}
fprintf(fp, "\n"); fprintf(fp, "\n");
} }
@ -700,7 +826,10 @@ void searchigp(gtime_t time __attribute__((unused)), const double *pos, const sb
trace(4, "searchigp: pos=%.3f %.3f\n", pos[0] * R2D, pos[1] * R2D); trace(4, "searchigp: pos=%.3f %.3f\n", pos[0] * R2D, pos[1] * R2D);
if (lon >= 180.0) lon -= 360.0; if (lon >= 180.0)
{
lon -= 360.0;
}
if (-55.0 <= lat && lat < 55.0) if (-55.0 <= lat && lat < 55.0)
{ {
latp[0] = static_cast<int>(floor(lat / 5.0)) * 5; latp[0] = static_cast<int>(floor(lat / 5.0)) * 5;
@ -730,29 +859,54 @@ void searchigp(gtime_t time __attribute__((unused)), const double *pos, const sb
} }
else if (lat >= 85.0) else if (lat >= 85.0)
{ {
for (i = 0; i < 4; i++) lonp[i] = static_cast<int>(floor(lon / 90.0)) * 90; for (i = 0; i < 4; i++)
{
lonp[i] = static_cast<int>(floor(lon / 90.0)) * 90;
}
} }
else if (lat < -85.0) else if (lat < -85.0)
{ {
for (i = 0; i < 4; i++) lonp[i] = static_cast<int>(floor((lon - 50.0) / 90.0)) * 90 + 40; for (i = 0; i < 4; i++)
{
lonp[i] = static_cast<int>(floor((lon - 50.0) / 90.0)) * 90 + 40;
}
} }
} }
for (i = 0; i < 4; i++) for (i = 0; i < 4; i++)
if (lonp[i] == 180) lonp[i] = -180; {
if (lonp[i] == 180)
{
lonp[i] = -180;
}
}
for (i = 0; i <= MAXBAND; i++) for (i = 0; i <= MAXBAND; i++)
{ {
for (p = ion[i].igp; p < ion[i].igp + ion[i].nigp; p++) for (p = ion[i].igp; p < ion[i].igp + ion[i].nigp; p++)
{ {
if (p->t0.time == 0) continue; if (p->t0.time == 0)
{
continue;
}
if (p->lat == latp[0] && p->lon == lonp[0] && p->give > 0) if (p->lat == latp[0] && p->lon == lonp[0] && p->give > 0)
{
igp[0] = p; igp[0] = p;
}
else if (p->lat == latp[1] && p->lon == lonp[1] && p->give > 0) else if (p->lat == latp[1] && p->lon == lonp[1] && p->give > 0)
{
igp[1] = p; igp[1] = p;
}
else if (p->lat == latp[0] && p->lon == lonp[2] && p->give > 0) else if (p->lat == latp[0] && p->lon == lonp[2] && p->give > 0)
{
igp[2] = p; igp[2] = p;
}
else if (p->lat == latp[1] && p->lon == lonp[3] && p->give > 0) else if (p->lat == latp[1] && p->lon == lonp[3] && p->give > 0)
{
igp[3] = p; igp[3] = p;
if (igp[0] && igp[1] && igp[2] && igp[3]) return; }
if (igp[0] && igp[1] && igp[2] && igp[3])
{
return;
}
} }
} }
} }
@ -783,7 +937,10 @@ int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos,
azel[0] * R2D, azel[1] * R2D); azel[0] * R2D, azel[1] * R2D);
*delay = *var = 0.0; *delay = *var = 0.0;
if (pos[2] < -100.0 || azel[1] <= 0) return 1; if (pos[2] < -100.0 || azel[1] <= 0)
{
return 1;
}
/* ipp (ionospheric pierce point) position */ /* ipp (ionospheric pierce point) position */
fp = ionppp(pos, azel, re, hion, posp); fp = ionppp(pos, azel, re, hion, posp);
@ -803,28 +960,42 @@ int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos,
{ {
w[1] = y; w[1] = y;
w[2] = x; w[2] = x;
if ((w[0] = 1.0 - w[1] - w[2]) < 0.0) err = 1; if ((w[0] = 1.0 - w[1] - w[2]) < 0.0)
{
err = 1;
}
} }
else if (igp[0] && igp[2] && igp[3]) else if (igp[0] && igp[2] && igp[3])
{ {
w[0] = 1.0 - x; w[0] = 1.0 - x;
w[3] = y; w[3] = y;
if ((w[2] = 1.0 - w[0] - w[3]) < 0.0) err = 1; if ((w[2] = 1.0 - w[0] - w[3]) < 0.0)
{
err = 1;
}
} }
else if (igp[0] && igp[1] && igp[3]) else if (igp[0] && igp[1] && igp[3])
{ {
w[0] = 1.0 - y; w[0] = 1.0 - y;
w[3] = x; w[3] = x;
if ((w[1] = 1.0 - w[0] - w[3]) < 0.0) err = 1; if ((w[1] = 1.0 - w[0] - w[3]) < 0.0)
{
err = 1;
}
} }
else if (igp[1] && igp[2] && igp[3]) else if (igp[1] && igp[2] && igp[3])
{ {
w[1] = 1.0 - x; w[1] = 1.0 - x;
w[2] = 1.0 - y; w[2] = 1.0 - y;
if ((w[3] = 1.0 - w[1] - w[2]) < 0.0) err = 1; if ((w[3] = 1.0 - w[1] - w[2]) < 0.0)
{
err = 1;
}
} }
else else
{
err = 1; err = 1;
}
if (err) if (err)
{ {
@ -834,7 +1005,10 @@ int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos,
} }
for (i = 0; i < 4; i++) for (i = 0; i < 4; i++)
{ {
if (!igp[i]) continue; if (!igp[i])
{
continue;
}
t = timediff(time, igp[i]->t0); t = timediff(time, igp[i]->t0);
*delay += w[i] * igp[i]->delay; *delay += w[i] * igp[i]->delay;
*var += w[i] * varicorr(igp[i]->give) * 9e-8 * fabs(t); *var += w[i] * varicorr(igp[i]->give) * 9e-8 * fabs(t);
@ -860,14 +1034,27 @@ void getmet(double lat, double *met)
double a; double a;
lat = fabs(lat); lat = fabs(lat);
if (lat <= 15.0) if (lat <= 15.0)
for (i = 0; i < 10; i++) met[i] = metprm[0][i]; {
for (i = 0; i < 10; i++)
{
met[i] = metprm[0][i];
}
}
else if (lat >= 75.0) else if (lat >= 75.0)
for (i = 0; i < 10; i++) met[i] = metprm[4][i]; {
for (i = 0; i < 10; i++)
{
met[i] = metprm[4][i];
}
}
else else
{ {
j = static_cast<int>(lat / 15.0); j = static_cast<int>(lat / 15.0);
a = (lat - j * 15.0) / 15.0; a = (lat - j * 15.0) / 15.0;
for (i = 0; i < 10; i++) met[i] = (1.0 - a) * metprm[j - 1][i] + a * metprm[j][i]; for (i = 0; i < 10; i++)
{
met[i] = (1.0 - a) * metprm[j - 1][i] + a * metprm[j][i];
}
} }
} }
@ -901,12 +1088,18 @@ double sbstropcorr(gtime_t time, const double *pos, const double *azel,
{ {
getmet(pos[0] * R2D, met); getmet(pos[0] * R2D, met);
c = cos(2.0 * PI * (time2doy(time) - (pos[0] >= 0.0 ? 28.0 : 211.0)) / 365.25); c = cos(2.0 * PI * (time2doy(time) - (pos[0] >= 0.0 ? 28.0 : 211.0)) / 365.25);
for (i = 0; i < 5; i++) met[i] -= met[i + 5] * c; for (i = 0; i < 5; i++)
{
met[i] -= met[i + 5] * c;
}
zh = 1e-6 * k1 * rd * met[0] / gm; zh = 1e-6 * k1 * rd * met[0] / gm;
zw = 1e-6 * k2 * rd / (gm * (met[4] + 1.0) - met[3] * rd) * met[2] / met[1]; zw = 1e-6 * k2 * rd / (gm * (met[4] + 1.0) - met[3] * rd) * met[2] / met[1];
zh *= pow(1.0 - met[3] * h / met[1], g / (rd * met[3])); zh *= pow(1.0 - met[3] * h / met[1], g / (rd * met[3]));
zw *= pow(1.0 - met[3] * h / met[1], (met[4] + 1.0) * g / (rd * met[3]) - 1.0); zw *= pow(1.0 - met[3] * h / met[1], (met[4] + 1.0) * g / (rd * met[3]) - 1.0);
for (i = 0; i < 3; i++) pos_[i] = pos[i]; for (i = 0; i < 3; i++)
{
pos_[i] = pos[i];
}
} }
m = 1.001 / sqrt(0.002001 + sinel * sinel); m = 1.001 / sqrt(0.002001 + sinel * sinel);
*var = 0.12 * 0.12 * m * m; *var = 0.12 * 0.12 * m * m;
@ -926,7 +1119,10 @@ int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
for (p = sbssat->sat; p < sbssat->sat + sbssat->nsat; p++) for (p = sbssat->sat; p < sbssat->sat + sbssat->nsat; p++)
{ {
if (p->sat != sat || p->lcorr.t0.time == 0) continue; if (p->sat != sat || p->lcorr.t0.time == 0)
{
continue;
}
t = timediff(time, p->lcorr.t0); t = timediff(time, p->lcorr.t0);
if (fabs(t) > MAXSBSAGEL) if (fabs(t) > MAXSBSAGEL)
{ {
@ -934,7 +1130,10 @@ int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
time_str(time, 0), sat, t); time_str(time, 0), sat, t);
return 0; return 0;
} }
for (i = 0; i < 3; i++) drs[i] = p->lcorr.dpos[i] + p->lcorr.dvel[i] * t; for (i = 0; i < 3; i++)
{
drs[i] = p->lcorr.dpos[i] + p->lcorr.dvel[i] * t;
}
*ddts = p->lcorr.daf0 + p->lcorr.daf1 * t; *ddts = p->lcorr.daf0 + p->lcorr.daf1 * t;
trace(5, "sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n", trace(5, "sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n",
@ -943,7 +1142,10 @@ int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
return 1; return 1;
} }
/* if sbas satellite without correction, no correction applied */ /* if sbas satellite without correction, no correction applied */
if (satsys(sat, nullptr) == SYS_SBS) return 1; if (satsys(sat, nullptr) == SYS_SBS)
{
return 1;
}
trace(2, "no sbas long-term correction: %s sat=%2d\n", time_str(time, 0), sat); trace(2, "no sbas long-term correction: %s sat=%2d\n", time_str(time, 0), sat);
return 0; return 0;
@ -961,12 +1163,21 @@ int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
for (p = sbssat->sat; p < sbssat->sat + sbssat->nsat; p++) for (p = sbssat->sat; p < sbssat->sat + sbssat->nsat; p++)
{ {
if (p->sat != sat) continue; if (p->sat != sat)
if (p->fcorr.t0.time == 0) break; {
continue;
}
if (p->fcorr.t0.time == 0)
{
break;
}
t = timediff(time, p->fcorr.t0) + sbssat->tlat; t = timediff(time, p->fcorr.t0) + sbssat->tlat;
/* expire age of correction or UDRE==14 (not monitored) */ /* expire age of correction or UDRE==14 (not monitored) */
if (fabs(t) > MAXSBSAGEF || p->fcorr.udre >= 15) continue; if (fabs(t) > MAXSBSAGEF || p->fcorr.udre >= 15)
{
continue;
}
*prc = p->fcorr.prc; *prc = p->fcorr.prc;
#ifdef RRCENA #ifdef RRCENA
if (p->fcorr.ai > 0 && fabs(t) <= 8.0 * p->fcorr.dt) if (p->fcorr.ai > 0 && fabs(t) <= 8.0 * p->fcorr.dt)
@ -1020,7 +1231,10 @@ int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs,
{ {
return 0; return 0;
} }
for (i = 0; i < 3; i++) rs[i] += drs[i]; for (i = 0; i < 3; i++)
{
rs[i] += drs[i];
}
dts[0] += dclk + prc / SPEED_OF_LIGHT; dts[0] += dclk + prc / SPEED_OF_LIGHT;
@ -1048,17 +1262,25 @@ int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words,
trace(5, "sbsdecodemsg: prn=%d\n", prn); trace(5, "sbsdecodemsg: prn=%d\n", prn);
if (time.time == 0) return 0; if (time.time == 0)
{
return 0;
}
tow = time2gpst(time, &sbsmsg->week); tow = time2gpst(time, &sbsmsg->week);
sbsmsg->tow = static_cast<int>(tow + DTTOL); sbsmsg->tow = static_cast<int>(tow + DTTOL);
sbsmsg->prn = prn; sbsmsg->prn = prn;
for (i = 0; i < 7; i++) for (i = 0; i < 7; i++)
{
for (j = 0; j < 4; j++) for (j = 0; j < 4; j++)
{ {
sbsmsg->msg[i * 4 + j] = static_cast<unsigned char>(words[i] >> ((3 - j) * 8)); sbsmsg->msg[i * 4 + j] = static_cast<unsigned char>(words[i] >> ((3 - j) * 8));
} }
}
sbsmsg->msg[28] = static_cast<unsigned char>(words[7] >> 18) & 0xC0; sbsmsg->msg[28] = static_cast<unsigned char>(words[7] >> 18) & 0xC0;
for (i = 28; i > 0; i--) f[i] = (sbsmsg->msg[i] >> 6) + (sbsmsg->msg[i - 1] << 2); for (i = 28; i > 0; i--)
{
f[i] = (sbsmsg->msg[i] >> 6) + (sbsmsg->msg[i - 1] << 2);
}
f[0] = sbsmsg->msg[0] >> 6; f[0] = sbsmsg->msg[0] >> 6;
return rtk_crc24q(f, 29) == (words[7] & 0xFFFFFF); /* check crc */ return rtk_crc24q(f, 29) == (words[7] & 0xFFFFFF); /* check crc */

528
src/algorithms/libs/rtklib/rtklib_solution.cc
View File

@ -79,9 +79,13 @@ static const int solq_nmea[] = {/* nmea quality flags to rtklib sol quality */
const char *opt2sep(const solopt_t *opt) const char *opt2sep(const solopt_t *opt)
{ {
if (!*opt->sep) if (!*opt->sep)
{
return " "; return " ";
}
if (!strcmp(opt->sep, "\\t")) if (!strcmp(opt->sep, "\\t"))
{
return "\t"; return "\t";
}
return opt->sep; return opt->sep;
} }
@ -94,9 +98,18 @@ int tonum(char *buff, const char *sep, double *v)
for (p = buff, n = 0; n < MAXFIELD; p = q + len) for (p = buff, n = 0; n < MAXFIELD; p = q + len)
{ {
if ((q = strstr(p, sep))) *q = '\0'; if ((q = strstr(p, sep)))
if (*p) v[n++] = atof(p); {
if (!q) break; *q = '\0';
}
if (*p)
{
v[n++] = atof(p);
}
if (!q)
{
break;
}
} }
return n; return n;
} }
@ -301,11 +314,17 @@ int decode_nmeagga(char **val, int n, sol_t *sol)
time = utc2gpst(epoch2time(ep)); time = utc2gpst(epoch2time(ep));
tt = timediff(time, sol->time); tt = timediff(time, sol->time);
if (tt < -43200.0) if (tt < -43200.0)
{
sol->time = timeadd(time, 86400.0); sol->time = timeadd(time, 86400.0);
}
else if (tt > 43200.0) else if (tt > 43200.0)
{
sol->time = timeadd(time, -86400.0); sol->time = timeadd(time, -86400.0);
}
else else
{
sol->time = time; sol->time = time;
}
pos2ecef(pos, sol->rr); pos2ecef(pos, sol->rr);
sol->stat = 0 <= solq && solq <= 8 ? solq_nmea[solq] : SOLQ_NONE; sol->stat = 0 <= solq && solq <= 8 ? solq_nmea[solq] : SOLQ_NONE;
sol->ns = nrcv; sol->ns = nrcv;
@ -337,8 +356,10 @@ int decode_nmea(char *buff, sol_t *sol)
*q = '\0'; *q = '\0';
} }
else else
{
break; break;
} }
}
/* decode nmea sentence */ /* decode nmea sentence */
if (!strcmp(val[0], "$GPRMC")) if (!strcmp(val[0], "$GPRMC"))
{ {
@ -362,9 +383,13 @@ char *decode_soltime(char *buff, const solopt_t *opt, gtime_t *time)
trace(4, "decode_soltime:\n"); trace(4, "decode_soltime:\n");
if (!strcmp(opt->sep, "\\t")) if (!strcmp(opt->sep, "\\t"))
{
strcpy(s, "\t"); strcpy(s, "\t");
}
else if (*opt->sep) else if (*opt->sep)
{
strcpy(s, opt->sep); strcpy(s, opt->sep);
}
len = static_cast<int>(strlen(s)); len = static_cast<int>(strlen(s));
/* yyyy/mm/dd hh:mm:ss or yyyy mm dd hh:mm:ss */ /* yyyy/mm/dd hh:mm:ss or yyyy mm dd hh:mm:ss */
@ -383,8 +408,14 @@ char *decode_soltime(char *buff, const solopt_t *opt, gtime_t *time)
{ {
*time = utc2gpst(timeadd(*time, -9 * 3600.0)); *time = utc2gpst(timeadd(*time, -9 * 3600.0));
} }
if (!(p = strchr(buff, ':')) || !(p = strchr(p + 1, ':'))) return nullptr; if (!(p = strchr(buff, ':')) || !(p = strchr(p + 1, ':')))
for (p++; isdigit(static_cast<int>(*p)) || *p == '.';) p++; {
return nullptr;
}
for (p++; isdigit(static_cast<int>(*p)) || *p == '.';)
{
p++;
}
return p + len; return p + len;
} }
if (opt->posf == SOLF_GSIF) if (opt->posf == SOLF_GSIF)
@ -394,16 +425,31 @@ char *decode_soltime(char *buff, const solopt_t *opt, gtime_t *time)
return nullptr; return nullptr;
} }
*time = timeadd(epoch2time(v), -12.0 * 3600.0); *time = timeadd(epoch2time(v), -12.0 * 3600.0);
if (!(p = strchr(buff, ':')) || !(p = strchr(p + 1, ':'))) return nullptr; if (!(p = strchr(buff, ':')) || !(p = strchr(p + 1, ':')))
for (p++; isdigit(static_cast<int>(*p)) || *p == '.';) p++; {
return nullptr;
}
for (p++; isdigit(static_cast<int>(*p)) || *p == '.';)
{
p++;
}
return p + len; return p + len;
} }
/* wwww ssss */ /* wwww ssss */
for (p = buff, n = 0; n < 2; p = q + len) for (p = buff, n = 0; n < 2; p = q + len)
{ {
if ((q = strstr(p, s))) *q = '\0'; if ((q = strstr(p, s)))
if (*p) v[n++] = atof(p); {
if (!q) break; *q = '\0';
}
if (*p)
{
v[n++] = atof(p);
}
if (!q)
{
break;
}
} }
if (n >= 2 && 0.0 <= v[0] && v[0] <= 3000.0 && 0.0 <= v[1] && v[1] < 604800.0) if (n >= 2 && 0.0 <= v[0] && v[0] <= 3000.0 && 0.0 <= v[1] && v[1] < 604800.0)
{ {
@ -423,14 +469,23 @@ int decode_solxyz(char *buff, const solopt_t *opt, sol_t *sol)
trace(4, "decode_solxyz:\n"); trace(4, "decode_solxyz:\n");
if ((n = tonum(buff, sep, val)) < 3) return 0; if ((n = tonum(buff, sep, val)) < 3)
{
return 0;
}
for (j = 0; j < 3; j++) for (j = 0; j < 3; j++)
{ {
sol->rr[j] = val[i++]; /* xyz */ sol->rr[j] = val[i++]; /* xyz */
} }
if (i < n) sol->stat = static_cast<unsigned char>(val[i++]); if (i < n)
if (i < n) sol->ns = static_cast<unsigned char>(val[i++]); {
sol->stat = static_cast<unsigned char>(val[i++]);
}
if (i < n)
{
sol->ns = static_cast<unsigned char>(val[i++]);
}
if (i + 3 < n) if (i + 3 < n)
{ {
P[0] = val[i] * val[i]; P[0] = val[i] * val[i];
@ -450,12 +505,21 @@ int decode_solxyz(char *buff, const solopt_t *opt, sol_t *sol)
} }
covtosol(P, sol); covtosol(P, sol);
} }
if (i < n) sol->age = static_cast<float>(val[i++]); if (i < n)
if (i < n) sol->ratio = static_cast<float>(val[i]); {
sol->age = static_cast<float>(val[i++]);
}
if (i < n)
{
sol->ratio = static_cast<float>(val[i]);
}
sol->type = 0; /* position type = xyz */ sol->type = 0; /* position type = xyz */
if (MAXSOLQ < sol->stat) sol->stat = SOLQ_NONE; if (MAXSOLQ < sol->stat)
{
sol->stat = SOLQ_NONE;
}
return 1; return 1;
} }
@ -473,22 +537,34 @@ int decode_solllh(char *buff, const solopt_t *opt, sol_t *sol)
if (!opt->degf) if (!opt->degf)
{ {
if (n < 3) return 0; if (n < 3)
{
return 0;
}
pos[0] = val[i++] * D2R; /* lat/lon/hgt (ddd.ddd) */ pos[0] = val[i++] * D2R; /* lat/lon/hgt (ddd.ddd) */
pos[1] = val[i++] * D2R; pos[1] = val[i++] * D2R;
pos[2] = val[i++]; pos[2] = val[i++];
} }
else else
{ {
if (n < 7) return 0; if (n < 7)
{
return 0;
}
pos[0] = dms2deg(val) * D2R; /* lat/lon/hgt (ddd mm ss) */ pos[0] = dms2deg(val) * D2R; /* lat/lon/hgt (ddd mm ss) */
pos[1] = dms2deg(val + 3) * D2R; pos[1] = dms2deg(val + 3) * D2R;
pos[2] = val[6]; pos[2] = val[6];
i += 7; i += 7;
} }
pos2ecef(pos, sol->rr); pos2ecef(pos, sol->rr);
if (i < n) sol->stat = static_cast<unsigned char>(val[i++]); if (i < n)
if (i < n) sol->ns = static_cast<unsigned char>(val[i++]); {
sol->stat = static_cast<unsigned char>(val[i++]);
}
if (i < n)
{
sol->ns = static_cast<unsigned char>(val[i++]);
}
if (i + 3 < n) if (i + 3 < n)
{ {
Q[4] = val[i] * val[i]; Q[4] = val[i] * val[i];
@ -509,12 +585,21 @@ int decode_solllh(char *buff, const solopt_t *opt, sol_t *sol)
covecef(pos, Q, P); covecef(pos, Q, P);
covtosol(P, sol); covtosol(P, sol);
} }
if (i < n) sol->age = static_cast<float>(val[i++]); if (i < n)
if (i < n) sol->ratio = static_cast<float>(val[i]); {
sol->age = static_cast<float>(val[i++]);
}
if (i < n)
{
sol->ratio = static_cast<float>(val[i]);
}
sol->type = 0; /* position type = xyz */ sol->type = 0; /* position type = xyz */
if (MAXSOLQ < sol->stat) sol->stat = SOLQ_NONE; if (MAXSOLQ < sol->stat)
{
sol->stat = SOLQ_NONE;
}
return 1; return 1;
} }
@ -528,14 +613,23 @@ int decode_solenu(char *buff, const solopt_t *opt, sol_t *sol)
trace(4, "decode_solenu:\n"); trace(4, "decode_solenu:\n");
if ((n = tonum(buff, sep, val)) < 3) return 0; if ((n = tonum(buff, sep, val)) < 3)
{
return 0;
}
for (j = 0; j < 3; j++) for (j = 0; j < 3; j++)
{ {
sol->rr[j] = val[i++]; /* enu */ sol->rr[j] = val[i++]; /* enu */
} }
if (i < n) sol->stat = static_cast<unsigned char>(val[i++]); if (i < n)
if (i < n) sol->ns = static_cast<unsigned char>(val[i++]); {
sol->stat = static_cast<unsigned char>(val[i++]);
}
if (i < n)
{
sol->ns = static_cast<unsigned char>(val[i++]);
}
if (i + 3 < n) if (i + 3 < n)
{ {
Q[0] = val[i] * val[i]; Q[0] = val[i] * val[i];
@ -555,12 +649,21 @@ int decode_solenu(char *buff, const solopt_t *opt, sol_t *sol)
} }
covtosol(Q, sol); covtosol(Q, sol);
} }
if (i < n) sol->age = static_cast<float>(val[i++]); if (i < n)
if (i < n) sol->ratio = static_cast<float>(val[i]); {
sol->age = static_cast<float>(val[i++]);
}
if (i < n)
{
sol->ratio = static_cast<float>(val[i]);
}
sol->type = 1; /* position type = enu */ sol->type = 1; /* position type = enu */
if (MAXSOLQ < sol->stat) sol->stat = SOLQ_NONE; if (MAXSOLQ < sol->stat)
{
sol->stat = SOLQ_NONE;
}
return 1; return 1;
} }
@ -573,7 +676,10 @@ int decode_solgsi(char *buff, const solopt_t *opt __attribute((unused)), sol_t *
trace(4, "decode_solgsi:\n"); trace(4, "decode_solgsi:\n");
if (tonum(buff, " ", val) < 3) return 0; if (tonum(buff, " ", val) < 3)
{
return 0;
}
for (j = 0; j < 3; j++) for (j = 0; j < 3; j++)
{ {
@ -624,11 +730,17 @@ void decode_refpos(char *buff, const solopt_t *opt, double *rb)
trace(3, "decode_refpos: buff=%s\n", buff); trace(3, "decode_refpos: buff=%s\n", buff);
if ((n = tonum(buff, sep, val)) < 3) return; if ((n = tonum(buff, sep, val)) < 3)
{
return;
}
if (opt->posf == SOLF_XYZ) if (opt->posf == SOLF_XYZ)
{ /* xyz */ { /* xyz */
for (i = 0; i < 3; i++) rb[i] = val[i]; for (i = 0; i < 3; i++)
{
rb[i] = val[i];
}
} }
else if (opt->degf == 0) else if (opt->degf == 0)
{ /* lat/lon/hgt (ddd.ddd) */ { /* lat/lon/hgt (ddd.ddd) */
@ -656,21 +768,36 @@ int decode_sol(char *buff, const solopt_t *opt, sol_t *sol, double *rb)
if (!strncmp(buff, COMMENTH, 1)) if (!strncmp(buff, COMMENTH, 1))
{ /* reference position */ { /* reference position */
if (!strstr(buff, "ref pos") && !strstr(buff, "slave pos")) return 0; if (!strstr(buff, "ref pos") && !strstr(buff, "slave pos"))
if (!(p = strchr(buff, ':'))) return 0; {
return 0;
}
if (!(p = strchr(buff, ':')))
{
return 0;
}
decode_refpos(p + 1, opt, rb); decode_refpos(p + 1, opt, rb);
return 0; return 0;
} }
if (!strncmp(buff, "$GP", 3)) if (!strncmp(buff, "$GP", 3))
{ /* decode nmea */ { /* decode nmea */
if (!decode_nmea(buff, sol)) return 0; if (!decode_nmea(buff, sol))
{
return 0;
}
/* for time update only */ /* for time update only */
if (opt->posf != SOLF_NMEA && !strncmp(buff, "$GPRMC", 6)) return 2; if (opt->posf != SOLF_NMEA && !strncmp(buff, "$GPRMC", 6))
{
return 2;
}
} }
else else
{ /* decode position record */ { /* decode position record */
if (!decode_solpos(buff, opt, sol)) return 0; if (!decode_solpos(buff, opt, sol))
{
return 0;
}
} }
return 1; return 1;
} }
@ -683,14 +810,23 @@ void decode_solopt(char *buff, solopt_t *opt)
trace(4, "decode_solhead: buff=%s\n", buff); trace(4, "decode_solhead: buff=%s\n", buff);
if (strncmp(buff, COMMENTH, 1) != 0 && strncmp(buff, "+", 1) != 0) return; if (strncmp(buff, COMMENTH, 1) != 0 && strncmp(buff, "+", 1) != 0)
{
return;
}
if (strstr(buff, "GPST")) if (strstr(buff, "GPST"))
{
opt->times = TIMES_GPST; opt->times = TIMES_GPST;
}
else if (strstr(buff, "UTC")) else if (strstr(buff, "UTC"))
{
opt->times = TIMES_UTC; opt->times = TIMES_UTC;
}
else if (strstr(buff, "JST")) else if (strstr(buff, "JST"))
{
opt->times = TIMES_JST; opt->times = TIMES_JST;
}
if ((p = strstr(buff, "x-ecef(m)"))) if ((p = strstr(buff, "x-ecef(m)")))
{ {
@ -771,7 +907,10 @@ int inputsol(unsigned char data, gtime_t ts, gtime_t te, double tint,
solbuf->nb = 0; solbuf->nb = 0;
} }
solbuf->buff[solbuf->nb++] = data; solbuf->buff[solbuf->nb++] = data;
if (data != '\n' && solbuf->nb < MAXSOLMSG) return 0; /* sync trailer */ if (data != '\n' && solbuf->nb < MAXSOLMSG)
{
return 0; /* sync trailer */
}
solbuf->buff[solbuf->nb] = '\0'; solbuf->buff[solbuf->nb] = '\0';
solbuf->nb = 0; solbuf->nb = 0;
@ -829,7 +968,10 @@ int sort_solbuf(solbuf_t *solbuf)
trace(4, "sort_solbuf: n=%d\n", solbuf->n); trace(4, "sort_solbuf: n=%d\n", solbuf->n);
if (solbuf->n <= 0) return 0; if (solbuf->n <= 0)
{
return 0;
}
if (!(solbuf_data = static_cast<sol_t *>(realloc(solbuf->data, sizeof(sol_t) * solbuf->n)))) if (!(solbuf_data = static_cast<sol_t *>(realloc(solbuf->data, sizeof(sol_t) * solbuf->n))))
{ {
@ -916,15 +1058,26 @@ int addsol(solbuf_t *solbuf, const sol_t *sol)
if (solbuf->cyclic) if (solbuf->cyclic)
{ /* ring buffer */ { /* ring buffer */
if (solbuf->nmax <= 1) return 0; if (solbuf->nmax <= 1)
{
return 0;
}
solbuf->data[solbuf->end] = *sol; solbuf->data[solbuf->end] = *sol;
if (++solbuf->end >= solbuf->nmax) solbuf->end = 0; if (++solbuf->end >= solbuf->nmax)
{
solbuf->end = 0;
}
if (solbuf->start == solbuf->end) if (solbuf->start == solbuf->end)
{ {
if (++solbuf->start >= solbuf->nmax) solbuf->start = 0; if (++solbuf->start >= solbuf->nmax)
{
solbuf->start = 0;
}
} }
else else
{
solbuf->n++; solbuf->n++;
}
return 1; return 1;
} }
@ -957,7 +1110,10 @@ sol_t *getsol(solbuf_t *solbuf, int index)
{ {
trace(4, "getsol: index=%d\n", index); trace(4, "getsol: index=%d\n", index);
if (index < 0 || solbuf->n <= index) return nullptr; if (index < 0 || solbuf->n <= index)
{
return nullptr;
}
if ((index = solbuf->start + index) >= solbuf->nmax) if ((index = solbuf->start + index) >= solbuf->nmax)
{ {
index -= solbuf->nmax; index -= solbuf->nmax;
@ -985,7 +1141,10 @@ void initsolbuf(solbuf_t *solbuf, int cyclic, int nmax)
solbuf->data = nullptr; solbuf->data = nullptr;
if (cyclic) if (cyclic)
{ {
if (nmax <= 2) nmax = 2; if (nmax <= 2)
{
nmax = 2;
}
if (!(solbuf->data = static_cast<sol_t *>(malloc(sizeof(sol_t) * nmax)))) if (!(solbuf->data = static_cast<sol_t *>(malloc(sizeof(sol_t) * nmax))))
{ {
trace(1, "initsolbuf: memory allocation error\n"); trace(1, "initsolbuf: memory allocation error\n");
@ -1037,7 +1196,10 @@ int sort_solstat(solstatbuf_t *statbuf)
trace(4, "sort_solstat: n=%d\n", statbuf->n); trace(4, "sort_solstat: n=%d\n", statbuf->n);
if (statbuf->n <= 0) return 0; if (statbuf->n <= 0)
{
return 0;
}
if (!(statbuf_data = static_cast<solstat_t *>(realloc(statbuf->data, sizeof(solstat_t) * statbuf->n)))) if (!(statbuf_data = static_cast<solstat_t *>(realloc(statbuf->data, sizeof(solstat_t) * statbuf->n))))
{ {
@ -1064,10 +1226,18 @@ int decode_solstat(char *buff, solstat_t *stat)
trace(4, "decode_solstat: buff=%s\n", buff); trace(4, "decode_solstat: buff=%s\n", buff);
if (strstr(buff, "$SAT") != buff) return 0; if (strstr(buff, "$SAT") != buff)
{
return 0;
}
for (p = buff; *p; p++) for (p = buff; *p; p++)
if (*p == ',') *p = ' '; {
if (*p == ',')
{
*p = ' ';
}
}
n = sscanf(buff, "$SAT%d%lf%s%d%lf%lf%lf%lf%d%d%d%d%d%d%d%d", n = sscanf(buff, "$SAT%d%lf%s%d%lf%lf%lf%lf%d%d%d%d%d%d%d%d",
&week, &tow, id, &frq, &az, &el, &resp, &resc, &vsat, &snr, &fix, &slip, &week, &tow, id, &frq, &az, &el, &resp, &resc, &vsat, &snr, &fix, &slip,
@ -1138,7 +1308,10 @@ int readsolstatdata(FILE *fp, gtime_t ts, gtime_t te, double tint,
while (fgets(buff, sizeof(buff), fp)) while (fgets(buff, sizeof(buff), fp))
{ {
/* decode solution status */ /* decode solution status */
if (!decode_solstat(buff, &stat)) continue; if (!decode_solstat(buff, &stat))
{
continue;
}
/* add solution to solution buffer */ /* add solution to solution buffer */
if (screent(stat.time, ts, te, tint)) if (screent(stat.time, ts, te, tint))
@ -1245,7 +1418,9 @@ int outpos(unsigned char *buff, const char *s, const sol_t *sol,
dms2[2]); dms2[2]);
} }
else else
{
p += sprintf(p, "%s%s%14.9f%s%14.9f", s, sep, pos[0] * R2D, sep, pos[1] * R2D); p += sprintf(p, "%s%s%14.9f%s%14.9f", s, sep, pos[0] * R2D, sep, pos[1] * R2D);
}
p += sprintf(p, "%s%10.4f%s%3d%s%3d%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%6.2f%s%6.1f\n", p += sprintf(p, "%s%10.4f%s%3d%s%3d%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%6.2f%s%6.1f\n",
sep, pos[2], sep, sol->stat, sep, sol->ns, sep, SQRT_SOL(Q[4]), sep, sep, pos[2], sep, sol->stat, sep, sol->ns, sep, SQRT_SOL(Q[4]), sep,
SQRT_SOL(Q[0]), sep, SQRT_SOL(Q[8]), sep, sqvar(Q[1]), sep, sqvar(Q[2]), SQRT_SOL(Q[0]), sep, SQRT_SOL(Q[8]), sep, sqvar(Q[1]), sep, sqvar(Q[2]),
@ -1265,7 +1440,10 @@ int outenu(unsigned char *buff, const char *s, const sol_t *sol,
trace(3, "outenu :\n"); trace(3, "outenu :\n");
for (i = 0; i < 3; i++) rr[i] = sol->rr[i] - rb[i]; for (i = 0; i < 3; i++)
{
rr[i] = sol->rr[i] - rb[i];
}
ecef2pos(rb, pos); ecef2pos(rb, pos);
soltocov(sol, P); soltocov(sol, P);
covenu(pos, P, Q); covenu(pos, P, Q);
@ -1291,7 +1469,10 @@ int outnmea_rmc(unsigned char *buff, const sol_t *sol)
if (sol->stat <= SOLQ_NONE) if (sol->stat <= SOLQ_NONE)
{ {
p += sprintf(p, "$GPRMC,,,,,,,,,,,,"); p += sprintf(p, "$GPRMC,,,,,,,,,,,,");
for (q = reinterpret_cast<char *>(buff) + 1, sum = 0; *q; q++) sum ^= *q; for (q = reinterpret_cast<char *>(buff) + 1, sum = 0; *q; q++)
{
sum ^= *q;
}
p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A); p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A);
return p - reinterpret_cast<char *>(buff); return p - reinterpret_cast<char *>(buff);
} }
@ -1308,11 +1489,16 @@ int outnmea_rmc(unsigned char *buff, const sol_t *sol)
if (vel >= 1.0) if (vel >= 1.0)
{ {
dir = atan2(enuv[0], enuv[1]) * R2D; dir = atan2(enuv[0], enuv[1]) * R2D;
if (dir < 0.0) dir += 360.0; if (dir < 0.0)
{
dir += 360.0;
}
dirp = dir; dirp = dir;
} }
else else
{
dir = dirp; dir = dirp;
}
deg2dms(fabs(pos[0]) * R2D, dms1); deg2dms(fabs(pos[0]) * R2D, dms1);
deg2dms(fabs(pos[1]) * R2D, dms2); deg2dms(fabs(pos[1]) * R2D, dms2);
p += sprintf(p, "$GPRMC,%02.0f%02.0f%05.2f,A,%02.0f%010.7f,%s,%03.0f%010.7f,%s,%4.2f,%4.2f,%02.0f%02.0f%02d,%.1f,%s,%s", p += sprintf(p, "$GPRMC,%02.0f%02.0f%05.2f,A,%02.0f%010.7f,%s,%03.0f%010.7f,%s,%4.2f,%4.2f,%02.0f%02.0f%02d,%.1f,%s,%s",
@ -1320,7 +1506,10 @@ int outnmea_rmc(unsigned char *buff, const sol_t *sol)
dms2[0], dms2[1] + dms2[2] / 60.0, pos[1] >= 0 ? "E" : "W", vel / KNOT2M, dir, dms2[0], dms2[1] + dms2[2] / 60.0, pos[1] >= 0 ? "E" : "W", vel / KNOT2M, dir,
ep[2], ep[1], static_cast<int>(ep[0]) % 100, amag, emag, ep[2], ep[1], static_cast<int>(ep[0]) % 100, amag, emag,
sol->stat == SOLQ_DGPS || sol->stat == SOLQ_FLOAT || sol->stat == SOLQ_FIX ? "D" : "A"); sol->stat == SOLQ_DGPS || sol->stat == SOLQ_FLOAT || sol->stat == SOLQ_FIX ? "D" : "A");
for (q = reinterpret_cast<char *>(buff) + 1, sum = 0; *q; q++) sum ^= *q; /* check-sum */ for (q = reinterpret_cast<char *>(buff) + 1, sum = 0; *q; q++)
{
sum ^= *q; /* check-sum */
}
p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A); p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A);
return p - reinterpret_cast<char *>(buff); return p - reinterpret_cast<char *>(buff);
} }
@ -1339,13 +1528,24 @@ int outnmea_gga(unsigned char *buff, const sol_t *sol)
if (sol->stat <= SOLQ_NONE) if (sol->stat <= SOLQ_NONE)
{ {
p += sprintf(p, "$GPGGA,,,,,,,,,,,,,,"); p += sprintf(p, "$GPGGA,,,,,,,,,,,,,,");
for (q = reinterpret_cast<char *>(buff) + 1, sum = 0; *q; q++) sum ^= *q; for (q = reinterpret_cast<char *>(buff) + 1, sum = 0; *q; q++)
{
sum ^= *q;
}
p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A); p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A);
return p - reinterpret_cast<char *>(buff); return p - reinterpret_cast<char *>(buff);
} }
for (solq = 0; solq < 8; solq++) for (solq = 0; solq < 8; solq++)
if (solq_nmea[solq] == sol->stat) break; {
if (solq >= 8) solq = 0; if (solq_nmea[solq] == sol->stat)
{
break;
}
}
if (solq >= 8)
{
solq = 0;
}
time = gpst2utc(sol->time); time = gpst2utc(sol->time);
if (time.sec >= 0.995) if (time.sec >= 0.995)
{ {
@ -1361,7 +1561,10 @@ int outnmea_gga(unsigned char *buff, const sol_t *sol)
ep[3], ep[4], ep[5], dms1[0], dms1[1] + dms1[2] / 60.0, pos[0] >= 0 ? "N" : "S", ep[3], ep[4], ep[5], dms1[0], dms1[1] + dms1[2] / 60.0, pos[0] >= 0 ? "N" : "S",
dms2[0], dms2[1] + dms2[2] / 60.0, pos[1] >= 0 ? "E" : "W", solq, dms2[0], dms2[1] + dms2[2] / 60.0, pos[1] >= 0 ? "E" : "W", solq,
sol->ns, dop, pos[2] - h, h, sol->age); sol->ns, dop, pos[2] - h, h, sol->age);
for (q = reinterpret_cast<char *>(buff) + 1, sum = 0; *q; q++) sum ^= *q; /* check-sum */ for (q = reinterpret_cast<char *>(buff) + 1, sum = 0; *q; q++)
{
sum ^= *q; /* check-sum */
}
p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A); p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A);
return p - reinterpret_cast<char *>(buff); return p - reinterpret_cast<char *>(buff);
} }
@ -1380,7 +1583,10 @@ int outnmea_gsa(unsigned char *buff, const sol_t *sol,
if (sol->stat <= SOLQ_NONE) if (sol->stat <= SOLQ_NONE)
{ {
p += sprintf(p, "$GPGSA,A,1,,,,,,,,,,,,,,,"); p += sprintf(p, "$GPGSA,A,1,,,,,,,,,,,,,,,");
for (q = reinterpret_cast<char *>(buff) + 1, sum = 0; *q; q++) sum ^= *q; for (q = reinterpret_cast<char *>(buff) + 1, sum = 0; *q; q++)
{
sum ^= *q;
}
p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A); p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A);
return p - reinterpret_cast<char *>(buff); return p - reinterpret_cast<char *>(buff);
} }
@ -1388,11 +1594,23 @@ int outnmea_gsa(unsigned char *buff, const sol_t *sol,
/* GPGSA: gps/sbas */ /* GPGSA: gps/sbas */
for (sat = 1, nsat = 0; sat <= MAXSAT && nsat < 12; sat++) for (sat = 1, nsat = 0; sat <= MAXSAT && nsat < 12; sat++)
{ {
if (!ssat[sat - 1].vs || ssat[sat - 1].azel[1] <= 0.0) continue; if (!ssat[sat - 1].vs || ssat[sat - 1].azel[1] <= 0.0)
{
continue;
}
sys = satsys(sat, prn + nsat); sys = satsys(sat, prn + nsat);
if (sys != SYS_GPS && sys != SYS_SBS) continue; if (sys != SYS_GPS && sys != SYS_SBS)
if (sys == SYS_SBS) prn[nsat] += 33 - MINPRNSBS; {
for (i = 0; i < 2; i++) azel[i + nsat * 2] = ssat[sat - 1].azel[i]; continue;
}
if (sys == SYS_SBS)
{
prn[nsat] += 33 - MINPRNSBS;
}
for (i = 0; i < 2; i++)
{
azel[i + nsat * 2] = ssat[sat - 1].azel[i];
}
nsat++; nsat++;
} }
if (nsat > 0) if (nsat > 0)
@ -1402,21 +1620,37 @@ int outnmea_gsa(unsigned char *buff, const sol_t *sol,
for (i = 0; i < 12; i++) for (i = 0; i < 12; i++)
{ {
if (i < nsat) if (i < nsat)
{
p += sprintf(p, ",%02d", prn[i]); p += sprintf(p, ",%02d", prn[i]);
}
else else
{
p += sprintf(p, ","); p += sprintf(p, ",");
} }
}
dops(nsat, azel, 0.0, dop); dops(nsat, azel, 0.0, dop);
p += sprintf(p, ",%3.1f,%3.1f,%3.1f,1", dop[1], dop[2], dop[3]); p += sprintf(p, ",%3.1f,%3.1f,%3.1f,1", dop[1], dop[2], dop[3]);
for (q = s + 1, sum = 0; *q; q++) sum ^= *q; /* check-sum */ for (q = s + 1, sum = 0; *q; q++)
{
sum ^= *q; /* check-sum */
}
p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A); p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A);
} }
/* GLGSA: glonass */ /* GLGSA: glonass */
for (sat = 1, nsat = 0; sat <= MAXSAT && nsat < 12; sat++) for (sat = 1, nsat = 0; sat <= MAXSAT && nsat < 12; sat++)
{ {
if (!ssat[sat - 1].vs || ssat[sat - 1].azel[1] <= 0.0) continue; if (!ssat[sat - 1].vs || ssat[sat - 1].azel[1] <= 0.0)
if (satsys(sat, prn + nsat) != SYS_GLO) continue; {
for (i = 0; i < 2; i++) azel[i + nsat * 2] = ssat[sat - 1].azel[i]; continue;
}
if (satsys(sat, prn + nsat) != SYS_GLO)
{
continue;
}
for (i = 0; i < 2; i++)
{
azel[i + nsat * 2] = ssat[sat - 1].azel[i];
}
nsat++; nsat++;
} }
if (nsat > 0) if (nsat > 0)
@ -1426,21 +1660,37 @@ int outnmea_gsa(unsigned char *buff, const sol_t *sol,
for (i = 0; i < 12; i++) for (i = 0; i < 12; i++)
{ {
if (i < nsat) if (i < nsat)
{
p += sprintf(p, ",%02d", prn[i] + 64); p += sprintf(p, ",%02d", prn[i] + 64);
}
else else
{
p += sprintf(p, ","); p += sprintf(p, ",");
} }
}
dops(nsat, azel, 0.0, dop); dops(nsat, azel, 0.0, dop);
p += sprintf(p, ",%3.1f,%3.1f,%3.1f,2", dop[1], dop[2], dop[3]); p += sprintf(p, ",%3.1f,%3.1f,%3.1f,2", dop[1], dop[2], dop[3]);
for (q = s + 1, sum = 0; *q; q++) sum ^= *q; /* check-sum */ for (q = s + 1, sum = 0; *q; q++)
{
sum ^= *q; /* check-sum */
}
p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A); p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A);
} }
/* GAGSA: galileo */ /* GAGSA: galileo */
for (sat = 1, nsat = 0; sat <= MAXSAT && nsat < 12; sat++) for (sat = 1, nsat = 0; sat <= MAXSAT && nsat < 12; sat++)
{ {
if (!ssat[sat - 1].vs || ssat[sat - 1].azel[1] <= 0.0) continue; if (!ssat[sat - 1].vs || ssat[sat - 1].azel[1] <= 0.0)
if (satsys(sat, prn + nsat) != SYS_GAL) continue; {
for (i = 0; i < 2; i++) azel[i + nsat * 2] = ssat[sat - 1].azel[i]; continue;
}
if (satsys(sat, prn + nsat) != SYS_GAL)
{
continue;
}
for (i = 0; i < 2; i++)
{
azel[i + nsat * 2] = ssat[sat - 1].azel[i];
}
nsat++; nsat++;
} }
if (nsat > 0) if (nsat > 0)
@ -1450,13 +1700,20 @@ int outnmea_gsa(unsigned char *buff, const sol_t *sol,
for (i = 0; i < 12; i++) for (i = 0; i < 12; i++)
{ {
if (i < nsat) if (i < nsat)
{
p += sprintf(p, ",%02d", prn[i]); p += sprintf(p, ",%02d", prn[i]);
}
else else
{
p += sprintf(p, ","); p += sprintf(p, ",");
} }
}
dops(nsat, azel, 0.0, dop); dops(nsat, azel, 0.0, dop);
p += sprintf(p, ",%3.1f,%3.1f,%3.1f,3", dop[1], dop[2], dop[3]); p += sprintf(p, ",%3.1f,%3.1f,%3.1f,3", dop[1], dop[2], dop[3]);
for (q = s + 1, sum = 0; *q; q++) sum ^= *q; /* check-sum */ for (q = s + 1, sum = 0; *q; q++)
{
sum ^= *q; /* check-sum */
}
p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A); p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A);
} }
return p - reinterpret_cast<char *>(buff); return p - reinterpret_cast<char *>(buff);
@ -1476,7 +1733,10 @@ int outnmea_gsv(unsigned char *buff, const sol_t *sol,
if (sol->stat <= SOLQ_NONE) if (sol->stat <= SOLQ_NONE)
{ {
p += sprintf(p, "$GPGSV,1,1,0,,,,,,,,,,,,,,,,"); p += sprintf(p, "$GPGSV,1,1,0,,,,,,,,,,,,,,,,");
for (q = reinterpret_cast<char *>(buff) + 1, sum = 0; *q; q++) sum ^= *q; for (q = reinterpret_cast<char *>(buff) + 1, sum = 0; *q; q++)
{
sum ^= *q;
}
p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A); p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A);
return p - reinterpret_cast<char *>(buff); return p - reinterpret_cast<char *>(buff);
} }
@ -1484,8 +1744,14 @@ int outnmea_gsv(unsigned char *buff, const sol_t *sol,
for (sat = 1, n = 0; sat < MAXSAT && n < 12; sat++) for (sat = 1, n = 0; sat < MAXSAT && n < 12; sat++)
{ {
sys = satsys(sat, &prn); sys = satsys(sat, &prn);
if (sys != SYS_GPS && sys != SYS_SBS) continue; if (sys != SYS_GPS && sys != SYS_SBS)
if (ssat[sat - 1].vs && ssat[sat - 1].azel[1] > 0.0) sats[n++] = sat; {
continue;
}
if (ssat[sat - 1].vs && ssat[sat - 1].azel[1] > 0.0)
{
sats[n++] = sat;
}
} }
nmsg = n <= 0 ? 0 : (n - 1) / 4 + 1; nmsg = n <= 0 ? 0 : (n - 1) / 4 + 1;
@ -1498,25 +1764,42 @@ int outnmea_gsv(unsigned char *buff, const sol_t *sol,
{ {
if (k < n) if (k < n)
{ {
if (satsys(sats[k], &prn) == SYS_SBS) prn += 33 - MINPRNSBS; if (satsys(sats[k], &prn) == SYS_SBS)
{
prn += 33 - MINPRNSBS;
}
az = ssat[sats[k] - 1].azel[0] * R2D; az = ssat[sats[k] - 1].azel[0] * R2D;
if (az < 0.0) az += 360.0; if (az < 0.0)
{
az += 360.0;
}
el = ssat[sats[k] - 1].azel[1] * R2D; el = ssat[sats[k] - 1].azel[1] * R2D;
snr = ssat[sats[k] - 1].snr[0] * 0.25; snr = ssat[sats[k] - 1].snr[0] * 0.25;
p += sprintf(p, ",%02d,%02.0f,%03.0f,%02.0f", prn, el, az, snr); p += sprintf(p, ",%02d,%02.0f,%03.0f,%02.0f", prn, el, az, snr);
} }
else else
{
p += sprintf(p, ",,,,"); p += sprintf(p, ",,,,");
} }
}
p += sprintf(p, ",1"); /* L1C/A */ p += sprintf(p, ",1"); /* L1C/A */
for (q = s + 1, sum = 0; *q; q++) sum ^= *q; /* check-sum */ for (q = s + 1, sum = 0; *q; q++)
{
sum ^= *q; /* check-sum */
}
p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A); p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A);
} }
/* GLGSV: glonass */ /* GLGSV: glonass */
for (sat = 1, n = 0; sat < MAXSAT && n < 12; sat++) for (sat = 1, n = 0; sat < MAXSAT && n < 12; sat++)
{ {
if (satsys(sat, &prn) != SYS_GLO) continue; if (satsys(sat, &prn) != SYS_GLO)
if (ssat[sat - 1].vs && ssat[sat - 1].azel[1] > 0.0) sats[n++] = sat; {
continue;
}
if (ssat[sat - 1].vs && ssat[sat - 1].azel[1] > 0.0)
{
sats[n++] = sat;
}
} }
nmsg = n <= 0 ? 0 : (n - 1) / 4 + 1; nmsg = n <= 0 ? 0 : (n - 1) / 4 + 1;
@ -1532,23 +1815,37 @@ int outnmea_gsv(unsigned char *buff, const sol_t *sol,
satsys(sats[k], &prn); satsys(sats[k], &prn);
prn += 64; /* 65-99 */ prn += 64; /* 65-99 */
az = ssat[sats[k] - 1].azel[0] * R2D; az = ssat[sats[k] - 1].azel[0] * R2D;
if (az < 0.0) az += 360.0; if (az < 0.0)
{
az += 360.0;
}
el = ssat[sats[k] - 1].azel[1] * R2D; el = ssat[sats[k] - 1].azel[1] * R2D;
snr = ssat[sats[k] - 1].snr[0] * 0.25; snr = ssat[sats[k] - 1].snr[0] * 0.25;
p += sprintf(p, ",%02d,%02.0f,%03.0f,%02.0f", prn, el, az, snr); p += sprintf(p, ",%02d,%02.0f,%03.0f,%02.0f", prn, el, az, snr);
} }
else else
{
p += sprintf(p, ",,,,"); p += sprintf(p, ",,,,");
} }
}
p += sprintf(p, ",1"); /* L1C/A */ p += sprintf(p, ",1"); /* L1C/A */
for (q = s + 1, sum = 0; *q; q++) sum ^= *q; /* check-sum */ for (q = s + 1, sum = 0; *q; q++)
{
sum ^= *q; /* check-sum */
}
p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A); p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A);
} }
/* GAGSV: galileo */ /* GAGSV: galileo */
for (sat = 1, n = 0; sat < MAXSAT && n < 12; sat++) for (sat = 1, n = 0; sat < MAXSAT && n < 12; sat++)
{ {
if (satsys(sat, &prn) != SYS_GAL) continue; if (satsys(sat, &prn) != SYS_GAL)
if (ssat[sat - 1].vs && ssat[sat - 1].azel[1] > 0.0) sats[n++] = sat; {
continue;
}
if (ssat[sat - 1].vs && ssat[sat - 1].azel[1] > 0.0)
{
sats[n++] = sat;
}
} }
nmsg = n <= 0 ? 0 : (n - 1) / 4 + 1; nmsg = n <= 0 ? 0 : (n - 1) / 4 + 1;
@ -1563,16 +1860,24 @@ int outnmea_gsv(unsigned char *buff, const sol_t *sol,
{ {
satsys(sats[k], &prn); /* 1-36 */ satsys(sats[k], &prn); /* 1-36 */
az = ssat[sats[k] - 1].azel[0] * R2D; az = ssat[sats[k] - 1].azel[0] * R2D;
if (az < 0.0) az += 360.0; if (az < 0.0)
{
az += 360.0;
}
el = ssat[sats[k] - 1].azel[1] * R2D; el = ssat[sats[k] - 1].azel[1] * R2D;
snr = ssat[sats[k] - 1].snr[0] * 0.25; snr = ssat[sats[k] - 1].snr[0] * 0.25;
p += sprintf(p, ",%02d,%02.0f,%03.0f,%02.0f", prn, el, az, snr); p += sprintf(p, ",%02d,%02.0f,%03.0f,%02.0f", prn, el, az, snr);
} }
else else
{
p += sprintf(p, ",,,,"); p += sprintf(p, ",,,,");
} }
}
p += sprintf(p, ",7"); /* L1BC */ p += sprintf(p, ",7"); /* L1BC */
for (q = s + 1, sum = 0; *q; q++) sum ^= *q; /* check-sum */ for (q = s + 1, sum = 0; *q; q++)
{
sum ^= *q; /* check-sum */
}
p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A); p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A);
} }
return p - reinterpret_cast<char *>(buff); return p - reinterpret_cast<char *>(buff);
@ -1633,7 +1938,10 @@ int outprcopts(unsigned char *buff, const prcopt_t *opt)
p += sprintf(p, "%s navi sys :", COMMENTH); p += sprintf(p, "%s navi sys :", COMMENTH);
for (i = 0; sys[i]; i++) for (i = 0; sys[i]; i++)
{ {
if (opt->navsys & sys[i]) p += sprintf(p, " %s", s7[i]); if (opt->navsys & sys[i])
{
p += sprintf(p, " %s", s7[i]);
}
} }
p += sprintf(p, "\n"); p += sprintf(p, "\n");
} }
@ -1656,7 +1964,10 @@ int outprcopts(unsigned char *buff, const prcopt_t *opt)
} }
for (i = 0; i < 2; i++) for (i = 0; i < 2; i++)
{ {
if (opt->mode == PMODE_SINGLE || (i >= 1 && opt->mode > PMODE_FIXED)) continue; if (opt->mode == PMODE_SINGLE || (i >= 1 && opt->mode > PMODE_FIXED))
{
continue;
}
p += sprintf(p, "%s antenna%d : %-21s (%7.4f %7.4f %7.4f)\n", COMMENTH, p += sprintf(p, "%s antenna%d : %-21s (%7.4f %7.4f %7.4f)\n", COMMENTH,
i + 1, opt->anttype[i], opt->antdel[i][0], opt->antdel[i][1], i + 1, opt->anttype[i], opt->antdel[i][0], opt->antdel[i][1],
opt->antdel[i][2]); opt->antdel[i][2]);
@ -1680,17 +1991,26 @@ int outsolheads(unsigned char *buff, const solopt_t *opt)
trace(3, "outsolheads:\n"); trace(3, "outsolheads:\n");
if (opt->posf == SOLF_NMEA) return 0; if (opt->posf == SOLF_NMEA)
{
return 0;
}
if (opt->outhead) if (opt->outhead)
{ {
p += sprintf(p, "%s (", COMMENTH); p += sprintf(p, "%s (", COMMENTH);
if (opt->posf == SOLF_XYZ) if (opt->posf == SOLF_XYZ)
{
p += sprintf(p, "x/y/z-ecef=WGS84"); p += sprintf(p, "x/y/z-ecef=WGS84");
}
else if (opt->posf == SOLF_ENU) else if (opt->posf == SOLF_ENU)
{
p += sprintf(p, "e/n/u-baseline=WGS84"); p += sprintf(p, "e/n/u-baseline=WGS84");
}
else else
{
p += sprintf(p, "lat/lon/height=%s/%s", s1[opt->datum], s2[opt->height]); p += sprintf(p, "lat/lon/height=%s/%s", s1[opt->datum], s2[opt->height]);
}
p += sprintf(p, ",Q=1:fix,2:float,3:sbas,4:dgps,5:single,6:ppp,ns=# of satellites)\n"); p += sprintf(p, ",Q=1:fix,2:float,3:sbas,4:dgps,5:single,6:ppp,ns=# of satellites)\n");
} }
p += sprintf(p, "%s %-*s%s", COMMENTH, (opt->timef ? 16 : 8) + timeu + 1, s3[opt->times], sep); p += sprintf(p, "%s %-*s%s", COMMENTH, (opt->timef ? 16 : 8) + timeu + 1, s3[opt->times], sep);
@ -1752,8 +2072,14 @@ int outsols(unsigned char *buff, const sol_t *sol, const double *rb,
if (opt->posf == SOLF_NMEA) if (opt->posf == SOLF_NMEA)
{ {
if (opt->nmeaintv[0] < 0.0) return 0; if (opt->nmeaintv[0] < 0.0)
if (!screent(sol->time, ts, ts, opt->nmeaintv[0])) return 0; {
return 0;
}
if (!screent(sol->time, ts, ts, opt->nmeaintv[0]))
{
return 0;
}
} }
if (sol->stat <= SOLQ_NONE || (opt->posf == SOLF_ENU && norm_rtk(rb, 3) <= 0.0)) if (sol->stat <= SOLQ_NONE || (opt->posf == SOLF_ENU && norm_rtk(rb, 3) <= 0.0))
{ {
@ -1762,11 +2088,19 @@ int outsols(unsigned char *buff, const sol_t *sol, const double *rb,
timeu = opt->timeu < 0 ? 0 : (opt->timeu > 20 ? 20 : opt->timeu); timeu = opt->timeu < 0 ? 0 : (opt->timeu > 20 ? 20 : opt->timeu);
time = sol->time; time = sol->time;
if (opt->times >= TIMES_UTC) time = gpst2utc(time); if (opt->times >= TIMES_UTC)
if (opt->times == TIMES_JST) time = timeadd(time, 9 * 3600.0); {
time = gpst2utc(time);
}
if (opt->times == TIMES_JST)
{
time = timeadd(time, 9 * 3600.0);
}
if (opt->timef) if (opt->timef)
{
time2str(time, s, timeu); time2str(time, s, timeu);
}
else else
{ {
gpst = time2gpst(time, &week); gpst = time2gpst(time, &week);
@ -1816,8 +2150,14 @@ int outsolexs(unsigned char *buff, const sol_t *sol, const ssat_t *ssat,
if (opt->posf == SOLF_NMEA) if (opt->posf == SOLF_NMEA)
{ {
if (opt->nmeaintv[1] < 0.0) return 0; if (opt->nmeaintv[1] < 0.0)
if (!screent(sol->time, ts, ts, opt->nmeaintv[1])) return 0; {
return 0;
}
if (!screent(sol->time, ts, ts, opt->nmeaintv[1]))
{
return 0;
}
} }
if (opt->posf == SOLF_NMEA) if (opt->posf == SOLF_NMEA)
{ {

738
src/algorithms/libs/rtklib/rtklib_stream.cc
View File

File diff suppressed because it is too large Load Diff

45
src/algorithms/libs/rtklib/rtklib_tides.cc
View File

@ -65,9 +65,15 @@ void tide_pl(const double *eu, const double *rp, double GMp,
trace(4, "tide_pl : pos=%.3f %.3f\n", pos[0] * R2D, pos[1] * R2D); trace(4, "tide_pl : pos=%.3f %.3f\n", pos[0] * R2D, pos[1] * R2D);
if ((r = norm_rtk(rp, 3)) <= 0.0) return; if ((r = norm_rtk(rp, 3)) <= 0.0)
{
return;
}
for (i = 0; i < 3; i++) ep[i] = rp[i] / r; for (i = 0; i < 3; i++)
{
ep[i] = rp[i] / r;
}
K2 = GMp / GME * std::pow(RE_WGS84, 2.04) * std::pow(RE_WGS84, 2.0) / (r * r * r); K2 = GMp / GME * std::pow(RE_WGS84, 2.04) * std::pow(RE_WGS84, 2.0) / (r * r * r);
K3 = K2 * RE_WGS84 / r; K3 = K2 * RE_WGS84 / r;
@ -181,8 +187,14 @@ void tide_oload(gtime_t tut, const double *odisp, double *denu)
for (i = 0; i < 11; i++) for (i = 0; i < 11; i++)
{ {
ang = 0.0; ang = 0.0;
for (j = 0; j < 5; j++) ang += a[j] * args[i][j]; for (j = 0; j < 5; j++)
for (j = 0; j < 3; j++) dp[j] += odisp[j + i * 6] * cos(ang - odisp[j + 3 + i * 6] * D2R); {
ang += a[j] * args[i][j];
}
for (j = 0; j < 3; j++)
{
dp[j] += odisp[j + i * 6] * cos(ang - odisp[j + 3 + i * 6] * D2R);
}
} }
denu[0] = -dp[1]; denu[0] = -dp[1];
denu[1] = -dp[2]; denu[1] = -dp[2];
@ -279,13 +291,19 @@ void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp,
trace(3, "tidedisp: tutc=%s\n", time_str(tutc, 0)); trace(3, "tidedisp: tutc=%s\n", time_str(tutc, 0));
if (erp) geterp(erp, tutc, erpv); if (erp)
{
geterp(erp, tutc, erpv);
}
tut = timeadd(tutc, erpv[2]); tut = timeadd(tutc, erpv[2]);
dr[0] = dr[1] = dr[2] = 0.0; dr[0] = dr[1] = dr[2] = 0.0;
if (norm_rtk(rr, 3) <= 0.0) return; if (norm_rtk(rr, 3) <= 0.0)
{
return;
}
pos[0] = asin(rr[2] / norm_rtk(rr, 3)); pos[0] = asin(rr[2] / norm_rtk(rr, 3));
pos[1] = atan2(rr[1], rr[0]); pos[1] = atan2(rr[1], rr[0]);
@ -309,19 +327,28 @@ void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp,
#else #else
tide_solid(rs, rm, pos, E, gmst, opt, drt); tide_solid(rs, rm, pos, E, gmst, opt, drt);
#endif #endif
for (i = 0; i < 3; i++) dr[i] += drt[i]; for (i = 0; i < 3; i++)
{
dr[i] += drt[i];
}
} }
if ((opt & 2) && odisp) if ((opt & 2) && odisp)
{ /* ocean tide loading */ { /* ocean tide loading */
tide_oload(tut, odisp, denu); tide_oload(tut, odisp, denu);
matmul("TN", 3, 1, 3, 1.0, E, denu, 0.0, drt); matmul("TN", 3, 1, 3, 1.0, E, denu, 0.0, drt);
for (i = 0; i < 3; i++) dr[i] += drt[i]; for (i = 0; i < 3; i++)
{
dr[i] += drt[i];
}
} }
if ((opt & 4) && erp) if ((opt & 4) && erp)
{ /* pole tide */ { /* pole tide */
tide_pole(tut, pos, erpv, denu); tide_pole(tut, pos, erpv, denu);
matmul("TN", 3, 1, 3, 1.0, E, denu, 0.0, drt); matmul("TN", 3, 1, 3, 1.0, E, denu, 0.0, drt);
for (i = 0; i < 3; i++) dr[i] += drt[i]; for (i = 0; i < 3; i++)
{
dr[i] += drt[i];
}
} }
trace(5, "tidedisp: dr=%.3f %.3f %.3f\n", dr[0], dr[1], dr[2]); trace(5, "tidedisp: dr=%.3f %.3f %.3f\n", dr[0], dr[1], dr[2]);
} }

5
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
View File

@ -576,7 +576,10 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
} }
} }
if (n_valid > 0) compute_pranges(epoch_data); if (n_valid > 0)
{
compute_pranges(epoch_data);
}
for (uint32_t n = 0; n < d_nchannels_out; n++) for (uint32_t n = 0; n < d_nchannels_out; n++)
{ {

10
src/algorithms/signal_source/adapters/file_signal_source.cc
View File

@ -60,8 +60,14 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration,
filename_ = configuration->property(role + ".filename", default_filename); filename_ = configuration->property(role + ".filename", default_filename);
// override value with commandline flag, if present // override value with commandline flag, if present
if (FLAGS_signal_source != "-") filename_ = FLAGS_signal_source; if (FLAGS_signal_source != "-")
if (FLAGS_s != "-") filename_ = FLAGS_s; {
filename_ = FLAGS_signal_source;
}
if (FLAGS_s != "-")
{
filename_ = FLAGS_s;
}
item_type_ = configuration->property(role + ".item_type", default_item_type); item_type_ = configuration->property(role + ".item_type", default_item_type);
repeat_ = configuration->property(role + ".repeat", false); repeat_ = configuration->property(role + ".repeat", false);

10
src/algorithms/signal_source/adapters/nsr_file_signal_source.cc
View File

@ -58,8 +58,14 @@ NsrFileSignalSource::NsrFileSignalSource(ConfigurationInterface* configuration,
filename_ = configuration->property(role + ".filename", default_filename); filename_ = configuration->property(role + ".filename", default_filename);
// override value with commandline flag, if present // override value with commandline flag, if present
if (FLAGS_signal_source != "-") filename_ = FLAGS_signal_source; if (FLAGS_signal_source != "-")
if (FLAGS_s != "-") filename_ = FLAGS_s; {
filename_ = FLAGS_signal_source;
}
if (FLAGS_s != "-")
{
filename_ = FLAGS_s;
}
item_type_ = configuration->property(role + ".item_type", default_item_type); item_type_ = configuration->property(role + ".item_type", default_item_type);
repeat_ = configuration->property(role + ".repeat", false); repeat_ = configuration->property(role + ".repeat", false);

10
src/algorithms/signal_source/adapters/spir_file_signal_source.cc
View File

@ -57,8 +57,14 @@ SpirFileSignalSource::SpirFileSignalSource(ConfigurationInterface* configuration
filename_ = configuration->property(role + ".filename", default_filename); filename_ = configuration->property(role + ".filename", default_filename);
// override value with commandline flag, if present // override value with commandline flag, if present
if (FLAGS_signal_source != "-") filename_ = FLAGS_signal_source; if (FLAGS_signal_source != "-")
if (FLAGS_s != "-") filename_ = FLAGS_s; {
filename_ = FLAGS_signal_source;
}
if (FLAGS_s != "-")
{
filename_ = FLAGS_s;
}
item_type_ = configuration->property(role + ".item_type", default_item_type); item_type_ = configuration->property(role + ".item_type", default_item_type);
repeat_ = configuration->property(role + ".repeat", false); repeat_ = configuration->property(role + ".repeat", false);

10
src/algorithms/signal_source/adapters/two_bit_cpx_file_signal_source.cc
View File

@ -62,8 +62,14 @@ TwoBitCpxFileSignalSource::TwoBitCpxFileSignalSource(ConfigurationInterface* con
filename_ = configuration->property(role + ".filename", default_filename); filename_ = configuration->property(role + ".filename", default_filename);
// override value with commandline flag, if present // override value with commandline flag, if present
if (FLAGS_signal_source != "-") filename_ = FLAGS_signal_source; if (FLAGS_signal_source != "-")
if (FLAGS_s != "-") filename_ = FLAGS_s; {
filename_ = FLAGS_signal_source;
}
if (FLAGS_s != "-")
{
filename_ = FLAGS_s;
}
item_type_ = configuration->property(role + ".item_type", default_item_type); item_type_ = configuration->property(role + ".item_type", default_item_type);
repeat_ = configuration->property(role + ".repeat", false); repeat_ = configuration->property(role + ".repeat", false);

10
src/algorithms/signal_source/adapters/two_bit_packed_file_signal_source.cc
View File

@ -66,8 +66,14 @@ TwoBitPackedFileSignalSource::TwoBitPackedFileSignalSource(ConfigurationInterfac
filename_ = configuration->property(role + ".filename", default_filename); filename_ = configuration->property(role + ".filename", default_filename);
// override value with commandline flag, if present // override value with commandline flag, if present
if (FLAGS_signal_source != "-") filename_ = FLAGS_signal_source; if (FLAGS_signal_source != "-")
if (FLAGS_s != "-") filename_ = FLAGS_s; {
filename_ = FLAGS_signal_source;
}
if (FLAGS_s != "-")
{
filename_ = FLAGS_s;
}
item_type_ = configuration->property(role + ".item_type", default_item_type); item_type_ = configuration->property(role + ".item_type", default_item_type);
big_endian_items_ = configuration->property(role + ".big_endian_items", true); big_endian_items_ = configuration->property(role + ".big_endian_items", true);

5
src/algorithms/signal_source/gnuradio_blocks/labsat23_source.cc
View File

@ -198,7 +198,10 @@ int labsat23_source::general_work(int noutput_items,
bool preamble_ok = true; bool preamble_ok = true;
for (int i = 0; i < 8; i++) for (int i = 0; i < 8; i++)
{ {
if (memblock[byte_counter] != 0x00) preamble_ok = false; if (memblock[byte_counter] != 0x00)
{
preamble_ok = false;
}
//std::cout << "H[" << i << "]:" << (int)memblock[byte_counter] << std::endl; //std::cout << "H[" << i << "]:" << (int)memblock[byte_counter] << std::endl;
byte_counter++; byte_counter++;
} }

2
src/algorithms/signal_source/gnuradio_blocks/unpack_2bit_samples.cc
View File

@ -65,7 +65,9 @@ bool systemBytesAreBigEndian()
byte_and_samples b{}; byte_and_samples b{};
b.byte = static_cast<int8_t>(0x01); b.byte = static_cast<int8_t>(0x01);
if (*reinterpret_cast<char *>(&b.byte) == 1) if (*reinterpret_cast<char *>(&b.byte) == 1)
{
return false; return false;
}
return true; return true;
} }

5
src/algorithms/signal_source/libs/gnss_sdr_valve.cc
View File

@ -94,7 +94,10 @@ int gnss_sdr_valve::work(int noutput_items,
return 0; // do not produce or consume return 0; // do not produce or consume
} }
uint64_t n = std::min(d_nitems - d_ncopied_items, static_cast<uint64_t>(noutput_items)); uint64_t n = std::min(d_nitems - d_ncopied_items, static_cast<uint64_t>(noutput_items));
if (n == 0) return 0; if (n == 0)
{
return 0;
}
memcpy(output_items[0], input_items[0], n * input_signature()->sizeof_stream_item(0)); memcpy(output_items[0], input_items[0], n * input_signature()->sizeof_stream_item(0));
d_ncopied_items += n; d_ncopied_items += n;
return n; return n;

5
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
View File

@ -203,7 +203,10 @@ bool gps_l1_ca_telemetry_decoder_cc::decode_subframe()
if (symbol_accumulator_counter == 20) if (symbol_accumulator_counter == 20)
{ {
// symbol to bit // symbol to bit
if (symbol_accumulator > 0) GPS_frame_4bytes += 1; // insert the telemetry bit in LSB if (symbol_accumulator > 0)
{
GPS_frame_4bytes += 1; // insert the telemetry bit in LSB
}
symbol_accumulator = 0; symbol_accumulator = 0;
symbol_accumulator_counter = 0; symbol_accumulator_counter = 0;

4
src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc
View File

@ -299,13 +299,17 @@ void sbas_l1_telemetry_decoder_cc::frame_detector::get_frame_candidates(const st
{ {
// invert bits // invert bits
for (int &candidate_bit_it : candidate) for (int &candidate_bit_it : candidate)
{
candidate_bit_it = candidate_bit_it == 0 ? 1 : 0; candidate_bit_it = candidate_bit_it == 0 ? 1 : 0;
} }
}
msg_candidates.emplace_back(relative_preamble_start, candidate); msg_candidates.emplace_back(relative_preamble_start, candidate);
ss.str(""); ss.str("");
ss << "preamble " << preample_it - preambles.begin() << (inv_preamble_detected ? " inverted" : " normal") << " detected! candidate="; ss << "preamble " << preample_it - preambles.begin() << (inv_preamble_detected ? " inverted" : " normal") << " detected! candidate=";
for (auto bit_it = candidate.begin(); bit_it < candidate.end(); ++bit_it) for (auto bit_it = candidate.begin(); bit_it < candidate.end(); ++bit_it)
{
ss << *bit_it; ss << *bit_it;
}
VLOG(EVENT) << ss.str(); VLOG(EVENT) << ss.str();
} }
} }

6
src/algorithms/telemetry_decoder/libs/convolutional.h
View File

@ -155,7 +155,9 @@ inline static float Gamma(const float rec_array[],
for (i = 0; i < nn; i++) for (i = 0; i < nn; i++)
{ {
if (symbol & mask) if (symbol & mask)
{
rm += rec_array[nn - i - 1]; rm += rec_array[nn - i - 1];
}
mask = mask << 1; mask = mask << 1;
} }
return (rm); return (rm);
@ -219,11 +221,15 @@ inline static void Viterbi(int output_u_int[],
for (t = 0; t < LL + mm; t++) for (t = 0; t < LL + mm; t++)
{ {
for (i = 0; i < nn; i++) for (i = 0; i < nn; i++)
{
rec_array[i] = static_cast<float>(input_c[nn * t + i]); rec_array[i] = static_cast<float>(input_c[nn * t + i]);
}
/* precompute all possible branch metrics */ /* precompute all possible branch metrics */
for (i = 0; i < number_symbols; i++) for (i = 0; i < number_symbols; i++)
{
metric_c[i] = Gamma(rec_array, i, nn); metric_c[i] = Gamma(rec_array, i, nn);
}
/* step through all states */ /* step through all states */
for (state = 0; state < states; state++) for (state = 0; state < states; state++)

33
src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c
View File

@ -30,7 +30,6 @@
*/ */
#include "bits.h" #include "bits.h"
#include <limits.h> #include <limits.h>
#include <string.h> #include <string.h>
@ -72,11 +71,11 @@ u8 parity(u32 x)
u32 getbitu(const u8 *buff, u32 pos, u8 len) u32 getbitu(const u8 *buff, u32 pos, u8 len)
{ {
u32 bits = 0; u32 bits = 0;
u32 i=0; u32 i = 0;
for (i = pos; i < pos + len; i++) for (i = pos; i < pos + len; i++)
{ {
bits = (bits << 1) + bits = (bits << 1) +
((buff[i/8] >> (7 - i%8)) & 1u); ((buff[i / 8] >> (7 - i % 8)) & 1u);
} }
return bits; return bits;
@ -118,14 +117,20 @@ void setbitu(u8 *buff, u32 pos, u32 len, u32 data)
u32 mask = 1u << (len - 1); u32 mask = 1u << (len - 1);
if (len <= 0 || 32 < len) if (len <= 0 || 32 < len)
{
return; return;
}
u32 i = 0; u32 i = 0;
for (i = pos; i < pos + len; i++, mask >>= 1) for (i = pos; i < pos + len; i++, mask >>= 1)
{ {
if (data & mask) if (data & mask)
buff[i/8] |= 1u << (7 - i % 8); {
buff[i / 8] |= 1u << (7 - i % 8);
}
else else
buff[i/8] &= ~(1u << (7 - i % 8)); {
buff[i / 8] &= ~(1u << (7 - i % 8));
}
} }
} }
@ -243,7 +248,9 @@ u8 count_bits_u64(u64 v, u8 bv)
u8 r = 0; u8 r = 0;
int i = 0; int i = 0;
for (i = 0; i < 16; i++) for (i = 0; i < 16; i++)
r += bitn[(v >> (i*4)) & 0xf]; {
r += bitn[(v >> (i * 4)) & 0xf];
}
return bv == 1 ? r : 64 - r; return bv == 1 ? r : 64 - r;
} }
@ -260,7 +267,9 @@ u8 count_bits_u32(u32 v, u8 bv)
u8 r = 0; u8 r = 0;
int i = 0; int i = 0;
for (i = 0; i < 8; i++) for (i = 0; i < 8; i++)
r += bitn[(v >> (i*4)) & 0xf]; {
r += bitn[(v >> (i * 4)) & 0xf];
}
return bv == 1 ? r : 32 - r; return bv == 1 ? r : 32 - r;
} }
@ -276,8 +285,10 @@ u8 count_bits_u16(u16 v, u8 bv)
{ {
u8 r = 0; u8 r = 0;
int i = 0; int i = 0;
for (i= 0; i < 4; i++) for (i = 0; i < 4; i++)
r += bitn[(v >> (i*4)) & 0xf]; {
r += bitn[(v >> (i * 4)) & 0xf];
}
return bv == 1 ? r : 16 - r; return bv == 1 ? r : 16 - r;
} }
@ -294,7 +305,9 @@ u8 count_bits_u8(u8 v, u8 bv)
u8 r = 0; u8 r = 0;
int i = 0; int i = 0;
for (i = 0; i < 2; i++) for (i = 0; i < 2; i++)
r += bitn[(v >> (i*4)) & 0xf]; {
r += bitn[(v >> (i * 4)) & 0xf];
}
return bv == 1 ? r : 8 - r; return bv == 1 ? r : 8 - r;
} }

5
src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c
View File

@ -71,8 +71,7 @@ static const u32 crc24qtab[256] = {
0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88,
0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1,
0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401,
0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538};
};
/** Calculate Qualcomm 24-bit Cyclical Redundancy Check (CRC-24Q). /** Calculate Qualcomm 24-bit Cyclical Redundancy Check (CRC-24Q).
* *
@ -93,7 +92,9 @@ u32 crc24q(const u8 *buf, u32 len, u32 crc)
{ {
u32 i = 0; u32 i = 0;
for (i = 0; i < len; i++) for (i = 0; i < len; i++)
{
crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[((crc >> 16) ^ buf[i]) & 0xff]; crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[((crc >> 16) ^ buf[i]) & 0xff];
}
return crc; return crc;
} }

69
src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c
View File

@ -30,8 +30,8 @@
*/ */
#include <stdlib.h>
#include "fec.h" #include "fec.h"
#include <stdlib.h>
static inline int parity(int x) static inline int parity(int x)
{ {
@ -52,10 +52,10 @@ void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2])
{ {
int state; int state;
for(state = 0; state < 32; state++) for (state = 0; state < 32; state++)
{ {
poly->c0[state] = (polynomial[0] < 0) ^ parity((2*state) & abs(polynomial[0])) ? 255 : 0; poly->c0[state] = (polynomial[0] < 0) ^ parity((2 * state) & abs(polynomial[0])) ? 255 : 0;
poly->c1[state] = (polynomial[1] < 0) ^ parity((2*state) & abs(polynomial[1])) ? 255 : 0; poly->c1[state] = (polynomial[1] < 0) ^ parity((2 * state) & abs(polynomial[1])) ? 255 : 0;
} }
} }
@ -83,28 +83,31 @@ void v27_init(v27_t *v, v27_decision_t *decisions, unsigned int decisions_count,
v->decisions_index = 0; v->decisions_index = 0;
v->decisions_count = decisions_count; v->decisions_count = decisions_count;
for(i = 0; i < 64; i++) for (i = 0; i < 64; i++)
{
v->old_metrics[i] = 63; v->old_metrics[i] = 63;
}
v->old_metrics[initial_state & 63] = 0; /* Bias known start state */ v->old_metrics[initial_state & 63] = 0; /* Bias known start state */
} }
/* C-language butterfly */ /* C-language butterfly */
#define BFLY(i) {\ #define BFLY(i) \
unsigned int metric,m0,m1,decision;\ { \
metric = (v->poly->c0[i] ^ sym0) + (v->poly->c1[i] ^ sym1);\ unsigned int metric, m0, m1, decision; \
m0 = v->old_metrics[i] + metric;\ metric = (v->poly->c0[i] ^ sym0) + (v->poly->c1[i] ^ sym1); \
m1 = v->old_metrics[(i)+32] + (510 - metric);\ m0 = v->old_metrics[i] + metric; \
decision = (signed int)(m0-m1) > 0;\ m1 = v->old_metrics[(i) + 32] + (510 - metric); \
v->new_metrics[2*(i)] = decision ? m1 : m0;\ decision = (signed int)(m0 - m1) > 0; \
d->w[(i)/16] |= decision << ((2*(i))&31);\ v->new_metrics[2 * (i)] = decision ? m1 : m0; \
m0 -= (metric+metric-510);\ d->w[(i) / 16] |= decision << ((2 * (i)) & 31); \
m1 += (metric+metric-510);\ m0 -= (metric + metric - 510); \
decision = (signed int)(m0-m1) > 0;\ m1 += (metric + metric - 510); \
v->new_metrics[2*(i)+1] = decision ? m1 : m0;\ decision = (signed int)(m0 - m1) > 0; \
d->w[(i)/16] |= decision << ((2*(i)+1)&31);\ v->new_metrics[2 * (i) + 1] = decision ? m1 : m0; \
} d->w[(i) / 16] |= decision << ((2 * (i) + 1) & 31); \
}
/** Update a v27_t decoder with a block of symbols. /** Update a v27_t decoder with a block of symbols.
* *
@ -119,7 +122,7 @@ void v27_update(v27_t *v, const unsigned char *syms, int nbits)
unsigned int *tmp; unsigned int *tmp;
int normalize = 0; int normalize = 0;
while(nbits--) while (nbits--)
{ {
v27_decision_t *d = &v->decisions[v->decisions_index]; v27_decision_t *d = &v->decisions[v->decisions_index];
@ -161,26 +164,32 @@ void v27_update(v27_t *v, const unsigned char *syms, int nbits)
BFLY(31); BFLY(31);
/* Normalize metrics if they are nearing overflow */ /* Normalize metrics if they are nearing overflow */
if(v->new_metrics[0] > (1 << 30)) if (v->new_metrics[0] > (1 << 30))
{ {
int i; int i;
unsigned int minmetric = 1 << 31; unsigned int minmetric = 1 << 31;
for(i = 0; i < 64; i++) for (i = 0; i < 64; i++)
{
if (v->new_metrics[i] < minmetric)
{ {
if(v->new_metrics[i] < minmetric)
minmetric = v->new_metrics[i]; minmetric = v->new_metrics[i];
} }
}
for(i = 0; i < 64; i++) for (i = 0; i < 64; i++)
{
v->new_metrics[i] -= minmetric; v->new_metrics[i] -= minmetric;
}
normalize += minmetric; normalize += minmetric;
} }
/* Advance decision index */ /* Advance decision index */
if(++v->decisions_index >= v->decisions_count) if (++v->decisions_index >= v->decisions_count)
{
v->decisions_index = 0; v->decisions_index = 0;
}
/* Swap pointers to old and new metrics */ /* Swap pointers to old and new metrics */
tmp = v->old_metrics; tmp = v->old_metrics;
@ -207,12 +216,10 @@ void v27_chainback_fixed(v27_t *v, unsigned char *data, unsigned int nbits,
final_state %= 64; final_state %= 64;
final_state <<= 2; final_state <<= 2;
while(nbits-- != 0) while (nbits-- != 0)
{ {
/* Decrement decision index */ /* Decrement decision index */
decisions_index = (decisions_index == 0) ? decisions_index = (decisions_index == 0) ? v->decisions_count - 1 : decisions_index - 1;
v->decisions_count-1 : decisions_index-1;
v27_decision_t *d = &v->decisions[decisions_index]; v27_decision_t *d = &v->decisions[decisions_index];
k = (d->w[(final_state >> 2) / 32] >> ((final_state >> 2) % 32)) & 1; k = (d->w[(final_state >> 2) / 32] >> ((final_state >> 2) % 32)) & 1;
@ -239,9 +246,9 @@ void v27_chainback_likely(v27_t *v, unsigned char *data, unsigned int nbits)
int i; int i;
unsigned int best_metric = 0xffffffff; unsigned int best_metric = 0xffffffff;
unsigned char best_state = 0; unsigned char best_state = 0;
for(i = 0; i < 64; i++) for (i = 0; i < 64; i++)
{ {
if(v->new_metrics[i] < best_metric) if (v->new_metrics[i] < best_metric)
{ {
best_metric = v->new_metrics[i]; best_metric = v->new_metrics[i];
best_state = i; best_state = i;

2
src/algorithms/telemetry_decoder/libs/viterbi_decoder.cc
View File

@ -199,7 +199,9 @@ int Viterbi_Decoder::do_acs(const double sym[], int nbits)
{ {
/* Temporarily store the received symbols current decoding step */ /* Temporarily store the received symbols current decoding step */
for (i = 0; i < d_nn; i++) for (i = 0; i < d_nn; i++)
{
d_rec_array[i] = static_cast<float>(sym[d_nn * t + i]); d_rec_array[i] = static_cast<float>(sym[d_nn * t + i]);
}
/* precompute all possible branch metrics */ /* precompute all possible branch metrics */
for (i = 0; i < d_number_symbols; i++) for (i = 0; i < d_number_symbols; i++)

30
src/algorithms/tracking/adapters/beidou_b1i_dll_pll_tracking.cc
View File

@ -59,14 +59,20 @@ BeidouB1iDllPllTracking::BeidouB1iDllPllTracking(
bool dump = configuration->property(role + ".dump", false); bool dump = configuration->property(role + ".dump", false);
trk_param.dump = dump; trk_param.dump = dump;
float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz); if (FLAGS_pll_bw_hz != 0.0)
{
pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
}
trk_param.pll_bw_hz = pll_bw_hz; trk_param.pll_bw_hz = pll_bw_hz;
float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0);
trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz; trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz;
float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0);
trk_param.dll_bw_narrow_hz = dll_bw_narrow_hz; trk_param.dll_bw_narrow_hz = dll_bw_narrow_hz;
float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz); if (FLAGS_dll_bw_hz != 0.0)
{
dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
}
trk_param.dll_bw_hz = dll_bw_hz; trk_param.dll_bw_hz = dll_bw_hz;
float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
trk_param.early_late_space_chips = early_late_space_chips; trk_param.early_late_space_chips = early_late_space_chips;
@ -105,16 +111,28 @@ BeidouB1iDllPllTracking::BeidouB1iDllPllTracking(
char sig_[3] = "B1"; char sig_[3] = "B1";
std::memcpy(trk_param.signal, sig_, 3); std::memcpy(trk_param.signal, sig_, 3);
int cn0_samples = configuration->property(role + ".cn0_samples", 20); int cn0_samples = configuration->property(role + ".cn0_samples", 20);
if (FLAGS_cn0_samples != 20) cn0_samples = FLAGS_cn0_samples; if (FLAGS_cn0_samples != 20)
{
cn0_samples = FLAGS_cn0_samples;
}
trk_param.cn0_samples = cn0_samples; trk_param.cn0_samples = cn0_samples;
int cn0_min = configuration->property(role + ".cn0_min", 25); int cn0_min = configuration->property(role + ".cn0_min", 25);
if (FLAGS_cn0_min != 25) cn0_min = FLAGS_cn0_min; if (FLAGS_cn0_min != 25)
{
cn0_min = FLAGS_cn0_min;
}
trk_param.cn0_min = cn0_min; trk_param.cn0_min = cn0_min;
int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); int max_lock_fail = configuration->property(role + ".max_lock_fail", 50);
if (FLAGS_max_lock_fail != 50) max_lock_fail = FLAGS_max_lock_fail; if (FLAGS_max_lock_fail != 50)
{
max_lock_fail = FLAGS_max_lock_fail;
}
trk_param.max_lock_fail = max_lock_fail; trk_param.max_lock_fail = max_lock_fail;
double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85);
if (FLAGS_carrier_lock_th != 0.85) carrier_lock_th = FLAGS_carrier_lock_th; if (FLAGS_carrier_lock_th != 0.85)
{
carrier_lock_th = FLAGS_carrier_lock_th;
}
trk_param.carrier_lock_th = carrier_lock_th; trk_param.carrier_lock_th = carrier_lock_th;
//################# MAKE TRACKING GNURadio object ################### //################# MAKE TRACKING GNURadio object ###################

30
src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc
View File

@ -76,10 +76,16 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking(
trk_param.smoother_length = configuration->property(role + ".smoother_length", 10); trk_param.smoother_length = configuration->property(role + ".smoother_length", 10);
} }
float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 5.0); float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 5.0);
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz); if (FLAGS_pll_bw_hz != 0.0)
{
pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
}
trk_param.pll_bw_hz = pll_bw_hz; trk_param.pll_bw_hz = pll_bw_hz;
float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.5); float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.5);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz); if (FLAGS_dll_bw_hz != 0.0)
{
dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
}
trk_param.dll_bw_hz = dll_bw_hz; trk_param.dll_bw_hz = dll_bw_hz;
float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0);
trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz; trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz;
@ -117,16 +123,28 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking(
char sig_[3] = "1B"; char sig_[3] = "1B";
std::memcpy(trk_param.signal, sig_, 3); std::memcpy(trk_param.signal, sig_, 3);
int cn0_samples = configuration->property(role + ".cn0_samples", 20); int cn0_samples = configuration->property(role + ".cn0_samples", 20);
if (FLAGS_cn0_samples != 20) cn0_samples = FLAGS_cn0_samples; if (FLAGS_cn0_samples != 20)
{
cn0_samples = FLAGS_cn0_samples;
}
trk_param.cn0_samples = cn0_samples; trk_param.cn0_samples = cn0_samples;
int cn0_min = configuration->property(role + ".cn0_min", 25); int cn0_min = configuration->property(role + ".cn0_min", 25);
if (FLAGS_cn0_min != 25) cn0_min = FLAGS_cn0_min; if (FLAGS_cn0_min != 25)
{
cn0_min = FLAGS_cn0_min;
}
trk_param.cn0_min = cn0_min; trk_param.cn0_min = cn0_min;
int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); int max_lock_fail = configuration->property(role + ".max_lock_fail", 50);
if (FLAGS_max_lock_fail != 50) max_lock_fail = FLAGS_max_lock_fail; if (FLAGS_max_lock_fail != 50)
{
max_lock_fail = FLAGS_max_lock_fail;
}
trk_param.max_lock_fail = max_lock_fail; trk_param.max_lock_fail = max_lock_fail;
double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85);
if (FLAGS_carrier_lock_th != 0.85) carrier_lock_th = FLAGS_carrier_lock_th; if (FLAGS_carrier_lock_th != 0.85)
{
carrier_lock_th = FLAGS_carrier_lock_th;
}
trk_param.carrier_lock_th = carrier_lock_th; trk_param.carrier_lock_th = carrier_lock_th;
//################# MAKE TRACKING GNURadio object ################### //################# MAKE TRACKING GNURadio object ###################

10
src/algorithms/tracking/adapters/galileo_e1_tcp_connector_tracking.cc
View File

@ -68,9 +68,15 @@ GalileoE1TcpConnectorTracking::GalileoE1TcpConnectorTracking(
fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
dump = configuration->property(role + ".dump", false); dump = configuration->property(role + ".dump", false);
pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz); if (FLAGS_pll_bw_hz != 0.0)
{
pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
}
dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz); if (FLAGS_dll_bw_hz != 0.0)
{
dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
}
early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.15); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.15);
very_early_late_space_chips = configuration->property(role + ".very_early_late_space_chips", 0.6); very_early_late_space_chips = configuration->property(role + ".very_early_late_space_chips", 0.6);
port_ch0 = configuration->property(role + ".port_ch0", 2060); port_ch0 = configuration->property(role + ".port_ch0", 2060);

30
src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc
View File

@ -76,10 +76,16 @@ GalileoE5aDllPllTracking::GalileoE5aDllPllTracking(
trk_param.smoother_length = configuration->property(role + ".smoother_length", 10); trk_param.smoother_length = configuration->property(role + ".smoother_length", 10);
} }
float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 20.0); float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 20.0);
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz); if (FLAGS_pll_bw_hz != 0.0)
{
pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
}
trk_param.pll_bw_hz = pll_bw_hz; trk_param.pll_bw_hz = pll_bw_hz;
float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 20.0); float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 20.0);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz); if (FLAGS_dll_bw_hz != 0.0)
{
dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
}
trk_param.dll_bw_hz = dll_bw_hz; trk_param.dll_bw_hz = dll_bw_hz;
float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 5.0); float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 5.0);
trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz; trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz;
@ -115,16 +121,28 @@ GalileoE5aDllPllTracking::GalileoE5aDllPllTracking(
char sig_[3] = "5X"; char sig_[3] = "5X";
std::memcpy(trk_param.signal, sig_, 3); std::memcpy(trk_param.signal, sig_, 3);
int cn0_samples = configuration->property(role + ".cn0_samples", 20); int cn0_samples = configuration->property(role + ".cn0_samples", 20);
if (FLAGS_cn0_samples != 20) cn0_samples = FLAGS_cn0_samples; if (FLAGS_cn0_samples != 20)
{
cn0_samples = FLAGS_cn0_samples;
}
trk_param.cn0_samples = cn0_samples; trk_param.cn0_samples = cn0_samples;
int cn0_min = configuration->property(role + ".cn0_min", 25); int cn0_min = configuration->property(role + ".cn0_min", 25);
if (FLAGS_cn0_min != 25) cn0_min = FLAGS_cn0_min; if (FLAGS_cn0_min != 25)
{
cn0_min = FLAGS_cn0_min;
}
trk_param.cn0_min = cn0_min; trk_param.cn0_min = cn0_min;
int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); int max_lock_fail = configuration->property(role + ".max_lock_fail", 50);
if (FLAGS_max_lock_fail != 50) max_lock_fail = FLAGS_max_lock_fail; if (FLAGS_max_lock_fail != 50)
{
max_lock_fail = FLAGS_max_lock_fail;
}
trk_param.max_lock_fail = max_lock_fail; trk_param.max_lock_fail = max_lock_fail;
double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85);
if (FLAGS_carrier_lock_th != 0.85) carrier_lock_th = FLAGS_carrier_lock_th; if (FLAGS_carrier_lock_th != 0.85)
{
carrier_lock_th = FLAGS_carrier_lock_th;
}
trk_param.carrier_lock_th = carrier_lock_th; trk_param.carrier_lock_th = carrier_lock_th;
//################# MAKE TRACKING GNURadio object ################### //################# MAKE TRACKING GNURadio object ###################

10
src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc
View File

@ -69,9 +69,15 @@ GlonassL1CaDllPllCAidTracking::GlonassL1CaDllPllCAidTracking(
fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
dump = configuration->property(role + ".dump", false); dump = configuration->property(role + ".dump", false);
pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz); if (FLAGS_pll_bw_hz != 0.0)
{
pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
}
dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz); if (FLAGS_dll_bw_hz != 0.0)
{
dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
}
pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0);
dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0);
int extend_correlation_ms; int extend_correlation_ms;

10
src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc
View File

@ -66,9 +66,15 @@ GlonassL1CaDllPllTracking::GlonassL1CaDllPllTracking(
fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
dump = configuration->property(role + ".dump", false); dump = configuration->property(role + ".dump", false);
pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz); if (FLAGS_pll_bw_hz != 0.0)
{
pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
}
dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz); if (FLAGS_dll_bw_hz != 0.0)
{
dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
}
early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
std::string default_dump_filename = "./track_ch"; std::string default_dump_filename = "./track_ch";
dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); dump_filename = configuration->property(role + ".dump_filename", default_dump_filename);

10
src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_c_aid_tracking.cc
View File

@ -67,9 +67,15 @@ GlonassL2CaDllPllCAidTracking::GlonassL2CaDllPllCAidTracking(
fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
dump = configuration->property(role + ".dump", false); dump = configuration->property(role + ".dump", false);
pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz); if (FLAGS_pll_bw_hz != 0.0)
{
pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
}
dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz); if (FLAGS_dll_bw_hz != 0.0)
{
dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
}
pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0);
dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0);
int extend_correlation_ms; int extend_correlation_ms;

10
src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_tracking.cc
View File

@ -64,9 +64,15 @@ GlonassL2CaDllPllTracking::GlonassL2CaDllPllTracking(
fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
dump = configuration->property(role + ".dump", false); dump = configuration->property(role + ".dump", false);
pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz); if (FLAGS_pll_bw_hz != 0.0)
{
pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
}
dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz); if (FLAGS_dll_bw_hz != 0.0)
{
dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
}
early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
std::string default_dump_filename = "./track_ch"; std::string default_dump_filename = "./track_ch";
dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); dump_filename = configuration->property(role + ".dump_filename", default_dump_filename);

10
src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc
View File

@ -68,9 +68,15 @@ GpsL1CaDllPllCAidTracking::GpsL1CaDllPllCAidTracking(
fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
dump = configuration->property(role + ".dump", false); dump = configuration->property(role + ".dump", false);
pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz); if (FLAGS_pll_bw_hz != 0.0)
{
pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
}
dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz); if (FLAGS_dll_bw_hz != 0.0)
{
dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
}
pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0);
dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0);
int extend_correlation_ms; int extend_correlation_ms;

30
src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc
View File

@ -76,14 +76,20 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking(
bool dump_mat = configuration->property(role + ".dump_mat", true); bool dump_mat = configuration->property(role + ".dump_mat", true);
trk_param.dump_mat = dump_mat; trk_param.dump_mat = dump_mat;
float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz); if (FLAGS_pll_bw_hz != 0.0)
{
pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
}
trk_param.pll_bw_hz = pll_bw_hz; trk_param.pll_bw_hz = pll_bw_hz;
float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0);
trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz; trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz;
float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0);
trk_param.dll_bw_narrow_hz = dll_bw_narrow_hz; trk_param.dll_bw_narrow_hz = dll_bw_narrow_hz;
float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz); if (FLAGS_dll_bw_hz != 0.0)
{
dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
}
trk_param.dll_bw_hz = dll_bw_hz; trk_param.dll_bw_hz = dll_bw_hz;
float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
trk_param.early_late_space_chips = early_late_space_chips; trk_param.early_late_space_chips = early_late_space_chips;
@ -119,16 +125,28 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking(
char sig_[3] = "1C"; char sig_[3] = "1C";
std::memcpy(trk_param.signal, sig_, 3); std::memcpy(trk_param.signal, sig_, 3);
int cn0_samples = configuration->property(role + ".cn0_samples", 20); int cn0_samples = configuration->property(role + ".cn0_samples", 20);
if (FLAGS_cn0_samples != 20) cn0_samples = FLAGS_cn0_samples; if (FLAGS_cn0_samples != 20)
{
cn0_samples = FLAGS_cn0_samples;
}
trk_param.cn0_samples = cn0_samples; trk_param.cn0_samples = cn0_samples;
int cn0_min = configuration->property(role + ".cn0_min", 30); int cn0_min = configuration->property(role + ".cn0_min", 30);
if (FLAGS_cn0_min != 25) cn0_min = FLAGS_cn0_min; if (FLAGS_cn0_min != 25)
{
cn0_min = FLAGS_cn0_min;
}
trk_param.cn0_min = cn0_min; trk_param.cn0_min = cn0_min;
int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); int max_lock_fail = configuration->property(role + ".max_lock_fail", 50);
if (FLAGS_max_lock_fail != 50) max_lock_fail = FLAGS_max_lock_fail; if (FLAGS_max_lock_fail != 50)
{
max_lock_fail = FLAGS_max_lock_fail;
}
trk_param.max_lock_fail = max_lock_fail; trk_param.max_lock_fail = max_lock_fail;
double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.80); double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.80);
if (FLAGS_carrier_lock_th != 0.85) carrier_lock_th = FLAGS_carrier_lock_th; if (FLAGS_carrier_lock_th != 0.85)
{
carrier_lock_th = FLAGS_carrier_lock_th;
}
trk_param.carrier_lock_th = carrier_lock_th; trk_param.carrier_lock_th = carrier_lock_th;
//################# MAKE TRACKING GNURadio object ################### //################# MAKE TRACKING GNURadio object ###################

5
src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc
View File

@ -77,7 +77,10 @@ GpsL1CaKfTracking::GpsL1CaKfTracking(
f_if = configuration->property(role + ".if", 0); f_if = configuration->property(role + ".if", 0);
dump = configuration->property(role + ".dump", false); dump = configuration->property(role + ".dump", false);
dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz); if (FLAGS_dll_bw_hz != 0.0)
{
dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
}
early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
std::string default_dump_filename = "./track_ch"; std::string default_dump_filename = "./track_ch";
dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); dump_filename = configuration->property(role + ".dump_filename", default_dump_filename);

30
src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc
View File

@ -66,10 +66,16 @@ GpsL2MDllPllTracking::GpsL2MDllPllTracking(
bool dump_mat = configuration->property(role + ".dump_mat", true); bool dump_mat = configuration->property(role + ".dump_mat", true);
trk_param.dump_mat = dump_mat; trk_param.dump_mat = dump_mat;
float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 2.0); float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 2.0);
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz); if (FLAGS_pll_bw_hz != 0.0)
{
pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
}
trk_param.pll_bw_hz = pll_bw_hz; trk_param.pll_bw_hz = pll_bw_hz;
float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.75); float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.75);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz); if (FLAGS_dll_bw_hz != 0.0)
{
dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
}
trk_param.dll_bw_hz = dll_bw_hz; trk_param.dll_bw_hz = dll_bw_hz;
float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
trk_param.early_late_space_chips = early_late_space_chips; trk_param.early_late_space_chips = early_late_space_chips;
@ -96,16 +102,28 @@ GpsL2MDllPllTracking::GpsL2MDllPllTracking(
char sig_[3] = "2S"; char sig_[3] = "2S";
std::memcpy(trk_param.signal, sig_, 3); std::memcpy(trk_param.signal, sig_, 3);
int cn0_samples = configuration->property(role + ".cn0_samples", 20); int cn0_samples = configuration->property(role + ".cn0_samples", 20);
if (FLAGS_cn0_samples != 20) cn0_samples = FLAGS_cn0_samples; if (FLAGS_cn0_samples != 20)
{
cn0_samples = FLAGS_cn0_samples;
}
trk_param.cn0_samples = cn0_samples; trk_param.cn0_samples = cn0_samples;
int cn0_min = configuration->property(role + ".cn0_min", 25); int cn0_min = configuration->property(role + ".cn0_min", 25);
if (FLAGS_cn0_min != 25) cn0_min = FLAGS_cn0_min; if (FLAGS_cn0_min != 25)
{
cn0_min = FLAGS_cn0_min;
}
trk_param.cn0_min = cn0_min; trk_param.cn0_min = cn0_min;
int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); int max_lock_fail = configuration->property(role + ".max_lock_fail", 50);
if (FLAGS_max_lock_fail != 50) max_lock_fail = FLAGS_max_lock_fail; if (FLAGS_max_lock_fail != 50)
{
max_lock_fail = FLAGS_max_lock_fail;
}
trk_param.max_lock_fail = max_lock_fail; trk_param.max_lock_fail = max_lock_fail;
double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85); double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85);
if (FLAGS_carrier_lock_th != 0.85) carrier_lock_th = FLAGS_carrier_lock_th; if (FLAGS_carrier_lock_th != 0.85)
{
carrier_lock_th = FLAGS_carrier_lock_th;
}
trk_param.carrier_lock_th = carrier_lock_th; trk_param.carrier_lock_th = carrier_lock_th;
//################# MAKE TRACKING GNURadio object ################### //################# MAKE TRACKING GNURadio object ###################

30
src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc
View File

@ -76,10 +76,16 @@ GpsL5DllPllTracking::GpsL5DllPllTracking(
trk_param.smoother_length = configuration->property(role + ".smoother_length", 10); trk_param.smoother_length = configuration->property(role + ".smoother_length", 10);
} }
float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz); if (FLAGS_pll_bw_hz != 0.0)
{
pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
}
trk_param.pll_bw_hz = pll_bw_hz; trk_param.pll_bw_hz = pll_bw_hz;
float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz); if (FLAGS_dll_bw_hz != 0.0)
{
dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
}
trk_param.dll_bw_hz = dll_bw_hz; trk_param.dll_bw_hz = dll_bw_hz;
float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0);
trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz; trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz;
@ -115,16 +121,28 @@ GpsL5DllPllTracking::GpsL5DllPllTracking(
char sig_[3] = "L5"; char sig_[3] = "L5";
std::memcpy(trk_param.signal, sig_, 3); std::memcpy(trk_param.signal, sig_, 3);
int cn0_samples = configuration->property(role + ".cn0_samples", 20); int cn0_samples = configuration->property(role + ".cn0_samples", 20);
if (FLAGS_cn0_samples != 20) cn0_samples = FLAGS_cn0_samples; if (FLAGS_cn0_samples != 20)
{
cn0_samples = FLAGS_cn0_samples;
}
trk_param.cn0_samples = cn0_samples; trk_param.cn0_samples = cn0_samples;
int cn0_min = configuration->property(role + ".cn0_min", 25); int cn0_min = configuration->property(role + ".cn0_min", 25);
if (FLAGS_cn0_min != 25) cn0_min = FLAGS_cn0_min; if (FLAGS_cn0_min != 25)
{
cn0_min = FLAGS_cn0_min;
}
trk_param.cn0_min = cn0_min; trk_param.cn0_min = cn0_min;
int max_lock_fail = configuration->property(role + ".max_lock_fail", 50); int max_lock_fail = configuration->property(role + ".max_lock_fail", 50);
if (FLAGS_max_lock_fail != 50) max_lock_fail = FLAGS_max_lock_fail; if (FLAGS_max_lock_fail != 50)
{
max_lock_fail = FLAGS_max_lock_fail;
}
trk_param.max_lock_fail = max_lock_fail; trk_param.max_lock_fail = max_lock_fail;
double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.75); double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.75);
if (FLAGS_carrier_lock_th != 0.85) carrier_lock_th = FLAGS_carrier_lock_th; if (FLAGS_carrier_lock_th != 0.85)
{
carrier_lock_th = FLAGS_carrier_lock_th;
}
trk_param.carrier_lock_th = carrier_lock_th; trk_param.carrier_lock_th = carrier_lock_th;
//################# MAKE TRACKING GNURadio object ################### //################# MAKE TRACKING GNURadio object ###################

14
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc
View File

@ -764,7 +764,10 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status(double coh_integration_
} }
else else
{ {
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; if (d_carrier_lock_fail_counter > 0)
{
d_carrier_lock_fail_counter--;
}
} }
if (d_carrier_lock_fail_counter > trk_parameters.max_lock_fail) if (d_carrier_lock_fail_counter > trk_parameters.max_lock_fail)
{ {
@ -853,7 +856,10 @@ void dll_pll_veml_tracking::run_dll_pll()
void dll_pll_veml_tracking::clear_tracking_vars() void dll_pll_veml_tracking::clear_tracking_vars()
{ {
std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0));
if (trk_parameters.track_pilot) d_Prompt_Data[0] = gr_complex(0.0, 0.0); if (trk_parameters.track_pilot)
{
d_Prompt_Data[0] = gr_complex(0.0, 0.0);
}
d_carr_error_hz = 0.0; d_carr_error_hz = 0.0;
d_carr_error_filt_hz = 0.0; d_carr_error_filt_hz = 0.0;
d_code_error_chips = 0.0; d_code_error_chips = 0.0;
@ -989,9 +995,13 @@ void dll_pll_veml_tracking::save_correlation_results()
} }
// If tracking pilot, disable Costas loop // If tracking pilot, disable Costas loop
if (trk_parameters.track_pilot) if (trk_parameters.track_pilot)
{
d_cloop = false; d_cloop = false;
}
else else
{
d_cloop = true; d_cloop = true;
}
} }

5
src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc
View File

@ -425,7 +425,10 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attri
} }
else else
{ {
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; if (d_carrier_lock_fail_counter > 0)
{
d_carrier_lock_fail_counter--;
}
} }
if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail)
{ {

5
src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc
View File

@ -808,7 +808,10 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
} }
else else
{ {
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; if (d_carrier_lock_fail_counter > 0)
{
d_carrier_lock_fail_counter--;
}
} }
if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail)
{ {

5
src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc
View File

@ -798,7 +798,10 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
} }
else else
{ {
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; if (d_carrier_lock_fail_counter > 0)
{
d_carrier_lock_fail_counter--;
}
} }
if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail)
{ {

5
src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc
View File

@ -667,7 +667,10 @@ int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
} }
else else
{ {
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; if (d_carrier_lock_fail_counter > 0)
{
d_carrier_lock_fail_counter--;
}
} }
if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail)
{ {

5
src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc
View File

@ -806,7 +806,10 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
} }
else else
{ {
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; if (d_carrier_lock_fail_counter > 0)
{
d_carrier_lock_fail_counter--;
}
} }
if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail)
{ {

5
src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc
View File

@ -797,7 +797,10 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
} }
else else
{ {
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; if (d_carrier_lock_fail_counter > 0)
{
d_carrier_lock_fail_counter--;
}
} }
if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail)
{ {

5
src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc
View File

@ -667,7 +667,10 @@ int Glonass_L2_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
} }
else else
{ {
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; if (d_carrier_lock_fail_counter > 0)
{
d_carrier_lock_fail_counter--;
}
} }
if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail)
{ {

5
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
View File

@ -787,7 +787,10 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __attrib
} }
else else
{ {
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; if (d_carrier_lock_fail_counter > 0)
{
d_carrier_lock_fail_counter--;
}
} }
if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail)
{ {

5
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc
View File

@ -788,7 +788,10 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __attrib
} }
else else
{ {
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; if (d_carrier_lock_fail_counter > 0)
{
d_carrier_lock_fail_counter--;
}
} }
if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail)
{ {

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