INSTINCT Code Coverage Report

Directory: src/
File: Nodes/Converter/GNSS/UbloxGnssObsConverter.cpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 55 57 96.5%
Functions: 7 8 87.5%
Branches: 51 86 59.3%
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 "UbloxGnssObsConverter.hpp"
10
11 #include <map>
12
13 #include "util/Logger.hpp"
14
15 #include "internal/FlowManager.hpp"
16
17 #include "NodeData/GNSS/UbloxObs.hpp"
18 #include "NodeData/GNSS/GnssObs.hpp"
19
20 115 NAV::UbloxGnssObsConverter::UbloxGnssObsConverter()
21
2/4
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 115 times.
✗ Branch 5 not taken.
 115 : Node(typeStatic())
22 {
23 LOG_TRACE("{}: called", name);
24 115 _hasConfig = false;
25
26
4/8
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 115 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 115 times.
✓ Branch 9 taken 115 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 345 CreateInputPin("UbloxObs", Pin::Type::Flow, { NAV::UbloxObs::type() }, &UbloxGnssObsConverter::receiveObs);
27
28
4/8
✓ Branch 2 taken 115 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 115 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 115 times.
✓ Branch 10 taken 115 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 460 CreateOutputPin("GnssObs", Pin::Type::Flow, { NAV::GnssObs::type() });
29 345 }
30
31 232 NAV::UbloxGnssObsConverter::~UbloxGnssObsConverter()
32 {
33 LOG_TRACE("{}: called", nameId());
34 232 }
35
36 229 std::string NAV::UbloxGnssObsConverter::typeStatic()
37 {
38
1/2
✓ Branch 1 taken 229 times.
✗ Branch 2 not taken.
 458 return "UbloxGnssObsConverter";
39 }
40
41 ✗ std::string NAV::UbloxGnssObsConverter::type() const
42 {
43 ✗ return typeStatic();
44 }
45
46 114 std::string NAV::UbloxGnssObsConverter::category()
47 {
48
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
 228 return "Converter";
49 }
50
51 3 bool NAV::UbloxGnssObsConverter::initialize()
52 {
53 LOG_TRACE("{}: called", nameId());
54
55 3 _lastEpochObs.clear();
56
57 3 return true;
58 }
59
60 8207 void NAV::UbloxGnssObsConverter::receiveObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
61 {
62
1/2
✓ Branch 1 taken 8207 times.
✗ Branch 2 not taken.
 8207 auto ubloxObs = std::static_pointer_cast<const UbloxObs>(queue.extract_front());
63
64 namespace ubx = vendor::ublox;
65
66
1/2
✓ Branch 1 taken 8207 times.
✗ Branch 2 not taken.
 8207 if (ubloxObs->msgClass == ubx::UBX_CLASS_RXM)
67 {
68
2/2
✓ Branch 1 taken 1600 times.
✓ Branch 2 taken 6607 times.
 8207 if (static_cast<ubx::UbxRxmMessages>(ubloxObs->msgId) == ubx::UbxRxmMessages::UBX_RXM_RAWX)
69 {
70 LOG_DATA("{}: Converting message at [{}]", nameId(), ubloxObs->insTime.toYMDHMS(GPST));
71
1/2
✓ Branch 1 taken 1600 times.
✗ Branch 2 not taken.
 1600 auto gnssObs = std::make_shared<GnssObs>();
72 1600 gnssObs->insTime = ubloxObs->insTime;
73
74
1/2
✓ Branch 2 taken 1600 times.
✗ Branch 3 not taken.
 1600 const auto& ubxRxmRawx = std::get<ubx::UbxRxmRawx>(ubloxObs->data);
75
76
3/4
✓ Branch 1 taken 1600 times.
✗ Branch 2 not taken.
✓ Branch 8 taken 11200 times.
✓ Branch 9 taken 1600 times.
 12800 for (const auto& satSys : SatelliteSystem::GetAll())
77 {
78 11200 std::map<SatSigId, GnssObs::ObservationData> sortedObsData;
79
2/2
✓ Branch 5 taken 200683 times.
✓ Branch 6 taken 11200 times.
 211883 for (const auto& data : ubxRxmRawx.data)
80 {
81
3/4
✓ Branch 1 taken 200683 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 172014 times.
✓ Branch 5 taken 28669 times.
 200683 if (ubx::getSatSys(data.gnssId) != satSys) { continue; }
82
83
1/2
✓ Branch 1 taken 28669 times.
✗ Branch 2 not taken.
 28669 SatSigId satSigId(ubx::getCode(data.gnssId, data.sigId), data.svId);
84 LOG_DATA("{}: [{}][{}], prValid {}, cpValid {}, halfCycValid {}, subHalfSubtractedFromPhase {}, trkStat {}, observedLastEpoch {}",
85 nameId(), ubloxObs->insTime.toYMDHMS(GPST), satSigId, data.prValid(), data.cpValid(), data.halfCycValid(), data.subHalfSubtractedFromPhase(), data.trkStat,
86 _lastEpochObs.contains(satSigId));
87 28669 GnssObs::ObservationData obsData(satSigId);
88
2/4
✓ Branch 1 taken 28669 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 28669 times.
✗ Branch 4 not taken.
 28669 if (data.prValid())
89 {
90
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 28669 times.
 28669 if (data.prMes > 100'000'000) { return; } // Sometimes at the start of the ublox receiver, it reports absurd high numbers
91 28669 obsData.pseudorange = GnssObs::ObservationData::Pseudorange{
92 28669 .value = data.prMes,
93 .SSI = 0,
94 28669 };
95 }
96
3/4
✓ Branch 1 taken 28669 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 28590 times.
✓ Branch 4 taken 79 times.
 28669 if (data.cpValid())
97 {
98 28590 std::bitset<4> LLI;
99
1/2
✓ Branch 1 taken 28590 times.
✗ Branch 2 not taken.
 28590 LLI[0] = !_lastEpochObs.contains(satSigId);
100
1/2
✓ Branch 1 taken 28590 times.
✗ Branch 2 not taken.
 28590 LLI[1] = !data.halfCycValid();
101 28590 obsData.carrierPhase = GnssObs::ObservationData::CarrierPhase{
102 28590 .value = data.cpMes,
103 .SSI = 0,
104 28590 .LLI = static_cast<uint8_t>(LLI.to_ulong()),
105 28590 };
106 }
107 28669 obsData.doppler = data.doMes;
108 28669 obsData.CN0 = data.cno;
109
110
1/2
✓ Branch 2 taken 28669 times.
✗ Branch 3 not taken.
 28669 sortedObsData.insert(std::make_pair(satSigId, obsData));
111
3/6
✓ Branch 1 taken 28669 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 28669 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 28669 times.
✗ Branch 9 not taken.
 28669 gnssObs->satData(satSigId.toSatId()).frequencies |= satSigId.freq();
112 }
113
3/6
✓ Branch 1 taken 11200 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200130 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 200130 times.
✗ Branch 8 not taken.
 211330 std::erase_if(_lastEpochObs, [&](const SatSigId& satSigId) { return satSigId.freq().getSatSys() == satSys; });
114
2/2
✓ Branch 5 taken 28669 times.
✓ Branch 6 taken 11200 times.
 39869 for (const auto& obsData : sortedObsData)
115 {
116 // LOG_DATA("{}: Adding [{}]", nameId(), obsData.second.satSigId);
117
1/2
✓ Branch 2 taken 28669 times.
✗ Branch 3 not taken.
 28669 gnssObs->data.push_back(obsData.second);
118
2/2
✓ Branch 1 taken 28590 times.
✓ Branch 2 taken 79 times.
 28669 if (obsData.second.carrierPhase)
119 {
120
1/2
✓ Branch 1 taken 28590 times.
✗ Branch 2 not taken.
 28590 _lastEpochObs.insert(obsData.first);
121 }
122 }
123 12800 }
124
2/2
✓ Branch 2 taken 669 times.
✓ Branch 3 taken 931 times.
 1600 if (gnssObs->data.empty()) { return; }
125
1/2
✓ Branch 2 taken 931 times.
✗ Branch 3 not taken.
 931 invokeCallbacks(OUTPUT_PORT_INDEX_GNSS_OBS, gnssObs);
126 1600 }
127 }
128 8207 }
129