mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Apply automated code formatting 

Documented at .clang-format
See http://clang.llvm.org/docs/ClangFormat.html and http://clang.llvm.org/docs/ClangFormatStyleOptions.html
This commit is contained in:
Carles Fernandez 2018-03-03 02:03:39 +01:00
parent c9ac8c78af
commit 34f24562cf
902 changed files with 36939 additions and 34573 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
.clang-format Normal file
View File

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

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

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

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

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

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

File diff suppressed because it is too large Load Diff

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

File diff suppressed because it is too large Load Diff

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

23
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc
View File

@ -42,9 +42,8 @@
using google::LogMessage;
GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga(
ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) :
role_(role), in_streams_(in_streams), out_streams_(out_streams)
ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
{
unsigned int code_length;
bool bit_transition_flag;
@ -72,7 +71,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga(
ifreq = configuration_->property(role + ".if", 0);
dump = configuration_->property(role + ".dump", false);
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);
// note : the FPGA is implemented according to bit transition flag = 0. Setting bit transition flag to 1 has no effect.
@ -88,7 +87,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga(
//--- Find number of samples per spreading code -------------------------
code_length = round(
fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
// code length has the same value as d_fft_size
float nbits;
@ -112,17 +111,17 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga(
{
item_size_ = sizeof(lv_16sc_t);
gps_acquisition_fpga_sc_ = gps_pcps_make_acquisition_fpga_sc(
sampled_ms, max_dwells_, doppler_max_, ifreq, fs_in,
code_length, code_length, vector_length_, nsamples_total,
bit_transition_flag, use_CFAR_algorithm_flag,
select_queue_Fpga, device_name, dump, dump_filename);
sampled_ms, max_dwells_, doppler_max_, ifreq, fs_in,
code_length, code_length, vector_length_, nsamples_total,
bit_transition_flag, use_CFAR_algorithm_flag,
select_queue_Fpga, device_name, dump, dump_filename);
DLOG(INFO) << "acquisition("
<< gps_acquisition_fpga_sc_->unique_id() << ")";
<< gps_acquisition_fpga_sc_->unique_id() << ")";
}
else
{
LOG(WARNING) << "item_type configured to " << item_type_ << "but FPGA implementation only accepts cshort";
throw std::invalid_argument( "Wrong input_type configuration. Should be cshort" );
throw std::invalid_argument("Wrong input_type configuration. Should be cshort");
}
channel_ = 0;
@ -219,7 +218,7 @@ float GpsL1CaPcpsAcquisitionFpga::calculate_threshold(float pfa)
//Calculate the threshold
unsigned int frequency_bins = 0;
for (int doppler = static_cast<int>(-doppler_max_); doppler <= static_cast<int>(doppler_max_);
doppler += doppler_step_)
doppler += doppler_step_)
{
frequency_bins++;
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

File diff suppressed because it is too large Load Diff

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

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

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

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

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

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

122
src/algorithms/acquisition/gnuradio_blocks/gps_pcps_acquisition_fpga_sc.cc
View File

@ -39,58 +39,57 @@
#include <volk/volk.h>
#include <volk_gnsssdr/volk_gnsssdr.h>
#include "control_message_factory.h"
#include "GPS_L1_CA.h" //GPS_TWO_PI
#include "GPS_L1_CA.h" //GPS_TWO_PI
using google::LogMessage;
void wait3(int seconds)
{
boost::this_thread::sleep_for(boost::chrono::seconds
{ seconds });
boost::this_thread::sleep_for(boost::chrono::seconds{seconds});
}
gps_pcps_acquisition_fpga_sc_sptr gps_pcps_make_acquisition_fpga_sc(
unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, int samples_per_ms,
int samples_per_code, int vector_length, unsigned int nsamples_total,
bool bit_transition_flag, bool use_CFAR_algorithm_flag,
unsigned int select_queue_Fpga, std::string device_name, bool dump,
std::string dump_filename)
unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, int samples_per_ms,
int samples_per_code, int vector_length, unsigned int nsamples_total,
bool bit_transition_flag, bool use_CFAR_algorithm_flag,
unsigned int select_queue_Fpga, std::string device_name, bool dump,
std::string dump_filename)
{
return gps_pcps_acquisition_fpga_sc_sptr(
new gps_pcps_acquisition_fpga_sc(sampled_ms, max_dwells,
doppler_max, freq, fs_in, samples_per_ms, samples_per_code,
vector_length, nsamples_total, bit_transition_flag,
use_CFAR_algorithm_flag, select_queue_Fpga, device_name,
dump, dump_filename));
new gps_pcps_acquisition_fpga_sc(sampled_ms, max_dwells,
doppler_max, freq, fs_in, samples_per_ms, samples_per_code,
vector_length, nsamples_total, bit_transition_flag,
use_CFAR_algorithm_flag, select_queue_Fpga, device_name,
dump, dump_filename));
}
gps_pcps_acquisition_fpga_sc::gps_pcps_acquisition_fpga_sc(
unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, int samples_per_ms,
int samples_per_code, int vector_length, unsigned int nsamples_total,
bool bit_transition_flag, bool use_CFAR_algorithm_flag,
unsigned int select_queue_Fpga, std::string device_name, bool dump,
std::string dump_filename) :
unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, int samples_per_ms,
int samples_per_code, int vector_length, unsigned int nsamples_total,
bool bit_transition_flag, bool use_CFAR_algorithm_flag,
unsigned int select_queue_Fpga, std::string device_name, bool dump,
std::string dump_filename) :
gr::block("pcps_acquisition_fpga_sc",
gr::io_signature::make(0, 0, sizeof(lv_16sc_t)),
gr::io_signature::make(0, 0, 0))
gr::block("pcps_acquisition_fpga_sc",
gr::io_signature::make(0, 0, sizeof(lv_16sc_t)),
gr::io_signature::make(0, 0, 0))
{
this->message_port_register_out(pmt::mp("events"));
d_sample_counter = 0; // SAMPLE COUNTER
d_sample_counter = 0; // SAMPLE COUNTER
d_active = false;
d_state = 0;
d_samples_per_code = samples_per_code;
d_max_dwells = max_dwells; // Note : d_max_dwells is not used in the FPGA implementation
d_max_dwells = max_dwells; // Note : d_max_dwells is not used in the FPGA implementation
d_well_count = 0;
d_doppler_max = doppler_max;
d_fft_size = sampled_ms * samples_per_ms;
d_mag = 0;
d_num_doppler_bins = 0;
d_bit_transition_flag = bit_transition_flag; // Note : bit transition flag is ignored and assumed 0 in the FPGA implementation
d_use_CFAR_algorithm_flag = use_CFAR_algorithm_flag; // Note : user CFAR algorithm flag is ignored and assumed 0 in the FPGA implementation
d_bit_transition_flag = bit_transition_flag; // Note : bit transition flag is ignored and assumed 0 in the FPGA implementation
d_use_CFAR_algorithm_flag = use_CFAR_algorithm_flag; // Note : user CFAR algorithm flag is ignored and assumed 0 in the FPGA implementation
d_threshold = 0.0;
d_doppler_step = 250;
d_channel = 0;
@ -102,8 +101,7 @@ gps_pcps_acquisition_fpga_sc::gps_pcps_acquisition_fpga_sc(
d_gnss_synchro = 0;
// instantiate HW accelerator class
acquisition_fpga_8sc = std::make_shared < gps_fpga_acquisition_8sc>
(device_name, vector_length, d_fft_size, nsamples_total, fs_in, freq, sampled_ms, select_queue_Fpga);
acquisition_fpga_8sc = std::make_shared<gps_fpga_acquisition_8sc>(device_name, vector_length, d_fft_size, nsamples_total, fs_in, freq, sampled_ms, select_queue_Fpga);
}
@ -136,9 +134,7 @@ void gps_pcps_acquisition_fpga_sc::init()
d_mag = 0.0;
d_num_doppler_bins = ceil(
static_cast<double>(static_cast<int>(d_doppler_max)
- static_cast<int>(-d_doppler_max))
/ static_cast<double>(d_doppler_step));
static_cast<double>(static_cast<int>(d_doppler_max) - static_cast<int>(-d_doppler_max)) / static_cast<double>(d_doppler_step));
acquisition_fpga_8sc->open_device();
@ -173,11 +169,11 @@ void gps_pcps_acquisition_fpga_sc::set_active(bool active)
float peak_to_noise_level = 0.0;
float input_power;
float test_statistics = 0.0;
acquisition_fpga_8sc->block_samples(); // block the samples to run the acquisition this is only necessary for the tests
acquisition_fpga_8sc->block_samples(); // block the samples to run the acquisition this is only necessary for the tests
d_active = active;
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
d_state = 1;
@ -196,25 +192,24 @@ void gps_pcps_acquisition_fpga_sc::set_active(bool active)
d_well_count++;
DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System
<< " " << d_gnss_synchro->PRN << " ,sample stamp: "
<< d_sample_counter << ", threshold: " << ", threshold: "
<< d_threshold << ", doppler_max: " << d_doppler_max
<< ", doppler_step: " << d_doppler_step;
<< " , doing acquisition of satellite: " << d_gnss_synchro->System
<< " " << d_gnss_synchro->PRN << " ,sample stamp: "
<< d_sample_counter << ", threshold: "
<< ", threshold: "
<< d_threshold << ", doppler_max: " << d_doppler_max
<< ", doppler_step: " << d_doppler_step;
// Doppler frequency search loop
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins;
doppler_index++)
doppler_index++)
{
doppler = -static_cast<int>(d_doppler_max)
+ d_doppler_step * doppler_index;
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
acquisition_fpga_8sc->set_phase_step(doppler_index);
acquisition_fpga_8sc->run_acquisition(); // runs acquisition and waits until it is finished
acquisition_fpga_8sc->run_acquisition(); // runs acquisition and waits until it is finished
acquisition_fpga_8sc->read_acquisition_results(&indext, &magt,
&initial_sample, &input_power);
&initial_sample, &input_power);
d_sample_counter = initial_sample;
@ -224,13 +219,12 @@ void gps_pcps_acquisition_fpga_sc::set_active(bool active)
peak_to_noise_level = temp_peak_to_noise_level;
d_mag = magt;
input_power = (input_power - d_mag)
/ (effective_fft_size - 1);
input_power = (input_power - d_mag) / (effective_fft_size - 1);
d_gnss_synchro->Acq_delay_samples =
static_cast<double>(indext % d_samples_per_code);
static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz =
static_cast<double>(doppler);
static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
test_statistics = d_mag / input_power;
}
@ -244,29 +238,29 @@ void gps_pcps_acquisition_fpga_sc::set_active(bool active)
boost::filesystem::path p = d_dump_filename;
filename << p.parent_path().string()
<< boost::filesystem::path::preferred_separator
<< p.stem().string() << "_"
<< d_gnss_synchro->System << "_"
<< d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler
<< p.extension().string();
<< boost::filesystem::path::preferred_separator
<< p.stem().string() << "_"
<< d_gnss_synchro->System << "_"
<< d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler
<< p.extension().string();
DLOG(INFO) << "Writing ACQ out to " << filename.str();
d_dump_file.open(filename.str().c_str(),
std::ios::out | std::ios::binary);
std::ios::out | std::ios::binary);
d_dump_file.close();
}
}
if (test_statistics > d_threshold)
{
d_state = 2; // Positive acquisition
d_state = 2; // Positive acquisition
// 6.1- Declare positive acquisition using a message port
DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " "
<< d_gnss_synchro->PRN;
<< d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
@ -280,16 +274,16 @@ void gps_pcps_acquisition_fpga_sc::set_active(bool active)
acquisition_message = 1;
this->message_port_pub(pmt::mp("events"),
pmt::from_long(acquisition_message));
pmt::from_long(acquisition_message));
}
else
{
d_state = 3; // Negative acquisition
d_state = 3; // Negative acquisition
// 6.2- Declare negative acquisition using a message port
DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " "
<< d_gnss_synchro->PRN;
<< d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
@ -303,7 +297,7 @@ void gps_pcps_acquisition_fpga_sc::set_active(bool active)
acquisition_message = 2;
this->message_port_pub(pmt::mp("events"),
pmt::from_long(acquisition_message));
pmt::from_long(acquisition_message));
}
acquisition_fpga_8sc->unblock_samples();
@ -315,8 +309,8 @@ void gps_pcps_acquisition_fpga_sc::set_active(bool active)
int gps_pcps_acquisition_fpga_sc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused)))
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused)))
{
// general work not used with the acquisition
return noutput_items;

53
src/algorithms/acquisition/gnuradio_blocks/gps_pcps_acquisition_fpga_sc.h
View File

@ -63,12 +63,12 @@ typedef boost::shared_ptr<gps_pcps_acquisition_fpga_sc> gps_pcps_acquisition_fpg
gps_pcps_acquisition_fpga_sc_sptr
gps_pcps_make_acquisition_fpga_sc(unsigned int sampled_ms,
unsigned int max_dwells, unsigned int doppler_max, long freq,
long fs_in, int samples_per_ms, int samples_per_code,
int vector_length_, unsigned int nsamples_total_,
bool bit_transition_flag, bool use_CFAR_algorithm_flag,
unsigned int select_queue_Fpga, std::string device_name, bool dump,
std::string dump_filename);
unsigned int max_dwells, unsigned int doppler_max, long freq,
long fs_in, int samples_per_ms, int samples_per_code,
int vector_length_, unsigned int nsamples_total_,
bool bit_transition_flag, bool use_CFAR_algorithm_flag,
unsigned int select_queue_Fpga, std::string device_name, bool dump,
std::string dump_filename);
/*!
* \brief This class implements a Parallel Code Phase Search Acquisition.
@ -81,20 +81,20 @@ class gps_pcps_acquisition_fpga_sc : public gr::block
private:
friend gps_pcps_acquisition_fpga_sc_sptr
gps_pcps_make_acquisition_fpga_sc(unsigned int sampled_ms,
unsigned int max_dwells, unsigned int doppler_max, long freq,
long fs_in, int samples_per_ms, int samples_per_code,
int vector_length, unsigned int nsamples_total,
bool bit_transition_flag, bool use_CFAR_algorithm_flag,
unsigned int select_queue_Fpga, std::string device_name, bool dump,
std::string dump_filename);
unsigned int max_dwells, unsigned int doppler_max, long freq,
long fs_in, int samples_per_ms, int samples_per_code,
int vector_length, unsigned int nsamples_total,
bool bit_transition_flag, bool use_CFAR_algorithm_flag,
unsigned int select_queue_Fpga, std::string device_name, bool dump,
std::string dump_filename);
gps_pcps_acquisition_fpga_sc(unsigned int sampled_ms,
unsigned int max_dwells, unsigned int doppler_max, long freq,
long fs_in, int samples_per_ms, int samples_per_code,
int vector_length, unsigned int nsamples_total,
bool bit_transition_flag, bool use_CFAR_algorithm_flag,
unsigned int select_queue_Fpga, std::string device_name, bool dump,
std::string dump_filename);
unsigned int max_dwells, unsigned int doppler_max, long freq,
long fs_in, int samples_per_ms, int samples_per_code,
int vector_length, unsigned int nsamples_total,
bool bit_transition_flag, bool use_CFAR_algorithm_flag,
unsigned int select_queue_Fpga, std::string device_name, bool dump,
std::string dump_filename);
int d_samples_per_code;
float d_threshold;
@ -107,9 +107,13 @@ private:
unsigned int d_num_doppler_bins;
Gnss_Synchro *d_gnss_synchro;
float d_mag;bool d_bit_transition_flag;bool d_use_CFAR_algorithm_flag;
std::ofstream d_dump_file;bool d_active;
int d_state;bool d_dump;
float d_mag;
bool d_bit_transition_flag;
bool d_use_CFAR_algorithm_flag;
std::ofstream d_dump_file;
bool d_active;
int d_state;
bool d_dump;
unsigned int d_channel;
std::string d_dump_filename;
@ -126,7 +130,7 @@ public:
* to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks.
*/
inline void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
inline void set_gnss_synchro(Gnss_Synchro *p_gnss_synchro)
{
d_gnss_synchro = p_gnss_synchro;
}
@ -207,9 +211,8 @@ public:
* \brief Parallel Code Phase Search Acquisition signal processing.
*/
int general_work(int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
};
#endif /* GNSS_SDR_PCPS_ACQUISITION_SC_H_*/

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

