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