INSTINCT Code Coverage Report

Directory: src/
File: Nodes/Converter/GNSS/UbloxGnssOrbitCollector.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 535 608 88.0%
Functions: 16 22 72.7%
Branches: 547 1186 46.1%
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 #include "UbloxGnssOrbitCollector.hpp"
10
11 #include <chrono>
12
13 #include "util/Logger.hpp"
14 #include "util/Container/STL.hpp"
15
16 #include "internal/NodeManager.hpp"
17 namespace nm = NAV::NodeManager;
18 #include "internal/FlowManager.hpp"
19
20 #include "Navigation/GNSS/Satellite/Ephemeris/GPSEphemeris.hpp"
21 #include "Navigation/GNSS/Satellite/Ephemeris/GalileoEphemeris.hpp"
22 #include "Navigation/GNSS/Satellite/Ephemeris/GLONASSEphemeris.hpp"
23 #include "Navigation/GNSS/Satellite/Ephemeris/BDSEphemeris.hpp"
24 #include "Navigation/GNSS/Functions.hpp"
25 #include "Navigation/Transformations/Units.hpp"
26
27 113 NAV::UbloxGnssOrbitCollector::UbloxGnssOrbitCollector()
28
2/4
✓ Branch 1 taken 113 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 113 times.
✗ Branch 5 not taken.
 113 : Node(typeStatic())
29 {
30 LOG_TRACE("{}: called", name);
31 113 _hasConfig = false;
32
33
4/8
✓ Branch 1 taken 113 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 113 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 113 times.
✓ Branch 9 taken 113 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 339 nm::CreateInputPin(this, "UbloxObs", Pin::Type::Flow, { NAV::UbloxObs::type() }, &UbloxGnssOrbitCollector::receiveObs);
34
35
5/10
✓ Branch 2 taken 113 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 113 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 113 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 113 times.
✓ Branch 15 taken 113 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 452 nm::CreateOutputPin(this, GnssNavInfo::type().c_str(), Pin::Type::Object, { GnssNavInfo::type() }, &_gnssNavInfo);
36 339 }
37
38 228 NAV::UbloxGnssOrbitCollector::~UbloxGnssOrbitCollector()
39 {
40 LOG_TRACE("{}: called", nameId());
41 228 }
42
43 225 std::string NAV::UbloxGnssOrbitCollector::typeStatic()
44 {
45
1/2
✓ Branch 1 taken 225 times.
✗ Branch 2 not taken.
 450 return "UbloxGnssOrbitCollector";
46 }
47
48 ✗ std::string NAV::UbloxGnssOrbitCollector::type() const
49 {
50 ✗ return typeStatic();
51 }
52
53 112 std::string NAV::UbloxGnssOrbitCollector::category()
54 {
55
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 return "Converter";
56 }
57
58 3 bool NAV::UbloxGnssOrbitCollector::initialize()
59 {
60 LOG_TRACE("{}: called", nameId());
61
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 2 times.
 3 if (!_postProcessingLock.has_value())
62 {
63
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 auto guard = requestOutputValueLock(OUTPUT_PORT_INDEX_GNSS_NAV_INFO);
64 1 _gnssNavInfo.reset();
65 1 }
66 else
67 {
68 2 _gnssNavInfo.reset();
69 }
70 3 _ephemerisBuilder.clear();
71 3 _lastAccessedBuilder.clear();
72 3 _warningsNotImplemented.clear();
73
74 3 if (inputPins.at(INPUT_PORT_INDEX_UBLOX_OBS).isPinLinked()
75
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 && !inputPins.at(INPUT_PORT_INDEX_UBLOX_OBS).link.connectedNode->isOnlyRealtime()
76
5/6
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 2 times.
 6 && !_postProcessingLock.has_value())
77 {
78 1 _postProcessingLock.emplace(outputPins.at(OUTPUT_PORT_INDEX_GNSS_NAV_INFO).dataAccessMutex);
79 }
80
81 3 return true;
82 }
83
84 1 void NAV::UbloxGnssOrbitCollector::onDeleteLink([[maybe_unused]] OutputPin& startPin, [[maybe_unused]] InputPin& endPin)
85 {
86 LOG_TRACE("{}: called for {} ==> {}", nameId(), size_t(startPin.id), size_t(endPin.id));
87
88
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (_postProcessingLock.has_value())
89 {
90 1 _postProcessingLock.reset();
91 }
92 1 }
93
94 718 NAV::UbloxGnssOrbitCollector::EphemerisBuilder& NAV::UbloxGnssOrbitCollector::getEphemerisBuilder(const SatId& satId, const InsTime& insTime, size_t IOD)
95 {
96
4/8
✓ Branch 1 taken 718 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 718 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 718 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 718 times.
✗ Branch 13 not taken.
 718 LOG_DEBUG("{}: Searching for [{}] at [{}]", nameId(), satId, insTime.toYMDHMS(GPST));
97
2/2
✓ Branch 0 taken 227 times.
✓ Branch 1 taken 491 times.
 718 if (IOD != 0)
98 {
99
1/2
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
 227 _lastAccessedBuilder[satId] = IOD;
100 }
101
102
1/2
✓ Branch 1 taken 718 times.
✗ Branch 2 not taken.
 718 auto iter = std::ranges::find_if(_ephemerisBuilder, [&](const auto& builder) {
103
4/4
✓ Branch 1 taken 875 times.
✓ Branch 2 taken 8649 times.
✓ Branch 5 taken 671 times.
✓ Branch 6 taken 204 times.
 9524 return builder.satId == satId && builder.navData->refTime == insTime;
104 });
105
2/2
✓ Branch 2 taken 47 times.
✓ Branch 3 taken 671 times.
 718 if (iter == _ephemerisBuilder.end())
106 {
107
3/6
✓ Branch 1 taken 47 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 47 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 47 times.
✗ Branch 9 not taken.
 47 LOG_DEBUG("{}: Constructing new builder", nameId());
108
109 47 std::shared_ptr<SatNavData> satNavData = nullptr;
110
2/9
✓ Branch 1 taken 19 times.
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 47 switch (SatelliteSystem_(satId.satSys))
111 {
112 19 case GPS:
113
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 satNavData = std::make_shared<GPSEphemeris>(insTime);
114 19 break;
115 28 case GAL:
116
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 28 satNavData = std::make_shared<GalileoEphemeris>(insTime);
117 28 break;
118 ✗ case GLO:
119 // satNavData = std::make_shared<GLONASSEphemeris>(insTime);
120 ✗ LOG_CRITICAL("{}: GLONASS not implemented yet.", nameId()); // TODO: Not yet supported
121 break;
122 ✗ case BDS:
123 // satNavData = std::make_shared<BDSEphemeris>(insTime);
124 ✗ LOG_CRITICAL("{}: BeiDou not implemented yet.", nameId()); // TODO: Not yet supported
125 break;
126 ✗ case QZSS:
127 ✗ LOG_CRITICAL("{}: QZSS not implemented yet.", nameId()); // TODO: Not yet supported
128 break;
129 ✗ case IRNSS:
130 ✗ LOG_CRITICAL("{}: IRNSS not implemented yet.", nameId()); // TODO: Not yet supported
131 break;
132 ✗ case SBAS:
133 ✗ LOG_CRITICAL("{}: SBAS not implemented yet.", nameId()); // TODO: Not yet supported
134 break;
135 ✗ case SatSys_None:
136 ✗ LOG_CRITICAL("{}: Satellite system cannot be none.", nameId());
137 break;
138 }
139
1/2
✓ Branch 1 taken 47 times.
✗ Branch 2 not taken.
 47 return _ephemerisBuilder.emplace_back(satId, satNavData);
140 47 }
141
142
3/6
✓ Branch 1 taken 671 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 671 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 671 times.
✗ Branch 9 not taken.
 671 LOG_DEBUG("{}: Found builder", nameId());
143 671 return *iter;
144 }
145
146 std::optional<std::reference_wrapper<NAV::UbloxGnssOrbitCollector::EphemerisBuilder>>
147 1182 NAV::UbloxGnssOrbitCollector::getEphemerisBuilder(const SatId& satId, size_t IOD)
148 {
149
3/6
✓ Branch 1 taken 1182 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1182 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1182 times.
✗ Branch 9 not taken.
 1182 LOG_DEBUG("{}: Searching for [{}] at Issue of Data [{}]", nameId(), satId, IOD);
150
1/2
✓ Branch 1 taken 1182 times.
✗ Branch 2 not taken.
 1182 _lastAccessedBuilder[satId] = IOD;
151
152
1/2
✓ Branch 1 taken 1182 times.
✗ Branch 2 not taken.
 1182 auto iter = std::ranges::find_if(_ephemerisBuilder, [&](const auto& builder) {
153
2/2
✓ Branch 1 taken 1827 times.
✓ Branch 2 taken 20319 times.
 22146 if (builder.satId == satId)
154 {
155
2/2
✓ Branch 1 taken 261 times.
✓ Branch 2 taken 1566 times.
 1827 if (builder.navData->type == SatNavData::GPSEphemeris)
156 {
157 261 auto ephemeris = std::dynamic_pointer_cast<GPSEphemeris>(builder.navData);
158
3/4
✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 245 times.
✓ Branch 5 taken 16 times.
 261 return ephemeris && ephemeris->IODE == IOD;
159 261 }
160
1/2
✓ Branch 1 taken 1566 times.
✗ Branch 2 not taken.
 1566 if (builder.navData->type == SatNavData::GalileoEphemeris)
161 {
162 1566 auto ephemeris = std::dynamic_pointer_cast<GalileoEphemeris>(builder.navData);
163
3/4
✓ Branch 1 taken 1566 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 876 times.
✓ Branch 5 taken 690 times.
 1566 return ephemeris && ephemeris->IODnav == IOD;
164 1566 }
165 }
166 20319 return false;
167 });
168
2/2
✓ Branch 2 taken 1121 times.
✓ Branch 3 taken 61 times.
 1182 if (iter != _ephemerisBuilder.end())
169 {
170
3/6
✓ Branch 1 taken 1121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1121 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1121 times.
✗ Branch 9 not taken.
 1121 LOG_DEBUG("{}: Found builder", nameId());
171 1121 return *iter;
172 }
173
3/6
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 61 times.
✗ Branch 9 not taken.
 61 LOG_DEBUG("{}: Could not find builder. Ignoring subframe.", nameId());
174 61 return std::nullopt;
175 }
176
177 std::optional<std::reference_wrapper<NAV::UbloxGnssOrbitCollector::EphemerisBuilder>>
178 236 NAV::UbloxGnssOrbitCollector::getLastEphemerisBuilder(const SatId& satId)
179 {
180
2/4
✓ Branch 2 taken 236 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 236 times.
✗ Branch 7 not taken.
 236 LOG_DEBUG("{}: Searching the last builder for [{}]", nameId(), satId);
181
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 if (_lastAccessedBuilder.contains(satId))
182 {
183 236 return getEphemerisBuilder(satId, _lastAccessedBuilder.at(satId));
184 }
185 ✗ LOG_DEBUG("{}: Could not find last accessed builder. Ignoring subframe.", nameId());
186 ✗ return std::nullopt;
187 }
188
189 8207 void NAV::UbloxGnssOrbitCollector::receiveObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
190 {
191
1/2
✓ Branch 1 taken 8207 times.
✗ Branch 2 not taken.
 8207 [[maybe_unused]] auto ubloxObs = std::static_pointer_cast<const UbloxObs>(queue.extract_front());
192
193
1/2
✓ Branch 1 taken 8207 times.
✗ Branch 2 not taken.
 8207 if (ubloxObs->msgClass == ubx::UBX_CLASS_RXM)
194 {
195
2/2
✓ Branch 1 taken 6607 times.
✓ Branch 2 taken 1600 times.
 8207 if (static_cast<ubx::UbxRxmMessages>(ubloxObs->msgId) == ubx::UbxRxmMessages::UBX_RXM_SFRBX)
196 {
197
1/2
✓ Branch 2 taken 6607 times.
✗ Branch 3 not taken.
 6607 const auto& sfrbx = std::get<ubx::UbxRxmSfrbx>(ubloxObs->data);
198
199
1/2
✓ Branch 1 taken 6607 times.
✗ Branch 2 not taken.
 6607 SatelliteSystem satSys = ubx::getSatSys(sfrbx.gnssId);
200 6607 SatId satId(satSys, sfrbx.svId);
201
5/10
✓ Branch 1 taken 6607 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6607 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 6607 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6607 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 6607 times.
✗ Branch 19 not taken.
 6607 LOG_DEBUG("{}: [{}] Converting message at [{}][{}]", nameId(), satId, ubloxObs->insTime.toYMDHMS(GPST), ubloxObs->insTime.toGPSweekTow(GPST));
202
203
2/9
✓ Branch 1 taken 2396 times.
✓ Branch 2 taken 4211 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 6607 switch (SatelliteSystem_(satSys))
204 {
205 2396 case GPS:
206
1/2
✓ Branch 2 taken 2396 times.
✗ Branch 3 not taken.
 2396 decryptGPS(satId, sfrbx, ubloxObs->insTime);
207 2396 break;
208 4211 case GAL:
209
1/2
✓ Branch 2 taken 4211 times.
✗ Branch 3 not taken.
 4211 decryptGalileo(satId, sfrbx, ubloxObs->insTime);
210 4211 break;
211 ✗ case GLO:
212 ✗ decryptGLONASS(satId, sfrbx, ubloxObs->insTime);
213 ✗ break;
214 ✗ case BDS:
215 ✗ decryptBeiDou(satId, sfrbx, ubloxObs->insTime);
216 ✗ break;
217 ✗ case QZSS:
218 ✗ decryptQZSS(satId, sfrbx, ubloxObs->insTime);
219 ✗ break;
220 ✗ case IRNSS:
221 ✗ decryptIRNSS(satId, sfrbx, ubloxObs->insTime);
222 ✗ break;
223 ✗ case SBAS:
224 ✗ decryptSBAS(satId, sfrbx, ubloxObs->insTime);
225 ✗ break;
226 ✗ case SatSys_None:
227 ✗ LOG_CRITICAL("{}: Satellite system cannot be none.", nameId());
228 break;
229 }
230 }
231 }
232
233 8207 if (_postProcessingLock.has_value()
234
3/6
✓ Branch 1 taken 8207 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8207 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 8207 times.
✗ Branch 7 not taken.
 8207 && inputPins.at(INPUT_PORT_INDEX_UBLOX_OBS).isPinLinked()
235
5/10
✓ Branch 0 taken 8207 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 8207 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8207 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 8207 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 8207 times.
 16414 && inputPins.at(INPUT_PORT_INDEX_UBLOX_OBS).link.getConnectedPin()->noMoreDataAvailable)
236 {
237 ✗ _postProcessingLock.reset();
238 }
239 8207 }
240
241 2396 void NAV::UbloxGnssOrbitCollector::decryptGPS(const SatId& satId, const ubx::UbxRxmSfrbx& sfrbx, const InsTime& insTime)
242 {
243 // u-blox 8 / u-blox M8: Receiver description - Including protocol specification, ch. 10.2, p. 30f
244 // > For GPS L1C/A signals, there is a fairly straightforward mapping between the reported subframe
245 // > and the structure of subframe and words described in the GPS ICD. Each subframe comprises ten
246 // > data words, which are reported in the same order they are received.
247 // >
248 // > MSB LSB
249 // > 1 to 10: Pad (2 bits) Data (24 bits) Parity (6 bits)
250 // >
251 // > Note that as the GPS data words only comprise 30 bits, the 2 most significant bits in each word
252 // > reported by UBX-RXM-SFRBX are padding and should be ignored.
253
254
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2396 times.
 2396 if (sfrbx.numWords != 10)
255 {
256 ✗ LOG_ERROR("{}: [{}] Received {} instead of 10 words", nameId(), satId, sfrbx.numWords);
257 1169 return;
258 }
259
260 2396 size_t w = 0; // Word counter
261
262 // Telemetry Word
263 // 30 bits long, occurs every 6 seconds ┌ Integrity Status Flag
264 // Preamble │ ┌ Reserved
265 // 1 0 0 0 1 0 1 1 MSB TLM Message LSB │ │ Parity
266 // ┌──────┴──────┐ ┌───────────────┴──────────────────────┐ │ │ ┌──────┴────────┐
267 // 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30
268 LOG_DATA("{}: [{}] tlm: {} {}", nameId(), satId, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
269 2396 constexpr uint8_t TLM_PREAMBLE = 0b10001011;
270
3/4
✓ Branch 1 taken 2396 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1169 times.
✓ Branch 4 taken 1227 times.
 2396 if (static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 22) & 0b11111111) != TLM_PREAMBLE) // Preamble found after the 2 padding bits
