INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataLink/udpSend.cpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 12 104 11.5%
Functions: 4 13 30.8%
Branches: 9 184 4.9%
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 "udpSend.hpp"
10
11 #include "NodeData/GNSS/GnssObs.hpp"
12 #include "NodeData/State/PosVelAtt.hpp"
13 #include "internal/Node/Pin.hpp"
14 #include "internal/FlowManager.hpp"
15 #include "NodeRegistry.hpp"
16
17 #include "util/Logger.hpp"
18 #include "internal/gui/widgets/imgui_ex.hpp"
19 #include "internal/gui/NodeEditorApplication.hpp"
20
21 #include <cstring>
22 #include <string>
23 #include <vector>
24
25 // ---------------------------------------------------------- Private variabels ------------------------------------------------------------
26
27 namespace NAV
28 {
29 /// List of supported data identifiers
30 const std::vector<std::string> supportedDataIdentifier{
31 PosVelAtt::type(),
32 PosVel::type(),
33 Pos::type(),
34 GnssObs::type()
35 };
36
37 } // namespace NAV
38
39 // ---------------------------------------------------------- Member functions -------------------------------------------------------------
40
41 114 NAV::UdpSend::UdpSend()
42
6/12
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 114 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 114 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 114 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 114 times.
✗ Branch 20 not taken.
 114 : Node(typeStatic()), _socket(_io_context, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 0)), _resolver(_io_context)
