-
Notifications
You must be signed in to change notification settings - Fork 41
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #44 from fixposition/feature/gpgga
Feature/gpgga
- Loading branch information
Showing
22 changed files
with
848 additions
and
4 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
64 changes: 64 additions & 0 deletions
64
fixposition_driver_lib/include/fixposition_driver_lib/converter/gpgga.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,64 @@ | ||
| /** | ||
| * @file | ||
| * @brief Declaration of GpggaConverter | ||
| * | ||
| * \verbatim | ||
| * ___ ___ | ||
| * \ \ / / | ||
| * \ \/ / Fixposition AG | ||
| * / /\ \ All right reserved. | ||
| * /__/ \__\ | ||
| * \endverbatim | ||
| * | ||
| */ | ||
|
|
||
| #ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__ | ||
| #define __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__ | ||
|
|
||
| /* SYSTEM / STL */ | ||
|
|
||
| /* EXTERNAL */ | ||
|
|
||
| /* PACKAGE */ | ||
| #include <fixposition_driver_lib/converter/base_converter.hpp> | ||
| #include <fixposition_driver_lib/msg_data.hpp> | ||
| #include <fixposition_driver_lib/time_conversions.hpp> | ||
|
|
||
| namespace fixposition { | ||
|
|
||
| class GpggaConverter : public BaseAsciiConverter { | ||
| public: | ||
| using GpggaObserver = std::function<void(const GpggaData&)>; | ||
| /** | ||
| * @brief Construct a new GpggaConverter | ||
| * | ||
| */ | ||
| GpggaConverter() {} | ||
|
|
||
| ~GpggaConverter() = default; | ||
|
|
||
| /** | ||
| * @brief Take comma-delimited tokens of GPGGA message, convert to Data structs and if available, | ||
| * call observers | ||
| * Example: | ||
| * $GPGGA,151229.40,4723.54108,N,00826.88485,E,4,12,00.98,473.5,M,,,,*3A\r\n | ||
| * | ||
| * @param[in] tokens message split in tokens | ||
| */ | ||
| void ConvertTokens(const std::vector<std::string>& tokens) final; | ||
|
|
||
| /** | ||
| * @brief Add Observer to call at the end of ConvertTokens() | ||
| * | ||
| * @param[in] ob | ||
| */ | ||
| void AddObserver(GpggaObserver ob) { obs_.push_back(ob); } | ||
|
|
||
| private: | ||
| GpggaData msg_; | ||
| std::vector<GpggaObserver> obs_; | ||
| const std::string header_ = "LLH"; | ||
| static constexpr const int kSize_ = 15; | ||
| }; | ||
| } // namespace fixposition | ||
| #endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPGGA__ |
64 changes: 64 additions & 0 deletions
64
fixposition_driver_lib/include/fixposition_driver_lib/converter/gprmc.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,64 @@ | ||
| /** | ||
| * @file | ||
| * @brief Declaration of GprmcConverter | ||
| * | ||
| * \verbatim | ||
| * ___ ___ | ||
| * \ \ / / | ||
| * \ \/ / Fixposition AG | ||
| * / /\ \ All right reserved. | ||
| * /__/ \__\ | ||
| * \endverbatim | ||
| * | ||
| */ | ||
|
|
||
| #ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_GPRMC__ | ||
| #define __FIXPOSITION_DRIVER_LIB_CONVERTER_GPRMC__ | ||
|
|
||
| /* SYSTEM / STL */ | ||
|
|
||
| /* EXTERNAL */ | ||
|
|
||
| /* PACKAGE */ | ||
| #include <fixposition_driver_lib/converter/base_converter.hpp> | ||
| #include <fixposition_driver_lib/msg_data.hpp> | ||
| #include <fixposition_driver_lib/time_conversions.hpp> | ||
|
|
||
| namespace fixposition { | ||
|
|
||
| class GprmcConverter : public BaseAsciiConverter { | ||
| public: | ||
| using GprmcObserver = std::function<void(const GprmcData&)>; | ||
| /** | ||
| * @brief Construct a new GprmcConverter | ||
| * | ||
| */ | ||
| GprmcConverter() {} | ||
|
|
||
| ~GprmcConverter() = default; | ||
|
|
||
| /** | ||
| * @brief Take comma-delimited tokens of GPRMC message, convert to Data structs and if available, | ||
| * call observers | ||
| * Example: | ||
| * $GPRMC,151227.40,A,4723.54036,N,00826.88672,E,0.0,81.6,111022,,,R*7C\r\n | ||
| * | ||
| * @param[in] tokens message split in tokens | ||
| */ | ||
| void ConvertTokens(const std::vector<std::string>& tokens) final; | ||
|
|
||
| /** | ||
| * @brief Add Observer to call at the end of ConvertTokens() | ||
| * | ||
| * @param[in] ob | ||
| */ | ||
| void AddObserver(GprmcObserver ob) { obs_.push_back(ob); } | ||
|
|
||
| private: | ||
| GprmcData msg_; | ||
| std::vector<GprmcObserver> obs_; | ||
| const std::string header_ = "LLH"; | ||
| static constexpr const int kSize_ = 13; | ||
| }; | ||
| } // namespace fixposition | ||
| #endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPRMC__ |
65 changes: 65 additions & 0 deletions
65
fixposition_driver_lib/include/fixposition_driver_lib/converter/gpzda.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,65 @@ | ||
| /** | ||
| * @file | ||
| * @brief Declaration of GpzdaConverter | ||
| * | ||
| * \verbatim | ||
| * ___ ___ | ||
| * \ \ / / | ||
| * \ \/ / Fixposition AG | ||
| * / /\ \ All right reserved. | ||
| * /__/ \__\ | ||
| * \endverbatim | ||
| * | ||
| */ | ||
|
|
||
| #ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_GPZDA__ | ||
| #define __FIXPOSITION_DRIVER_LIB_CONVERTER_GPZDA__ | ||
|
|
||
| /* SYSTEM / STL */ | ||
|
|
||
| /* EXTERNAL */ | ||
|
|
||
| /* PACKAGE */ | ||
| #include <fixposition_driver_lib/converter/base_converter.hpp> | ||
| #include <fixposition_driver_lib/msg_data.hpp> | ||
| #include <fixposition_driver_lib/time_conversions.hpp> | ||
| #include <chrono> | ||
|
|
||
| namespace fixposition { | ||
|
|
||
| class GpzdaConverter : public BaseAsciiConverter { | ||
| public: | ||
| using GpzdaObserver = std::function<void(const GpzdaData&)>; | ||
| /** | ||
| * @brief Construct a new GpzdaConverter | ||
| * | ||
| */ | ||
| GpzdaConverter() {} | ||
|
|
||
| ~GpzdaConverter() = default; | ||
|
|
||
| /** | ||
| * @brief Take comma-delimited tokens of GPZDA message, convert to Data structs and if available, | ||
| * call observers | ||
| * Example: | ||
| * $GPZDA,090411.0001,10,10,2023,00,00*69\r\n | ||
| * | ||
| * @param[in] tokens message split in tokens | ||
| */ | ||
| void ConvertTokens(const std::vector<std::string>& tokens) final; | ||
|
|
||
| /** | ||
| * @brief Add Observer to call at the end of ConvertTokens() | ||
| * | ||
| * @param[in] ob | ||
| */ | ||
| void AddObserver(GpzdaObserver ob) { obs_.push_back(ob); } | ||
|
|
||
| private: | ||
| GpzdaData msg_; | ||
| std::vector<GpzdaObserver> obs_; | ||
| const std::string header_ = "LLH"; | ||
| static constexpr const int kSize_ = 7; | ||
| }; | ||
| } // namespace fixposition | ||
| #endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_GPZDA__ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,82 @@ | ||
| /** | ||
| * @file | ||
| * @brief Implementation of GpggaConverter converter | ||
| * | ||
| * \verbatim | ||
| * ___ ___ | ||
| * \ \ / / | ||
| * \ \/ / Fixposition AG | ||
| * / /\ \ All right reserved. | ||
| * /__/ \__\ | ||
| * \endverbatim | ||
| * | ||
| */ | ||
|
|
||
| /* SYSTEM / STL */ | ||
| #include <iostream> | ||
|
|
||
| /* PACKAGE */ | ||
| #include <fixposition_driver_lib/converter/gpgga.hpp> | ||
|
|
||
| namespace fixposition { | ||
|
|
||
| /// msg field indices | ||
| static constexpr const int time_idx = 1; | ||
| static constexpr const int lat_idx = 2; | ||
| static constexpr const int lat_ns_idx = 3; | ||
| static constexpr const int lon_idx = 4; | ||
| static constexpr const int lon_ew_idx = 5; | ||
| static constexpr const int quality_idx = 6; | ||
| static constexpr const int num_sv_idx = 7; | ||
| static constexpr const int hdop_idx = 8; | ||
| static constexpr const int alt_idx = 9; | ||
| static constexpr const int alt_unit_idx = 10; | ||
| static constexpr const int sep_idx = 11; | ||
| static constexpr const int sep_unit_idx = 12; | ||
| static constexpr const int diff_age_idx = 13; | ||
| static constexpr const int diff_sta_idx = 14; | ||
|
|
||
| void GpggaConverter::ConvertTokens(const std::vector<std::string>& tokens) { | ||
| // Check if message size is wrong | ||
| bool ok = tokens.size() == kSize_; | ||
| if (!ok) { | ||
| std::cout << "Error in parsing GPGGA string with " << tokens.size() << " fields! GPGGA message will be empty.\n"; | ||
| msg_ = GpggaData(); | ||
| return; | ||
| } | ||
|
|
||
| // Header stamps | ||
| msg_.time = tokens.at(time_idx); | ||
|
|
||
| // LLH coordinates | ||
| const std::string _latstr = tokens.at(lat_idx); | ||
| double _lat = StringToDouble(_latstr.substr(0,2)) + StringToDouble((_latstr.substr(2))) / 60; | ||
| if (tokens.at(lat_ns_idx).compare("S") == 0) _lat *= -1; | ||
| msg_.latitude = _lat; | ||
|
|
||
| const std::string _lonstr = tokens.at(lon_idx); | ||
| double _lon = StringToDouble(_lonstr.substr(0,3)) + StringToDouble((_lonstr.substr(3))) / 60; | ||
| if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1; | ||
| msg_.longitude = _lon; | ||
|
|
||
| msg_.altitude = StringToDouble(tokens.at(alt_idx)); | ||
|
|
||
| // Covariance diagonals | ||
| const double hdop = StringToDouble(tokens.at(hdop_idx)); | ||
| msg_.cov(0, 0) = hdop * hdop; | ||
| msg_.cov(1, 1) = hdop * hdop; | ||
| msg_.cov(2, 2) = 4 * hdop * hdop; | ||
|
|
||
| // Rest of covariance fields | ||
| msg_.cov(0, 1) = msg_.cov(1, 0) = 0.0; | ||
| msg_.cov(0, 2) = msg_.cov(2, 0) = 0.0; | ||
| msg_.cov(1, 2) = msg_.cov(2, 1) = 0.0; | ||
| msg_.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED | ||
|
|
||
| // Process all observers | ||
| for (auto& ob : obs_) { | ||
| ob(msg_); | ||
| } | ||
| } | ||
|
|
||
| } // namespace fixposition |
Oops, something went wrong.