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