271 {
272
3/6
✓ Branch 1 taken 1169 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1169 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1169 times.
✗ Branch 9 not taken.
 1169 LOG_DEBUG("{}: [{}] Wrong telemetry word preamble. Ignoring SFRBX message.", nameId(), satId);
273 1169 return;
274 }
275
276 // Handover Word (HOW)
277 // 30 bits long, occurs every 6 seconds anti-spoof (A-S) flag
278 // alert flag │
279 // MSB TOW-Count Message LSB │ │ Subframe ID Parity
280 // ┌──────────────────┴────────────────────┐ │ │ ┌───┴──┐ ┌──────┴────────┐
281 // 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30
282 1227 w++;
283 LOG_DATA("{}: [{}] how: {} {}", nameId(), satId, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
284
1/2
✓ Branch 1 taken 1227 times.
✗ Branch 2 not taken.
 1227 auto subFrameId = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 8) & 0b111);
285 LOG_DATA("{}: [{}] subFrameId: {}", nameId(), satId, subFrameId);
286
287
2/4
✓ Branch 0 taken 1227 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 1227 times.
 1227 if (subFrameId == 0 || subFrameId > 5)
288 {
289 ✗ LOG_ERROR("{}: [{}] The subFrameId has to be in the range [1..=5] but it is {}", nameId(), satId, subFrameId);
290 ✗ return;
291 }
292
293
1/2
✓ Branch 2 taken 1227 times.
✗ Branch 3 not taken.
 1227 auto gpsWeekToW = insTime.toGPSweekTow(GPST);
294
295 736 auto finishSubFrame = [&]<size_t N>(const std::shared_ptr<SatNavData>& ephemeris, size_t subFrameId, std::bitset<N>& subframesFound) {
296
2/4
✓ Branch 0 taken 736 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 736 times.
✗ Branch 3 not taken.
 736 if (1 <= subFrameId && subFrameId <= 3)
297 {
298 736 subframesFound.set(subFrameId - 1);
299
300 LOG_DATA("{}: [{}] subframesFound: {}", nameId(), satId, subframesFound);
301
2/2
✓ Branch 1 taken 698 times.
✓ Branch 2 taken 38 times.
 736 if (subframesFound.count() == 3)
302 {
303 LOG_DATA("{}: [{}] [{}] All subframes found. Updating gnnsNavInfo", nameId(), satId, ephemeris->refTime.toYMDHMS(GPST));
304
1/2
✓ Branch 1 taken 698 times.
✗ Branch 2 not taken.
 698 std::unique_lock guard(outputPins.at(OUTPUT_PORT_INDEX_GNSS_NAV_INFO).dataAccessMutex, std::defer_lock);
305
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 698 times.
 698 if (!_postProcessingLock.has_value())
306 {
307 ✗ guard.lock();
308 }
309 698 _gnssNavInfo.satelliteSystems |= satId.satSys;
310
1/2
✓ Branch 1 taken 698 times.
✗ Branch 2 not taken.
 698 _gnssNavInfo.addSatelliteNavData(satId, ephemeris);
311 698 }
312 }
313 736 };
314
315
2/2
✓ Branch 0 taken 245 times.
✓ Branch 1 taken 982 times.
 1227 if (subFrameId == 1) // Third through tenth words of subframe 1 shall each contain 6 parity bits as their LSBs
316 {
317 // IS-GPS-200M: Figure 20-1. Data Format (sheet 1 of 11), p. 79
318 // IS-GPS-200M: 20.3.3.3 Subframe 1, p. 91ff
319 // IS-GPS-200M: Table 20-I. Subframe 1 Parameters, p. 96
320
321 // T_GD, af2, af1, af0 shall be two's complement, with the sign bit (+ or -) occupying the MSB
322
323 245 w++;
324 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 3 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
325
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 [[maybe_unused]] auto WN = static_cast<uint16_t>((sfrbx.dwrd.at(w) >> 20) & 0b1111111111); // 10 BITS - Transmission Week Number
326
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto L2ChannelCodes = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 18) & 0b11); // 2 BITS
327
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto uraIndex = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 14) & 0b1111); // 4 BITS
328
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto svHealth = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 8) & 0b111111); // 6 BITS
329
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto IODC = static_cast<uint16_t>(((sfrbx.dwrd.at(w) >> 6) & 0b11) << 8); // 2 MSBs - 10 BITS TOTAL
330 LOG_DATA("{}: [{}] WN {}, L2ChannelCodes {}, uraIndex {}, svHealth {}, IODC {}", nameId(), satId, WN, L2ChannelCodes, uraIndex, svHealth, std::bitset<10>(IODC));
331
332 245 w++;
333 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 4 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
334
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto L2DataFlagPCode = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 29) & 0b1); // 1 BIT
335 LOG_DATA("{}: [{}] L2DataFlagPCode {}", nameId(), satId, L2DataFlagPCode);
336
337 245 w++;
338 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 5 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
339
340 245 w++;
341 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 6 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
342
343 245 w++;
344 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 7 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
345
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto T_GD = static_cast<int8_t>((sfrbx.dwrd.at(w) >> 6) & 0b11111111);
346 LOG_DATA("{}: [{}] T_GD {}", nameId(), satId, T_GD);
347
348 245 w++;
349 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 8 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
350
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 IODC |= static_cast<uint16_t>((sfrbx.dwrd.at(w) >> 22) & 0b11111111); // 8 LSBs - 10 BITS TOTAL
351
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto toc = static_cast<uint16_t>((sfrbx.dwrd.at(w) >> 6) & 0b1111111111111111); // 16 LSBs
352 LOG_DATA("{}: [{}] IODC {} ({}), toc {}", nameId(), satId, IODC, std::bitset<10>(IODC), toc);
353
354 245 w++;
355 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 9 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
356
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto af2 = static_cast<int8_t>((sfrbx.dwrd.at(w) >> 22) & 0b11111111); // 8 BITS
357
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto af1 = static_cast<int16_t>((sfrbx.dwrd.at(w) >> 6) & 0b1111111111111111); // 16 BITS
358 LOG_DATA("{}: [{}] af2 {}, af1 {}", nameId(), satId, af2, af1);
359
360 245 w++;
361 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 10 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
362
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto af0 = math::interpretAs<int32_t, 22>(sfrbx.dwrd.at(w) >> 8); // 22 BITS
363 LOG_DATA("{}: [{}] af0 {}", nameId(), satId, af0);
364
365
1/2
✓ Branch 3 taken 245 times.
✗ Branch 4 not taken.
 245 InsTime insTimeToc(gpsWeekToW.gpsCycle, gpsWeekToW.gpsWeek, toc * std::pow(2, 4), GPST);
366
367
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto& ephemerisBuilder = getEphemerisBuilder(satId, insTimeToc);
368 245 auto ephemeris = std::dynamic_pointer_cast<GPSEphemeris>(ephemerisBuilder.navData);
369 245 ephemeris->toc = insTimeToc;
370
371 245 ephemeris->L2ChannelCodes = L2ChannelCodes;
372
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 ephemeris->svAccuracy = gpsUraIdx2Val(uraIndex);
373 245 ephemeris->svHealth = svHealth;
374 245 ephemeris->IODC = IODC;
375 245 ephemeris->L2DataFlagPCode = L2DataFlagPCode;
376 245 ephemeris->T_GD = T_GD * std::pow(2, -31);
377 245 ephemeris->refTime = ephemeris->toc;
378 245 ephemeris->a = {
379 245 af0 * std::pow(2, -31),
380 245 af1 * std::pow(2, -43),
381 245 af2 * std::pow(2, -55),
382 };
383 LOG_DATA("{}: [{}] svAccuracy [{} m], svHealth [{}], IODC [{}], tgd [{:.3e} s]", nameId(), satId,
384 ephemeris->svAccuracy, ephemeris->svHealth, ephemeris->IODC, ephemeris->T_GD);
385 LOG_DATA("{}: [{}] toc [{}], a0 [{:.3e} s], a1 [{:.3e} s/s], a2 [{:.3e} s/s^2]", nameId(), satId,
386 ephemeris->toc.toYMDHMS(GPST), ephemeris->a[0], ephemeris->a[1], ephemeris->a[2]);
387
388
1/2
✓ Branch 2 taken 245 times.
✗ Branch 3 not taken.
 245 finishSubFrame(ephemeris, subFrameId, ephemerisBuilder.subframes);
