INSTINCT Code Coverage Report

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