66
src/algorithms/acquisition/libs/gps_fpga_acquisition_8sc.cc
View File

@ -65,7 +65,7 @@
#include "GPS_L1_CA.h"
#define PAGE_SIZE 0x10000
#define MAX_PHASE_STEP_RAD 0.999999999534339 // 1 - pow(2,-31);
#define MAX_PHASE_STEP_RAD 0.999999999534339 // 1 - pow(2,-31);
#define NUM_PRNs 32
#define TEST_REGISTER_ACQ_WRITEVAL 0x55AA
@ -78,30 +78,29 @@ bool gps_fpga_acquisition_8sc::init()
bool gps_fpga_acquisition_8sc::set_local_code(unsigned int PRN)
{
// select the code with the chosen PRN
gps_fpga_acquisition_8sc::fpga_configure_acquisition_local_code(
&d_all_fft_codes[d_vector_length * PRN]);
&d_all_fft_codes[d_vector_length * PRN]);
return true;
}
gps_fpga_acquisition_8sc::gps_fpga_acquisition_8sc(std::string device_name,
unsigned int vector_length, unsigned int nsamples,
unsigned int nsamples_total, long fs_in, long freq,
unsigned int sampled_ms, unsigned select_queue)
unsigned int vector_length, unsigned int nsamples,
unsigned int nsamples_total, long fs_in, long freq,
unsigned int sampled_ms, unsigned select_queue)
{
// initial values
d_device_name = device_name;
d_freq = freq;
d_fs_in = fs_in;
d_vector_length = vector_length;
d_nsamples = nsamples; // number of samples not including padding
d_nsamples = nsamples; // number of samples not including padding
d_select_queue = select_queue;
d_doppler_max = 0;
d_doppler_step = 0;
d_fd = 0; // driver descriptor
d_map_base = nullptr; // driver memory map
d_fd = 0; // driver descriptor
d_map_base = nullptr; // driver memory map
// compute all the possible code ffts
@ -110,35 +109,35 @@ gps_fpga_acquisition_8sc::gps_fpga_acquisition_8sc(std::string device_name,
// allocate memory to compute all the PRNs
// and compute all the possible codes
std::complex<float>* code = new std::complex<float>[nsamples_total]; // buffer for the local code
std::complex<float> * code_total = new gr_complex[vector_length]; // buffer for the local code repeate every number of ms
std::complex<float>* code = new std::complex<float>[nsamples_total]; // buffer for the local code
std::complex<float>* code_total = new gr_complex[vector_length]; // buffer for the local code repeate every number of ms
gr_complex* d_fft_codes_padded = static_cast<gr_complex*>(volk_gnsssdr_malloc(vector_length * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_all_fft_codes = new lv_16sc_t[vector_length * NUM_PRNs]; // memory containing all the possible fft codes for PRN 0 to 32
d_all_fft_codes = new lv_16sc_t[vector_length * NUM_PRNs]; // memory containing all the possible fft codes for PRN 0 to 32
float max; // temporary maxima search
float max; // temporary maxima search
for (unsigned int PRN = 0; PRN < NUM_PRNs; PRN++)
{
gps_l1_ca_code_gen_complex_sampled(code, PRN, fs_in, 0); // generate PRN code
gps_l1_ca_code_gen_complex_sampled(code, PRN, fs_in, 0); // generate PRN code
for (unsigned int i = 0; i < sampled_ms; i++)
{
memcpy(&(code_total[i * nsamples_total]), code, sizeof(gr_complex) * nsamples_total); // repeat for each ms
memcpy(&(code_total[i * nsamples_total]), code, sizeof(gr_complex) * nsamples_total); // repeat for each ms
}
int offset = 0;
memcpy(d_fft_if->get_inbuf() + offset, code_total, sizeof(gr_complex) * vector_length); // copy to FFT buffer
memcpy(d_fft_if->get_inbuf() + offset, code_total, sizeof(gr_complex) * vector_length); // copy to FFT buffer
d_fft_if->execute(); // Run the FFT of local code
d_fft_if->execute(); // Run the FFT of local code
volk_32fc_conjugate_32fc(d_fft_codes_padded, d_fft_if->get_outbuf(), vector_length); // conjugate values
volk_32fc_conjugate_32fc(d_fft_codes_padded, d_fft_if->get_outbuf(), vector_length); // conjugate values
max = 0; // initialize maximum value
max = 0; // initialize maximum value
for (unsigned int i = 0; i < vector_length; i++) // search for maxima
for (unsigned int i = 0; i < vector_length; i++) // search for maxima
{
if (std::abs(d_fft_codes_padded[i].real()) > max)
{
@ -150,12 +149,11 @@ gps_fpga_acquisition_8sc::gps_fpga_acquisition_8sc(std::string device_name,
}
}
for (unsigned int i = 0; i < vector_length; i++) // map the FFT to the dynamic range of the fixed point values an copy to buffer containing all FFTs
for (unsigned int i = 0; i < vector_length; i++) // map the FFT to the dynamic range of the fixed point values an copy to buffer containing all FFTs
{
d_all_fft_codes[i + vector_length * PRN] = lv_16sc_t(static_cast<int>(d_fft_codes_padded[i].real() * (pow(2, 7) - 1) / max),
static_cast<int>(d_fft_codes_padded[i].imag() * (pow(2, 7) - 1) / max));
static_cast<int>(d_fft_codes_padded[i].imag() * (pow(2, 7) - 1) / max));
}
}
// temporary buffers that we can delete
@ -201,7 +199,7 @@ void gps_fpga_acquisition_8sc::fpga_configure_acquisition_local_code(lv_16sc_t f
{
tmp = fft_local_code[k].real();
tmp2 = fft_local_code[k].imag();
local_code = (tmp & 0xFF) | ((tmp2 * 256) & 0xFF00); // put together the real part and the imaginary part
local_code = (tmp & 0xFF) | ((tmp2 * 256) & 0xFF00); // put together the real part and the imaginary part
d_map_base[4] = 0x0C000000 | (local_code & 0xFFFF);
}
}
@ -213,7 +211,7 @@ void gps_fpga_acquisition_8sc::run_acquisition(void)
int reenable = 1;
write(d_fd, reinterpret_cast<void*>(&reenable), sizeof(int));
d_map_base[5] = 0; // writing anything to reg 4 launches the acquisition process
d_map_base[5] = 0; // writing anything to reg 4 launches the acquisition process
int irq_count;
ssize_t nb;
@ -254,15 +252,15 @@ void gps_fpga_acquisition_8sc::set_phase_step(unsigned int doppler_index)
{
phase_step_rad_real = MAX_PHASE_STEP_RAD;
}
phase_step_rad_int_temp = phase_step_rad_real * 4; // * 2^2
phase_step_rad_int = (int32_t) (phase_step_rad_int_temp * (536870912)); // * 2^29 (in total it makes x2^31 in two steps to avoid the warnings
phase_step_rad_int_temp = phase_step_rad_real * 4; // * 2^2
phase_step_rad_int = (int32_t)(phase_step_rad_int_temp * (536870912)); // * 2^29 (in total it makes x2^31 in two steps to avoid the warnings
d_map_base[3] = phase_step_rad_int;
}
void gps_fpga_acquisition_8sc::read_acquisition_results(uint32_t* max_index,
float* max_magnitude, unsigned *initial_sample, float *power_sum)
float* max_magnitude, unsigned* initial_sample, float* power_sum)
{
unsigned readval = 0;
readval = d_map_base[0];
@ -279,26 +277,25 @@ void gps_fpga_acquisition_8sc::read_acquisition_results(uint32_t* max_index,
void gps_fpga_acquisition_8sc::block_samples()
{
d_map_base[14] = 1; // block the samples
d_map_base[14] = 1; // block the samples
}
void gps_fpga_acquisition_8sc::unblock_samples()
{
d_map_base[14] = 0; // unblock the samples
d_map_base[14] = 0; // unblock the samples
}
void gps_fpga_acquisition_8sc::open_device()
{
if ((d_fd = open(d_device_name.c_str(), O_RDWR | O_SYNC)) == -1)
{
LOG(WARNING) << "Cannot open deviceio" << d_device_name;
}
d_map_base = reinterpret_cast<volatile unsigned *>(mmap(NULL, PAGE_SIZE,
PROT_READ | PROT_WRITE, MAP_SHARED, d_fd, 0));
d_map_base = reinterpret_cast<volatile unsigned*>(mmap(NULL, PAGE_SIZE,
PROT_READ | PROT_WRITE, MAP_SHARED, d_fd, 0));
if (d_map_base == reinterpret_cast<void*>(-1))
{
@ -328,11 +325,10 @@ void gps_fpga_acquisition_8sc::open_device()
void gps_fpga_acquisition_8sc::close_device()
{
unsigned * aux = const_cast<unsigned*>(d_map_base);
unsigned* aux = const_cast<unsigned*>(d_map_base);
if (munmap(static_cast<void*>(aux), PAGE_SIZE) == -1)
{
printf("Failed to unmap memory uio\n");
}
close(d_fd);
}

38
src/algorithms/acquisition/libs/gps_fpga_acquisition_8sc.h
View File

@ -48,16 +48,18 @@ class gps_fpga_acquisition_8sc
{
public:
gps_fpga_acquisition_8sc(std::string device_name,
unsigned int vector_length, unsigned int nsamples,
unsigned int nsamples_total, long fs_in, long freq,
unsigned int sampled_ms, unsigned select_queue);
~gps_fpga_acquisition_8sc();bool init();bool set_local_code(
unsigned int PRN); //int code_length_chips, const lv_16sc_t* local_code_in, float *shifts_chips);
unsigned int vector_length, unsigned int nsamples,
unsigned int nsamples_total, long fs_in, long freq,
unsigned int sampled_ms, unsigned select_queue);
~gps_fpga_acquisition_8sc();
bool init();
bool set_local_code(
unsigned int PRN); //int code_length_chips, const lv_16sc_t* local_code_in, float *shifts_chips);
bool free();
void run_acquisition(void);
void set_phase_step(unsigned int doppler_index);
void read_acquisition_results(uint32_t* max_index, float* max_magnitude,
unsigned *initial_sample, float *power_sum);
void read_acquisition_results(uint32_t *max_index, float *max_magnitude,
unsigned *initial_sample, float *power_sum);
void block_samples();
void unblock_samples();
void open_device();
@ -82,27 +84,25 @@ public:
}
private:
long d_freq;
long d_fs_in;
gr::fft::fft_complex* d_fft_if; // function used to run the fft of the local codes
gr::fft::fft_complex *d_fft_if; // function used to run the fft of the local codes
// data related to the hardware module and the driver
int d_fd; // driver descriptor
volatile unsigned *d_map_base; // driver memory map
lv_16sc_t *d_all_fft_codes; // memory that contains all the code ffts
unsigned int d_vector_length; // number of samples incluing padding and number of ms
unsigned int d_nsamples; // number of samples not including padding
unsigned int d_select_queue; // queue selection
std::string d_device_name; // HW device name
unsigned int d_doppler_max; // max doppler
unsigned int d_doppler_step; // doppler step
int d_fd; // driver descriptor
volatile unsigned *d_map_base; // driver memory map
lv_16sc_t *d_all_fft_codes; // memory that contains all the code ffts
unsigned int d_vector_length; // number of samples incluing padding and number of ms
unsigned int d_nsamples; // number of samples not including padding
unsigned int d_select_queue; // queue selection
std::string d_device_name; // HW device name
unsigned int d_doppler_max; // max doppler
unsigned int d_doppler_step; // doppler step
// FPGA private functions
unsigned fpga_acquisition_test_register(unsigned writeval);
void fpga_configure_acquisition_local_code(lv_16sc_t fft_local_code[]);
void configure_acquisition();
};
#endif /* GNSS_SDR_FPGA_MULTICORRELATOR_H_ */

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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