389 245 }
390
2/2
✓ Branch 0 taken 246 times.
✓ Branch 1 taken 736 times.
 982 else if (subFrameId == 2) // The third through tenth words of subframes 2 and 3 shall each contain six parity bits as their LSBs
391 {
392 // IS-GPS-200M: Figure 20-1. Data Format (sheet 2 of 11), p. 80
393 // IS-GPS-200M: 20.3.3.4 Subframes 2 and 3, p. 101ff
394 // IS-GPS-200M: Table 20-III. Ephemeris Parameters, p. 105
395
396 // Crs, delta_n, M_0, Cuc, Cus shall be two's complement, with the sign bit (+ or -) occupying the MSB
397
398 246 w++;
399 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 3 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
400
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 auto IODE = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 22) & 0b11111111); // 8 BITS
401
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 auto Crs = static_cast<int16_t>((sfrbx.dwrd.at(w) >> 6) & 0b1111111111111111); // 16 BITS
402 LOG_DATA("{}: [{}] IODE {}, Crs {}", nameId(), satId, IODE, Crs);
403
404 246 w++;
405 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 4 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
406
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 auto delta_n = static_cast<int16_t>((sfrbx.dwrd.at(w) >> 14) & 0b1111111111111111); // 16 BITS
407
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 auto M_0 = static_cast<int32_t>(((sfrbx.dwrd.at(w) >> 6) & 0b11111111) << 24); // 8 MSBs - 32 BITS TOTAL
408 LOG_DATA("{}: [{}] delta_n {}, M_0 {}", nameId(), satId, delta_n, std::bitset<32>(static_cast<uint32_t>(M_0)));
409
410 246 w++;
411 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 5 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
412
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 M_0 |= static_cast<int32_t>((sfrbx.dwrd.at(w) >> 6) & 0b111111111111111111111111); // 24 LSBs - 32 BITS TOTAL
413 LOG_DATA("{}: [{}] M_0 {} ({})", nameId(), satId, M_0, std::bitset<32>(static_cast<uint32_t>(M_0)));
414
415 246 w++;
416 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 6 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
417
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 auto Cuc = static_cast<int16_t>((sfrbx.dwrd.at(w) >> 14) & 0b1111111111111111); // 16 BITS
418
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 uint32_t e = ((sfrbx.dwrd.at(w) >> 6) & 0b11111111) << 24; // 8 MSBs - 32 BITS TOTAL
419 LOG_DATA("{}: [{}] Cuc {}, e {}", nameId(), satId, Cuc, std::bitset<32>(e));
420
421 246 w++;
422 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 7 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
423
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 e |= (sfrbx.dwrd.at(w) >> 6) & 0b111111111111111111111111; // 24 LSBs - 32 BITS TOTAL
424 LOG_DATA("{}: [{}] e {} ({})", nameId(), satId, e, std::bitset<32>(e));
425
426 246 w++;
427 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 8 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
428
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 auto Cus = static_cast<int16_t>((sfrbx.dwrd.at(w) >> 14) & 0b1111111111111111); // 16 BITS
429
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 uint32_t sqrt_A = ((sfrbx.dwrd.at(w) >> 6) & 0b11111111) << 24; // 8 MSBs - 32 BITS TOTAL
430 LOG_DATA("{}: [{}] Cus {}, sqrt_A {}", nameId(), satId, Cus, std::bitset<32>(sqrt_A));
431
432 246 w++;
433 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 9 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
434
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 sqrt_A |= (sfrbx.dwrd.at(w) >> 6) & 0b111111111111111111111111; // 24 LSBs - 32 BITS TOTAL
435 LOG_DATA("{}: [{}] sqrt_A {} ({})", nameId(), satId, sqrt_A, std::bitset<32>(sqrt_A));
436
437 246 w++;
438 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 10 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
439
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 auto toe = static_cast<uint16_t>((sfrbx.dwrd.at(w) >> 14) & 0b1111111111111111); // 16 BITS
440
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 auto fitInterval = static_cast<bool>((sfrbx.dwrd.at(w) >> 13) & 0b1); // 1 BIT
441
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 [[maybe_unused]] auto AODO = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 8) & 0b11111); // 5 BITS
442 LOG_DATA("{}: [{}] toe {}, fitInterval {}, AODO {}", nameId(), satId, toe, fitInterval, AODO);
443
444
1/2
✓ Branch 3 taken 246 times.
✗ Branch 4 not taken.
 246 InsTime insTimeToe(gpsWeekToW.gpsCycle, gpsWeekToW.gpsWeek, toe * std::pow(2, 4), GPST);
445
446
1/2
✓ Branch 1 taken 246 times.
✗ Branch 2 not taken.
 246 auto& ephemerisBuilder = getEphemerisBuilder(satId, insTimeToe);
447 246 auto ephemeris = std::dynamic_pointer_cast<GPSEphemeris>(ephemerisBuilder.navData);
448 246 ephemeris->toe = insTimeToe;
449
450 246 ephemeris->IODE = IODE;
451 246 ephemeris->Crs = Crs * std::pow(2, -5);
452 246 ephemeris->delta_n = semicircles2rad(delta_n * std::pow(2, -43));
453 246 ephemeris->M_0 = semicircles2rad(M_0 * std::pow(2, -31));
454 246 ephemeris->Cuc = Cuc * std::pow(2, -29);
455 246 ephemeris->e = e * std::pow(2, -33);
456 246 ephemeris->Cus = Cus * std::pow(2, -29);
457 246 ephemeris->sqrt_A = sqrt_A * std::pow(2, -19);
458
1/2
✓ Branch 3 taken 246 times.
✗ Branch 4 not taken.
 246 ephemeris->toe = InsTime(gpsWeekToW.gpsCycle, gpsWeekToW.gpsWeek, toe * std::pow(2, 4), GPST);
459
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 246 times.
 246 ephemeris->fitInterval = fitInterval ? 8.0 : 4.0;
460 // AODO
461
462 LOG_DATA("{}: [{}] IODE [{}], Crs [{:.3e} m], delta_n [{:.3e} rad/s], M_0 [{:.3e} rad], Cuc [{:.3e} rad]", nameId(), satId,
463 ephemeris->IODE, ephemeris->Crs, ephemeris->delta_n, ephemeris->M_0, ephemeris->Cuc);
464 LOG_DATA("{}: [{}] e [{:.3e}], Cus [{:.3e} rad], sqrt_A [{:.3e} m^(1/2)], toe [{}], fitInterval [{:.1f}]", nameId(), satId,
465 ephemeris->e, ephemeris->Cus, ephemeris->sqrt_A, ephemeris->toe.toGPSweekTow(GPST), ephemeris->fitInterval);
466
467
1/2
✓ Branch 2 taken 246 times.
✗ Branch 3 not taken.
 246 finishSubFrame(ephemeris, subFrameId, ephemerisBuilder.subframes);
468 246 }
469
2/2
✓ Branch 0 taken 245 times.
✓ Branch 1 taken 491 times.
 736 else if (subFrameId == 3) // The third through tenth words of subframes 2 and 3 shall each contain six parity bits as their LSBs
470 {
471 // IS-GPS-200M: Figure 20-1. Data Format (sheet 3 of 11), p. 81
472 // IS-GPS-200M: 20.3.3.4 Subframes 2 and 3, p. 101ff
473 // IS-GPS-200M: Table 20-III. Ephemeris Parameters, p. 105
474
475 // Cic, Omega_0, Cis, i_0, Crc, omega, Omega_dot, IDOT shall be two's complement, with the sign bit (+ or -) occupying the MSB
476
477 245 w++;
478 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 3 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
479
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto Cic = static_cast<int16_t>((sfrbx.dwrd.at(w) >> 14) & 0b1111111111111111); // 16 BITS
480
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto Omega_0 = static_cast<int32_t>(((sfrbx.dwrd.at(w) >> 6) & 0b11111111) << 24); // 8 MSBs - 32 BITS TOTAL
481 LOG_DATA("{}: [{}] Cic {}, Omega_0 {}", nameId(), satId, Cic, std::bitset<32>(static_cast<uint32_t>(Omega_0)));
482
483 245 w++;
484 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 4 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
485
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 Omega_0 |= static_cast<int32_t>((sfrbx.dwrd.at(w) >> 6) & 0b111111111111111111111111); // 24 LSBs - 32 BITS TOTAL
486 LOG_DATA("{}: [{}] Omega_0 {} ({})", nameId(), satId, Omega_0, std::bitset<32>(static_cast<uint32_t>(Omega_0)));
487
488 245 w++;
489 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 5 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
490
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto Cis = static_cast<int16_t>((sfrbx.dwrd.at(w) >> 14) & 0b1111111111111111); // 16 BITS
491
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto i_0 = static_cast<int32_t>(((sfrbx.dwrd.at(w) >> 6) & 0b11111111) << 24); // 8 MSBs - 32 BITS TOTAL
492 LOG_DATA("{}: [{}] Cis {}, i_0 {}", nameId(), satId, Cis, std::bitset<32>(static_cast<uint32_t>(i_0)));
493
494 245 w++;
495 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 6 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
496
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 i_0 |= static_cast<int32_t>((sfrbx.dwrd.at(w) >> 6) & 0b111111111111111111111111); // 24 LSBs - 32 BITS TOTAL
497 LOG_DATA("{}: [{}] i_0 {} ({})", nameId(), satId, i_0, std::bitset<32>(static_cast<uint32_t>(i_0)));
498
499 245 w++;
500 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 7 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
501
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto Crc = static_cast<int16_t>((sfrbx.dwrd.at(w) >> 14) & 0b1111111111111111); // 16 BITS
502
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto omega = static_cast<int32_t>(((sfrbx.dwrd.at(w) >> 6) & 0b11111111) << 24); // 8 MSBs - 32 BITS TOTAL
503 LOG_DATA("{}: [{}] Crc {}, omega {}", nameId(), satId, Crc, std::bitset<32>(static_cast<uint32_t>(omega)));
504
505 245 w++;
506 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 8 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
507
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 omega |= static_cast<int32_t>((sfrbx.dwrd.at(w) >> 6) & 0b111111111111111111111111); // 24 LSBs - 32 BITS TOTAL
508 LOG_DATA("{}: [{}] omega {} ({})", nameId(), satId, omega, std::bitset<32>(static_cast<uint32_t>(omega)));
509
510 245 w++;
511 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 9 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
512
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto Omega_dot = math::interpretAs<int32_t, 24>(sfrbx.dwrd.at(w) >> 6); // 24 BITS
513 LOG_DATA("{}: [{}] Omega_dot {} ({})", nameId(), satId, Omega_dot, std::bitset<32>(static_cast<uint32_t>(Omega_dot)));
514
515 245 w++;
516 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 10 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
517
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto IODE = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 22) & 0b11111111); // 8 BITS
518
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto i_dot = math::interpretAs<int16_t, 14>(sfrbx.dwrd.at(w) >> 8); // 14 BITS
519 LOG_DATA("{}: [{}] IODE {}, i_dot {} ({})", nameId(), satId, IODE, i_dot, std::bitset<16>(static_cast<uint16_t>(i_dot)));
520
521
1/2
✓ Branch 1 taken 245 times.
✗ Branch 2 not taken.
 245 auto ephemerisBuilder = getEphemerisBuilder(satId, IODE);
522
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 245 times.
 245 if (!ephemerisBuilder.has_value())
523 {
524 ✗ LOG_WARN("{}: [{}] Could not find Ephemeris builder for IODE {}", nameId(), satId, IODE);
525 ✗ return;
526 }
527 245 auto ephemeris = std::dynamic_pointer_cast<GPSEphemeris>(ephemerisBuilder->get().navData);
528
529 245 ephemeris->Cic = Cic * std::pow(2, -29);
530 245 ephemeris->Omega_0 = semicircles2rad(Omega_0 * std::pow(2, -31));
531 245 ephemeris->Cis = Cis * std::pow(2, -29);
532 245 ephemeris->i_0 = semicircles2rad(i_0 * std::pow(2, -31));
533 245 ephemeris->Crc = Crc * std::pow(2, -5);
534 245 ephemeris->omega = semicircles2rad(omega * std::pow(2, -31));
535 245 ephemeris->Omega_dot = semicircles2rad(Omega_dot * std::pow(2, -43));
536 245 ephemeris->IODE = IODE;
537 245 ephemeris->i_dot = semicircles2rad(i_dot * std::pow(2, -43));
538
539 LOG_DATA("{}: [{}] Cic [{:.3e} rad], Omega_0 [{:.3e} rad], Cis [{:.3e} rad], i_0 [{:.3e} rad], Crc [{:.3e} m]", nameId(), satId,
540 ephemeris->Cic, ephemeris->Omega_0, ephemeris->Cis, ephemeris->i_0, ephemeris->Crc);
541 LOG_DATA("{}: [{}] omega [{:.3e} rad], Omega_dot [{:.3e} rad/s], IODE [{}], i_dot [{:.3e} rad/s]", nameId(), satId,
542 ephemeris->omega, ephemeris->Omega_dot, ephemeris->IODE, ephemeris->i_dot);
543
544
1/2
✓ Branch 4 taken 245 times.
✗ Branch 5 not taken.
 245 finishSubFrame(ephemeris, subFrameId, ephemerisBuilder->get().subframes);