43 {
44 LOG_TRACE("{}: called", name);
45
46 114 _hasConfig = true;
47 114 _guiConfigDefaultWindowSize = { 202, 96 };
48
49
1/2
✓ Branch 2 taken 114 times.
✗ Branch 3 not taken.
 114 CreateInputPin("Data", Pin::Type::Flow, supportedDataIdentifier, &UdpSend::receiveData);
50 114 }
51
52 228 NAV::UdpSend::~UdpSend()
53 {
54 LOG_TRACE("{}: called", nameId());
55 228 }
56
57 228 std::string NAV::UdpSend::typeStatic()
58 {
59
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 456 return "UdpSend";
60 }
61
62 ✗ std::string NAV::UdpSend::type() const
63 {
64 ✗ return typeStatic();
65 }
66
67 114 std::string NAV::UdpSend::category()
68 {
69
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
 228 return "Data Link";
70 }
71
72 ✗ void NAV::UdpSend::guiConfig()
73 {
74 ✗ ImGui::SetNextItemWidth(150 * gui::NodeEditorApplication::windowFontRatio());
75 ✗ if (ImGui::InputInt4L(fmt::format("IPv4##{}", size_t(id)).c_str(), _ip.data(), IP_LIMITS[0], IP_LIMITS[1]))
76 {
77 ✗ flow::ApplyChanges();
78 }
79 ✗ ImGui::SetNextItemWidth(150 * gui::NodeEditorApplication::windowFontRatio());
80 ✗ if (ImGui::InputIntL(fmt::format("Port##{}", size_t(id)).c_str(), &_port, UdpUtil::PORT_LIMITS[0], UdpUtil::PORT_LIMITS[1]))
81 {
82 ✗ flow::ApplyChanges();
83 }
84 ✗ }
85
86 ✗ bool NAV::UdpSend::resetNode()
87 {
88 ✗ return true;
89 }
90
91 ✗ json NAV::UdpSend::save() const
92 {
93 LOG_TRACE("{}: called", nameId());
94
95 ✗ json j;
96
97 ✗ j["ip"] = _ip;
98 ✗ j["port"] = _port;
99
100 ✗ return j;
101 ✗ }
102
103 ✗ void NAV::UdpSend::restore(json const& j)
104 {
105 LOG_TRACE("{}: called", nameId());
106 ✗ if (j.contains("ip"))
107 {
108 ✗ j.at("ip").get_to(_ip);
109 }
110 ✗ if (j.contains("port"))
111 {
112 ✗ j.at("port").get_to(_port);
113 }
114 ✗ }
115
116 ✗ bool NAV::UdpSend::initialize()
117 {
118 LOG_TRACE("{}: called", nameId());
119
120 ✗ std::string ipString{};
121 ✗ for (size_t i = 0; i < 4; i++)
122 {
123 ✗ ipString.append(std::to_string(_ip.at(i)));
124 ✗ i < 3 ? ipString.append(".") : ipString.append("");
125 }
126
127 ✗ _endpoints = _resolver.resolve(boost::asio::ip::udp::v4(), ipString, std::to_string(_port));
128
129 ✗ return true;
130 ✗ }
131
132 ✗ void NAV::UdpSend::deinitialize()
133 {
134 ✗ _io_context.stop();
135
136 LOG_TRACE("{}: called", nameId());
137 ✗ }
138
139 ✗ void NAV::UdpSend::receiveData(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
140 {
141 ✗ auto data = queue.extract_front();
142
143 ✗ std::vector<char> data2send{};
144
145 // Identify message type
146 ✗ if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf({ data->getType() }, { PosVelAtt::type() }))
147 {
148 ✗ _msgType = UdpUtil::MessageType::PosVelAtt;
149 ✗ data2send.resize(UdpUtil::Size::TOTAL_POSVELATT);
150 ✗ setMsgTypeAndTime(data2send, data->insTime);
151 }
152 ✗ else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf({ data->getType() }, { PosVel::type() }))
153 {
154 ✗ _msgType = UdpUtil::MessageType::PosVel;
155 ✗ data2send.resize(UdpUtil::Size::TOTAL_POSVEL);
156 ✗ setMsgTypeAndTime(data2send, data->insTime);
157 }
158 ✗ else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf({ data->getType() }, { Pos::type() }))
159 {
160 ✗ _msgType = UdpUtil::MessageType::Pos;
161 ✗ data2send.resize(UdpUtil::Size::TOTAL_POS);
162 ✗ setMsgTypeAndTime(data2send, data->insTime);
163 }
164 ✗ else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf({ data->getType() }, { GnssObs::type() }))
165 {
166 ✗ _msgType = UdpUtil::MessageType::GnssObs;
167 }
168
169 // Copy data
170 ✗ if (_msgType == UdpUtil::MessageType::Pos)
171 {
172 ✗ auto pos = std::static_pointer_cast<const Pos>(data);
173 ✗ std::memcpy(data2send.data() + UdpUtil::Offset::POS, pos->lla_position().data(), UdpUtil::Size::POS);
174 ✗ }
175 ✗ else if (_msgType == UdpUtil::MessageType::PosVel)
176 {
177 ✗ auto posVel = std::static_pointer_cast<const PosVel>(data);
178 ✗ std::memcpy(data2send.data() + UdpUtil::Offset::POS, posVel->lla_position().data(), UdpUtil::Size::POS);
179 ✗ std::memcpy(data2send.data() + UdpUtil::Offset::VEL, posVel->n_velocity().data(), UdpUtil::Size::VEL);
180 ✗ }
181 ✗ else if (_msgType == UdpUtil::MessageType::PosVelAtt)
182 {
183 ✗ auto posVelAtt = std::static_pointer_cast<const PosVelAtt>(data);
184 ✗ std::memcpy(data2send.data() + UdpUtil::Offset::POS, posVelAtt->lla_position().data(), UdpUtil::Size::POS);
185 ✗ std::memcpy(data2send.data() + UdpUtil::Offset::VEL, posVelAtt->n_velocity().data(), UdpUtil::Size::VEL);
186 ✗ std::memcpy(data2send.data() + UdpUtil::Offset::QUAT, &posVelAtt->n_Quat_b().x(), UdpUtil::Size::QUAT);
187 ✗ std::memcpy(data2send.data() + UdpUtil::Offset::QUAT + UdpUtil::Size::QUAT, &posVelAtt->n_Quat_b().y(), UdpUtil::Size::QUAT);
188 ✗ std::memcpy(data2send.data() + UdpUtil::Offset::QUAT + 2 * UdpUtil::Size::QUAT, &posVelAtt->n_Quat_b().z(), UdpUtil::Size::QUAT);
189 ✗ std::memcpy(data2send.data() + UdpUtil::Offset::QUAT + 3 * UdpUtil::Size::QUAT, &posVelAtt->n_Quat_b().w(), UdpUtil::Size::QUAT);
190 ✗ }
191 ✗ else if (_msgType == UdpUtil::MessageType::GnssObs)
192 {
193 ✗ auto gnssObs = std::static_pointer_cast<const GnssObs>(data);
194 ✗ const size_t sizeGnssData = UdpUtil::Size::SINGLE_OBSERVATION_DATA * gnssObs->data.size();
195
196 ✗ auto sizeTotal = sizeGnssData + UdpUtil::Size::MSGTYPE + UdpUtil::Size::GPSCYCLE + UdpUtil::Size::GPSWEEK + UdpUtil::Size::GPSTOW + UdpUtil::Size::SIZE;
197 ✗ if (sizeTotal > UdpUtil::MAXIMUM_BYTES)
198 {
199 ✗ LOG_ERROR("{}: gnssObs msg is bigger than the maximum size of a single UDP package: {} bytes.", nameId(), sizeTotal);
200 }
201
202 ✗ data2send.resize(sizeTotal);
203
204 ✗ setMsgTypeAndTime(data2send, data->insTime);
205
206 ✗ std::memcpy(data2send.data() + UdpUtil::Offset::SIZE, &sizeGnssData, UdpUtil::Size::SIZE);
207 ✗ std::memcpy(data2send.data() + UdpUtil::Offset::GNSSDATA, gnssObs->data.data(), sizeGnssData);
208 ✗ }
209 ✗ _socket.send_to(boost::asio::buffer(data2send), *_endpoints.begin());
210 ✗ }
211
212 ✗ void NAV::UdpSend::setMsgTypeAndTime(std::vector<char>& data2send, const InsTime& insTime)
213 {
214 ✗ auto gpsCycle = insTime.toGPSweekTow().gpsCycle;
215 ✗ auto gpsWeek = insTime.toGPSweekTow().gpsWeek;
216 ✗ auto gpsTow = static_cast<double>(insTime.toGPSweekTow().tow);
217
218 ✗ std::memcpy(data2send.data(), &_msgType, UdpUtil::Size::MSGTYPE);
219
220 ✗ std::memcpy(data2send.data() + UdpUtil::Offset::GPSCYCLE, &gpsCycle, UdpUtil::Size::GPSCYCLE);
221 ✗ std::memcpy(data2send.data() + UdpUtil::Offset::GPSWEEK, &gpsWeek, UdpUtil::Size::GPSWEEK);
222 ✗ std::memcpy(data2send.data() + UdpUtil::Offset::GPSTOW, &gpsTow, UdpUtil::Size::GPSTOW);
223 ✗ }
224