| 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 GnssObs.hpp | ||
| 10 | /// @brief GNSS Observation messages | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2022-04-26 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <cstdint> | ||
| 17 | #include <limits> | ||
| 18 | #include <optional> | ||
| 19 | #include <vector> | ||
| 20 | #include <algorithm> | ||
| 21 | #include <Eigen/Core> | ||
| 22 | |||
| 23 | #include "NodeData/NodeData.hpp" | ||
| 24 | |||
| 25 | #include "Navigation/GNSS/Core/SatelliteIdentifier.hpp" | ||
| 26 | #include "Navigation/GNSS/Core/Code.hpp" | ||
| 27 | #include "util/Assert.h" | ||
| 28 | |||
| 29 | namespace NAV | ||
| 30 | { | ||
| 31 | /// GNSS Observation message information | ||
| 32 | class GnssObs : public NodeData | ||
| 33 | { | ||
| 34 | public: | ||
| 35 | /// @brief Observation types | ||
| 36 | enum ObservationType : uint8_t | ||
| 37 | { | ||
| 38 | Pseudorange, ///< Pseudorange | ||
| 39 | Carrier, ///< Carrier-Phase | ||
| 40 | Doppler, ///< Doppler (Pseudorange rate) | ||
| 41 | ObservationType_COUNT, ///< Count | ||
| 42 | }; | ||
| 43 | |||
| 44 | /// @brief Stores the satellites observations | ||
| 45 | struct ObservationData | ||
| 46 | { | ||
| 47 | /// Pseudorange | ||
| 48 | struct Pseudorange | ||
| 49 | { | ||
| 50 | /// Pseudorange measurement [m] | ||
| 51 | double value = 0.0; | ||
| 52 | |||
| 53 | /// @brief Signal Strength Indicator (SSI) projected into interval 1-9 | ||
| 54 | /// | ||
| 55 | /// Carrier to Noise ratio(dbHz) | Carrier to Noise ratio(RINEX) | ||
| 56 | /// :-: | --- | ||
| 57 | /// - | 0 or blank: not known, don't care | ||
| 58 | /// < 12 | 1 (minimum possible signal strength) | ||
| 59 | /// 12-17 | 2 | ||
| 60 | /// 18-23 | 3 | ||
| 61 | /// 24-29 | 4 | ||
| 62 | /// 30-35 | 5 (threshold for good tracking) | ||
| 63 | /// 36-41 | 6 | ||
| 64 | /// 42-47 | 7 | ||
| 65 | /// 48-53 | 8 | ||
| 66 | /// ≥ 54 | 9 (maximum possible signal strength) | ||
| 67 | uint8_t SSI = 0; | ||
| 68 | }; | ||
| 69 | |||
| 70 | /// Carrier phase | ||
| 71 | struct CarrierPhase | ||
| 72 | { | ||
| 73 | /// Carrier phase measurement [cycles] | ||
| 74 | double value = 0.0; | ||
| 75 | |||
| 76 | /// @brief Signal Strength Indicator (SSI) projected into interval 1-9 | ||
| 77 | /// | ||
| 78 | /// Carrier to Noise ratio(dbHz) | Carrier to Noise ratio(RINEX) | ||
| 79 | /// :-: | --- | ||
| 80 | /// - | 0 or blank: not known, don't care | ||
| 81 | /// < 12 | 1 (minimum possible signal strength) | ||
| 82 | /// 12-17 | 2 | ||
| 83 | /// 18-23 | 3 | ||
| 84 | /// 24-29 | 4 | ||
| 85 | /// 30-35 | 5 (threshold for good tracking) | ||
| 86 | /// 36-41 | 6 | ||
| 87 | /// 42-47 | 7 | ||
| 88 | /// 48-53 | 8 | ||
| 89 | /// ≥ 54 | 9 (maximum possible signal strength) | ||
| 90 | uint8_t SSI = 0; | ||
| 91 | |||
| 92 | /// Loss of Lock Indicator [0...6] (only associated with the phase observation) | ||
| 93 | uint8_t LLI = 0; | ||
| 94 | }; | ||
| 95 | |||
| 96 | /// @brief Constructor | ||
| 97 | /// @param[in] satSigId Satellite signal identifier (frequency and satellite number) | ||
| 98 | 522246 | explicit ObservationData(const SatSigId& satSigId) : satSigId(satSigId) {} | |
| 99 | |||
| 100 | #ifdef TESTING | ||
| 101 | /// @brief Constructor | ||
| 102 | /// @param[in] satSigId Satellite signal identifier (frequency and satellite number) | ||
| 103 | /// @param[in] pseudorange Pseudorange measurement [m] and Signal Strength Indicator (SSI) | ||
| 104 | /// @param[in] carrierPhase Carrier phase measurement [cycles], Signal Strength Indicator (SSI) and Loss of Lock Indicator (LLI) | ||
| 105 | /// @param[in] doppler Doppler measurement [Hz] | ||
| 106 | /// @param[in] CN0 Carrier-to-Noise density [dBHz] | ||
| 107 | 23672 | ObservationData(const SatSigId& satSigId, | |
| 108 | std::optional<Pseudorange> pseudorange, | ||
| 109 | std::optional<CarrierPhase> carrierPhase, | ||
| 110 | std::optional<double> doppler, | ||
| 111 | std::optional<double> CN0) | ||
| 112 | 23672 | : satSigId(satSigId), | |
| 113 | 23672 | pseudorange(pseudorange), | |
| 114 | 23672 | carrierPhase(carrierPhase), | |
| 115 | 23672 | doppler(doppler), | |
| 116 | 23672 | CN0(CN0) | |
| 117 | 23672 | {} | |
| 118 | #endif | ||
| 119 | |||
| 120 | SatSigId satSigId = { Code::None, 0 }; ///< SignalId and satellite number | ||
| 121 | std::optional<Pseudorange> pseudorange; ///< Pseudorange measurement [m] | ||
| 122 | std::optional<CarrierPhase> carrierPhase; ///< Carrier phase measurement [cycles] | ||
| 123 | std::optional<double> doppler; ///< Doppler measurement [Hz] | ||
| 124 | std::optional<double> CN0; ///< Carrier-to-Noise density [dBHz] | ||
| 125 | }; | ||
| 126 | |||
| 127 | /// @brief Useful information of the satellites | ||
| 128 | struct SatelliteData | ||
| 129 | { | ||
| 130 | SatId satId; ///< Satellite identifier | ||
| 131 | Frequency frequencies = Freq_None; ///< Frequencies transmitted by this satellite | ||
| 132 | }; | ||
| 133 | |||
| 134 | #ifdef TESTING | ||
| 135 | /// Default constructor | ||
| 136 | 9045 | GnssObs() = default; | |
| 137 | |||
| 138 | /// @brief Constructor | ||
| 139 | /// @param[in] insTime Epoch time | ||
| 140 | /// @param[in] data Observation data | ||
| 141 | /// @param[in] satData Satellite data | ||
| 142 | 807 | GnssObs(const InsTime& insTime, std::vector<ObservationData> data, std::vector<SatelliteData> satData) | |
| 143 | 807 | : data(std::move(data)), _satData(std::move(satData)) | |
| 144 | { | ||
| 145 | 807 | this->insTime = insTime; | |
| 146 | 807 | } | |
| 147 | #endif | ||
| 148 | /// @brief Returns the type of the data class | ||
| 149 | /// @return The data type | ||
| 150 | 583140 | [[nodiscard]] static std::string type() | |
| 151 | { | ||
| 152 |
1/2✓ Branch 1 taken 583140 times.
✗ Branch 2 not taken.
|
1166280 | return "GnssObs"; |
| 153 | } | ||
| 154 | |||
| 155 | /// @brief Returns the type of the data class | ||
| 156 | /// @return The data type | ||
| 157 | ✗ | [[nodiscard]] std::string getType() const override { return type(); } | |
| 158 | |||
| 159 | /// @brief Returns the parent types of the data class | ||
| 160 | /// @return The parent data types | ||
| 161 | 112 | [[nodiscard]] static std::vector<std::string> parentTypes() | |
| 162 | { | ||
| 163 |
3/6✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 112 times.
✓ Branch 4 taken 112 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
336 | return { NodeData::type() }; |
| 164 | 112 | } | |
| 165 | |||
| 166 | /// @brief Satellite observations | ||
| 167 | std::vector<ObservationData> data; | ||
| 168 | |||
| 169 | /// @brief Access or insert the satellite data | ||
| 170 | /// @param satId Satellite identifier | ||
| 171 | /// @return The satellite data | ||
| 172 | 1875890 | SatelliteData& satData(const SatId& satId) | |
| 173 | { | ||
| 174 |
1/2✓ Branch 1 taken 1875923 times.
✗ Branch 2 not taken.
|
1875890 | auto iter = std::ranges::find_if(_satData, [&satId](const SatelliteData& sat) { |
| 175 | 33280141 | return sat.satId == satId; | |
| 176 | }); | ||
| 177 |
2/2✓ Branch 2 taken 1618027 times.
✓ Branch 3 taken 258004 times.
|
1875923 | if (iter != _satData.end()) |
| 178 | { | ||
| 179 | 1618027 | return *iter; | |
| 180 | } | ||
| 181 | |||
| 182 |
1/2✓ Branch 1 taken 257985 times.
✗ Branch 2 not taken.
|
258004 | _satData.emplace_back(); |
| 183 | 257985 | _satData.back().satId = satId; | |
| 184 | 257990 | return _satData.back(); | |
| 185 | } | ||
| 186 | |||
| 187 | /// @brief Access the satellite data | ||
| 188 | /// @param satId Satellite identifier | ||
| 189 | /// @return The satellite data if in the list | ||
| 190 | [[nodiscard]] std::optional<std::reference_wrapper<const SatelliteData>> satData(const SatId& satId) const | ||
| 191 | { | ||
| 192 | auto iter = std::ranges::find_if(_satData, [&satId](const SatelliteData& sat) { | ||
| 193 | return sat.satId == satId; | ||
| 194 | }); | ||
| 195 | if (iter != _satData.end()) | ||
| 196 | { | ||
| 197 | return *iter; | ||
| 198 | } | ||
| 199 | return std::nullopt; | ||
| 200 | } | ||
| 201 | |||
| 202 | /// @brief Checks if an element with the identifier exists | ||
| 203 | /// @param[in] satSigId Signal id | ||
| 204 | /// @return True if the element exists | ||
| 205 | 14221972 | [[nodiscard]] bool contains(const SatSigId& satSigId) const | |
| 206 | { | ||
| 207 |
1/2✓ Branch 1 taken 14221300 times.
✗ Branch 2 not taken.
|
14221972 | auto iter = std::ranges::find_if(data, [&satSigId](const ObservationData& idData) { |
| 208 | 700897311 | return idData.satSigId == satSigId; | |
| 209 | }); | ||
| 210 | 14221300 | return iter != data.end(); | |
| 211 | } | ||
| 212 | |||
| 213 | /// @brief Return the element with the identifier or a newly constructed one if it did not exist | ||
| 214 | /// @param[in] satSigId Signal id | ||
| 215 | /// @return The element found in the observations or a newly constructed one | ||
| 216 | 1846786 | ObservationData& operator()(const SatSigId& satSigId) | |
| 217 | { | ||
| 218 |
1/2✓ Branch 1 taken 1847268 times.
✗ Branch 2 not taken.
|
1846786 | auto iter = std::ranges::find_if(data, [&satSigId](const ObservationData& idData) { |
| 219 | 86940988 | return idData.satSigId == satSigId; | |
| 220 | }); | ||
| 221 |
2/2✓ Branch 2 taken 1353799 times.
✓ Branch 3 taken 493590 times.
|
1847268 | if (iter != data.end()) |
| 222 | { | ||
| 223 | 1353799 | return *iter; | |
| 224 | } | ||
| 225 | |||
| 226 |
1/2✓ Branch 1 taken 493559 times.
✗ Branch 2 not taken.
|
493590 | data.emplace_back(satSigId); |
| 227 | 493559 | return data.back(); | |
| 228 | } | ||
| 229 | |||
| 230 | /// @brief Return the element with the identifier | ||
| 231 | /// @param[in] satSigId Signal id | ||
| 232 | /// @return The element found in the observations | ||
| 233 | ✗ | [[nodiscard]] std::optional<std::reference_wrapper<const ObservationData>> operator()(const SatSigId& satSigId) const | |
| 234 | { | ||
| 235 | ✗ | auto iter = std::ranges::find_if(data, [&satSigId](const ObservationData& idData) { | |
| 236 | ✗ | return idData.satSigId == satSigId; | |
| 237 | }); | ||
| 238 | |||
| 239 | ✗ | if (iter != data.end()) | |
| 240 | { | ||
| 241 | ✗ | return *iter; | |
| 242 | } | ||
| 243 | ✗ | return std::nullopt; | |
| 244 | } | ||
| 245 | |||
| 246 | /// @brief Useful information of the satellites | ||
| 247 | 270766 | [[nodiscard]] const std::vector<SatelliteData>& getSatData() const { return _satData; } | |
| 248 | |||
| 249 | /// @brief Returns a vector of data descriptors for the dynamic data | ||
| 250 | ✗ | [[nodiscard]] std::vector<std::string> dynamicDataDescriptors() const override | |
| 251 | { | ||
| 252 | ✗ | std::vector<std::string> descriptors; | |
| 253 | ✗ | descriptors.reserve(data.size() * 7); | |
| 254 | |||
| 255 | ✗ | for (const auto& obsData : data) | |
| 256 | { | ||
| 257 | ✗ | descriptors.push_back(fmt::format("{} Pseudorange [m]", obsData.satSigId)); | |
| 258 | ✗ | descriptors.push_back(fmt::format("{} Pseudorange SSI", obsData.satSigId)); | |
| 259 | |||
| 260 | ✗ | descriptors.push_back(fmt::format("{} Carrier-phase [cycles]", obsData.satSigId)); | |
| 261 | ✗ | descriptors.push_back(fmt::format("{} Carrier-phase SSI", obsData.satSigId)); | |
| 262 | ✗ | descriptors.push_back(fmt::format("{} Carrier-phase LLI", obsData.satSigId)); | |
| 263 | |||
| 264 | ✗ | descriptors.push_back(fmt::format("{} Doppler [Hz]", obsData.satSigId)); | |
| 265 | ✗ | descriptors.push_back(fmt::format("{} Carrier-to-Noise density [dBHz]", obsData.satSigId)); | |
| 266 | } | ||
| 267 | |||
| 268 | ✗ | return descriptors; | |
| 269 | ✗ | } | |
| 270 | |||
| 271 | /// @brief Get the value for the descriptor | ||
| 272 | /// @return Value if in the observation | ||
| 273 | ✗ | [[nodiscard]] std::optional<double> getDynamicDataAt(const std::string& descriptor) const override | |
| 274 | { | ||
| 275 | ✗ | for (const auto& obsData : data) | |
| 276 | { | ||
| 277 | ✗ | if (descriptor == fmt::format("{} Pseudorange [m]", obsData.satSigId) && obsData.pseudorange) | |
| 278 | { | ||
| 279 | ✗ | return obsData.pseudorange->value; | |
| 280 | } | ||
| 281 | ✗ | if (descriptor == fmt::format("{} Pseudorange SSI", obsData.satSigId) && obsData.pseudorange) | |
| 282 | { | ||
| 283 | ✗ | return obsData.pseudorange->SSI; | |
| 284 | } | ||
| 285 | ✗ | if (descriptor == fmt::format("{} Carrier-phase [cycles]", obsData.satSigId) && obsData.carrierPhase) | |
| 286 | { | ||
| 287 | ✗ | return obsData.carrierPhase->value; | |
| 288 | } | ||
| 289 | ✗ | if (descriptor == fmt::format("{} Carrier-phase SSI", obsData.satSigId) && obsData.carrierPhase) | |
| 290 | { | ||
| 291 | ✗ | return obsData.carrierPhase->SSI; | |
| 292 | } | ||
| 293 | ✗ | if (descriptor == fmt::format("{} Carrier-phase LLI", obsData.satSigId) && obsData.carrierPhase) | |
| 294 | { | ||
| 295 | ✗ | return obsData.carrierPhase->LLI; | |
| 296 | } | ||
| 297 | ✗ | if (descriptor == fmt::format("{} Doppler [Hz]", obsData.satSigId)) | |
| 298 | { | ||
| 299 | ✗ | return obsData.doppler; | |
| 300 | } | ||
| 301 | ✗ | if (descriptor == fmt::format("{} Carrier-to-Noise density [dBHz]", obsData.satSigId)) | |
| 302 | { | ||
| 303 | ✗ | return obsData.CN0; | |
| 304 | } | ||
| 305 | } | ||
| 306 | ✗ | return std::nullopt; | |
| 307 | } | ||
| 308 | |||
| 309 | /// @brief Returns a vector of data descriptors and values for the dynamic data | ||
| 310 | ✗ | [[nodiscard]] std::vector<std::pair<std::string, double>> getDynamicData() const override | |
| 311 | { | ||
| 312 | ✗ | std::vector<std::pair<std::string, double>> dynData; | |
| 313 | ✗ | dynData.reserve(data.size() * 7); | |
| 314 | ✗ | for (const auto& obsData : data) | |
| 315 | { | ||
| 316 | ✗ | if (obsData.pseudorange) { dynData.emplace_back(fmt::format("{} Pseudorange [m]", obsData.satSigId), obsData.pseudorange->value); } | |
| 317 | ✗ | if (obsData.pseudorange) { dynData.emplace_back(fmt::format("{} Pseudorange SSI", obsData.satSigId), obsData.pseudorange->SSI); } | |
| 318 | |||
| 319 | ✗ | if (obsData.carrierPhase) { dynData.emplace_back(fmt::format("{} Carrier-phase [cycles]", obsData.satSigId), obsData.carrierPhase->value); } | |
| 320 | ✗ | if (obsData.carrierPhase) { dynData.emplace_back(fmt::format("{} Carrier-phase SSI", obsData.satSigId), obsData.carrierPhase->SSI); } | |
| 321 | ✗ | if (obsData.carrierPhase) { dynData.emplace_back(fmt::format("{} Carrier-phase LLI", obsData.satSigId), obsData.carrierPhase->LLI); } | |
| 322 | |||
| 323 | ✗ | if (obsData.doppler) { dynData.emplace_back(fmt::format("{} Doppler [Hz]", obsData.satSigId), obsData.doppler.value()); } | |
| 324 | |||
| 325 | ✗ | if (obsData.CN0) { dynData.emplace_back(fmt::format("{} Carrier-to-Noise density [dBHz]", obsData.satSigId), obsData.CN0.value()); } | |
| 326 | } | ||
| 327 | ✗ | return dynData; | |
| 328 | ✗ | } | |
| 329 | |||
| 330 | /// Receiver Information, e.g. from RINEX header | ||
| 331 | struct ReceiverInfo | ||
| 332 | { | ||
| 333 | ///< Approximate receiver position in [m], e.g. from RINEX header | ||
| 334 | std::optional<Eigen::Vector3d> e_approxPos; | ||
| 335 | |||
| 336 | /// Antenna Type. Empty if unknown | ||
| 337 | std::string antennaType; | ||
| 338 | |||
| 339 | /// @brief Antenna Delta (North, East, Up) in [m] | ||
| 340 | /// | ||
| 341 | /// - Horizontal eccentricity of ARP relative to the marker (north/east) | ||
| 342 | /// - Height of the antenna reference point (ARP) above the marker | ||
| 343 | Eigen::Vector3d antennaDeltaNEU = Eigen::Vector3d::Zero(); | ||
| 344 | }; | ||
| 345 | |||
| 346 | /// Optional Receiver Information, e.g. from RINEX header | ||
| 347 | std::optional<std::reference_wrapper<ReceiverInfo>> receiverInfo; | ||
| 348 | |||
| 349 | private: | ||
| 350 | /// @brief Useful information of the satellites | ||
| 351 | std::vector<SatelliteData> _satData; | ||
| 352 | }; | ||
| 353 | |||
| 354 | /// @brief Converts the enum to a string | ||
| 355 | /// @param[in] obsType Enum value to convert into text | ||
| 356 | /// @return String representation of the enum | ||
| 357 | ✗ | constexpr const char* to_string(GnssObs::ObservationType obsType) | |
| 358 | { | ||
| 359 | ✗ | switch (obsType) | |
| 360 | { | ||
| 361 | ✗ | case GnssObs::Pseudorange: | |
| 362 | ✗ | return "Pseudorange"; | |
| 363 | ✗ | case GnssObs::Carrier: | |
| 364 | ✗ | return "Carrier"; | |
| 365 | ✗ | case GnssObs::Doppler: | |
| 366 | ✗ | return "Doppler"; | |
| 367 | ✗ | case GnssObs::ObservationType_COUNT: | |
| 368 | ✗ | return "COUNT"; | |
| 369 | } | ||
| 370 | ✗ | return ""; | |
| 371 | } | ||
| 372 | |||
| 373 | } // namespace NAV | ||
| 374 | |||
| 375 | #ifndef DOXYGEN_IGNORE | ||
| 376 | |||
| 377 | template<> | ||
| 378 | struct fmt::formatter<NAV::GnssObs::ObservationType> : fmt::formatter<const char*> | ||
| 379 | { | ||
| 380 | /// @brief Defines how to format Frequency structs | ||
| 381 | /// @param[in] obsType Struct to format | ||
| 382 | /// @param[in, out] ctx Format context | ||
| 383 | /// @return Output iterator | ||
| 384 | template<typename FormatContext> | ||
| 385 | ✗ | auto format(const NAV::GnssObs::ObservationType& obsType, FormatContext& ctx) const | |
| 386 | { | ||
| 387 | ✗ | return fmt::formatter<const char*>::format(to_string(obsType), ctx); | |
| 388 | } | ||
| 389 | }; | ||
| 390 | |||
| 391 | #endif | ||
| 392 | |||
| 393 | /// @brief Stream insertion operator overload | ||
| 394 | /// @param[in, out] os Output stream object to stream the time into | ||
| 395 | /// @param[in] obj Object to print | ||
| 396 | /// @return Returns the output stream object in order to chain stream insertions | ||
| 397 | std::ostream& operator<<(std::ostream& os, const NAV::GnssObs::ObservationType& obj); | ||
| 398 |