545 245 }
546
2/2
✓ Branch 0 taken 237 times.
✓ Branch 1 taken 254 times.
 491 else if (subFrameId == 4) // Words three through ten of each page contain six parity bits as their LSBs
547 {
548 // Page description
549 // 1, 6, 11, 16 and 21: Reserved
550 // 2, 3, 4, 5, 7, 8, 9 and 10: almanac data for SV 25 through 32 respectively
551 // 12, 19, 20, 22, 23 and 24: Reserved
552 // 13: NMCT
553 // 14 and 15: Reserved for system use
554 // 17: Special messages
555 // 18: Ionospheric and UTC data
556 // 25: A-S flags/SV configurations for 32 SVs, plus SV health for SV 25 through 32
557 //
558 // IS-GPS-200M: Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, p. 114
559 // IS-GPS-200M: 20.3.3.5 Subframes 4 and 5, p. 112ff
560
561 237 w++;
562 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 3 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
563
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 [[maybe_unused]] auto dataId = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 28) & 0b11); // 2 BITS
564
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 auto svId = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 22) & 0b111111); // 6 BITS
565 LOG_DATA("{}: [{}] dataId {}, svId {}", nameId(), satId, dataId, svId);
566
567
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 229 times.
 237 if (svId == 56) // IS-GPS-200M: Figure 20-1. Data Format (sheet 8 of 11), p. 86 - for page 18
568 {
569 // IS-GPS-200M: 20.3.3.5.1.6 Coordinated Universal Time (UTC) Parameters, p. 121
570 // IS-GPS-200M: Table 20-IX. UTC Parameters, p. 122
571
572 // IS-GPS-200M: 20.3.3.5.1.7 Ionospheric Data, p. 121
573 // IS-GPS-200M: Table 20-X. onospheric Parameters, p. 123
574
575
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 auto alpha0 = static_cast<int8_t>((sfrbx.dwrd.at(w) >> 14) & 0b11111111); // 8 BITS
576
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 auto alpha1 = static_cast<int8_t>((sfrbx.dwrd.at(w) >> 6) & 0b11111111); // 8 BITS
577 LOG_DATA("{}: [{}] alpha0 {}, alpha1 {}", nameId(), satId, alpha0, alpha1);
578
579 8 w++;
580 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 4 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
581
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 auto alpha2 = static_cast<int8_t>((sfrbx.dwrd.at(w) >> 22) & 0b11111111); // 8 BITS
582
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 auto alpha3 = static_cast<int8_t>((sfrbx.dwrd.at(w) >> 14) & 0b11111111); // 8 BITS
583
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 auto beta0 = static_cast<int8_t>((sfrbx.dwrd.at(w) >> 6) & 0b11111111); // 8 BITS
584 LOG_DATA("{}: [{}] alpha2 {}, alpha3 {}, beta0 {}", nameId(), satId, alpha2, alpha3, beta0);
585
586 8 w++;
587 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 5 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
588
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 auto beta1 = static_cast<int8_t>((sfrbx.dwrd.at(w) >> 22) & 0b11111111); // 8 BITS
589
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 auto beta2 = static_cast<int8_t>((sfrbx.dwrd.at(w) >> 14) & 0b11111111); // 8 BITS
590
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 auto beta3 = static_cast<int8_t>((sfrbx.dwrd.at(w) >> 6) & 0b11111111); // 8 BITS
591 LOG_DATA("{}: [{}] beta1 {}, beta2 {}, beta3 {}", nameId(), satId, beta1, beta2, beta3);
592
593 8 w++;
594 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 6 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
595
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 auto A1 = math::interpretAs<int32_t, 24>(sfrbx.dwrd.at(w) >> 6); // 24 BITS
596 LOG_DATA("{}: [{}] A1 {}", nameId(), satId, A1);
597
598 8 w++;
599 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 7 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
600
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 auto A0 = static_cast<int32_t>(((sfrbx.dwrd.at(w) >> 6) & 0b111111111111111111111111) << 8); // 24 MSBs - 32 BITS TOTAL
601 LOG_DATA("{}: [{}] A0 {}", nameId(), satId, std::bitset<32>(static_cast<uint32_t>(A0)));
602
603 8 w++;
604 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 8 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
605
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 A0 |= static_cast<int32_t>((sfrbx.dwrd.at(w) >> 22) & 0b11111111); // 8 LSBs - 32 BITS TOTAL
606 LOG_DATA("{}: [{}] A0 {} ({})", nameId(), satId, A0, std::bitset<32>(static_cast<uint32_t>(A0)));
607
608 8 w++;
609 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 9 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
610
611 8 w++;
612 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 10 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
613
614
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 std::unique_lock guard(outputPins.at(OUTPUT_PORT_INDEX_GNSS_NAV_INFO).dataAccessMutex, std::defer_lock);
615
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
 8 if (!_postProcessingLock.has_value())
616 {
617 ✗ guard.lock();
618 }
619
620 ✗ _gnssNavInfo.ionosphericCorrections.insert(satId.satSys, IonosphericCorrections::Alpha,
621 {
622 8 alpha0 * std::pow(2, -30) /* [s] */,
623 8 alpha1 * std::pow(2, -27) /* [s/semi-circle] */,
624 8 alpha2 * std::pow(2, -24) /* [s/semi-circle^2] */,
625
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 alpha3 * std::pow(2, -24) /* [s/semi-circle^3] */,
626 });
627 ✗ _gnssNavInfo.ionosphericCorrections.insert(satId.satSys, IonosphericCorrections::Beta,
628 {
629 8 beta0 * std::pow(2, 11) /* [s] */,
630 8 beta1 * std::pow(2, 14) /* [s/semi-circle] */,
631 8 beta2 * std::pow(2, 16) /* [s/semi-circle^2] */,
632
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 beta3 * std::pow(2, 16) /* [s/semi-circle^3] */,
633 });
634
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 8 _gnssNavInfo.timeSysCorr[{ satId.satSys.getTimeSystem(), UTC }] = GnssNavInfo::TimeSystemCorrections{
635 8 .a0 = A0 * std::pow(2, -30),
636 8 .a1 = A1 * std::pow(2, -50),
637 };
638 8 }
639 else
640 {
641 // IS-GPS-200M: Figure 20-1. Data Format (sheet 6 of 11), p. 84 - for pages 1, 6, 11, 16 and 21
642 // IS-GPS-200M: Figure 20-1. Data Format (sheet 7 of 11), p. 85 - for pages 12, 19, 20, 22, 23 and 24
643 // IS-GPS-200M: Figure 20-1. Data Format (sheet 9 of 11), p. 87 - for page 25
644 // IS-GPS-200M: Figure 20-1. Data Format (sheet 10 of 11), p. 88 - for page 13
645 // IS-GPS-200M: Figure 20-1. Data Format (sheet 11 of 11), p. 89 - for pages 14, 15 and 17
646
647 229 w++;
648 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 4 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
649 229 w++;
650 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 5 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
651 229 w++;
652 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 6 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
653 229 w++;
654 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 7 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
655 229 w++;
656 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 8 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
657 229 w++;
658 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 9 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
659 229 w++;
660 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 10 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
661 }
662 }
663
1/2
✓ Branch 0 taken 254 times.
✗ Branch 1 not taken.
 254 else if (subFrameId == 5) // Words three through ten of each page contain six parity bits as their LSBs
664 {
665 // Page description
666 // 1 through 24: almanac data for SV 1 through 24
667 // 25: SV health data for SV 1 through 24, the almanac reference time, the almanac reference week number
668 //
669 // IS-GPS-200M: Figure 20-1. Data Format (sheet 4 of 11), p. 82 - for page 1 through 24
670 // IS-GPS-200M: Figure 20-1. Data Format (sheet 5 of 11), p. 83 - for page 25
671 // IS-GPS-200M: 20.3.3.5 Subframes 4 and 5, p. 112ff
672
673 254 w++;
674 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 3 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
675 254 w++;
676 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 4 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
677 254 w++;
678 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 5 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
679 254 w++;
680 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 6 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
681 254 w++;
682 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 7 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
683 254 w++;
684 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 8 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
685 254 w++;
686 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 9 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
687 254 w++;
688 LOG_DATA("{}: [{}] word {:2}: {} {}", nameId(), satId, w + 1 /* 10 */, std::bitset<2>(sfrbx.dwrd.at(w) >> 30), std::bitset<30>(sfrbx.dwrd.at(w)));
689 }
690 }
691
692 4211 void NAV::UbloxGnssOrbitCollector::decryptGalileo(const SatId& satId, const ubx::UbxRxmSfrbx& sfrbx, const InsTime& insTime)
693 {
694 // u-blox 8 / u-blox M8: Receiver description - Including protocol specification, ch. 10.5, p. 32f
695 // > The Galileo E1OS and E5b signals both transmit the I/NAV message but in different configurations.
696 // > For Galileo E1OS signals, each reported subframe contains a pair of I/NAV pages as described in
697 // > the Galileo ICD.
698 // > Galileo pages can either be "Nominal" or "Alert" pages. For Nominal pages the eight words are
699 // > arranged as follows:
700 // >
701 // > MSB LSB
702 // > 1: Even/Odd (1 bit) Page type (1 bit) Word type (6 bits) Data (122 - 99) (24 bits)
703 // > 2: Data (98 - 67) (32 bits)
704 // > 3: Data (66 - 35) (32 bits)
705 // > 4: Data (34 - 17) (18 bits) Tail (6 bits) Pad (8 bits)
706 // > 5: Even/Odd (1 bit) Page type (1 bit) Data (16 - 1) (16 bits) Reserved 1 (40 - 27) (14 bits)
707 // > 6: Reserved 2 (26 - 1) (26 bits) SAR (22 - 17) (6 bits)
708 // > 7: SAR (16 - 1) (16 bits) Spare (2 bits) CRC (24 - 11) (14 bits)
709 // > 8: CRC (10 - 1) (10 bits) Reserved 2 (8 bits) Tail (6 bits) Pad (8 bits)
710 // >
711 // > Alert pages are reported in very similar manner, but the page type bits will have value 1 and the
712 // > structure of the eight words will be slightly different (as indicated by the Galileo ICD).
713
714
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4211 times.
 4211 if (sfrbx.numWords != 8)
715 {
716 ✗ LOG_ERROR("{}: [{}] Received {} instead of 8 words", nameId(), satId, sfrbx.numWords);
717 695 return;
718 }
719
720 4211 size_t w = 0; // Word counter
721
722
1/2
✓ Branch 1 taken 4211 times.
✗ Branch 2 not taken.
 4211 auto even = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 31) & 0b1);
723
1/2
✓ Branch 1 taken 4211 times.
✗ Branch 2 not taken.
 4211 auto pageTypeEven = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 30) & 0b1);
724
1/2
✓ Branch 1 taken 4211 times.
✗ Branch 2 not taken.
 4211 auto odd = static_cast<uint8_t>((sfrbx.dwrd.at(4) >> 31) & 0b1);
725
1/2
✓ Branch 1 taken 4211 times.
✗ Branch 2 not taken.
 4211 auto pageTypeOdd = static_cast<uint8_t>((sfrbx.dwrd.at(4) >> 30) & 0b1);
726
1/2
✓ Branch 1 taken 4211 times.
✗ Branch 2 not taken.
 4211 auto wordType = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 24) & 0b111111);
727
3/6
✓ Branch 1 taken 4211 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4211 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 4211 times.
✗ Branch 9 not taken.
 4211 LOG_DEBUG("{}: [{}] wordType: {:2}, even {} ({} pageType), odd {} ({} pageType)", nameId(), satId, wordType, even, pageTypeEven, odd, pageTypeOdd);
