INSTINCT Code Coverage Report

Directory: src/
File: Nodes/Converter/GNSS/UbloxGnssOrbitCollector.hpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 4 4 100.0%
Functions: 1 1 100.0%
Branches: 0 0 -%
Line Branch Exec Source
1 // This file is part of INSTINCT, the INS Toolkit for Integrated
2 // Navigation Concepts and Training by the Institute of Navigation of
3 // the University of Stuttgart, Germany.
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9 /// @file UbloxGnssOrbitCollector.hpp
10 /// @brief Collects UBX-RXM-SFRBX messages and provides the Orbit information
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2024-02-05
13
14 #pragma once
15
16 #include <mutex>
17 #include <bitset>
18 #include <memory>
19 #include <unordered_map>
20 #include <set>
21
22 #include "internal/Node/Node.hpp"
23 #include "NodeData/GNSS/GnssNavInfo.hpp"
24 #include "NodeData/GNSS/UbloxObs.hpp"
25
26 #include "Navigation/GNSS/Satellite/internal/SatNavData.hpp"
27
28 namespace ubx = NAV::vendor::ublox;
29
30 namespace NAV
31 {
32 /// Collects UBX-RXM-SFRBX messages and provides the Orbit information
33 class UbloxGnssOrbitCollector : public Node
34 {
35 public:
36 /// @brief Default constructor
37 UbloxGnssOrbitCollector();
38 /// @brief Destructor
39 ~UbloxGnssOrbitCollector() override;
40 /// @brief Copy constructor
41 UbloxGnssOrbitCollector(const UbloxGnssOrbitCollector&) = delete;
42 /// @brief Move constructor
43 UbloxGnssOrbitCollector(UbloxGnssOrbitCollector&&) = delete;
44 /// @brief Copy assignment operator
45 UbloxGnssOrbitCollector& operator=(const UbloxGnssOrbitCollector&) = delete;
46 /// @brief Move assignment operator
47 UbloxGnssOrbitCollector& operator=(UbloxGnssOrbitCollector&&) = delete;
48
49 /// @brief String representation of the Class Type
50 [[nodiscard]] static std::string typeStatic();
51
52 /// @brief String representation of the Class Type
53 [[nodiscard]] std::string type() const override;
54
55 /// @brief String representation of the Class Category
56 [[nodiscard]] static std::string category();
57
58 private:
59 constexpr static size_t INPUT_PORT_INDEX_UBLOX_OBS = 0; ///< @brief Flow (UbloxObs)
60 constexpr static size_t OUTPUT_PORT_INDEX_GNSS_NAV_INFO = 0; ///< @brief Object
61
62 /// @brief Initialize the node
63 bool initialize() override;
64
65 /// @brief Called when a link is to be deleted
66 /// @param[in] startPin Pin where the link starts
67 /// @param[in] endPin Pin where the link ends
68 void onDeleteLink(OutputPin& startPin, InputPin& endPin) override;
69
70 /// @brief Data object to share over the output pin
71 GnssNavInfo _gnssNavInfo;
72
73 /// Mutex to lock if the connected ublox obs provider is a file reader
74 std::optional<std::unique_lock<std::mutex>> _postProcessingLock;
75
76 /// @brief Ephemeris builder to store unfinished ephemeris data till all subframes are collected
77 struct EphemerisBuilder
78 {
79 /// @brief Constructor
80 /// @param[in] satId Satellite identifier
81 /// @param[in] navData Nav data to store
82 47 EphemerisBuilder(const SatId& satId, std::shared_ptr<SatNavData> navData)
83 47 : satId(satId), navData(std::move(navData))
84 {
85 47 subframes.reset();
86 47 }
87
88 SatId satId; ///< Satellite Identifier
89 std::shared_ptr<SatNavData> navData; ///< Navigation data pointer
90 std::bitset<5> subframes; ///< Flags for which subframes were received already. e.g. Subframes 1, 2, 3 for GPS
91 };
92
93 /// @brief Map of ephemeris build helper for each satellite
94 std::vector<EphemerisBuilder> _ephemerisBuilder;
95
96 /// @brief List of IOD for each satellite
97 std::unordered_map<SatId, size_t> _lastAccessedBuilder;
98
99 /// List of satellite systems to emit warnings because conversion is not implemented yet
100 std::set<SatelliteSystem> _warningsNotImplemented;
101
102 /// @brief Searches the ephemeris builder for the given satellite and time. If nothing found returns a new instance.
103 /// @param[in] satId Satellite identifier
104 /// @param[in] insTime Time to search for (either time of ephemeris or time of clock)
105 /// @param[in] IOD Issue of Data (Ephemeris, Nav, ...)
106 /// @return Reference to the ephemeris builder
107 EphemerisBuilder& getEphemerisBuilder(const SatId& satId, const InsTime& insTime, size_t IOD = 0);
108
109 /// @brief Searches the ephemeris builder for the given Issue of Data Ephemeris
110 /// @param[in] satId Satellite Identifier
111 /// @param[in] IOD Issue of Data (Ephemeris, Nav, ...)
112 /// @return Reference to the ephemeris builder if it was found
113 std::optional<std::reference_wrapper<EphemerisBuilder>> getEphemerisBuilder(const SatId& satId, size_t IOD);
114
115 /// @brief Searches the most recent ephemeris builder for the given satellite
116 /// @param[in] satId Satellite identifier
117 /// @return Reference to the ephemeris builder if it was found
118 std::optional<std::reference_wrapper<EphemerisBuilder>> getLastEphemerisBuilder(const SatId& satId);
119
120 /// @brief Data receive function
121 /// @param[in] queue Queue with all the received data messages
122 /// @param[in] pinIdx Index of the pin the data is received on
123 void receiveObs(InputPin::NodeDataQueue& queue, size_t pinIdx);
124
125 /// @brief Decrypt the GPS SFRBX message
126 /// @param[in] satId Satellite Identifier
127 /// @param[in] sfrbx RAWX-SFRBX message
128 /// @param[in] insTime Time of the message
129 void decryptGPS(const SatId& satId, const ubx::UbxRxmSfrbx& sfrbx, const InsTime& insTime);
130
131 /// @brief Decrypt the Galileo SFRBX message
132 /// @param[in] satId Satellite Identifier
133 /// @param[in] sfrbx RAWX-SFRBX message
134 /// @param[in] insTime Time of the message
135 void decryptGalileo(const SatId& satId, const ubx::UbxRxmSfrbx& sfrbx, const InsTime& insTime);
136
137 /// @brief Decrypt the GLONASS SFRBX message
138 /// @param[in] satId Satellite Identifier
139 /// @param[in] sfrbx RAWX-SFRBX message
140 /// @param[in] insTime Time of the message
141 void decryptGLONASS(const SatId& satId, const ubx::UbxRxmSfrbx& sfrbx, const InsTime& insTime);
142
143 /// @brief Decrypt the BeiDou SFRBX message
144 /// @param[in] satId Satellite Identifier
145 /// @param[in] sfrbx RAWX-SFRBX message
146 /// @param[in] insTime Time of the message
147 void decryptBeiDou(const SatId& satId, const ubx::UbxRxmSfrbx& sfrbx, const InsTime& insTime);
148
149 /// @brief Decrypt the QZSS SFRBX message
150 /// @param[in] satId Satellite Identifier
151 /// @param[in] sfrbx RAWX-SFRBX message
152 /// @param[in] insTime Time of the message
153 void decryptQZSS(const SatId& satId, const ubx::UbxRxmSfrbx& sfrbx, const InsTime& insTime);
154
155 /// @brief Decrypt the IRNSS SFRBX message
156 /// @param[in] satId Satellite Identifier
157 /// @param[in] sfrbx RAWX-SFRBX message
158 /// @param[in] insTime Time of the message
159 void decryptIRNSS(const SatId& satId, const ubx::UbxRxmSfrbx& sfrbx, const InsTime& insTime);
160
161 /// @brief Decrypt the SBAS SFRBX message
162 /// @param[in] satId Satellite Identifier
163 /// @param[in] sfrbx RAWX-SFRBX message
164 /// @param[in] insTime Time of the message
165 void decryptSBAS(const SatId& satId, const ubx::UbxRxmSfrbx& sfrbx, const InsTime& insTime);
166 };
167
168 } // namespace NAV
169