0.5.1
Loading...
Searching...
No Matches
VectorNavDataLogger.cpp
Go to the documentation of this file.
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 "VectorNavDataLogger.hpp"
10
11#include "NodeData/IMU/VectorNavBinaryOutput.hpp"
12
13#include "util/Logger.hpp"
14#include "util/StringUtil.hpp"
15
16#include <iomanip> // std::setprecision
17#include <limits>
18
19#include "internal/FlowManager.hpp"
20
21#include <imgui_internal.h>
22
35
40
41std::string NAV::VectorNavDataLogger::typeStatic()
42{
43 return "VectorNavDataLogger";
44}
45
46std::string NAV::VectorNavDataLogger::type() const
47{
48 return typeStatic();
49}
50
51std::string NAV::VectorNavDataLogger::category()
52{
53 return "Data Logger";
54}
55
56void NAV::VectorNavDataLogger::guiConfig()
57{
58 if (FileWriter::guiConfig(".vnb", { ".vnb" }, size_t(id), nameId()))
59 {
60 flow::ApplyChanges();
61 doDeinitialize();
62 }
63
64 if (CommonLog::ShowOriginInput(nameId().c_str()))
65 {
66 flow::ApplyChanges();
67 }
68}
69
70[[nodiscard]] json NAV::VectorNavDataLogger::save() const
71{
72 LOG_TRACE("{}: called", nameId());
73
74 json j;
75
76 j["FileWriter"] = FileWriter::save();
77
78 return j;
79}
80
81void NAV::VectorNavDataLogger::restore(json const& j)
82{
83 LOG_TRACE("{}: called", nameId());
84
85 if (j.contains("FileWriter"))
86 {
87 FileWriter::restore(j.at("FileWriter"));
88 }
89}
90
91bool NAV::VectorNavDataLogger::onCreateLink([[maybe_unused]] OutputPin& startPin, [[maybe_unused]] InputPin& endPin)
92{
93 LOG_TRACE("{}: called for {} ==> {}", nameId(), size_t(startPin.id), size_t(endPin.id));
94
95 if (isInitialized())
96 {
97 deinitialize();
98 initialize();
99 }
100
101 return true;
102}
103
104void NAV::VectorNavDataLogger::flush()
105{
106 _filestream.flush();
107}
108
109bool NAV::VectorNavDataLogger::initialize()
110{
111 LOG_TRACE("{}: called", nameId());
112
113 if (!FileWriter::initialize())
114 {
115 return false;
116 }
117
118 CommonLog::initialize();
119
120 _headerWritten = false;
121
122 return true;
123}
124
125void NAV::VectorNavDataLogger::deinitialize()
126{
127 LOG_TRACE("{}: called", nameId());
128
129 FileWriter::deinitialize();
130}
131
132void NAV::VectorNavDataLogger::writeObservation(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
133{
134 auto obs = std::static_pointer_cast<const VectorNavBinaryOutput>(queue.extract_front());
135
136 if (_fileType == FileType::BINARY)
137 {
138 std::array<const char, 8> zeroData{};
139 if (!_headerWritten)
140 {
141 _filestream.write(obs->timeOutputs ? reinterpret_cast<const char*>(&obs->timeOutputs->timeField) : zeroData.data(), sizeof(vn::protocol::uart::TimeGroup));
142 _filestream.write(obs->imuOutputs ? reinterpret_cast<const char*>(&obs->imuOutputs->imuField) : zeroData.data(), sizeof(vn::protocol::uart::ImuGroup));
143 _filestream.write(obs->gnss1Outputs ? reinterpret_cast<const char*>(&obs->gnss1Outputs->gnssField) : zeroData.data(), sizeof(vn::protocol::uart::GpsGroup));
144 _filestream.write(obs->attitudeOutputs ? reinterpret_cast<const char*>(&obs->attitudeOutputs->attitudeField) : zeroData.data(), sizeof(vn::protocol::uart::AttitudeGroup));
145 _filestream.write(obs->insOutputs ? reinterpret_cast<const char*>(&obs->insOutputs->insField) : zeroData.data(), sizeof(vn::protocol::uart::InsGroup));
146 _filestream.write(obs->gnss2Outputs ? reinterpret_cast<const char*>(&obs->gnss2Outputs->gnssField) : zeroData.data(), sizeof(vn::protocol::uart::GpsGroup));
147 _headerWritten = true;
148 }
149
150 if (!obs->insTime.empty())
151 {
152 auto insTimeGPS = obs->insTime.toGPSweekTow();
153 auto tow = static_cast<double>(insTimeGPS.tow);
154 _filestream.write(reinterpret_cast<const char*>(&insTimeGPS.gpsCycle), sizeof(insTimeGPS.gpsCycle));
155 _filestream.write(reinterpret_cast<const char*>(&insTimeGPS.gpsWeek), sizeof(insTimeGPS.gpsWeek));
156 _filestream.write(reinterpret_cast<const char*>(&tow), sizeof(tow));
157 }
158 else
159 {
160 _filestream.write(zeroData.data(), sizeof(int32_t));
161 _filestream.write(zeroData.data(), sizeof(int32_t));
162 _filestream.write(zeroData.data(), sizeof(double));
163 }
164
165 // Group 2 (Time)
166 if (obs->timeOutputs)
167 {
168 if (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP)
169 {
170 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->timeStartup), sizeof(obs->timeOutputs->timeStartup));
171 }
172 if (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEGPS)
173 {
174 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->timeGps), sizeof(obs->timeOutputs->timeGps));
175 }
176 if (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW)
177 {
178 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->gpsTow), sizeof(obs->timeOutputs->gpsTow));
179 }
180 if (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK)
181 {
182 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->gpsWeek), sizeof(obs->timeOutputs->gpsWeek));
183 }
184 if (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESYNCIN)
185 {
186 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->timeSyncIn), sizeof(obs->timeOutputs->timeSyncIn));
187 }
188 if (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEGPSPPS)
189 {
190 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->timePPS), sizeof(obs->timeOutputs->timePPS));
191 }
192 if (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEUTC)
193 {
194 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->timeUtc.year), sizeof(obs->timeOutputs->timeUtc.year));
195 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->timeUtc.month), sizeof(obs->timeOutputs->timeUtc.month));
196 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->timeUtc.day), sizeof(obs->timeOutputs->timeUtc.day));
197 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->timeUtc.hour), sizeof(obs->timeOutputs->timeUtc.hour));
198 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->timeUtc.min), sizeof(obs->timeOutputs->timeUtc.min));
199 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->timeUtc.sec), sizeof(obs->timeOutputs->timeUtc.sec));
200 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->timeUtc.ms), sizeof(obs->timeOutputs->timeUtc.ms));
201 }
202 if (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_SYNCINCNT)
203 {
204 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->syncInCnt), sizeof(obs->timeOutputs->syncInCnt));
205 }
206 if (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_SYNCOUTCNT)
207 {
208 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->syncOutCnt), sizeof(obs->timeOutputs->syncOutCnt));
209 }
210 if (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS)
211 {
212 _filestream.write(reinterpret_cast<const char*>(&obs->timeOutputs->timeStatus.status()), sizeof(obs->timeOutputs->timeStatus.status()));
213 }
214 }
215 // Group 3 (IMU)
216 if (obs->imuOutputs)
217 {
218 if (obs->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_IMUSTATUS)
219 {
220 _filestream.write(reinterpret_cast<const char*>(&obs->imuOutputs->imuStatus), sizeof(obs->imuOutputs->imuStatus));
221 }
222 if (obs->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPMAG)
223 {
224 _filestream.write(reinterpret_cast<const char*>(obs->imuOutputs->uncompMag.data()), sizeof(obs->imuOutputs->uncompMag));
225 }
226 if (obs->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPACCEL)
227 {
228 _filestream.write(reinterpret_cast<const char*>(obs->imuOutputs->uncompAccel.data()), sizeof(obs->imuOutputs->uncompAccel));
229 }
230 if (obs->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPGYRO)
231 {
232 _filestream.write(reinterpret_cast<const char*>(obs->imuOutputs->uncompGyro.data()), sizeof(obs->imuOutputs->uncompGyro));
233 }
234 if (obs->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_TEMP)
235 {
236 _filestream.write(reinterpret_cast<const char*>(&obs->imuOutputs->temp), sizeof(obs->imuOutputs->temp));
237 }
238 if (obs->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_PRES)
239 {
240 _filestream.write(reinterpret_cast<const char*>(&obs->imuOutputs->pres), sizeof(obs->imuOutputs->pres));
241 }
242 if (obs->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTATHETA)
243 {
244 _filestream.write(reinterpret_cast<const char*>(&obs->imuOutputs->deltaTime), sizeof(obs->imuOutputs->deltaTime));
245 _filestream.write(reinterpret_cast<const char*>(obs->imuOutputs->deltaTheta.data()), sizeof(obs->imuOutputs->deltaTheta));
246 }
247 if (obs->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTAVEL)
248 {
249 _filestream.write(reinterpret_cast<const char*>(obs->imuOutputs->deltaV.data()), sizeof(obs->imuOutputs->deltaV));
250 }
251 if (obs->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_MAG)
252 {
253 _filestream.write(reinterpret_cast<const char*>(obs->imuOutputs->mag.data()), sizeof(obs->imuOutputs->mag));
254 }
255 if (obs->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_ACCEL)
256 {
257 _filestream.write(reinterpret_cast<const char*>(obs->imuOutputs->accel.data()), sizeof(obs->imuOutputs->accel));
258 }
259 if (obs->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_ANGULARRATE)
260 {
261 _filestream.write(reinterpret_cast<const char*>(obs->imuOutputs->angularRate.data()), sizeof(obs->imuOutputs->angularRate));
262 }
263 }
264 // Group 4 (GNSS1)
265 if (obs->gnss1Outputs)
266 {
267 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_UTC)
268 {
269 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->timeUtc.year), sizeof(obs->gnss1Outputs->timeUtc.year));
270 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->timeUtc.month), sizeof(obs->gnss1Outputs->timeUtc.month));
271 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->timeUtc.day), sizeof(obs->gnss1Outputs->timeUtc.day));
272 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->timeUtc.hour), sizeof(obs->gnss1Outputs->timeUtc.hour));
273 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->timeUtc.min), sizeof(obs->gnss1Outputs->timeUtc.min));
274 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->timeUtc.sec), sizeof(obs->gnss1Outputs->timeUtc.sec));
275 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->timeUtc.ms), sizeof(obs->gnss1Outputs->timeUtc.ms));
276 }
277 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_TOW)
278 {
279 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->tow), sizeof(obs->gnss1Outputs->tow));
280 }
281 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_WEEK)
282 {
283 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->week), sizeof(obs->gnss1Outputs->week));
284 }
285 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_NUMSATS)
286 {
287 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->numSats), sizeof(obs->gnss1Outputs->numSats));
288 }
289 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_FIX)
290 {
291 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->fix), sizeof(obs->gnss1Outputs->fix));
292 }
293 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_POSLLA)
294 {
295 _filestream.write(reinterpret_cast<const char*>(obs->gnss1Outputs->posLla.data()), sizeof(obs->gnss1Outputs->posLla));
296 }
297 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_POSECEF)
298 {
299 _filestream.write(reinterpret_cast<const char*>(obs->gnss1Outputs->posEcef.data()), sizeof(obs->gnss1Outputs->posEcef));
300 }
301 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_VELNED)
302 {
303 _filestream.write(reinterpret_cast<const char*>(obs->gnss1Outputs->velNed.data()), sizeof(obs->gnss1Outputs->velNed));
304 }
305 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_VELECEF)
306 {
307 _filestream.write(reinterpret_cast<const char*>(obs->gnss1Outputs->velEcef.data()), sizeof(obs->gnss1Outputs->velEcef));
308 }
309 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_POSU)
310 {
311 _filestream.write(reinterpret_cast<const char*>(obs->gnss1Outputs->posU.data()), sizeof(obs->gnss1Outputs->posU));
312 }
313 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_VELU)
314 {
315 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->velU), sizeof(obs->gnss1Outputs->velU));
316 }
317 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_TIMEU)
318 {
319 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->timeU), sizeof(obs->gnss1Outputs->timeU));
320 }
321 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO)
322 {
323 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->timeInfo.status.status()), sizeof(obs->gnss1Outputs->timeInfo.status.status()));
324 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->timeInfo.leapSeconds), sizeof(obs->gnss1Outputs->timeInfo.leapSeconds));
325 }
326 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_DOP)
327 {
328 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->dop.gDop), sizeof(obs->gnss1Outputs->dop.gDop));
329 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->dop.pDop), sizeof(obs->gnss1Outputs->dop.pDop));
330 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->dop.tDop), sizeof(obs->gnss1Outputs->dop.tDop));
331 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->dop.vDop), sizeof(obs->gnss1Outputs->dop.vDop));
332 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->dop.hDop), sizeof(obs->gnss1Outputs->dop.hDop));
333 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->dop.nDop), sizeof(obs->gnss1Outputs->dop.nDop));
334 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->dop.eDop), sizeof(obs->gnss1Outputs->dop.eDop));
335 }
336 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_SATINFO)
337 {
338 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->satInfo.numSats), sizeof(obs->gnss1Outputs->satInfo.numSats));
339
340 for (auto& satellite : obs->gnss1Outputs->satInfo.satellites)
341 {
342 _filestream.write(reinterpret_cast<const char*>(&satellite.sys), sizeof(satellite.sys));
343 _filestream.write(reinterpret_cast<const char*>(&satellite.svId), sizeof(satellite.svId));
344 _filestream.write(reinterpret_cast<const char*>(&satellite.flags), sizeof(satellite.flags));
345 _filestream.write(reinterpret_cast<const char*>(&satellite.cno), sizeof(satellite.cno));
346 _filestream.write(reinterpret_cast<const char*>(&satellite.qi), sizeof(satellite.qi));
347 _filestream.write(reinterpret_cast<const char*>(&satellite.el), sizeof(satellite.el));
348 _filestream.write(reinterpret_cast<const char*>(&satellite.az), sizeof(satellite.az));
349 }
350 }
351 if (obs->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_RAWMEAS)
352 {
353 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->raw.tow), sizeof(obs->gnss1Outputs->raw.tow));
354 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->raw.week), sizeof(obs->gnss1Outputs->raw.week));
355 _filestream.write(reinterpret_cast<const char*>(&obs->gnss1Outputs->raw.numSats), sizeof(obs->gnss1Outputs->raw.numSats));
356
357 for (auto& satellite : obs->gnss1Outputs->raw.satellites)
358 {
359 _filestream.write(reinterpret_cast<const char*>(&satellite.sys), sizeof(satellite.sys));
360 _filestream.write(reinterpret_cast<const char*>(&satellite.svId), sizeof(satellite.svId));
361 _filestream.write(reinterpret_cast<const char*>(&satellite.freq), sizeof(satellite.freq));
362 _filestream.write(reinterpret_cast<const char*>(&satellite.chan), sizeof(satellite.chan));
363 _filestream.write(reinterpret_cast<const char*>(&satellite.slot), sizeof(satellite.slot));
364 _filestream.write(reinterpret_cast<const char*>(&satellite.cno), sizeof(satellite.cno));
365 _filestream.write(reinterpret_cast<const char*>(&satellite.flags), sizeof(satellite.flags));
366 _filestream.write(reinterpret_cast<const char*>(&satellite.pr), sizeof(satellite.pr));
367 _filestream.write(reinterpret_cast<const char*>(&satellite.cp), sizeof(satellite.cp));
368 _filestream.write(reinterpret_cast<const char*>(&satellite.dp), sizeof(satellite.dp));
369 }
370 }
371 }
372 // Group 5 (Attitude)
373 if (obs->attitudeOutputs)
374 {
375 if (obs->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_VPESTATUS)
376 {
377 _filestream.write(reinterpret_cast<const char*>(&obs->attitudeOutputs->vpeStatus.status()), sizeof(obs->attitudeOutputs->vpeStatus.status()));
378 }
379 if (obs->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YAWPITCHROLL)
380 {
381 _filestream.write(reinterpret_cast<const char*>(obs->attitudeOutputs->ypr.data()), sizeof(obs->attitudeOutputs->ypr));
382 }
383 if (obs->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_QUATERNION)
384 {
385 _filestream.write(reinterpret_cast<const char*>(obs->attitudeOutputs->qtn.coeffs().data()), sizeof(obs->attitudeOutputs->qtn));
386 }
387 if (obs->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_DCM)
388 {
389 _filestream.write(reinterpret_cast<const char*>(obs->attitudeOutputs->dcm.data()), sizeof(obs->attitudeOutputs->dcm));
390 }
391 if (obs->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_MAGNED)
392 {
393 _filestream.write(reinterpret_cast<const char*>(obs->attitudeOutputs->magNed.data()), sizeof(obs->attitudeOutputs->magNed));
394 }
395 if (obs->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_ACCELNED)
396 {
397 _filestream.write(reinterpret_cast<const char*>(obs->attitudeOutputs->accelNed.data()), sizeof(obs->attitudeOutputs->accelNed));
398 }
399 if (obs->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY)
400 {
401 _filestream.write(reinterpret_cast<const char*>(obs->attitudeOutputs->linearAccelBody.data()), sizeof(obs->attitudeOutputs->linearAccelBody));
402 }
403 if (obs->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELNED)
404 {
405 _filestream.write(reinterpret_cast<const char*>(obs->attitudeOutputs->linearAccelNed.data()), sizeof(obs->attitudeOutputs->linearAccelNed));
406 }
407 if (obs->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YPRU)
408 {
409 _filestream.write(reinterpret_cast<const char*>(obs->attitudeOutputs->yprU.data()), sizeof(obs->attitudeOutputs->yprU));
410 }
411 }
412 // Group 6 (INS)
413 if (obs->insOutputs)
414 {
415 if (obs->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_INSSTATUS)
416 {
417 _filestream.write(reinterpret_cast<const char*>(&obs->insOutputs->insStatus.status()), sizeof(obs->insOutputs->insStatus.status()));
418 }
419 if (obs->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_POSLLA)
420 {
421 _filestream.write(reinterpret_cast<const char*>(obs->insOutputs->posLla.data()), sizeof(obs->insOutputs->posLla));
422 }
423 if (obs->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_POSECEF)
424 {
425 _filestream.write(reinterpret_cast<const char*>(obs->insOutputs->posEcef.data()), sizeof(obs->insOutputs->posEcef));
426 }
427 if (obs->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_VELBODY)
428 {
429 _filestream.write(reinterpret_cast<const char*>(obs->insOutputs->velBody.data()), sizeof(obs->insOutputs->velBody));
430 }
431 if (obs->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_VELNED)
432 {
433 _filestream.write(reinterpret_cast<const char*>(obs->insOutputs->velNed.data()), sizeof(obs->insOutputs->velNed));
434 }
435 if (obs->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_VELECEF)
436 {
437 _filestream.write(reinterpret_cast<const char*>(obs->insOutputs->velEcef.data()), sizeof(obs->insOutputs->velEcef));
438 }
439 if (obs->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_MAGECEF)
440 {
441 _filestream.write(reinterpret_cast<const char*>(obs->insOutputs->magEcef.data()), sizeof(obs->insOutputs->magEcef));
442 }
443 if (obs->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_ACCELECEF)
444 {
445 _filestream.write(reinterpret_cast<const char*>(obs->insOutputs->accelEcef.data()), sizeof(obs->insOutputs->accelEcef));
446 }
447 if (obs->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_LINEARACCELECEF)
448 {
449 _filestream.write(reinterpret_cast<const char*>(obs->insOutputs->linearAccelEcef.data()), sizeof(obs->insOutputs->linearAccelEcef));
450 }
451 if (obs->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_POSU)
452 {
453 _filestream.write(reinterpret_cast<const char*>(&obs->insOutputs->posU), sizeof(obs->insOutputs->posU));
454 }
455 if (obs->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_VELU)
456 {
457 _filestream.write(reinterpret_cast<const char*>(&obs->insOutputs->velU), sizeof(obs->insOutputs->velU));
458 }
459 }
460 // Group 7 (GNSS2)
461 if (obs->gnss2Outputs)
462 {
463 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_UTC)
464 {
465 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->timeUtc.year), sizeof(obs->gnss2Outputs->timeUtc.year));
466 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->timeUtc.month), sizeof(obs->gnss2Outputs->timeUtc.month));
467 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->timeUtc.day), sizeof(obs->gnss2Outputs->timeUtc.day));
468 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->timeUtc.hour), sizeof(obs->gnss2Outputs->timeUtc.hour));
469 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->timeUtc.min), sizeof(obs->gnss2Outputs->timeUtc.min));
470 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->timeUtc.sec), sizeof(obs->gnss2Outputs->timeUtc.sec));
471 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->timeUtc.ms), sizeof(obs->gnss2Outputs->timeUtc.ms));
472 }
473 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_TOW)
474 {
475 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->tow), sizeof(obs->gnss2Outputs->tow));
476 }
477 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_WEEK)
478 {
479 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->week), sizeof(obs->gnss2Outputs->week));
480 }
481 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_NUMSATS)
482 {
483 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->numSats), sizeof(obs->gnss2Outputs->numSats));
484 }
485 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_FIX)
486 {
487 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->fix), sizeof(obs->gnss2Outputs->fix));
488 }
489 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_POSLLA)
490 {
491 _filestream.write(reinterpret_cast<const char*>(obs->gnss2Outputs->posLla.data()), sizeof(obs->gnss2Outputs->posLla));
492 }
493 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_POSECEF)
494 {
495 _filestream.write(reinterpret_cast<const char*>(obs->gnss2Outputs->posEcef.data()), sizeof(obs->gnss2Outputs->posEcef));
496 }
497 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_VELNED)
498 {
499 _filestream.write(reinterpret_cast<const char*>(obs->gnss2Outputs->velNed.data()), sizeof(obs->gnss2Outputs->velNed));
500 }
501 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_VELECEF)
502 {
503 _filestream.write(reinterpret_cast<const char*>(obs->gnss2Outputs->velEcef.data()), sizeof(obs->gnss2Outputs->velEcef));
504 }
505 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_POSU)
506 {
507 _filestream.write(reinterpret_cast<const char*>(obs->gnss2Outputs->posU.data()), sizeof(obs->gnss2Outputs->posU));
508 }
509 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_VELU)
510 {
511 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->velU), sizeof(obs->gnss2Outputs->velU));
512 }
513 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_TIMEU)
514 {
515 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->timeU), sizeof(obs->gnss2Outputs->timeU));
516 }
517 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO)
518 {
519 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->timeInfo.status.status()), sizeof(obs->gnss2Outputs->timeInfo.status.status()));
520 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->timeInfo.leapSeconds), sizeof(obs->gnss2Outputs->timeInfo.leapSeconds));
521 }
522 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_DOP)
523 {
524 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->dop.gDop), sizeof(obs->gnss2Outputs->dop.gDop));
525 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->dop.pDop), sizeof(obs->gnss2Outputs->dop.pDop));
526 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->dop.tDop), sizeof(obs->gnss2Outputs->dop.tDop));
527 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->dop.vDop), sizeof(obs->gnss2Outputs->dop.vDop));
528 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->dop.hDop), sizeof(obs->gnss2Outputs->dop.hDop));
529 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->dop.nDop), sizeof(obs->gnss2Outputs->dop.nDop));
530 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->dop.eDop), sizeof(obs->gnss2Outputs->dop.eDop));
531 }
532 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_SATINFO)
533 {
534 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->satInfo.numSats), sizeof(obs->gnss2Outputs->satInfo.numSats));
535
536 for (auto& satellite : obs->gnss2Outputs->satInfo.satellites)
537 {
538 _filestream.write(reinterpret_cast<const char*>(&satellite.sys), sizeof(satellite.sys));
539 _filestream.write(reinterpret_cast<const char*>(&satellite.svId), sizeof(satellite.svId));
540 _filestream.write(reinterpret_cast<const char*>(&satellite.flags), sizeof(satellite.flags));
541 _filestream.write(reinterpret_cast<const char*>(&satellite.cno), sizeof(satellite.cno));
542 _filestream.write(reinterpret_cast<const char*>(&satellite.qi), sizeof(satellite.qi));
543 _filestream.write(reinterpret_cast<const char*>(&satellite.el), sizeof(satellite.el));
544 _filestream.write(reinterpret_cast<const char*>(&satellite.az), sizeof(satellite.az));
545 }
546 }
547 if (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_RAWMEAS)
548 {
549 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->raw.tow), sizeof(obs->gnss2Outputs->raw.tow));
550 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->raw.week), sizeof(obs->gnss2Outputs->raw.week));
551 _filestream.write(reinterpret_cast<const char*>(&obs->gnss2Outputs->raw.numSats), sizeof(obs->gnss2Outputs->raw.numSats));
552
553 for (auto& satellite : obs->gnss2Outputs->raw.satellites)
554 {
555 _filestream.write(reinterpret_cast<const char*>(&satellite.sys), sizeof(satellite.sys));
556 _filestream.write(reinterpret_cast<const char*>(&satellite.svId), sizeof(satellite.svId));
557 _filestream.write(reinterpret_cast<const char*>(&satellite.freq), sizeof(satellite.freq));
558 _filestream.write(reinterpret_cast<const char*>(&satellite.chan), sizeof(satellite.chan));
559 _filestream.write(reinterpret_cast<const char*>(&satellite.slot), sizeof(satellite.slot));
560 _filestream.write(reinterpret_cast<const char*>(&satellite.cno), sizeof(satellite.cno));
561 _filestream.write(reinterpret_cast<const char*>(&satellite.flags), sizeof(satellite.flags));
562 _filestream.write(reinterpret_cast<const char*>(&satellite.pr), sizeof(satellite.pr));
563 _filestream.write(reinterpret_cast<const char*>(&satellite.cp), sizeof(satellite.cp));
564 _filestream.write(reinterpret_cast<const char*>(&satellite.dp), sizeof(satellite.dp));
565 }
566 }
567 }
568 }
569 else
570 {
571 LOG_INFO("{}: If you want to log csv instead of binary, use the 'CsvLogger' instead.", nameId());
572 }
573
574 LOG_DATA("{}: Message logged", nameId());
575}
FlowManager.hpp
Save/Load the Nodes.
json
nlohmann::json json
json namespace
Definition FlowManager.hpp:23
Logger.hpp
Utility class for logging to console and file.
LOG_DATA
#define LOG_DATA
All output which occurs repeatedly every time observations are received.
Definition Logger.hpp:29
LOG_INFO
#define LOG_INFO
Info to the user on the state of the program.
Definition Logger.hpp:69
LOG_TRACE
#define LOG_TRACE
Detailled info to trace the execution of the program. Should not be called on functions which receive...
Definition Logger.hpp:65
StringUtil.hpp
Utility functions for working with std::strings.
VectorNavBinaryOutput.hpp
Binary Outputs from VectorNav Sensors.
VectorNavDataLogger.hpp
Data Logger for VectorNav observations.
NAV::CommonLog::ShowOriginInput
static bool ShowOriginInput(const char *id)
Shows a GUI to input a origin location.
Definition CommonLog.cpp:115
NAV::CommonLog::initialize
void initialize() const
Initialize the common log variables.
Definition CommonLog.cpp:51
NAV::FileWriter::_fileType
FileType _fileType
File Type.
Definition FileWriter.hpp:88
NAV::FileWriter::deinitialize
void deinitialize()
Deinitialize the file reader.
Definition FileWriter.cpp:105
NAV::FileWriter::guiConfig
bool guiConfig(const char *vFilters, const std::vector< std::string > &extensions, size_t id, const std::string &nameId)
ImGui config.
Definition FileWriter.cpp:20
NAV::FileWriter::restore
void restore(const json &j)
Restores the node from a json object.
Definition FileWriter.cpp:57
NAV::FileWriter::save
json save() const
Saves the node into a json object.
Definition FileWriter.cpp:45
NAV::FileWriter::initialize
bool initialize()
Initialize the file reader.
Definition FileWriter.cpp:71
NAV::FileWriter::_filestream
std::ofstream _filestream
File stream to write the file.
Definition FileWriter.hpp:85
NAV::InputPin
Input pins of nodes.
Definition Pin.hpp:491
NAV::InputPin::NodeDataQueue
TsDeque< std::shared_ptr< const NAV::NodeData > > NodeDataQueue
Node data queue type.
Definition Pin.hpp:707
NAV::Node::isInitialized
bool isInitialized() const
Checks if the node is initialized.
Definition Node.cpp:574
NAV::Node::doDeinitialize
bool doDeinitialize(bool wait=false)
Asks the node worker to deinitialize the node.
Definition Node.cpp:465
NAV::Node::_guiConfigDefaultWindowSize
ImVec2 _guiConfigDefaultWindowSize
Definition Node.hpp:522
NAV::Node::Node
Node(std::string name)
Constructor.
Definition Node.cpp:29
NAV::Node::nameId
std::string nameId() const
Node name and id.
Definition Node.cpp:323
NAV::Node::name
std::string name
Name of the Node.
Definition Node.hpp:507
NAV::Node::CreateInputPin
InputPin * CreateInputPin(const char *name, Pin::Type pinType, const std::vector< std::string > &dataIdentifier={}, InputPin::Callback callback=static_cast< InputPin::FlowFirableCallbackFunc >(nullptr), InputPin::FlowFirableCheckFunc firable=nullptr, int priority=0, int idx=-1)
Create an Input Pin object.
Definition Node.cpp:252
NAV::Node::_hasConfig
bool _hasConfig
Flag if the config window should be shown.
Definition Node.hpp:525
NAV::OutputPin
Output pins of nodes.
Definition Pin.hpp:338
NAV::TsDeque::extract_front
auto extract_front()
Returns a copy of the first element in the container and removes it from the container.
Definition TsDeque.hpp:494
NAV::VectorNavBinaryOutput::type
static std::string type()
Returns the type of the data class.
Definition VectorNavBinaryOutput.hpp:48
NAV::VectorNavDataLogger::category
static std::string category()
String representation of the Class Category.
Definition VectorNavDataLogger.cpp:51
NAV::VectorNavDataLogger::restore
void restore(const json &j) override
Restores the node from a json object.
Definition VectorNavDataLogger.cpp:81
NAV::VectorNavDataLogger::guiConfig
void guiConfig() override
ImGui config window which is shown on double click.
Definition VectorNavDataLogger.cpp:56
NAV::VectorNavDataLogger::~VectorNavDataLogger
~VectorNavDataLogger() override
Destructor.
Definition VectorNavDataLogger.cpp:36
NAV::VectorNavDataLogger::onCreateLink
bool onCreateLink(OutputPin &startPin, InputPin &endPin) override
Called when a new link is to be established.
Definition VectorNavDataLogger.cpp:91
NAV::VectorNavDataLogger::type
std::string type() const override
String representation of the Class Type.
Definition VectorNavDataLogger.cpp:46
NAV::VectorNavDataLogger::deinitialize
void deinitialize() override
Deinitialize the node.
Definition VectorNavDataLogger.cpp:125
NAV::VectorNavDataLogger::flush
void flush() override
Function called by the flow executer after finishing to flush out remaining data.
Definition VectorNavDataLogger.cpp:104
NAV::VectorNavDataLogger::typeStatic
static std::string typeStatic()
String representation of the Class Type.
Definition VectorNavDataLogger.cpp:41
NAV::VectorNavDataLogger::_headerWritten
bool _headerWritten
Flag to write the header once.
Definition VectorNavDataLogger.hpp:83
NAV::VectorNavDataLogger::initialize
bool initialize() override
Initialize the node.
Definition VectorNavDataLogger.cpp:109
NAV::VectorNavDataLogger::writeObservation
void writeObservation(InputPin::NodeDataQueue &queue, size_t pinIdx)
Write Observation to the file.
Definition VectorNavDataLogger.cpp:132
NAV::VectorNavDataLogger::save
json save() const override
Saves the node into a json object.
Definition VectorNavDataLogger.cpp:70
NAV::VectorNavDataLogger::VectorNavDataLogger
VectorNavDataLogger()
Default constructor.
Definition VectorNavDataLogger.cpp:23
NAV::flow::ApplyChanges
void ApplyChanges()
Signals that there have been changes to the flow.
Definition FlowManager.cpp:682
NAV::Pin::Type::Flow
@ Flow
NodeData Trigger.
Definition Pin.hpp:52