728
729
2/4
✓ Branch 0 taken 4211 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 4211 times.
✗ Branch 3 not taken.
 4211 if (even != 0 || pageTypeEven == 1
730
4/4
✓ Branch 0 taken 3814 times.
✓ Branch 1 taken 397 times.
✓ Branch 2 taken 237 times.
✓ Branch 3 taken 3577 times.
 4211 || odd != 1 || pageTypeOdd == 1)
731 {
732
3/6
✓ Branch 1 taken 634 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 634 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 634 times.
✗ Branch 9 not taken.
 634 LOG_DEBUG("{}: [{}] Ignoring message, because one of the page types is alert page", nameId(), satId);
733 634 return;
734 }
735
736
1/2
✓ Branch 2 taken 3577 times.
✗ Branch 3 not taken.
 3577 auto gpsWeekToW = insTime.toGPSweekTow(GST);
737
738 1103 auto finishWord = [&]<size_t N>(const std::shared_ptr<SatNavData>& ephemeris, size_t wordType, std::bitset<N>& subframesFound) {
739
2/4
✓ Branch 0 taken 1103 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 1103 times.
✗ Branch 3 not taken.
 1103 if (1 <= wordType && wordType <= 5)
740 {
741 1103 subframesFound.set(wordType - 1);
742
743 LOG_DATA("{}: [{}] subframesFound: {}", nameId(), satId, subframesFound);
744
2/2
✓ Branch 1 taken 989 times.
✓ Branch 2 taken 114 times.
 1103 if (subframesFound.count() == 5)
745 {
746 LOG_DATA("{}: [{}] [{}] All words found. Updating gnnsNavInfo", nameId(), satId, ephemeris->refTime.toYMDHMS(GPST));
747
1/2
✓ Branch 1 taken 989 times.
✗ Branch 2 not taken.
 989 std::unique_lock guard(outputPins.at(OUTPUT_PORT_INDEX_GNSS_NAV_INFO).dataAccessMutex, std::defer_lock);
748
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 989 times.
 989 if (!_postProcessingLock.has_value())
749 {
750 ✗ guard.lock();
751 }
752 989 _gnssNavInfo.satelliteSystems |= satId.satSys;
753
1/2
✓ Branch 1 taken 989 times.
✗ Branch 2 not taken.
 989 _gnssNavInfo.addSatelliteNavData(satId, ephemeris);
754 989 }
755 }
756 1103 };
757
758
2/2
✓ Branch 0 taken 227 times.
✓ Branch 1 taken 3350 times.
 3577 if (wordType == 1) // Ephemeris (1/4)
759 {
760 // Galileo-OS-SIS-ICD-v2.0: ch. 4.3.5, Table 40 Bits Allocation for I/NAV Word Type 1, p. 37
761 //
762 // Even/Odd PageType WordType IODnav toe M_0 e sqrt_A Tail Pad Even/Odd PageType sqrt_A Reserved Reserved 1
763 // 1 1 6 10 14 32 32 18 6 8 1 1 14 2 14
764 // └───────────────────┬───────────────────┘ │ │ └───────┬───────┘ └───────────────────────┬──────────────────────┘
765 // Ublox 1 2 3 4 5
766
767 // Galileo-OS-SIS-ICD-v2.0: ch. 5.1.1, Table 60 Ephemeris Parameters, p. 43f
768 // > M_0 shall be two's complement, with the sign bit (+ or -) occupying the MSB
769
770
4/8
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 227 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 227 times.
✗ Branch 13 not taken.
 227 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 1 */, std::bitset<32>(sfrbx.dwrd.at(w)));
771
1/2
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
 227 auto IODnav = static_cast<uint16_t>((sfrbx.dwrd.at(w) >> 14) & 0b1111111111);
772
1/2
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
 227 auto toe = static_cast<uint16_t>(sfrbx.dwrd.at(w) & 0b11111111111111);
773
3/6
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 227 times.
✗ Branch 9 not taken.
 227 LOG_DEBUG("{}: [{}] IODnav {}, toe {}", nameId(), satId, IODnav, toe);
774
775 227 w++;
776
4/8
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 227 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 227 times.
✗ Branch 13 not taken.
 227 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 2 */, std::bitset<32>(sfrbx.dwrd.at(w)));
777
1/2
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
 227 auto M_0 = static_cast<int32_t>(sfrbx.dwrd.at(w));
778
3/6
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 227 times.
✗ Branch 9 not taken.
 227 LOG_DEBUG("{}: [{}] M_0 {}", nameId(), satId, M_0);
779
780 227 w++;
781
4/8
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 227 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 227 times.
✗ Branch 13 not taken.
 227 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 3 */, std::bitset<32>(sfrbx.dwrd.at(w)));
782
1/2
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
 227 uint32_t e = sfrbx.dwrd.at(w);
783
3/6
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 227 times.
✗ Branch 9 not taken.
 227 LOG_DEBUG("{}: [{}] e {}", nameId(), satId, e);
784
785 227 w++;
786
4/8
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 227 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 227 times.
✗ Branch 13 not taken.
 227 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 4 */, std::bitset<32>(sfrbx.dwrd.at(w)));
787
1/2
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
 227 uint32_t sqrt_A = ((sfrbx.dwrd.at(w) >> 14) & 0b111111111111111111) << 14;
788
3/6
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 227 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 227 times.
✗ Branch 10 not taken.
 227 LOG_DEBUG("{}: [{}] sqrt_A {}", nameId(), satId, std::bitset<32>(sqrt_A));
789
790 227 w++;
791
4/8
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 227 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 227 times.
✗ Branch 13 not taken.
 227 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 5 */, std::bitset<32>(sfrbx.dwrd.at(w)));
792
1/2
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
 227 sqrt_A |= (sfrbx.dwrd.at(w) >> 16) & 0b11111111111111;
793
3/6
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 227 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 227 times.
✗ Branch 10 not taken.
 227 LOG_DEBUG("{}: [{}] sqrt_A {} ({})", nameId(), satId, sqrt_A, std::bitset<32>(sqrt_A));
794
795
1/2
✓ Branch 2 taken 227 times.
✗ Branch 3 not taken.
 227 InsTime insTimeToe(gpsWeekToW.gpsCycle, gpsWeekToW.gpsWeek, toe * 60.0, GST);
796
797
1/2
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
 227 auto& ephemerisBuilder = getEphemerisBuilder(satId, insTimeToe, IODnav);
798 227 auto ephemeris = std::dynamic_pointer_cast<GalileoEphemeris>(ephemerisBuilder.navData);
799 227 ephemeris->toe = insTimeToe;
800
801 227 ephemeris->IODnav = IODnav;
802 227 ephemeris->M_0 = semicircles2rad(M_0 * std::pow(2.0, -31));
803 227 ephemeris->e = e * std::pow(2.0, -33);
804 227 ephemeris->sqrt_A = sqrt_A * std::pow(2.0, -19);
805
806
4/8
✓ Branch 1 taken 227 times.
✗ Branch 2 not taken.
✓ Branch 9 taken 227 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 227 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 227 times.
✗ Branch 18 not taken.
 227 LOG_DEBUG("{}: [{}] IODnav [{}], toe [{}], M_0 [{:.3e} rad], e [{:.3e}], sqrt_A [{:.3e} m^(1/2)]", nameId(), satId,
807 ephemeris->IODnav, ephemeris->toe.toYMDHMS(GPST), ephemeris->M_0, ephemeris->e, ephemeris->sqrt_A);
808
809
1/2
✓ Branch 2 taken 227 times.
✗ Branch 3 not taken.
 227 finishWord(ephemeris, wordType, ephemerisBuilder.subframes);
810 227 }
811
2/2
✓ Branch 0 taken 236 times.
✓ Branch 1 taken 3114 times.
 3350 else if (wordType == 2) // Ephemeris (2/4)
812 {
813 // Galileo-OS-SIS-ICD-v2.0: ch. 4.3.5, Table 41 Bits Allocation for I/NAV Word Type 2, p. 37
814 //
815 // Even/Odd PageType WordType IODnav Omega_0 Omega_0 i_0 i_0 omega omega Tail Pad Even/Odd PageType i_dot Reserved Reserved 1
816 // 1 1 6 10 14 18 14 18 14 18 6 8 1 1 14 2 14
817 // └──────────────────────┬────────────────────┘ └────┬─────┘ └────┬───┘ └───────┬────────┘ └──────────────────────┬──────────────────────┘
818 // Ublox 1 2 3 4 5
819
820 // Galileo-OS-SIS-ICD-v2.0: ch. 5.1.1, Table 60 Ephemeris Parameters, p. 43f
821 // > Omega_0, i_0, omega, i_dot shall be two's complement, with the sign bit (+ or -) occupying the MSB
822
823
4/8
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 236 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 236 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 236 times.
✗ Branch 13 not taken.
 236 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 1 */, std::bitset<32>(sfrbx.dwrd.at(w)));
824
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto IODnav = static_cast<uint16_t>((sfrbx.dwrd.at(w) >> 14) & 0b1111111111);
825
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto Omega_0 = static_cast<int32_t>((sfrbx.dwrd.at(w) & 0b11111111111111) << 18);
826
3/6
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 236 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 236 times.
✗ Branch 10 not taken.
 236 LOG_DEBUG("{}: [{}] IODnav {}, Omega_0 {}", nameId(), satId, IODnav, std::bitset<32>(static_cast<uint32_t>(Omega_0)));
827
828 236 w++;
829
4/8
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 236 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 236 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 236 times.
✗ Branch 13 not taken.
 236 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 2 */, std::bitset<32>(sfrbx.dwrd.at(w)));
830
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 Omega_0 |= static_cast<int32_t>((sfrbx.dwrd.at(w) >> 14) & 0b111111111111111111);
831
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto i_0 = static_cast<int32_t>((sfrbx.dwrd.at(w) & 0b11111111111111) << 18);
832
3/6
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 236 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 236 times.
✗ Branch 11 not taken.
 236 LOG_DEBUG("{}: [{}] Omega_0 {} ({}), i_0 {}", nameId(), satId, Omega_0, std::bitset<32>(static_cast<uint32_t>(Omega_0)), std::bitset<32>(static_cast<uint32_t>(i_0)));
833
834 236 w++;
835
4/8
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 236 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 236 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 236 times.
✗ Branch 13 not taken.
 236 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 3 */, std::bitset<32>(sfrbx.dwrd.at(w)));
836
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 i_0 |= static_cast<int32_t>((sfrbx.dwrd.at(w) >> 14) & 0b111111111111111111);
837
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto omega = static_cast<int32_t>((sfrbx.dwrd.at(w) & 0b11111111111111) << 18);
838
3/6
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 236 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 236 times.
✗ Branch 11 not taken.
 236 LOG_DEBUG("{}: [{}] i_0 {} ({}), omega {}", nameId(), satId, i_0, std::bitset<32>(static_cast<uint32_t>(i_0)), std::bitset<32>(static_cast<uint32_t>(omega)));
839
840 236 w++;
841
4/8
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 236 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 236 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 236 times.
✗ Branch 13 not taken.
 236 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 4 */, std::bitset<32>(sfrbx.dwrd.at(w)));
842
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 omega |= static_cast<int32_t>((sfrbx.dwrd.at(w) >> 14) & 0b111111111111111111);
843
3/6
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 236 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 236 times.
✗ Branch 10 not taken.
 236 LOG_DEBUG("{}: [{}] omega {} ({})", nameId(), satId, omega, std::bitset<32>(static_cast<uint32_t>(omega)));
844
845 236 w++;
846
4/8
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 236 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 236 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 236 times.
✗ Branch 13 not taken.
 236 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 5 */, std::bitset<32>(sfrbx.dwrd.at(w)));
847
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto i_dot = math::interpretAs<int16_t, 14>(sfrbx.dwrd.at(w) >> 16);
848
3/6
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 236 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 236 times.
✗ Branch 10 not taken.
 236 LOG_DEBUG("{}: [{}] i_dot {} ({})", nameId(), satId, i_dot, std::bitset<16>(static_cast<uint16_t>(i_dot)));
849
850
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto ephemerisBuilder = getEphemerisBuilder(satId, IODnav);
851
2/2
✓ Branch 1 taken 27 times.
✓ Branch 2 taken 209 times.
 236 if (!ephemerisBuilder.has_value())
852 {
853
3/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
 27 LOG_WARN("{}: [{}] Could not find Ephemeris builder for IODnav {}", nameId(), satId, IODnav);
854 27 return;
855 }
856 209 auto ephemeris = std::dynamic_pointer_cast<GalileoEphemeris>(ephemerisBuilder->get().navData);
857
858 209 ephemeris->Omega_0 = semicircles2rad(Omega_0 * std::pow(2.0, -31));
859 209 ephemeris->i_0 = semicircles2rad(i_0 * std::pow(2.0, -31));
860 209 ephemeris->omega = semicircles2rad(omega * std::pow(2.0, -31));
861 209 ephemeris->i_dot = semicircles2rad(i_dot * std::pow(2.0, -43));
862
863
3/6
✓ Branch 1 taken 209 times.
✗ Branch 2 not taken.
✓ Branch 9 taken 209 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 209 times.
✗ Branch 14 not taken.
 209 LOG_DEBUG("{}: [{}] IODnav [{}], Omega_0 [{} rad], i_0 [{:.3e} rad], omega [{:.3e} rad], i_dot [{:.3e} rad/s]", nameId(), satId,
864 ephemeris->IODnav, ephemeris->Omega_0, ephemeris->i_0, ephemeris->omega, ephemeris->i_dot);
865
866
1/2
✓ Branch 4 taken 209 times.
✗ Branch 5 not taken.
 209 finishWord(ephemeris, wordType, ephemerisBuilder->get().subframes);
867 209 }
868
2/2
✓ Branch 0 taken 228 times.
✓ Branch 1 taken 2886 times.
 3114 else if (wordType == 3) // Ephemeris (3/4) and SISA
869 {
870 // Galileo-OS-SIS-ICD-v2.0: ch. 4.3.5, Table 42 Bits Allocation for I/NAV Word Type 3, p. 38
871 //
872 // Even/Odd PageType WordType IODnav Omega_dot Omega_dot delta_n Cuc Cuc Cus Crc Crc Crs Tail Pad Even/Odd PageType Crs SISA(E1,E5b) Reserved 1
873 // 1 1 6 10 14 10 16 6 10 16 6 10 8 6 8 1 1 8 8 14
874 // └───────────────────────┬─────────────────────┘ └──────────┬──────────┘ └─────┬─────┘ └────────┬────────┘ └───────────────────────┬───────────────────────┘
875 // Ublox 1 2 3 4 5
876
877 // Galileo-OS-SIS-ICD-v2.0: ch. 5.1.1, Table 60 Ephemeris Parameters, p. 43f
878 // > Omega_dot, delta_n, Cuc, Cus, Crc, Crs shall be two's complement, with the sign bit (+ or -) occupying the MSB
879
880
4/8
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 228 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 228 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 228 times.
✗ Branch 13 not taken.
 228 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 1 */, std::bitset<32>(sfrbx.dwrd.at(w)));
881
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 228 auto IODnav = static_cast<uint16_t>((sfrbx.dwrd.at(w) >> 14) & 0b1111111111);
882
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 228 auto Omega_dot = static_cast<int32_t>((sfrbx.dwrd.at(w) & 0b11111111111111) << 10);
883
3/6
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 228 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 228 times.
✗ Branch 10 not taken.
 228 LOG_DEBUG("{}: [{}] IODnav {}, Omega_dot {}", nameId(), satId, IODnav, std::bitset<32>(static_cast<uint32_t>(Omega_dot)));
884
885 228 w++;
886
4/8
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 228 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 228 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 228 times.
✗ Branch 13 not taken.
 228 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 2 */, std::bitset<32>(sfrbx.dwrd.at(w)));
887
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 228 Omega_dot |= static_cast<int32_t>((sfrbx.dwrd.at(w) >> 22) & 0b1111111111);
888 228 Omega_dot = math::interpretAs<int32_t, 24>(Omega_dot);
889
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 228 auto delta_n = static_cast<int16_t>((sfrbx.dwrd.at(w) >> 6) & 0b1111111111111111);
890
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 228 auto Cuc = static_cast<int16_t>((sfrbx.dwrd.at(w) & 0b111111) << 10);
891
3/6
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 228 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 228 times.
✗ Branch 11 not taken.
 228 LOG_DEBUG("{}: [{}] Omega_dot {} ({}), delta_n {}, Cuc {}", nameId(), satId,
892 Omega_dot, std::bitset<32>(static_cast<uint32_t>(Omega_dot)), delta_n, std::bitset<16>(static_cast<uint16_t>(Cuc)));
893
894 228 w++;
895
4/8
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 228 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 228 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 228 times.
✗ Branch 13 not taken.
 228 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 3 */, std::bitset<32>(sfrbx.dwrd.at(w)));
896
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 228 Cuc |= static_cast<int16_t>((sfrbx.dwrd.at(w) >> 22) & 0b1111111111); // NOLINT(bugprone-narrowing-conversions,cppcoreguidelines-narrowing-conversions)
897
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 228 auto Cus = static_cast<int16_t>((sfrbx.dwrd.at(w) >> 6) & 0b1111111111111111);
898
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 228 auto Crc = static_cast<int16_t>((sfrbx.dwrd.at(w) & 0b111111) << 10);
899
3/6
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 228 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 228 times.
✗ Branch 11 not taken.
 228 LOG_DEBUG("{}: [{}] Cuc {} ({}), Cus {}, Crc {}", nameId(), satId, Cuc, std::bitset<16>(static_cast<uint16_t>(Cuc)), Cus, std::bitset<16>(static_cast<uint16_t>(Crc)));
900
901 228 w++;
902
4/8
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 228 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 228 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 228 times.
✗ Branch 13 not taken.
 228 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 4 */, std::bitset<32>(sfrbx.dwrd.at(w)));
903
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 228 Crc |= static_cast<int16_t>((sfrbx.dwrd.at(w) >> 22) & 0b1111111111); // NOLINT(bugprone-narrowing-conversions,cppcoreguidelines-narrowing-conversions)
904
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 228 auto Crs = static_cast<int16_t>(((sfrbx.dwrd.at(w) >> 14) & 0b11111111) << 8);
905
3/6
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 228 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 228 times.
✗ Branch 11 not taken.
 228 LOG_DEBUG("{}: [{}] Crc {} ({}), Crs {}", nameId(), satId, Crc, std::bitset<16>(static_cast<uint16_t>(Crc)), std::bitset<16>(static_cast<uint16_t>(Crs)));
906
907 228 w++;
908
4/8
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 228 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 228 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 228 times.
✗ Branch 13 not taken.
 228 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 5 */, std::bitset<32>(sfrbx.dwrd.at(w)));
909
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 228 Crs |= static_cast<int16_t>((sfrbx.dwrd.at(w) >> 22) & 0b11111111); // NOLINT(bugprone-narrowing-conversions,cppcoreguidelines-narrowing-conversions)
910
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 228 auto SISA = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 14) & 0b11111111);
911
3/6
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 228 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 228 times.
✗ Branch 10 not taken.
 228 LOG_DEBUG("{}: [{}] Crs {} ({}), SISA {}", nameId(), satId, Crs, std::bitset<16>(static_cast<uint16_t>(Crs)), SISA);
912
913
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 228 auto ephemerisBuilder = getEphemerisBuilder(satId, IODnav);
914
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 225 times.
 228 if (!ephemerisBuilder.has_value())
915 {
916
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
 3 LOG_WARN("{}: [{}] Could not find Ephemeris builder for IODnav {}", nameId(), satId, IODnav);
917 3 return;
918 }
919 225 auto ephemeris = std::dynamic_pointer_cast<GalileoEphemeris>(ephemerisBuilder->get().navData);
920
921 225 ephemeris->Omega_dot = semicircles2rad(Omega_dot * std::pow(2.0, -43));
922 225 ephemeris->delta_n = semicircles2rad(delta_n * std::pow(2.0, -43));
923 225 ephemeris->Cuc = Cuc * std::pow(2.0, -29);
924 225 ephemeris->Cus = Cus * std::pow(2.0, -29);
925 225 ephemeris->Crc = Crc * std::pow(2.0, -5);
926 225 ephemeris->Crs = Crs * std::pow(2.0, -5);
927
1/2
✓ Branch 1 taken 225 times.
✗ Branch 2 not taken.
 225 ephemeris->signalAccuracy = galSisaIdx2Val(SISA);
928
929
3/6
✓ Branch 1 taken 225 times.
✗ Branch 2 not taken.
✓ Branch 12 taken 225 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 225 times.
✗ Branch 17 not taken.
 225 LOG_DEBUG("{}: [{}] IODnav [{}], Omega_dot [{} rad/s], delta_n [{:.3e} rad/s], Cuc [{:.3e} rad], Cus [{:.3e} rad/s], Crc [{:.3e} m], Crs [{:.3e} m], SISA [{:.3e} m]", nameId(), satId,
930 ephemeris->IODnav, ephemeris->Omega_dot, ephemeris->delta_n, ephemeris->Cuc, ephemeris->Cus, ephemeris->Crc, ephemeris->Crs, ephemeris->signalAccuracy);
931
932
1/2
✓ Branch 4 taken 225 times.
✗ Branch 5 not taken.
 225 finishWord(ephemeris, wordType, ephemerisBuilder->get().subframes);
933 225 }
934
2/2
✓ Branch 0 taken 237 times.
✓ Branch 1 taken 2649 times.
 2886 else if (wordType == 4) // SVID, Ephemeris (4/4), and Clock correction parameters
935 {
936 // Galileo-OS-SIS-ICD-v2.0: ch. 4.3.5, Table 43 Bits Allocation for I/NAV Word Type 4, p. 38
937 //
938 // Even/Odd PageType WordType IODnav SVID Cic Cic Cis toc toc af0 af0 af1 Tail Pad Even/Odd PageType af1 af2 Spare Reserved 1
939 // 1 1 6 10 6 8 8 16 8 6 26 5 13 6 8 1 1 8 6 2 14
940 // └───────────────────────┬─────────────────────┘ └─────┬─────┘ └──┬───┘ └────────┬────────┘ └───────────────────────┬─────────────────────┘
941 // Ublox 1 2 3 4 5
942
943 // Galileo-OS-SIS-ICD-v2.0: ch. 5.1.1, Table 60 Ephemeris Parameters, p. 43f
944 // Galileo-OS-SIS-ICD-v2.0: ch. 5.1.3, Table 63 Galileo Clock Correction Parameters, p. 46
945 // > Cic, Cis, af0, af1, af2 shall be two's complement, with the sign bit (+ or -) occupying the MSB
946
947
4/8
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 237 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 237 times.
✗ Branch 13 not taken.
 237 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 1 */, std::bitset<32>(sfrbx.dwrd.at(w)));
948
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 auto IODnav = static_cast<uint16_t>((sfrbx.dwrd.at(w) >> 14) & 0b1111111111);
949
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 [[maybe_unused]] auto SVID = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 8) & 0b111111);
950
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 auto Cic = static_cast<int16_t>((sfrbx.dwrd.at(w) & 0b11111111) << 8);
951
3/6
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 237 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 237 times.
✗ Branch 10 not taken.
 237 LOG_DEBUG("{}: [{}] IODnav {}, SVID {}, Cic {}", nameId(), satId, IODnav, SVID, std::bitset<16>(static_cast<uint16_t>(Cic)));
952
953 237 w++;
954
4/8
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 237 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 237 times.
✗ Branch 13 not taken.
 237 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 2 */, std::bitset<32>(sfrbx.dwrd.at(w)));
955
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 Cic |= static_cast<int16_t>((sfrbx.dwrd.at(w) >> 24) & 0b11111111); // NOLINT(bugprone-narrowing-conversions,cppcoreguidelines-narrowing-conversions)
956
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 auto Cis = static_cast<int16_t>((sfrbx.dwrd.at(w) >> 8) & 0b1111111111111111);
957
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 auto toc = static_cast<uint16_t>((sfrbx.dwrd.at(w) & 0b11111111) << 6);
958
3/6
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 237 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 237 times.
✗ Branch 11 not taken.
 237 LOG_DEBUG("{}: [{}] Cic {} ({}), Cis {}, toc {}", nameId(), satId, Cic, std::bitset<16>(static_cast<uint16_t>(Cic)), Cis, std::bitset<16>(toc));
959
960 237 w++;
961
4/8
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 237 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 237 times.
✗ Branch 13 not taken.
 237 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 3 */, std::bitset<32>(sfrbx.dwrd.at(w)));
962
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 toc |= static_cast<uint16_t>((sfrbx.dwrd.at(w) >> 26) & 0b111111);
963
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 auto af0 = static_cast<int32_t>((sfrbx.dwrd.at(w) & 0b11111111111111111111111111) << 5);
964
3/6
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 237 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 237 times.
✗ Branch 11 not taken.
 237 LOG_DEBUG("{}: [{}] toc {} ({}), af0 {}", nameId(), satId, toc, std::bitset<16>(toc), std::bitset<32>(static_cast<uint32_t>(af0)));
965
966 237 w++;
967
4/8
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 237 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 237 times.
✗ Branch 13 not taken.
 237 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 4 */, std::bitset<32>(sfrbx.dwrd.at(w)));
968
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 af0 |= static_cast<int32_t>((sfrbx.dwrd.at(w) >> 27) & 0b11111);
969 237 af0 = math::interpretAs<int32_t, 31>(af0);
970
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 auto af1 = static_cast<int32_t>(((sfrbx.dwrd.at(w) >> 14) & 0b1111111111111) << 8);
971
3/6
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 237 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 237 times.
✗ Branch 11 not taken.
 237 LOG_DEBUG("{}: [{}] af0 {} ({}), af1 {}", nameId(), satId, af0, std::bitset<32>(static_cast<uint32_t>(af0)), std::bitset<32>(static_cast<uint32_t>(af1)));
972
973 237 w++;
974
4/8
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 237 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 237 times.
✗ Branch 13 not taken.
 237 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 5 */, std::bitset<32>(sfrbx.dwrd.at(w)));
975
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 af1 |= static_cast<int32_t>((sfrbx.dwrd.at(w) >> 22) & 0b11111111);
976 237 af1 = math::interpretAs<int32_t, 21>(af1);
977
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 auto af2 = math::interpretAs<int8_t, 6>(sfrbx.dwrd.at(w) >> 16);
978
3/6
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 237 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 237 times.
✗ Branch 11 not taken.
 237 LOG_DEBUG("{}: [{}] af1 {} ({}), af1 {}", nameId(), satId, af1, std::bitset<32>(static_cast<uint32_t>(af1)), std::bitset<8>(static_cast<uint8_t>(af2)));
979
980
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 auto ephemerisBuilder = getEphemerisBuilder(satId, IODnav);
981
2/2
✓ Branch 1 taken 28 times.
✓ Branch 2 taken 209 times.
 237 if (!ephemerisBuilder.has_value())
982 {
983
3/6
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 28 times.
✗ Branch 9 not taken.
 28 LOG_WARN("{}: [{}] Could not find Ephemeris builder for IODnav {}", nameId(), satId, IODnav);
984 28 return;
985 }
986 209 auto ephemeris = std::dynamic_pointer_cast<GalileoEphemeris>(ephemerisBuilder->get().navData);
987
988
1/2
✓ Branch 2 taken 209 times.
✗ Branch 3 not taken.
 209 InsTime insTimeToc(gpsWeekToW.gpsCycle, gpsWeekToW.gpsWeek, toc * 60.0, GST);
989
990 209 ephemeris->Cic = Cic * std::pow(2.0, -29);
991 209 ephemeris->Cis = Cis * std::pow(2.0, -29);
992 209 ephemeris->toc = insTimeToc;
993 209 ephemeris->refTime = ephemeris->toc;
994 209 ephemeris->a = {
995 209 af0 * std::pow(2, -34),
996 209 af1 * std::pow(2, -46),
997 209 af2 * std::pow(2, -59),
998 };
999
1000
3/6
✓ Branch 1 taken 209 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 209 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 209 times.
✗ Branch 12 not taken.
 209 LOG_DEBUG("{}: [{}] IODnav [{}], Cic [{:.3e} rad], Cis [{:.3e} rad]", nameId(), satId, ephemeris->IODnav, ephemeris->Cic, ephemeris->Cis);
1001
4/8
✓ Branch 1 taken 209 times.
✗ Branch 2 not taken.
✓ Branch 12 taken 209 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 209 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 209 times.
✗ Branch 20 not taken.
 209 LOG_DEBUG("{}: [{}] toc [{}], a0 [{:.3e} s], a1 [{:.3e} s/s], a2 [{:.3e} s/s^2]", nameId(), satId,
1002 ephemeris->toc.toYMDHMS(GPST), ephemeris->a[0], ephemeris->a[1], ephemeris->a[2]);
1003
1004
1/2
✓ Branch 4 taken 209 times.
✗ Branch 5 not taken.
 209 finishWord(ephemeris, wordType, ephemerisBuilder->get().subframes);
1005 209 }
1006
2/2
✓ Branch 0 taken 236 times.
✓ Branch 1 taken 2413 times.
 2649 else if (wordType == 5) // Ionospheric correction, BGD, signal health and data validity status and GST
1007 {
1008 // Galileo-OS-SIS-ICD-v2.0: ch. 4.3.5, Table 44 Bits Allocation for I/NAV Word Type 5, p. 38
1009 //
1010 // Even/Odd PageType WordType ai0 ai1 ai2 ai2 Region 1-5 BGD(E1,E5a) BGD(E1,E5b) BGD(E1,E5b) E5b_HS E1b_HS E5b_DVS E1b_DVS WN TOW TOW Spare Tail Pad Even/Odd PageType Spare Reserved 1
1011 // 1 1 6 11 11 2 12 5x1 10 5 5 2 2 1 1 12 9 11 7 13 6 1 1 16 14
1012 // └─────────────────────┬───────────────────┘ └───────────────────┬───────────────────┘ └─────────────────────────┬──────────────────────────┘ └─────────┬─────────┘ └──────────────────┬────────────────┘
1013 // Ublox 1 2 3 4 5
1014
1015 // Galileo-OS-SIS-ICD-v2.0: ch. 5.1.6, Table 67 Ionospheric Correction Parameters, p. 48
1016 // Galileo-OS-SIS-ICD-v2.0: ch. 5.1.5, Table 65 BGD Parameters, p. 47
1017 // > ai1, ai2, BGD(E1,E5a), BGD(E1,E5b) shall be two's complement, with the sign bit (+ or -) occupying the MSB
1018
1019
4/8
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 236 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 236 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 236 times.
✗ Branch 13 not taken.
 236 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 1 */, std::bitset<32>(sfrbx.dwrd.at(w)));
1020
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto ai0 = static_cast<uint16_t>((sfrbx.dwrd.at(w) >> 13) & 0b11111111111);
1021
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto ai1 = math::interpretAs<int16_t, 11>((sfrbx.dwrd.at(w) >> 2) & 0b11111111111);
1022
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto ai2 = static_cast<int16_t>((sfrbx.dwrd.at(w) & 0b11) << 12);
1023
3/6
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 236 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 236 times.
✗ Branch 12 not taken.
 236 LOG_DEBUG("{}: [{}] ai0 {} ({}), ai1 {} ({}), ai2 {}", nameId(), satId,
1024 ai0, std::bitset<16>(ai0), ai1, std::bitset<16>(static_cast<uint16_t>(ai1)), std::bitset<16>(static_cast<uint16_t>(ai2)));
1025
1026 236 w++;
1027
4/8
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 236 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 236 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 236 times.
✗ Branch 13 not taken.
 236 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 2 */, std::bitset<32>(sfrbx.dwrd.at(w)));
1028
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 ai2 |= static_cast<int16_t>((sfrbx.dwrd.at(w) >> 20) & 0b111111111111); // NOLINT(bugprone-narrowing-conversions,cppcoreguidelines-narrowing-conversions)
1029 236 ai2 = math::interpretAs<int16_t, 14>(ai2);
1030
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto BGD_E1_E5a = math::interpretAs<int16_t, 10>((sfrbx.dwrd.at(w) >> 5) & 0b1111111111);
1031
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto BGD_E1_E5b = static_cast<int16_t>((sfrbx.dwrd.at(w) & 0b11111) << 5);
1032
3/6
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 236 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 236 times.
✗ Branch 11 not taken.
 236 LOG_DEBUG("{}: [{}] ai2 {} ({}), BGD_E1_E5a {}, BGD_E1_E5b {}", nameId(), satId, ai2, std::bitset<16>(static_cast<uint16_t>(ai2)), BGD_E1_E5a, std::bitset<16>(static_cast<uint16_t>(BGD_E1_E5b)));
1033
1034 236 w++;
1035
4/8
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 236 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 236 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 236 times.
✗ Branch 13 not taken.
 236 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 3 */, std::bitset<32>(sfrbx.dwrd.at(w)));
1036
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 BGD_E1_E5b |= static_cast<int16_t>((sfrbx.dwrd.at(w) >> 27) & 0b11111); // NOLINT(bugprone-narrowing-conversions,cppcoreguidelines-narrowing-conversions)
1037 236 BGD_E1_E5b = math::interpretAs<int16_t, 10>(BGD_E1_E5b);
1038
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto E5b_HS = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 25) & 0b11);
1039
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto E1b_HS = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 23) & 0b11);
1040
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto E5b_DVS = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 22) & 0b1);
1041
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto E1b_DVS = static_cast<uint8_t>((sfrbx.dwrd.at(w) >> 21) & 0b1);
1042
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 [[maybe_unused]] auto WN = static_cast<uint16_t>((sfrbx.dwrd.at(w) >> 9) & 0b111111111111);
1043
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 [[maybe_unused]] uint32_t TOW = (sfrbx.dwrd.at(w) & 0b111111111) << 11;
1044
3/6
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 236 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 236 times.
✗ Branch 11 not taken.
 236 LOG_DEBUG("{}: [{}] BGD_E1_E5b {} ({}), E5b_HS {}, E1b_HS {}, E5b_DVS {}, E1b_DVS {}, WN {}, TOW {}", nameId(), satId,
1045 BGD_E1_E5b, std::bitset<16>(static_cast<uint16_t>(BGD_E1_E5b)), E5b_HS, E1b_HS, E5b_DVS, E1b_DVS, WN, std::bitset<32>(TOW));
1046
1047 236 w++;
1048
4/8
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 236 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 236 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 236 times.
✗ Branch 13 not taken.
 236 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 4 */, std::bitset<32>(sfrbx.dwrd.at(w)));
1049
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 TOW |= (sfrbx.dwrd.at(w) >> 21) & 0b11111111111;
1050
3/6
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 236 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 236 times.
✗ Branch 10 not taken.
 236 LOG_DEBUG("{}: [{}] TOW {} ({})", nameId(), satId, TOW, std::bitset<32>(TOW));
1051
1052 236 w++;
1053
4/8
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 236 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 236 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 236 times.
✗ Branch 13 not taken.
 236 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 5 */, std::bitset<32>(sfrbx.dwrd.at(w)));
1054
1055
1/2
✓ Branch 1 taken 236 times.
✗ Branch 2 not taken.
 236 auto ephemerisBuilder = getLastEphemerisBuilder(satId);
1056
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 233 times.
 236 if (!ephemerisBuilder.has_value())
1057 {
1058
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
 3 LOG_WARN("{}: [{}] Could not find any ephemeris builder", nameId(), satId);
1059 3 return;
1060 }
1061 233 auto ephemeris = std::dynamic_pointer_cast<GalileoEphemeris>(ephemerisBuilder->get().navData);
1062
1063 233 ephemeris->BGD_E1_E5a = BGD_E1_E5a * std::pow(2.0, -32);
1064 233 ephemeris->BGD_E1_E5b = BGD_E1_E5b * std::pow(2.0, -32);
1065 233 ephemeris->svHealth = {
1066 .E5a_DataValidityStatus = {},
1067 233 .E5b_DataValidityStatus = static_cast<GalileoEphemeris::SvHealth::DataValidityStatus>(E5b_DVS),
1068 233 .E1B_DataValidityStatus = static_cast<GalileoEphemeris::SvHealth::DataValidityStatus>(E1b_DVS),
1069 .E5a_SignalHealthStatus = {},
1070 233 .E5b_SignalHealthStatus = static_cast<GalileoEphemeris::SvHealth::SignalHealthStatus>(E5b_HS),
1071 233 .E1B_SignalHealthStatus = static_cast<GalileoEphemeris::SvHealth::SignalHealthStatus>(E1b_HS),
1072 };
1073 233 ephemeris->dataSource[0] = true; // I/NAV E1-B
1074 233 ephemeris->dataSource[9] = true; // af0-af2, Toc, SISA are for E5b,E1
1075
1076 {
1077
1/2
✓ Branch 1 taken 233 times.
✗ Branch 2 not taken.
 233 std::unique_lock guard(outputPins.at(OUTPUT_PORT_INDEX_GNSS_NAV_INFO).dataAccessMutex, std::defer_lock);
1078
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 233 times.
 233 if (!_postProcessingLock.has_value())
1079 {
1080 ✗ guard.lock();
1081 }
1082 // ‘sfu’ (solar flux unit) is not a SI unit but can be converted as: 1 sfu = 10e-22 W/(m2*Hz)
1083 ✗ _gnssNavInfo.ionosphericCorrections.insert(satId.satSys, IonosphericCorrections::Alpha,
1084 {
1085 233 ai0 * std::pow(2.0, -2) /* [sfu] */,
1086 233 ai1 * std::pow(2.0, -8) /* [sfu/degree] */,
1087
1/2
✓ Branch 2 taken 233 times.
✗ Branch 3 not taken.
 233 ai2 * std::pow(2.0, -15) /* [sfu/degree^2] */,
1088 0.0,
1089 });
1090 233 }
1091
1092
3/6
✓ Branch 1 taken 233 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 233 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 233 times.
✗ Branch 11 not taken.
 233 LOG_DEBUG("{}: [{}] BGD_E1_E5a [{:.3e} s], BGD_E1_E5b [{:.3e} rad]", nameId(), satId, ephemeris->BGD_E1_E5a, ephemeris->BGD_E1_E5b);
1093
1094
1/2
✓ Branch 4 taken 233 times.
✗ Branch 5 not taken.
 233 finishWord(ephemeris, wordType, ephemerisBuilder->get().subframes);
1095 233 }
1096
2/2
✓ Branch 0 taken 237 times.
✓ Branch 1 taken 2176 times.
 2413 else if (wordType == 6) // GST-UTC conversion parameters
1097 {
1098 // Galileo-OS-SIS-ICD-v2.0: ch. 4.3.5, Table 44 Bits Allocation for I/NAV Word Type 6, p. 38
1099 //
1100 // Even/Odd PageType WordType A0 A0 A1 dt_LS t0t WNot WN_LSF DN dt_LSF TOW Tail Pad Even/Odd PageType TOW Spare Reserved 1
1101 // 1 1 6 24 8 24 8 8 8 8 3 8 7 6 8 1 1 13 3 14
1102 // └───────────────┬──────────────┘ └──┬─┘ └──────────┬───────────┘ └────────────┬───────────┘ └────────────────────┬───────────────────┘
1103 // Ublox 1 2 3 4 5
1104
1105 // Galileo-OS-SIS-ICD-v2.0: ch. 5.1.7, Table 68 Ionospheric Correction Parameters, p. 49
1106 // > A0, A1, dt_LS, dt_LSF shall be two's complement, with the sign bit (+ or -) occupying the MSB
1107
1108
4/8
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 237 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 237 times.
✗ Branch 13 not taken.
 237 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 1 */, std::bitset<32>(sfrbx.dwrd.at(w)));
1109
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 auto A0 = static_cast<int32_t>((sfrbx.dwrd.at(w) & 0b111111111111111111111111) << 8);
1110
3/6
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 237 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 237 times.
✗ Branch 10 not taken.
 237 LOG_DEBUG("{}: [{}] A0 {}", nameId(), satId, std::bitset<32>(static_cast<uint32_t>(A0)));
1111
1112 237 w++;
1113
4/8
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 237 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 237 times.
✗ Branch 13 not taken.
 237 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 2 */, std::bitset<32>(sfrbx.dwrd.at(w)));
1114
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 A0 |= static_cast<int32_t>((sfrbx.dwrd.at(w) >> 24) & 0b11111111);
1115
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 auto A1 = math::interpretAs<int32_t, 24>(sfrbx.dwrd.at(w) & 0b111111111111111111111111);
1116
3/6
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 237 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 237 times.
✗ Branch 11 not taken.
 237 LOG_DEBUG("{}: [{}] A0 {} ({}), A1 {} ({})", nameId(), satId, A0, std::bitset<32>(static_cast<uint32_t>(A0)), A1, std::bitset<32>(static_cast<uint32_t>(A1)));
1117
1118 237 w++;
1119
4/8
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 237 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 237 times.
✗ Branch 13 not taken.
 237 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 3 */, std::bitset<32>(sfrbx.dwrd.at(w)));
1120
1121 237 w++;
1122
4/8
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 237 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 237 times.
✗ Branch 13 not taken.
 237 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 4 */, std::bitset<32>(sfrbx.dwrd.at(w)));
1123
1124 237 w++;
1125
4/8
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 237 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 237 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 237 times.
✗ Branch 13 not taken.
 237 LOG_DEBUG("{}: [{}] word {:2}: {}", nameId(), satId, w + 1 /* 5 */, std::bitset<32>(sfrbx.dwrd.at(w)));
1126
1127
1/2
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
 237 std::unique_lock guard(outputPins.at(OUTPUT_PORT_INDEX_GNSS_NAV_INFO).dataAccessMutex, std::defer_lock);
1128
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 237 times.
 237 if (!_postProcessingLock.has_value())
1129 {
1130 ✗ guard.lock();
1131 }
1132
1133
2/4
✓ Branch 1 taken 237 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 237 times.
✗ Branch 6 not taken.
 237 _gnssNavInfo.timeSysCorr[{ satId.satSys.getTimeSystem(), UTC }] = GnssNavInfo::TimeSystemCorrections{
1134 237 .a0 = A0 * std::pow(2, -30),
1135 237 .a1 = A1 * std::pow(2, -50),
1136 };
1137 237 }
1138 }
1139
1140 ✗ void NAV::UbloxGnssOrbitCollector::decryptGLONASS([[maybe_unused]] const SatId& satId, const ubx::UbxRxmSfrbx& /* sfrbx */, const InsTime& /* insTime */)
1141 {
1142 // u-blox 8 / u-blox M8: Receiver description - Including protocol specification, ch. 10.3, p. 31
1143 // > For GLONASS L1OF and L2OF signals, each reported subframe contains a string as described in
1144 // > the GLONASS ICD. This string comprises 85 data bits which are reported over three 32 bit words
1145 // > in the UBX-RXM-SFRBX message. Data bits 1 to 8 are always a hamming code, whilst bits 81 to 84
1146 // > are a string number and bit 85 is the idle chip, which should always have a value of zero. The
1147 // > meaning of other bits vary with string and frame number.
1148 // > The fourth and final 32 bit word in the UBX-RXM-SFRBX message contains frame and superframe
1149 // > numbers (where available). These values aren't actually transmitted by the SVs, but are deduced
1150 // > by the receiver and are included to aid decoding of the transmitted data. However, the receiver
1151 // > does not always know these values, in which case a value of zero is reported.
1152 // >
1153 // > MSB LSB
1154 // > 1: Idle chip (1 bit, always 0) String # (4 bits) Data (80 - 54) (27 bits)
1155 // > 2: Data (53 - 22) (32 bits)
1156 // > 3: Data (21 - 9) (13 bits) Hammering code (8 bits) Pad (11 bits)
1157 // > 4: Superframe # (16 bits) Pad (8 bits) Frame # (8 bits)
1158 // >
1159 // > In some circumstances, (especially on startup) the receiver may be able to decode data from a
1160 // > GLONASS SV before it can identify the SV. When this occurs UBX-RXM-SFRBX messages will be
1161 // > issued with an svId of 255 to indicate "unknown".
1162
1163 ✗ if (!_warningsNotImplemented.contains(satId.satSys))
1164 {
1165 ✗ LOG_WARN("{}: ubx orbit collector is not implemented for GLONASS yet.", nameId()); // TODO: not implemented yet.
1166 ✗ _warningsNotImplemented.insert(satId.satSys);
1167 }
1168
1169 // auto ephemeris = std::dynamic_pointer_cast<GLONASSEphemeris>(ephemerisBuilder->get().navData);
1170
1171 // sfrbx.freqId;
1172 // _gnssNavInfo.addSatelliteNavData({ satSys, satNum }, std::make_shared<GLONASSEphemeris>(epoch, tau_c,
1173 // -m_tau_n, gamma_n, static_cast<bool>(health),
1174 // pos, vel, accelLuniSolar,
1175 // frequencyNumber_accuracyCode));
1176 ✗ }
1177
1178 ✗ void NAV::UbloxGnssOrbitCollector::decryptBeiDou([[maybe_unused]] const SatId& satId, const ubx::UbxRxmSfrbx& /* sfrbx */, const InsTime& /* insTime */)
1179 {
1180 // u-blox 8 / u-blox M8: Receiver description - Including protocol specification, ch. 10.4, p. 32
1181 // > For BeiDou (B1I) signals, there is a fairly straightforward mapping between the reported subframe
1182 // > and the structure of subframe and words described in the BeiDou ICD. Each subframe comprises
1183 // > ten data words, which are reported in the same order they are received.
1184 // >
1185 // > MSB LSB
1186 // > 1 to 10: Pad (2 bits) Data (22 bits) Parity (8 bits)
1187 // >
1188 // > Note that as the BeiDou data words only comprise 30 bits, the 2 most significant bits in each word
1189 // > reported by UBX-RXM-SFRBX are padding and should be ignored.
1190
1191 ✗ if (!_warningsNotImplemented.contains(satId.satSys))
1192 {
1193 ✗ LOG_WARN("{}: ubx orbit collector is not implemented for BeiDou yet.", nameId()); // TODO: not implemented yet.
1194 ✗ _warningsNotImplemented.insert(satId.satSys);
1195 }
1196
1197 // auto ephemeris = std::dynamic_pointer_cast<BDSEphemeris>(ephemerisBuilder->get().navData);
1198
1199 // _gnssNavInfo.addSatelliteNavData({ satSys, satNum }, std::make_shared<BDSEphemeris>(epoch, toe,
1200 // IODE_IODnav_AODE_IODEC, fitInterval_AODC, a,
1201 // sqrt_A, e, i_0, Omega_0, omega, M_0,
1202 // delta_n, Omega_dot, i_dot, Cus, Cuc,
1203 // Cis, Cic, Crs, Crc,
1204 // signalAccuracy, svHealth,
1205 // tgd_bgd5a_TGD1, IODC_bgd5b_TGD2));
1206 ✗ }
1207
1208 ✗ void NAV::UbloxGnssOrbitCollector::decryptQZSS([[maybe_unused]] const SatId& satId, const ubx::UbxRxmSfrbx& /* sfrbx */, const InsTime& /* insTime */)
1209 {
1210 // u-blox 8 / u-blox M8: Receiver description - Including protocol specification, ch. 10.7, p. 34
1211 // > The structure of the data delivered by QZSS L1C/A signals is effectively identical to that of GPS
1212 // > (L1C/A). Similarly the QZSS L2C signal is effectively identical to the GPS (L2C).
1213 // > The QZSS (L1SAIF) signal is different and uses the same data block format as used by SBAS
1214 // > (L1C/A). QZSS (SAIF) signals can be distinguished from QZSS (L1C/A and L2C) by noting that they
1215 // > have 8 words, instead of 10 for QZSS (L1C/A and L2C).
1216
1217 ✗ if (!_warningsNotImplemented.contains(satId.satSys))
1218 {
1219 ✗ LOG_WARN("{}: ubx orbit collector is not implemented for QZSS yet.", nameId()); // TODO: not implemented yet.
1220 ✗ _warningsNotImplemented.insert(satId.satSys);
1221 }
1222 ✗ }
1223
1224 ✗ void NAV::UbloxGnssOrbitCollector::decryptIRNSS([[maybe_unused]] const SatId& satId, const ubx::UbxRxmSfrbx& /* sfrbx */, const InsTime& /* insTime */)
1225 {
1226 ✗ if (!_warningsNotImplemented.contains(satId.satSys))
1227 {
1228 ✗ LOG_WARN("{}: ubx orbit collector is not implemented for IRNSS yet.", nameId()); // TODO: not implemented yet.
1229 ✗ _warningsNotImplemented.insert(satId.satSys);
1230 }
1231 ✗ }
1232
1233 ✗ void NAV::UbloxGnssOrbitCollector::decryptSBAS([[maybe_unused]] const SatId& satId, const ubx::UbxRxmSfrbx& /* sfrbx */, const InsTime& /* insTime */)
1234 {
1235 // u-blox 8 / u-blox M8: Receiver description - Including protocol specification, ch. 10.6, p. 33f
1236
1237 ✗ if (!_warningsNotImplemented.contains(satId.satSys))
1238 {
1239 ✗ LOG_WARN("{}: ubx orbit collector is not implemented for SBAS yet.", nameId()); // TODO: not implemented yet.
1240 ✗ _warningsNotImplemented.insert(satId.satSys);
1241 }
1242 ✗ }
1243