0.5.1
Loading...
Searching...
No Matches
VectorNavSensor.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 "VectorNavSensor.hpp"
10
11#include "util/Logger.hpp"
12#include "vn/searcher.h"
13#include "vn/util.h"
14#include "util/Vendor/VectorNav/VectorNavTypes.hpp"
15
16#include "internal/gui/widgets/HelpMarker.hpp"
17
18#include <vn/types.h>
19#include "internal/FlowManager.hpp"
20
21#include <imgui_internal.h>
22
23#include "NodeData/General/StringObs.hpp"
24
25#include "util/Time/TimeBase.hpp"
26#include "Navigation/Transformations/CoordinateFrames.hpp"
27#include "Navigation/Transformations/Units.hpp"
28
29#include <map>
30
31// to_json / from_json
32namespace vn
33{
34namespace math
35{
36static void to_json(json& j, const vn::math::vec3f& vec) // NOLINT(misc-use-anonymous-namespace)
37{
38 j = vec.c;
39}
40static void from_json(const json& j, vn::math::vec3f& vec) // NOLINT(misc-use-anonymous-namespace)
41{
42 j.get_to(vec.c);
43}
44static void to_json(json& j, const vn::math::vec3d& vec) // NOLINT(misc-use-anonymous-namespace)
45{
46 j = vec.c;
47}
48static void from_json(const json& j, vn::math::vec3d& vec) // NOLINT(misc-use-anonymous-namespace)
49{
50 j.get_to(vec.c);
51}
52static void to_json(json& j, const vn::math::mat3f& mat) // NOLINT(misc-use-anonymous-namespace)
53{
54 j = mat.e;
55}
56static void from_json(const json& j, vn::math::mat3f& mat) // NOLINT(misc-use-anonymous-namespace)
57{
58 j.get_to(mat.e);
59}
60} // namespace math
61
62namespace sensors
63{
64// ###########################################################################################################
65// SYSTEM MODULE
66// ###########################################################################################################
67
68static void to_json(json& j, const SynchronizationControlRegister& synchronizationControlRegister) // NOLINT(misc-use-anonymous-namespace)
69{
70 j = json{
71 { "syncInMode", synchronizationControlRegister.syncInMode },
72 { "syncInEdge", synchronizationControlRegister.syncInEdge },
73 { "syncInSkipFactor", synchronizationControlRegister.syncInSkipFactor },
74 { "syncOutMode", synchronizationControlRegister.syncOutMode },
75 { "syncOutPolarity", synchronizationControlRegister.syncOutPolarity },
76 { "syncOutSkipFactor", synchronizationControlRegister.syncOutSkipFactor },
77 { "syncOutPulseWidth", synchronizationControlRegister.syncOutPulseWidth },
78 };
79}
80static void from_json(const json& j, SynchronizationControlRegister& synchronizationControlRegister) // NOLINT(misc-use-anonymous-namespace)
81{
82 if (j.contains("syncInMode"))
83 {
84 j.at("syncInMode").get_to(synchronizationControlRegister.syncInMode);
85 }
86 if (j.contains("syncInEdge"))
87 {
88 j.at("syncInEdge").get_to(synchronizationControlRegister.syncInEdge);
89 }
90 if (j.contains("syncInSkipFactor"))
91 {
92 j.at("syncInSkipFactor").get_to(synchronizationControlRegister.syncInSkipFactor);
93 }
94 if (j.contains("syncOutMode"))
95 {
96 j.at("syncOutMode").get_to(synchronizationControlRegister.syncOutMode);
97 }
98 if (j.contains("syncOutPolarity"))
99 {
100 j.at("syncOutPolarity").get_to(synchronizationControlRegister.syncOutPolarity);
101 }
102 if (j.contains("syncOutSkipFactor"))
103 {
104 j.at("syncOutSkipFactor").get_to(synchronizationControlRegister.syncOutSkipFactor);
105 }
106 if (j.contains("syncOutPulseWidth"))
107 {
108 j.at("syncOutPulseWidth").get_to(synchronizationControlRegister.syncOutPulseWidth);
109 }
110}
111
112static void to_json(json& j, const CommunicationProtocolControlRegister& communicationProtocolControlRegister) // NOLINT(misc-use-anonymous-namespace)
113{
114 j = json{
115 { "serialCount", communicationProtocolControlRegister.serialCount },
116 { "serialStatus", communicationProtocolControlRegister.serialStatus },
117 { "spiCount", communicationProtocolControlRegister.spiCount },
118 { "spiStatus", communicationProtocolControlRegister.spiStatus },
119 { "serialChecksum", communicationProtocolControlRegister.serialChecksum },
120 { "spiChecksum", communicationProtocolControlRegister.spiChecksum },
121 { "errorMode", communicationProtocolControlRegister.errorMode },
122 };
123}
124static void from_json(const json& j, CommunicationProtocolControlRegister& communicationProtocolControlRegister) // NOLINT(misc-use-anonymous-namespace)
125{
126 if (j.contains("serialCount"))
127 {
128 j.at("serialCount").get_to(communicationProtocolControlRegister.serialCount);
129 }
130 if (j.contains("serialStatus"))
131 {
132 j.at("serialStatus").get_to(communicationProtocolControlRegister.serialStatus);
133 }
134 if (j.contains("spiCount"))
135 {
136 j.at("spiCount").get_to(communicationProtocolControlRegister.spiCount);
137 }
138 if (j.contains("spiStatus"))
139 {
140 j.at("spiStatus").get_to(communicationProtocolControlRegister.spiStatus);
141 }
142 if (j.contains("serialChecksum"))
143 {
144 j.at("serialChecksum").get_to(communicationProtocolControlRegister.serialChecksum);
145 }
146 if (j.contains("spiChecksum"))
147 {
148 j.at("spiChecksum").get_to(communicationProtocolControlRegister.spiChecksum);
149 }
150 if (j.contains("errorMode"))
151 {
152 j.at("errorMode").get_to(communicationProtocolControlRegister.errorMode);
153 }
154}
155
156static void to_json(json& j, const BinaryOutputRegister& binaryOutputRegister) // NOLINT(misc-use-anonymous-namespace)
157{
158 j = json{
159 { "asyncMode", binaryOutputRegister.asyncMode },
160 { "rateDivisor", binaryOutputRegister.rateDivisor },
161 { "commonField", binaryOutputRegister.commonField },
162 { "timeField", binaryOutputRegister.timeField },
163 { "imuField", binaryOutputRegister.imuField },
164 { "gpsField", binaryOutputRegister.gpsField },
165 { "attitudeField", binaryOutputRegister.attitudeField },
166 { "insField", binaryOutputRegister.insField },
167 { "gps2Field", binaryOutputRegister.gps2Field },
168 };
169}
170static void from_json(const json& j, BinaryOutputRegister& binaryOutputRegister) // NOLINT(misc-use-anonymous-namespace)
171{
172 if (j.contains("asyncMode"))
173 {
174 j.at("asyncMode").get_to(binaryOutputRegister.asyncMode);
175 }
176 if (j.contains("rateDivisor"))
177 {
178 j.at("rateDivisor").get_to(binaryOutputRegister.rateDivisor);
179 }
180 if (j.contains("commonField"))
181 {
182 j.at("commonField").get_to(binaryOutputRegister.commonField);
183 }
184 if (j.contains("timeField"))
185 {
186 j.at("timeField").get_to(binaryOutputRegister.timeField);
187 }
188 if (j.contains("imuField"))
189 {
190 j.at("imuField").get_to(binaryOutputRegister.imuField);
191 }
192 if (j.contains("gpsField"))
193 {
194 j.at("gpsField").get_to(binaryOutputRegister.gpsField);
195 }
196 if (j.contains("attitudeField"))
197 {
198 j.at("attitudeField").get_to(binaryOutputRegister.attitudeField);
199 }
200 if (j.contains("insField"))
201 {
202 j.at("insField").get_to(binaryOutputRegister.insField);
203 }
204 if (j.contains("gps2Field"))
205 {
206 j.at("gps2Field").get_to(binaryOutputRegister.gps2Field);
207 }
208}
209
210// ###########################################################################################################
211// IMU SUBSYSTEM
212// ###########################################################################################################
213
214static void to_json(json& j, const ImuFilteringConfigurationRegister& imuFilteringConfigurationRegister) // NOLINT(misc-use-anonymous-namespace)
215{
216 j = json{
217 { "magWindowSize", imuFilteringConfigurationRegister.magWindowSize },
218 { "accelWindowSize", imuFilteringConfigurationRegister.accelWindowSize },
219 { "gyroWindowSize", imuFilteringConfigurationRegister.gyroWindowSize },
220 { "tempWindowSize", imuFilteringConfigurationRegister.tempWindowSize },
221 { "presWindowSize", imuFilteringConfigurationRegister.presWindowSize },
222 { "magFilterMode", imuFilteringConfigurationRegister.magFilterMode },
223 { "accelFilterMode", imuFilteringConfigurationRegister.accelFilterMode },
224 { "gyroFilterMode", imuFilteringConfigurationRegister.gyroFilterMode },
225 { "tempFilterMode", imuFilteringConfigurationRegister.tempFilterMode },
226 { "presFilterMode", imuFilteringConfigurationRegister.presFilterMode },
227 };
228}
229static void from_json(const json& j, ImuFilteringConfigurationRegister& imuFilteringConfigurationRegister) // NOLINT(misc-use-anonymous-namespace)
230{
231 if (j.contains("magWindowSize"))
232 {
233 j.at("magWindowSize").get_to(imuFilteringConfigurationRegister.magWindowSize);
234 }
235 if (j.contains("accelWindowSize"))
236 {
237 j.at("accelWindowSize").get_to(imuFilteringConfigurationRegister.accelWindowSize);
238 }
239 if (j.contains("gyroWindowSize"))
240 {
241 j.at("gyroWindowSize").get_to(imuFilteringConfigurationRegister.gyroWindowSize);
242 }
243 if (j.contains("tempWindowSize"))
244 {
245 j.at("tempWindowSize").get_to(imuFilteringConfigurationRegister.tempWindowSize);
246 }
247 if (j.contains("presWindowSize"))
248 {
249 j.at("presWindowSize").get_to(imuFilteringConfigurationRegister.presWindowSize);
250 }
251 if (j.contains("magFilterMode"))
252 {
253 j.at("magFilterMode").get_to(imuFilteringConfigurationRegister.magFilterMode);
254 }
255 if (j.contains("accelFilterMode"))
256 {
257 j.at("accelFilterMode").get_to(imuFilteringConfigurationRegister.accelFilterMode);
258 }
259 if (j.contains("gyroFilterMode"))
260 {
261 j.at("gyroFilterMode").get_to(imuFilteringConfigurationRegister.gyroFilterMode);
262 }
263 if (j.contains("tempFilterMode"))
264 {
265 j.at("tempFilterMode").get_to(imuFilteringConfigurationRegister.tempFilterMode);
266 }
267 if (j.contains("presFilterMode"))
268 {
269 j.at("presFilterMode").get_to(imuFilteringConfigurationRegister.presFilterMode);
270 }
271}
272
273static void to_json(json& j, const DeltaThetaAndDeltaVelocityConfigurationRegister& deltaThetaAndDeltaVelocityConfigurationRegister) // NOLINT(misc-use-anonymous-namespace)
274{
275 j = json{
276 { "integrationFrame", deltaThetaAndDeltaVelocityConfigurationRegister.integrationFrame },
277 { "gyroCompensation", deltaThetaAndDeltaVelocityConfigurationRegister.gyroCompensation },
278 { "accelCompensation", deltaThetaAndDeltaVelocityConfigurationRegister.accelCompensation },
279 { "earthRateCorrection", deltaThetaAndDeltaVelocityConfigurationRegister.earthRateCorrection },
280 };
281}
282static void from_json(const json& j, DeltaThetaAndDeltaVelocityConfigurationRegister& deltaThetaAndDeltaVelocityConfigurationRegister) // NOLINT(misc-use-anonymous-namespace)
283{
284 if (j.contains("integrationFrame"))
285 {
286 j.at("integrationFrame").get_to(deltaThetaAndDeltaVelocityConfigurationRegister.integrationFrame);
287 }
288 if (j.contains("gyroCompensation"))
289 {
290 j.at("gyroCompensation").get_to(deltaThetaAndDeltaVelocityConfigurationRegister.gyroCompensation);
291 }
292 if (j.contains("accelCompensation"))
293 {
294 j.at("accelCompensation").get_to(deltaThetaAndDeltaVelocityConfigurationRegister.accelCompensation);
295 }
296 if (j.contains("earthRateCorrection"))
297 {
298 j.at("earthRateCorrection").get_to(deltaThetaAndDeltaVelocityConfigurationRegister.earthRateCorrection);
299 }
300}
301
302// ###########################################################################################################
303// GNSS SUBSYSTEM
304// ###########################################################################################################
305
306static void to_json(json& j, const GpsConfigurationRegister& gpsConfigurationRegister) // NOLINT(misc-use-anonymous-namespace)
307{
308 j = json{
309 { "mode", gpsConfigurationRegister.mode },
310 { "ppsSource", gpsConfigurationRegister.ppsSource },
311 { "rate", gpsConfigurationRegister.rate },
312 { "antPow", gpsConfigurationRegister.antPow },
313 };
314}
315static void from_json(const json& j, GpsConfigurationRegister& gpsConfigurationRegister) // NOLINT(misc-use-anonymous-namespace)
316{
317 if (j.contains("mode"))
318 {
319 j.at("mode").get_to(gpsConfigurationRegister.mode);
320 }
321 if (j.contains("ppsSource"))
322 {
323 j.at("ppsSource").get_to(gpsConfigurationRegister.ppsSource);
324 }
325 if (j.contains("rate"))
326 {
327 j.at("rate").get_to(gpsConfigurationRegister.rate);
328 }
329 if (j.contains("antPow"))
330 {
331 j.at("antPow").get_to(gpsConfigurationRegister.antPow);
332 }
333}
334
335static void to_json(json& j, const GpsCompassBaselineRegister& gpsCompassBaselineRegister) // NOLINT(misc-use-anonymous-namespace)
336{
337 j = json{
338 { "position", gpsCompassBaselineRegister.position },
339 { "uncertainty", gpsCompassBaselineRegister.uncertainty },
340 };
341}
342static void from_json(const json& j, GpsCompassBaselineRegister& gpsCompassBaselineRegister) // NOLINT(misc-use-anonymous-namespace)
343{
344 if (j.contains("position"))
345 {
346 j.at("position").get_to(gpsCompassBaselineRegister.position);
347 }
348 if (j.contains("uncertainty"))
349 {
350 j.at("uncertainty").get_to(gpsCompassBaselineRegister.uncertainty);
351 }
352}
353
354// ###########################################################################################################
355// ATTITUDE SUBSYSTEM
356// ###########################################################################################################
357
358static void to_json(json& j, const VpeBasicControlRegister& vpeBasicControlRegister) // NOLINT(misc-use-anonymous-namespace)
359{
360 j = json{
361 { "enable", vpeBasicControlRegister.enable },
362 { "headingMode", vpeBasicControlRegister.headingMode },
363 { "filteringMode", vpeBasicControlRegister.filteringMode },
364 { "tuningMode", vpeBasicControlRegister.tuningMode },
365 };
366}
367static void from_json(const json& j, VpeBasicControlRegister& vpeBasicControlRegister) // NOLINT(misc-use-anonymous-namespace)
368{
369 if (j.contains("enable"))
370 {
371 j.at("enable").get_to(vpeBasicControlRegister.enable);
372 }
373 if (j.contains("headingMode"))
374 {
375 j.at("headingMode").get_to(vpeBasicControlRegister.headingMode);
376 }
377 if (j.contains("filteringMode"))
378 {
379 j.at("filteringMode").get_to(vpeBasicControlRegister.filteringMode);
380 }
381 if (j.contains("tuningMode"))
382 {
383 j.at("tuningMode").get_to(vpeBasicControlRegister.tuningMode);
384 }
385}
386
387static void to_json(json& j, const VpeMagnetometerBasicTuningRegister& vpeMagnetometerBasicTuningRegister) // NOLINT(misc-use-anonymous-namespace)
388{
389 j = json{
390 { "baseTuning", vpeMagnetometerBasicTuningRegister.baseTuning },
391 { "adaptiveTuning", vpeMagnetometerBasicTuningRegister.adaptiveTuning },
392 { "adaptiveFiltering", vpeMagnetometerBasicTuningRegister.adaptiveFiltering },
393 };
394}
395static void from_json(const json& j, VpeMagnetometerBasicTuningRegister& vpeMagnetometerBasicTuningRegister) // NOLINT(misc-use-anonymous-namespace)
396{
397 if (j.contains("baseTuning"))
398 {
399 j.at("baseTuning").get_to(vpeMagnetometerBasicTuningRegister.baseTuning);
400 }
401 if (j.contains("adaptiveTuning"))
402 {
403 j.at("adaptiveTuning").get_to(vpeMagnetometerBasicTuningRegister.adaptiveTuning);
404 }
405 if (j.contains("adaptiveFiltering"))
406 {
407 j.at("adaptiveFiltering").get_to(vpeMagnetometerBasicTuningRegister.adaptiveFiltering);
408 }
409}
410
411static void to_json(json& j, const VpeAccelerometerBasicTuningRegister& vpeAccelerometerBasicTuningRegister) // NOLINT(misc-use-anonymous-namespace)
412{
413 j = json{
414 { "baseTuning", vpeAccelerometerBasicTuningRegister.baseTuning },
415 { "adaptiveTuning", vpeAccelerometerBasicTuningRegister.adaptiveTuning },
416 { "adaptiveFiltering", vpeAccelerometerBasicTuningRegister.adaptiveFiltering },
417 };
418}
419static void from_json(const json& j, VpeAccelerometerBasicTuningRegister& vpeAccelerometerBasicTuningRegister) // NOLINT(misc-use-anonymous-namespace)
420{
421 if (j.contains("baseTuning"))
422 {
423 j.at("baseTuning").get_to(vpeAccelerometerBasicTuningRegister.baseTuning);
424 }
425 if (j.contains("adaptiveTuning"))
426 {
427 j.at("adaptiveTuning").get_to(vpeAccelerometerBasicTuningRegister.adaptiveTuning);
428 }
429 if (j.contains("adaptiveFiltering"))
430 {
431 j.at("adaptiveFiltering").get_to(vpeAccelerometerBasicTuningRegister.adaptiveFiltering);
432 }
433}
434
435static void to_json(json& j, const VpeGyroBasicTuningRegister& vpeGyroBasicTuningRegister) // NOLINT(misc-use-anonymous-namespace)
436{
437 j = json{
438 { "angularWalkVariance", vpeGyroBasicTuningRegister.angularWalkVariance },
439 { "baseTuning", vpeGyroBasicTuningRegister.baseTuning },
440 { "adaptiveTuning", vpeGyroBasicTuningRegister.adaptiveTuning },
441 };
442}
443static void from_json(const json& j, VpeGyroBasicTuningRegister& vpeGyroBasicTuningRegister) // NOLINT(misc-use-anonymous-namespace)
444{
445 if (j.contains("angularWalkVariance"))
446 {
447 j.at("angularWalkVariance").get_to(vpeGyroBasicTuningRegister.angularWalkVariance);
448 }
449 if (j.contains("baseTuning"))
450 {
451 j.at("baseTuning").get_to(vpeGyroBasicTuningRegister.baseTuning);
452 }
453 if (j.contains("adaptiveTuning"))
454 {
455 j.at("adaptiveTuning").get_to(vpeGyroBasicTuningRegister.adaptiveTuning);
456 }
457}
458
459// ###########################################################################################################
460// INS SUBSYSTEM
461// ###########################################################################################################
462
463static void to_json(json& j, const InsBasicConfigurationRegisterVn300& insBasicConfigurationRegisterVn300) // NOLINT(misc-use-anonymous-namespace)
464{
465 j = json{
466 { "scenario", insBasicConfigurationRegisterVn300.scenario },
467 { "ahrsAiding", insBasicConfigurationRegisterVn300.ahrsAiding },
468 { "estBaseline", insBasicConfigurationRegisterVn300.estBaseline },
469 };
470}
471static void from_json(const json& j, InsBasicConfigurationRegisterVn300& insBasicConfigurationRegisterVn300) // NOLINT(misc-use-anonymous-namespace)
472{
473 if (j.contains("scenario"))
474 {
475 j.at("scenario").get_to(insBasicConfigurationRegisterVn300.scenario);
476 }
477 if (j.contains("ahrsAiding"))
478 {
479 j.at("ahrsAiding").get_to(insBasicConfigurationRegisterVn300.ahrsAiding);
480 }
481 if (j.contains("estBaseline"))
482 {
483 j.at("estBaseline").get_to(insBasicConfigurationRegisterVn300.estBaseline);
484 }
485}
486
487static void to_json(json& j, const StartupFilterBiasEstimateRegister& startupFilterBiasEstimateRegister) // NOLINT(misc-use-anonymous-namespace)
488{
489 j = json{
490 { "gyroBias", startupFilterBiasEstimateRegister.gyroBias },
491 { "accelBias", startupFilterBiasEstimateRegister.accelBias },
492 { "pressureBias", startupFilterBiasEstimateRegister.pressureBias },
493 };
494}
495static void from_json(const json& j, StartupFilterBiasEstimateRegister& startupFilterBiasEstimateRegister) // NOLINT(misc-use-anonymous-namespace)
496{
497 if (j.contains("gyroBias"))
498 {
499 j.at("gyroBias").get_to(startupFilterBiasEstimateRegister.gyroBias);
500 }
501 if (j.contains("accelBias"))
502 {
503 j.at("accelBias").get_to(startupFilterBiasEstimateRegister.accelBias);
504 }
505 if (j.contains("pressureBias"))
506 {
507 j.at("pressureBias").get_to(startupFilterBiasEstimateRegister.pressureBias);
508 }
509}
510
511// ###########################################################################################################
512// HARD/SOFT IRON ESTIMATOR SUBSYSTEM
513// ###########################################################################################################
514
515static void to_json(json& j, const MagnetometerCalibrationControlRegister& magnetometerCalibrationControlRegister) // NOLINT(misc-use-anonymous-namespace)
516{
517 j = json{
518 { "hsiMode", magnetometerCalibrationControlRegister.hsiMode },
519 { "hsiOutput", magnetometerCalibrationControlRegister.hsiOutput },
520 { "convergeRate", magnetometerCalibrationControlRegister.convergeRate },
521 };
522}
523static void from_json(const json& j, MagnetometerCalibrationControlRegister& magnetometerCalibrationControlRegister) // NOLINT(misc-use-anonymous-namespace)
524{
525 if (j.contains("hsiMode"))
526 {
527 j.at("hsiMode").get_to(magnetometerCalibrationControlRegister.hsiMode);
528 }
529 if (j.contains("hsiOutput"))
530 {
531 j.at("hsiOutput").get_to(magnetometerCalibrationControlRegister.hsiOutput);
532 }
533 if (j.contains("convergeRate"))
534 {
535 j.at("convergeRate").get_to(magnetometerCalibrationControlRegister.convergeRate);
536 }
537}
538
539// ###########################################################################################################
540// WORLD MAGNETIC & GRAVITY MODULE
541// ###########################################################################################################
542
543static void to_json(json& j, const MagneticAndGravityReferenceVectorsRegister& magneticAndGravityReferenceVectorsRegister) // NOLINT(misc-use-anonymous-namespace)
544{
545 j = json{
546 { "magRef", magneticAndGravityReferenceVectorsRegister.magRef },
547 { "accRef", magneticAndGravityReferenceVectorsRegister.accRef },
548 };
549}
550static void from_json(const json& j, MagneticAndGravityReferenceVectorsRegister& magneticAndGravityReferenceVectorsRegister) // NOLINT(misc-use-anonymous-namespace)
551{
552 if (j.contains("magRef"))
553 {
554 j.at("magRef").get_to(magneticAndGravityReferenceVectorsRegister.magRef);
555 }
556 if (j.contains("accRef"))
557 {
558 j.at("accRef").get_to(magneticAndGravityReferenceVectorsRegister.accRef);
559 }
560}
561
562static void to_json(json& j, const ReferenceVectorConfigurationRegister& referenceVectorConfigurationRegister) // NOLINT(misc-use-anonymous-namespace)
563{
564 j = json{
565 { "useMagModel", referenceVectorConfigurationRegister.useMagModel },
566 { "useGravityModel", referenceVectorConfigurationRegister.useGravityModel },
567 { "recalcThreshold", referenceVectorConfigurationRegister.recalcThreshold },
568 { "year", referenceVectorConfigurationRegister.year },
569 { "position", referenceVectorConfigurationRegister.position },
570 };
571}
572static void from_json(const json& j, ReferenceVectorConfigurationRegister& referenceVectorConfigurationRegister) // NOLINT(misc-use-anonymous-namespace)
573{
574 if (j.contains("useMagModel"))
575 {
576 j.at("useMagModel").get_to(referenceVectorConfigurationRegister.useMagModel);
577 }
578 if (j.contains("useGravityModel"))
579 {
580 j.at("useGravityModel").get_to(referenceVectorConfigurationRegister.useGravityModel);
581 }
582 if (j.contains("recalcThreshold"))
583 {
584 j.at("recalcThreshold").get_to(referenceVectorConfigurationRegister.recalcThreshold);
585 }
586 if (j.contains("year"))
587 {
588 j.at("year").get_to(referenceVectorConfigurationRegister.year);
589 }
590 if (j.contains("position"))
591 {
592 j.at("position").get_to(referenceVectorConfigurationRegister.position);
593 }
594}
595
596// ###########################################################################################################
597// VELOCITY AIDING
598// ###########################################################################################################
599
600static void to_json(json& j, const VelocityCompensationControlRegister& velocityCompensationControlRegister) // NOLINT(misc-use-anonymous-namespace)
601{
602 j = json{
603 { "mode", velocityCompensationControlRegister.mode },
604 { "velocityTuning", velocityCompensationControlRegister.velocityTuning },
605 { "rateTuning", velocityCompensationControlRegister.rateTuning },
606 };
607}
608static void from_json(const json& j, VelocityCompensationControlRegister& velocityCompensationControlRegister) // NOLINT(misc-use-anonymous-namespace)
609{
610 if (j.contains("mode"))
611 {
612 j.at("mode").get_to(velocityCompensationControlRegister.mode);
613 }
614 if (j.contains("velocityTuning"))
615 {
616 j.at("velocityTuning").get_to(velocityCompensationControlRegister.velocityTuning);
617 }
618 if (j.contains("rateTuning"))
619 {
620 j.at("rateTuning").get_to(velocityCompensationControlRegister.rateTuning);
621 }
622}
623
624} // namespace sensors
625} // namespace vn
626
627void NAV::VectorNavSensor::coupleImuFilterRates(NAV::VectorNavSensor* sensor, vn::sensors::BinaryOutputRegister& bor, uint32_t& /* binaryField */)
628{
629 // Set 'TimeStartup' and 'TimeStatus' always
630 (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS);
631
632 if (sensor->_sensorModel == VectorNavModel::VN310)
633 {
634 (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK);
635 }
636
637 if (sensor->_coupleImuRateOutput)
638 {
639 if (bor.imuField & (vn::protocol::uart::IMUGROUP_ACCEL | vn::protocol::uart::IMUGROUP_UNCOMPACCEL))
640 {
641 sensor->_imuFilteringConfigurationRegister.accelWindowSize = bor.rateDivisor;
642 }
643 if (bor.imuField & (vn::protocol::uart::IMUGROUP_ANGULARRATE | vn::protocol::uart::IMUGROUP_UNCOMPGYRO))
644 {
645 sensor->_imuFilteringConfigurationRegister.gyroWindowSize = bor.rateDivisor;
646 }
647 if (bor.imuField & (vn::protocol::uart::IMUGROUP_MAG | vn::protocol::uart::IMUGROUP_UNCOMPMAG))
648 {
649 sensor->_imuFilteringConfigurationRegister.magWindowSize = bor.rateDivisor;
650 }
651 if (bor.imuField & (vn::protocol::uart::IMUGROUP_MAG | vn::protocol::uart::IMUGROUP_UNCOMPMAG))
652 {
653 sensor->_imuFilteringConfigurationRegister.magWindowSize = bor.rateDivisor;
654 }
655 if (bor.imuField & vn::protocol::uart::IMUGROUP_TEMP)
656 {
657 sensor->_imuFilteringConfigurationRegister.tempWindowSize = bor.rateDivisor;
658 }
659 if (bor.imuField & vn::protocol::uart::IMUGROUP_PRES)
660 {
661 sensor->_imuFilteringConfigurationRegister.presWindowSize = bor.rateDivisor;
662 }
663 LOG_DATA("Frequency of ImuFilter matches output frequency (coupled 'rateDivisor' and 'windowSize' of moving average filter.)");
664
665 if (sensor->isInitialized() && sensor->_vs.isConnected() && sensor->_vs.verifySensorConnectivity())
666 {
667 try
668 {
669 sensor->_vs.writeImuFilteringConfiguration(sensor->_imuFilteringConfigurationRegister);
670 }
671 catch (const std::exception& e)
672 {
673 LOG_ERROR("Could not configure the imuFilteringConfigurationRegister: {}", e.what());
674 sensor->doDeinitialize();
675 }
676 }
677 else
678 {
679 sensor->doDeinitialize();
680 }
681 }
682};
683
684// NOLINTBEGIN
685
686// const std::array<NAV::VectorNavSensor::BinaryGroupData, 15> NAV::VectorNavSensor::_binaryGroupCommon = { {
687// { /* 0 */ "TimeStartup", vn::protocol::uart::CommonGroup::COMMONGROUP_TIMESTARTUP,
688// []() { ImGui::TextUnformatted("Time since startup.\n\nThe system time since startup measured in nano seconds. The time since startup is based upon the internal\nTXCO oscillator for the MCU. The accuracy of the internal TXCO is +/- 20ppm (-40C to 85C). This field is\nequivalent to the TimeStartup field in group 2."); } },
689// { /* 1 */ "TimeGps", vn::protocol::uart::CommonGroup::COMMONGROUP_TIMEGPS,
690// []() { ImGui::TextUnformatted("GPS time.\n\nThe absolute GPS time since start of GPS epoch 1980 expressed in nano seconds. This field is equivalent to\nthe TimeGps field in group 2."); },
691// [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
692// [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& /* binaryField */) { (bor.commonField & vn::protocol::uart::CommonGroup::COMMONGROUP_TIMEGPS) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS); } },
693// { /* 2 */ "TimeSyncIn", vn::protocol::uart::CommonGroup::COMMONGROUP_TIMESYNCIN,
694// []() { ImGui::TextUnformatted("Time since last SyncIn trigger.\n\nThe time since the last SyncIn trigger event expressed in nano seconds. This field is equivalent to the\nTimeSyncIn field in group 2."); } },
695// { /* 3 */ "YawPitchRoll", vn::protocol::uart::CommonGroup::COMMONGROUP_YAWPITCHROLL,
696// []() { ImGui::TextUnformatted("Estimated attitude as yaw pitch and roll angles.\n\nThe estimated attitude Yaw, Pitch, and Roll angles measured in degrees. The attitude is given as a 3,2,1 Euler\nangle sequence describing the body frame with respect to the local North East Down (NED) frame. This field\nis equivalent to the YawPitchRoll field in group 5.\n\nYaw [+/- 180°]\nPitch [+/- 90°]\nRoll [+/- 180°]"); } },
697// { /* 4 */ "Quaternion", vn::protocol::uart::CommonGroup::COMMONGROUP_QUATERNION,
698// []() { ImGui::TextUnformatted("Estimated attitude as a quaternion.\n\nThe estimated attitude quaternion. The last term is the scalar value. The attitude is given as the body frame\nwith respect to the local North East Down (NED) frame. This field is equivalent to the Quaternion field in\ngroup 5."); } },
699// { /* 5 */ "AngularRate", vn::protocol::uart::CommonGroup::COMMONGROUP_ANGULARRATE,
700// []() { ImGui::TextUnformatted("Compensated angular rate.\n\nThe estimated angular rate measured in rad/s. The angular rates are compensated by the onboard filter bias\nestimates. The angular rate is expressed in the body frame. This field is equivalent to the AngularRate field\nin group 3."); } },
701// { /* 6 */ "Position", vn::protocol::uart::CommonGroup::COMMONGROUP_POSITION,
702// []() { ImGui::TextUnformatted("Estimated position. (LLA)\n\nThe estimated position given as latitude, longitude, and altitude given in [deg, deg, m] respectively. This field\nis equivalent to the PosLla field in group 6."); },
703// [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
704// [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& /* binaryField */) { (bor.commonField & vn::protocol::uart::CommonGroup::COMMONGROUP_POSITION) && (bor.commonField |= vn::protocol::uart::CommonGroup::COMMONGROUP_INSSTATUS); } },
705// { /* 7 */ "Velocity", vn::protocol::uart::CommonGroup::COMMONGROUP_VELOCITY,
706// []() { ImGui::TextUnformatted("Estimated velocity. (NED)\n\nThe estimated velocity in the North East Down (NED) frame, given in m/s. This field is equivalent to the\nVelNed field in group 6."); },
707// [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
708// [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& /* binaryField */) { (bor.commonField & vn::protocol::uart::CommonGroup::COMMONGROUP_VELOCITY) && (bor.commonField |= vn::protocol::uart::CommonGroup::COMMONGROUP_INSSTATUS); } },
709// { /* 8 */ "Accel", vn::protocol::uart::CommonGroup::COMMONGROUP_ACCEL,
710// []() { ImGui::TextUnformatted("Estimated acceleration (compensated). (Body)\n\nThe estimated acceleration in the body frame, given in m/s^2. This acceleration includes gravity and has\nbeen bias compensated by the onboard INS Kalman filter. This field is equivalent to the Accel field in group 3."); } },
711// { /* 9 */ "Imu", vn::protocol::uart::CommonGroup::COMMONGROUP_IMU,
712// []() { ImGui::TextUnformatted("Calibrated uncompensated gyro and accelerometer measurements.\n\nThe uncompensated IMU acceleration and angular rate measurements. The acceleration is given in m/s^2,\nand the angular rate is given in rad/s. These measurements correspond to the calibrated angular rate and\nacceleration measurements straight from the IMU. The measurements have not been corrected for bias\noffset by the onboard Kalman filter. These are equivalent to the UncompAccel and UncompGyro fields in\ngroup 3."); } },
713// { /* 10 */ "MagPres", vn::protocol::uart::CommonGroup::COMMONGROUP_MAGPRES,
714// []() { ImGui::TextUnformatted("Calibrated magnetic (compensated), temperature, and pressure measurements.\n\nThe compensated magnetic, temperature, and pressure measurements from the IMU. The magnetic\nmeasurement is given in Gauss, and has been corrected for hard/soft iron corrections (if enabled). The\ntemperature measurement is given in Celsius. The pressure measurement is given in kPa. This field is\nequivalent to the Mag, Temp, and Pres fields in group 3.\n\nThe IP-68 enclosure on the tactical series forms an airtight (hermetic) seal isolating the internal\nsensors from the external environment. The pressure sensor is internal to this seal, and as such\nwill not measure the outside environment atmospheric pressure. It will instead read the pressure\ninside the sealed enclosure. The purpose of this sensor is to provide a means of ensuring the\nseal integrity over the lifetime of the product. Based on the Ideal Gas Law the ratio of pressure\ndivided by temperature should remain constant over both time and environmental temperature.\nWhen this is no longer the case, it can be assumed that the seal integrity has been compromised."); } },
715// { /* 11 */ "DeltaTheta", vn::protocol::uart::CommonGroup::COMMONGROUP_DELTATHETA,
716// []() { ImGui::TextUnformatted("Delta time, theta, and velocity.\n\nThe delta time, angle, and velocity measurements. The delta time (dtime) is the time interval that the delta\nangle and velocities are integrated over. The delta theta (dtheta) is the delta rotation angles incurred due to\nrotation, by the local body reference frame, since the last time the values were outputted by the device. The\ndelta velocity (dvel) is the delta velocity incurred due to motion, by the local body reference frame, since the\nlast time the values were outputted by the device. The frame of reference of these delta measurements are\ndetermined by the IntegrationFrame field in the Delta Theta and Delta Velocity Configuration Register\n(Register 82). These delta angles and delta velocities are calculated based upon the onboard coning and\nsculling integration performed onboard the sensor at the full IMU rate (default 800Hz). The integration for\nboth the delta angles and velocities are reset each time either of the values are either polled or sent out due\nto a scheduled asynchronous ASCII or binary output. Delta Theta and Delta Velocity values correctly capture\nthe nonlinearities involved in measuring motion from a rotating strapdown platform (as opposed to the older\nmechanically inertial navigation systems), thus providing you with the ability to integrate velocity and angular\nrate at much lower speeds (say for example 10 Hz, reducing bandwidth and computational complexity), while\nstill maintaining the same numeric precision as if you had performed the integration at the full IMU\nmeasurement rate of 800Hz. This field is equivalent to the DeltaTheta and DeltaVel fields in group 3 with the\ninclusion of the additional delta time parameter."); } },
717// { /* 12 */ "Ins/VpeStatus", vn::protocol::uart::CommonGroup::COMMONGROUP_INSSTATUS,
718// []() { ImGui::TextUnformatted("INS status (VN310).\n\nThe INS status bitfield. This field is equivalent to the InsSatus field in group 6.\nSee INS Solution LLA Register for more information on the individual bits in this field.");
719// ImGui::Separator();
720// ImGui::TextUnformatted("VPE Status (VN100).\n\nThe VPE status bitfield. This field is equivalent to the VpeStatus field in group 5.\nSee Group 5 - VPE for more information on the individual bits in this field."); },
721// [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t binaryField) { return !(static_cast<vn::protocol::uart::CommonGroup>(binaryField) & vn::protocol::uart::CommonGroup::COMMONGROUP_POSITION) && !(static_cast<vn::protocol::uart::CommonGroup>(binaryField) & vn::protocol::uart::CommonGroup::COMMONGROUP_VELOCITY); } },
722// { /* 13 */ "SyncInCnt", vn::protocol::uart::CommonGroup::COMMONGROUP_SYNCINCNT,
723// []() { ImGui::TextUnformatted("SyncIn count.\n\nThe number of SyncIn trigger events that have occurred. This field is equivalent to the SyncInCnt field in\ngroup 2."); } },
724// { /* 14 */ "TimeGpsPps", vn::protocol::uart::CommonGroup::COMMONGROUP_TIMEGPSPPS,
725// []() { ImGui::TextUnformatted("Time since last GNSS PPS trigger.\n\nThe time since the last GPS PPS trigger event expressed in nano seconds. This field is equivalent to the\nTimePPS field in group 2."); },
726// [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; } },
727// } };
728
729const std::array<NAV::VectorNavSensor::BinaryGroupData, 10> NAV::VectorNavSensor::_binaryGroupTime = { {
730 { /* 0 */ "TimeStartup", vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP,
731 []() { ImGui::TextUnformatted("Time since startup.\n\nThe system time since startup measured in nano seconds. The time since startup is based upon the internal\nTXCO oscillator for the MCU. The accuracy of the internal TXCO is +/- 20ppm (-40C to 85C)."); },
732 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& bor, uint32_t /* binaryField */) { return (sensorModel != VectorNavModel::VN310
733 || !((bor.gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_TOW)
734 || (bor.gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_WEEK)
735 || (bor.gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_TOW)
736 || (bor.gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_WEEK)))
737 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPMAG)
738 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPACCEL)
739 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPGYRO)
740 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_TEMP)
741 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_PRES)
742 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTATHETA)
743 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTAVEL)
744 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_MAG)
745 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_ACCEL)
746 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_ANGULARRATE)
747 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YAWPITCHROLL)
748 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_QUATERNION)
749 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_DCM)
750 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_MAGNED)
751 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_ACCELNED)
752 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY)
753 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELNED)
754 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YPRU)
755 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_INSSTATUS)
756 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_POSLLA)
757 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_POSECEF)
758 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_VELBODY)
759 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_VELNED)
760 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_VELECEF)
761 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_MAGECEF)
762 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_ACCELECEF)
763 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_LINEARACCELECEF)
764 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_POSU)
765 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_VELU); } },
766 { /* 1 */ "TimeGps", vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEGPS,
767 []() { ImGui::TextUnformatted("Absolute GPS time.\n\nThe absolute GPS time since start of GPS epoch 1980 expressed in nano seconds."); },
768 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
769 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& /* binaryField */) { (bor.timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEGPS) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS); } },
770 { /* 2 */ "GpsTow", vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW,
771 []() { ImGui::TextUnformatted("Time since start of GPS week.\n\nThe time since the start of the current GPS time week expressed in nano seconds."); },
772 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& bor, uint32_t /* binaryField */) { return (sensorModel == VectorNavModel::VN310)
773 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPMAG)
774 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPACCEL)
775 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPGYRO)
776 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_TEMP)
777 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_PRES)
778 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTATHETA)
779 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTAVEL)
780 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_MAG)
781 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_ACCEL)
782 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_ANGULARRATE)
783 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YAWPITCHROLL)
784 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_QUATERNION)
785 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_DCM)
786 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_MAGNED)
787 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_ACCELNED)
788 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY)
789 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELNED)
790 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YPRU)
791 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_INSSTATUS)
792 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_POSLLA)
793 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_POSECEF)
794 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_VELBODY)
795 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_VELNED)
796 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_VELECEF)
797 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_MAGECEF)
798 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_ACCELECEF)
799 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_LINEARACCELECEF)
800 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_POSU)
801 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_VELU); },
802 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& /* binaryField */) { (bor.timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS); } },
803 { /* 3 */ "GpsWeek", vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK,
804 []() { ImGui::TextUnformatted("GPS week.\n\nThe current GPS week."); },
805 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& bor, uint32_t /* binaryField */) { return (sensorModel == VectorNavModel::VN310)
806 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPMAG)
807 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPACCEL)
808 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPGYRO)
809 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_TEMP)
810 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_PRES)
811 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTATHETA)
812 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTAVEL)
813 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_MAG)
814 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_ACCEL)
815 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_ANGULARRATE)
816 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YAWPITCHROLL)
817 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_QUATERNION)
818 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_DCM)
819 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_MAGNED)
820 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_ACCELNED)
821 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY)
822 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELNED)
823 && !(bor.attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YPRU)
824 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_INSSTATUS)
825 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_POSLLA)
826 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_POSECEF)
827 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_VELBODY)
828 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_VELNED)
829 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_VELECEF)
830 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_MAGECEF)
831 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_ACCELECEF)
832 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_LINEARACCELECEF)
833 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_POSU)
834 && !(bor.insField & vn::protocol::uart::InsGroup::INSGROUP_VELU); },
835 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& /* binaryField */) { (bor.timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS); } },
836 { /* 4 */ "TimeSyncIn", vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESYNCIN,
837 []() { ImGui::TextUnformatted("Time since last SyncIn trigger.\n\nThe time since the last SyncIn event trigger expressed in nano seconds."); } },
838 { /* 5 */ "TimeGpsPps", vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEGPSPPS,
839 []() { ImGui::TextUnformatted("Time since last GPS PPS trigger.\n\nThe time since the last GPS PPS trigger event expressed in nano seconds."); },
840 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; } },
841 { /* 6 */ "TimeUTC", vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEUTC,
842 []() { ImGui::TextUnformatted("UTC time.\n\nThe current UTC time. The year is given as a signed byte year offset from the year 2000. For example the\nyear 2013 would be given as year 13."); },
843 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return false; },
844 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& /* binaryField */) { (bor.timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEUTC) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS); } },
845 { /* 7 */ "SyncInCnt", vn::protocol::uart::TimeGroup::TIMEGROUP_SYNCINCNT,
846 []() { ImGui::TextUnformatted("SyncIn trigger count.\n\nThe number of SyncIn trigger events that have occurred."); } },
847 { /* 8 */ "SyncOutCnt", vn::protocol::uart::TimeGroup::TIMEGROUP_SYNCOUTCNT,
848 []() { ImGui::TextUnformatted("SyncOut trigger count.\n\nThe number of SyncOut trigger events that have occurred."); } },
849 { /* 9 */ "TimeStatus", vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS,
850 []() { ImGui::TextUnformatted("Time valid status flags.");
851 if (ImGui::BeginTable("VectorNavTimeStatusTooltip", 3, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg))
852 {
853 ImGui::TableSetupColumn("Bit Offset", ImGuiTableColumnFlags_WidthFixed);
854 ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed);
855 ImGui::TableSetupColumn("Description", ImGuiTableColumnFlags_WidthFixed);
856 ImGui::TableHeadersRow();
857
858 ImGui::TableNextColumn(); ImGui::TextUnformatted("0");
859 ImGui::TableNextColumn(); ImGui::TextUnformatted("timeOk");
860 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 - GpsTow is valid");
861
862 ImGui::TableNextColumn(); ImGui::TextUnformatted("1");
863 ImGui::TableNextColumn(); ImGui::TextUnformatted("dateOk");
864 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 - TimeGps and GpsWeek are valid");
865
866 ImGui::TableNextColumn(); ImGui::TextUnformatted("2");
867 ImGui::TableNextColumn(); ImGui::TextUnformatted("utcTimeValid");
868 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 - UTC time is valid");
869
870 ImGui::TableNextColumn(); ImGui::TextUnformatted("3 - 7");
871 ImGui::TableNextColumn(); ImGui::TextUnformatted("resv");
872 ImGui::TableNextColumn(); ImGui::TextUnformatted("Reserved for future use");
873
874 ImGui::EndTable();
875 } },
876 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& bor, uint32_t /* binaryField */) { return !(bor.commonField & vn::protocol::uart::CommonGroup::COMMONGROUP_TIMEGPS)
877 && !(bor.timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW)
878 && !(bor.timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK)
879 && !(bor.timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEGPS)
880 && !(bor.timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEUTC)
881 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPMAG)
882 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPACCEL)
883 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPGYRO)
884 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_TEMP)
885 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_PRES)
886 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTATHETA)
887 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTAVEL)
888 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_MAG)
889 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_ACCEL)
890 && !(bor.imuField & vn::protocol::uart::ImuGroup::IMUGROUP_ANGULARRATE); } },
891} };
892
893const std::array<NAV::VectorNavSensor::BinaryGroupData, 11> NAV::VectorNavSensor::_binaryGroupIMU{ {
894 { /* 0 */ "ImuStatus", vn::protocol::uart::ImuGroup::IMUGROUP_IMUSTATUS,
895 []() { ImGui::TextUnformatted("Status is reserved for future use. Not currently used in the current code, as such will always report 0."); },
896 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return false; } },
897 { /* 1 */ "UncompMag", vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPMAG,
898 []() { ImGui::TextUnformatted("Uncompensated magnetic measurement.\n\nThe IMU magnetic field measured in units of Gauss, given in the body frame. This measurement is\ncompensated by the static calibration (individual factory calibration stored in flash), and the user\ncompensation, however it is not compensated by the onboard Hard/Soft Iron estimator."); },
899 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; }, coupleImuFilterRates },
900 { /* 2 */ "UncompAccel", vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPACCEL,
901 []() { ImGui::TextUnformatted("Uncompensated acceleration measurement.\n\nThe IMU acceleration measured in units of m/s^2, given in the body frame. This measurement is\ncompensated by the static calibration (individual factory calibration stored in flash), however it is not\ncompensated by any dynamic calibration such as bias compensation from the onboard INS Kalman filter."); },
902 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; }, coupleImuFilterRates },
903 { /* 3 */ "UncompGyro", vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPGYRO,
904 []() { ImGui::TextUnformatted("Uncompensated angular rate measurement.\n\nThe IMU angular rate measured in units of rad/s, given in the body frame. This measurement is compensated\nby the static calibration (individual factory calibration stored in flash), however it is not compensated by any\ndynamic calibration such as the bias compensation from the onboard AHRS/INS Kalman filters."); },
905 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; }, coupleImuFilterRates },
906 { /* 4 */ "Temp", vn::protocol::uart::ImuGroup::IMUGROUP_TEMP,
907 []() { ImGui::TextUnformatted("Temperature measurement.\n\nThe IMU temperature measured in units of Celsius."); },
908 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; }, coupleImuFilterRates },
909 { /* 5 */ "Pres", vn::protocol::uart::ImuGroup::IMUGROUP_PRES,
910 []() { ImGui::TextUnformatted("Pressure measurement.\n\nThe IMU pressure measured in kilopascals. This is an absolute pressure measurement. Typical pressure at sea level would be around 100 kPa."); },
911 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; }, coupleImuFilterRates },
912 { /* 6 */ "DeltaTheta", vn::protocol::uart::ImuGroup::IMUGROUP_DELTATHETA,
913 []() { ImGui::TextUnformatted("Delta theta angles.\n\nThe delta time (dtime) is the time interval that the delta angle and velocities are integrated over. The delta\ntheta (dtheta) is the delta rotation angles incurred due to rotation, by the local body reference frame, since\nthe last time the values were outputted by the device. The delta velocity (dvel) is the delta velocity incurred\ndue to motion, by the local body reference frame, since the last time the values were outputted by the device.\nThe frame of reference of these delta measurements are determined by the IntegrationFrame field in the\nDelta Theta and Delta Velocity Configuration Register (Register 82). These delta angles and delta velocities\nare calculated based upon the onboard coning and sculling integration performed onboard the sensor at the\nfull IMU rate (default 800Hz). The integration for both the delta angles and velocities are reset each time\neither of the values are either polled or sent out due to a scheduled asynchronous ASCII or binary output.\nDelta Theta and Delta Velocity values correctly capture the nonlinearities involved in measuring motion from\na rotating strapdown platform (as opposed to the older mechanically inertial navigation systems), thus\nproviding you with the ability to integrate velocity and angular rate at much lower speeds (say for example\n10 Hz, reducing bandwidth and computational complexity), while still maintaining the same numeric\nprecision as if you had performed the integration at the full IMU measurement rate of 800Hz. Time is given\nin seconds. Delta angles are given in degrees."); } },
914 { /* 7 */ "DeltaVel", vn::protocol::uart::ImuGroup::IMUGROUP_DELTAVEL,
915 []() { ImGui::TextUnformatted("Delta velocity.\n\nThe delta velocity (dvel) is the delta velocity incurred due to motion, since the last time the values were output\nby the device. The delta velocities are calculated based upon the onboard conning and sculling integration\nperformed onboard the sensor at the IMU sampling rate (nominally 800Hz). The integration for the delta\nvelocities are reset each time the values are either polled or sent out due to a scheduled asynchronous ASCII\nor binary output. Delta velocity is given in meters per second."); } },
916 { /* 8 */ "Mag", vn::protocol::uart::ImuGroup::IMUGROUP_MAG,
917 []() { ImGui::TextUnformatted("Compensated magnetic measurement.\n\nThe IMU compensated magnetic field measured units of Gauss, and given in the body frame. This\nmeasurement is compensated by the static calibration (individual factory calibration stored in flash), the user\ncompensation, and the dynamic calibration from the onboard Hard/Soft Iron estimator."); },
918 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; }, coupleImuFilterRates },
919 { /* 9 */ "Accel", vn::protocol::uart::ImuGroup::IMUGROUP_ACCEL,
920 []() { ImGui::TextUnformatted("Compensated acceleration measurement.\n\nThe compensated acceleration measured in units of m/s^2, and given in the body frame. This measurement\nis compensated by the static calibration (individual factory calibration stored in flash), the user\ncompensation, and the dynamic bias compensation from the onboard INS Kalman filter."); },
921 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; }, coupleImuFilterRates },
922 { /* 10 */ "AngularRate", vn::protocol::uart::ImuGroup::IMUGROUP_ANGULARRATE,
923 []() { ImGui::TextUnformatted("Compensated angular rate measurement.\n\nThe compensated angular rate measured in units of rad/s, and given in the body frame. This measurement\nis compensated by the static calibration (individual factor calibration stored in flash), the user compensation,\nand the dynamic bias compensation from the onboard INS Kalman filter."); },
924 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; }, coupleImuFilterRates },
925} };
926
927const std::array<NAV::VectorNavSensor::BinaryGroupData, 16> NAV::VectorNavSensor::_binaryGroupGNSS{ {
928 { /* 0 */ "UTC", vn::protocol::uart::GpsGroup::GPSGROUP_UTC,
929 []() { ImGui::TextUnformatted("GPS UTC Time\n\nThe current UTC time. The year is given as a signed byte year offset from the year 2000. For example the\nyear 2013 would be given as year 13."); },
930 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return false; },
931 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& /* bor */, uint32_t& binaryField) { (static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_UTC) && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO); } },
932 { /* 1 */ "Tow", vn::protocol::uart::GpsGroup::GPSGROUP_TOW,
933 []() { ImGui::TextUnformatted("GPS time of week\n\nThe GPS time of week given in nano seconds."); },
934 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t binaryField) { return sensorModel == VectorNavModel::VN310
935 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_POSLLA)
936 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_POSECEF)
937 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_VELNED)
938 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_VELECEF)
939 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_POSU)
940 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_VELU); },
941 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_TOW) && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO) && ((bor.commonField & vn::protocol::uart::CommonGroup::COMMONGROUP_TIMESTARTUP) || (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP)); } },
942 { /* 2 */ "Week", vn::protocol::uart::GpsGroup::GPSGROUP_WEEK,
943 []() { ImGui::TextUnformatted("GPS week\n\nThe current GPS week."); },
944 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t binaryField) { return sensorModel == VectorNavModel::VN310
945 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_POSLLA)
946 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_POSECEF)
947 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_VELNED)
948 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_VELECEF)
949 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_POSU)
950 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_VELU); },
951 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_WEEK) && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO) && ((bor.commonField & vn::protocol::uart::CommonGroup::COMMONGROUP_TIMESTARTUP) || (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP)); } },
952 { /* 3 */ "NumSats", vn::protocol::uart::GpsGroup::GPSGROUP_NUMSATS,
953 []() { ImGui::TextUnformatted("Number of tracked satellites\n\nThe number of tracked GNSS satellites."); },
954 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; } },
955 { /* 4 */ "Fix", vn::protocol::uart::GpsGroup::GPSGROUP_FIX,
956 []() { ImGui::TextUnformatted("GNSS fix\n\nThe current GNSS fix.");
957 if (ImGui::BeginTable("VectorNavFixTooltip", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg))
958 {
959 ImGui::TableSetupColumn("Value", ImGuiTableColumnFlags_WidthFixed);
960 ImGui::TableSetupColumn("Description", ImGuiTableColumnFlags_WidthFixed);
961 ImGui::TableHeadersRow();
962
963 ImGui::TableNextColumn(); ImGui::TextUnformatted("0");
964 ImGui::TableNextColumn(); ImGui::TextUnformatted("No fix");
965
966 ImGui::TableNextColumn(); ImGui::TextUnformatted("1");
967 ImGui::TableNextColumn(); ImGui::TextUnformatted("Time only");
968
969 ImGui::TableNextColumn(); ImGui::TextUnformatted("2");
970 ImGui::TableNextColumn(); ImGui::TextUnformatted("2D");
971
972 ImGui::TableNextColumn(); ImGui::TextUnformatted("3");
973 ImGui::TableNextColumn(); ImGui::TextUnformatted("3D");
974
975 ImGui::TableNextColumn(); ImGui::TextUnformatted("4");
976 ImGui::TableNextColumn(); ImGui::TextUnformatted("SBAS");
977
978 ImGui::TableNextColumn(); ImGui::TextUnformatted("7");
979 ImGui::TableNextColumn(); ImGui::TextUnformatted("RTK Float (only GNSS1)");
980
981 ImGui::TableNextColumn(); ImGui::TextUnformatted("8");
982 ImGui::TableNextColumn(); ImGui::TextUnformatted("RTK Fixed (only GNSS1)");
983
984 ImGui::EndTable();
985 } },
986 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t binaryField) { return sensorModel == VectorNavModel::VN310
987 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_POSLLA)
988 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_POSECEF)
989 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_VELNED)
990 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_VELECEF)
991 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_POSU)
992 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_VELU); } },
993 { /* 5 */ "PosLla", vn::protocol::uart::GpsGroup::GPSGROUP_POSLLA,
994 []() { ImGui::TextUnformatted("GNSS position (latitude, longitude, altitude)\n\nThe current GNSS position measurement given as the geodetic latitude, longitude and altitude above the\nellipsoid. The units are in [deg, deg, m] respectively."); },
995 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
996 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) {
997 (static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_POSLLA)
998 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_FIX)
999 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TOW)
1000 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_WEEK)
1001 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO);
1002 bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP;
1003 } },
1004 { /* 6 */ "PosEcef", vn::protocol::uart::GpsGroup::GPSGROUP_POSECEF,
1005 []() { ImGui::TextUnformatted("GNSS position (ECEF)\n\nThe current GNSS position given in the Earth centered Earth fixed (ECEF) coordinate frame, given in meters."); },
1006 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1007 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) {
1008 (static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_POSECEF)
1009 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_FIX)
1010 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TOW)
1011 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_WEEK)
1012 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO);
1013 bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP;
1014 } },
1015 { /* 7 */ "VelNed", vn::protocol::uart::GpsGroup::GPSGROUP_VELNED,
1016 []() { ImGui::TextUnformatted("GNSS velocity (NED)\n\nThe current GNSS velocity in the North East Down (NED) coordinate frame, given in m/s."); },
1017 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1018 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) {
1019 (static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_VELNED)
1020 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_FIX)
1021 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TOW)
1022 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_WEEK)
1023 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO);
1024 bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP;
1025 } },
1026 { /* 8 */ "VelEcef", vn::protocol::uart::GpsGroup::GPSGROUP_VELECEF,
1027 []() { ImGui::TextUnformatted("GNSS velocity (ECEF)\n\nThe current GNSS velocity in the Earth centered Earth fixed (ECEF) coordinate frame, given in m/s."); },
1028 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1029 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) {
1030 (static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_VELECEF)
1031 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_FIX)
1032 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TOW)
1033 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_WEEK)
1034 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO);
1035 bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP;
1036 } },
1037 { /* 9 */ "PosU", vn::protocol::uart::GpsGroup::GPSGROUP_POSU,
1038 []() { ImGui::TextUnformatted("GNSS position uncertainty (NED)\n\nThe current GNSS position uncertainty in the North East Down (NED) coordinate frame, given in meters (1 Sigma)."); },
1039 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1040 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) {
1041 (static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_POSU)
1042 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_FIX)
1043 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TOW)
1044 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_WEEK)
1045 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO);
1046 bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP;
1047 } },
1048 { /* 10 */ "VelU", vn::protocol::uart::GpsGroup::GPSGROUP_VELU,
1049 []() { ImGui::TextUnformatted("GNSS velocity uncertainty\n\nThe current GNSS velocity uncertainty, given in m/s (1 Sigma)."); },
1050 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1051 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) {
1052 (static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_VELU)
1053 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_FIX)
1054 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TOW)
1055 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_WEEK)
1056 && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO);
1057 bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP;
1058 } },
1059 { /* 11 */ "TimeU", vn::protocol::uart::GpsGroup::GPSGROUP_TIMEU,
1060 []() { ImGui::TextUnformatted("GNSS time uncertainty\n\nThe current GPS time uncertainty, given in seconds (1 Sigma)."); },
1061 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1062 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& /* bor */, uint32_t& binaryField) { (static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_TIMEU) && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO); } },
1063 { /* 12 */ "TimeInfo", vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO,
1064 []() { ImGui::TextUnformatted("GNSS time status and leap seconds\n\nFlags for valid GPS TOW, week number and UTC and current leap seconds.");
1065 if (ImGui::BeginTable("VectorNavTimeInfoTooltip", 3, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
1066 {
1067 ImGui::TableSetupColumn("Bit Offset");
1068 ImGui::TableSetupColumn("Field");
1069 ImGui::TableSetupColumn("Description");
1070 ImGui::TableHeadersRow();
1071
1072 ImGui::TableNextColumn(); ImGui::TextUnformatted("0");
1073 ImGui::TableNextColumn(); ImGui::TextUnformatted("timeOk");
1074 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 - GpsTow is valid");
1075
1076 ImGui::TableNextColumn(); ImGui::TextUnformatted("1");
1077 ImGui::TableNextColumn(); ImGui::TextUnformatted("dateOk");
1078 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 - TimeGps and GpsWeek are valid");
1079
1080 ImGui::TableNextColumn(); ImGui::TextUnformatted("2");
1081 ImGui::TableNextColumn(); ImGui::TextUnformatted("utcTimeValid");
1082 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 - UTC time is valid");
1083
1084 ImGui::TableNextColumn(); ImGui::TextUnformatted("3 - 7");
1085 ImGui::TableNextColumn(); ImGui::TextUnformatted("resv");
1086 ImGui::TableNextColumn(); ImGui::TextUnformatted("Reserved for future use");
1087
1088 ImGui::EndTable();
1089 } },
1090 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t binaryField) { return sensorModel == VectorNavModel::VN310
1091 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_TOW)
1092 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_WEEK)
1093 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_UTC)
1094 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_TIMEU)
1095 && !(static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_RAWMEAS); } },
1096 { /* 13 */ "DOP", vn::protocol::uart::GpsGroup::GPSGROUP_DOP,
1097 []() { ImGui::TextUnformatted("Dilution of precision"); },
1098 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; } },
1099 { /* 14 */ "SatInfo", vn::protocol::uart::GpsGroup::GPSGROUP_SATINFO,
1100 []() { ImGui::TextUnformatted("Satellite Information\n\nInformation and measurements pertaining to each GNSS satellite in view.\n\nSatInfo Element:");
1101 if (ImGui::BeginTable("VectorNavSatInfoTooltip", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
1102 {
1103 ImGui::TableSetupColumn("Name");
1104 ImGui::TableSetupColumn("Description");
1105 ImGui::TableHeadersRow();
1106
1107 ImGui::TableNextColumn(); ImGui::TextUnformatted("sys");
1108 ImGui::TableNextColumn(); ImGui::TextUnformatted("GNSS constellation indicator. See table below for details.");
1109
1110 ImGui::TableNextColumn(); ImGui::TextUnformatted("svId");
1111 ImGui::TableNextColumn(); ImGui::TextUnformatted("Space vehicle Id");
1112
1113 ImGui::TableNextColumn(); ImGui::TextUnformatted("flags");
1114 ImGui::TableNextColumn(); ImGui::TextUnformatted("Tracking info flags. See table below for details.");
1115
1116 ImGui::TableNextColumn(); ImGui::TextUnformatted("cno");
1117 ImGui::TableNextColumn(); ImGui::TextUnformatted("Carrier-to-noise density ratio (signal strength) [dB-Hz]");
1118
1119 ImGui::TableNextColumn(); ImGui::TextUnformatted("qi");
1120 ImGui::TableNextColumn(); ImGui::TextUnformatted("Quality Indicator. See table below for details.");
1121
1122 ImGui::TableNextColumn(); ImGui::TextUnformatted("el");
1123 ImGui::TableNextColumn(); ImGui::TextUnformatted("Elevation in degrees");
1124
1125 ImGui::TableNextColumn(); ImGui::TextUnformatted("az");
1126 ImGui::TableNextColumn(); ImGui::TextUnformatted("Azimuth angle in degrees");
1127
1128 ImGui::EndTable();
1129 }
1130 ImGui::BeginChild("VectorNavSatInfoTooltipGNSSConstelationChild", ImVec2(230, 217));
1131 ImGui::TextUnformatted("\nGNSS constellation indicator:");
1132 if (ImGui::BeginTable("VectorNavSatInfoTooltipGNSSConstelation", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
1133 {
1134 ImGui::TableSetupColumn("Value");
1135 ImGui::TableSetupColumn("Description");
1136 ImGui::TableHeadersRow();
1137
1138 ImGui::TableNextColumn(); ImGui::TextUnformatted("0");
1139 ImGui::TableNextColumn(); ImGui::TextUnformatted("GPS");
1140
1141 ImGui::TableNextColumn(); ImGui::TextUnformatted("1");
1142 ImGui::TableNextColumn(); ImGui::TextUnformatted("SBAS");
1143
1144 ImGui::TableNextColumn(); ImGui::TextUnformatted("2");
1145 ImGui::TableNextColumn(); ImGui::TextUnformatted("Galileo");
1146
1147 ImGui::TableNextColumn(); ImGui::TextUnformatted("3");
1148 ImGui::TableNextColumn(); ImGui::TextUnformatted("BeiDou");
1149
1150 ImGui::TableNextColumn(); ImGui::TextUnformatted("4");
1151 ImGui::TableNextColumn(); ImGui::TextUnformatted("IMES");
1152
1153 ImGui::TableNextColumn(); ImGui::TextUnformatted("5");
1154 ImGui::TableNextColumn(); ImGui::TextUnformatted("QZSS");
1155
1156 ImGui::TableNextColumn(); ImGui::TextUnformatted("6");
1157 ImGui::TableNextColumn(); ImGui::TextUnformatted("GLONASS");
1158
1159 ImGui::EndTable();
1160 }
1161 ImGui::EndChild();
1162 ImGui::SameLine();
1163 ImGui::BeginChild("VectorNavSatInfoTooltipFlagsChild", ImVec2(260, 217));
1164 ImGui::TextUnformatted("\nTracking info flags:");
1165 if (ImGui::BeginTable("VectorNavSatInfoTooltipFlags", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
1166 {
1167 ImGui::TableSetupColumn("Bit Offset");
1168 ImGui::TableSetupColumn("Description");
1169 ImGui::TableHeadersRow();
1170
1171 ImGui::TableNextColumn(); ImGui::TextUnformatted("0");
1172 ImGui::TableNextColumn(); ImGui::TextUnformatted("Healthy");
1173
1174 ImGui::TableNextColumn(); ImGui::TextUnformatted("1");
1175 ImGui::TableNextColumn(); ImGui::TextUnformatted("Almanac");
1176
1177 ImGui::TableNextColumn(); ImGui::TextUnformatted("2");
1178 ImGui::TableNextColumn(); ImGui::TextUnformatted("Ephemeris");
1179
1180 ImGui::TableNextColumn(); ImGui::TextUnformatted("3");
1181 ImGui::TableNextColumn(); ImGui::TextUnformatted("Differential Correction");
1182
1183 ImGui::TableNextColumn(); ImGui::TextUnformatted("4");
1184 ImGui::TableNextColumn(); ImGui::TextUnformatted("Used for Navigation");
1185
1186 ImGui::TableNextColumn(); ImGui::TextUnformatted("5");
1187 ImGui::TableNextColumn(); ImGui::TextUnformatted("Azimuth / Elevation Valid");
1188
1189 ImGui::TableNextColumn(); ImGui::TextUnformatted("6");
1190 ImGui::TableNextColumn(); ImGui::TextUnformatted("Used for RTK");
1191
1192 ImGui::EndTable();
1193 }
1194 ImGui::EndChild();
1195 ImGui::TextUnformatted("\nQuality Indicators:");
1196 if (ImGui::BeginTable("VectorNavSatInfoTooltipQuality", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
1197 {
1198 ImGui::TableSetupColumn("Value");
1199 ImGui::TableSetupColumn("Description");
1200 ImGui::TableHeadersRow();
1201
1202 ImGui::TableNextColumn(); ImGui::TextUnformatted("0");
1203 ImGui::TableNextColumn(); ImGui::TextUnformatted("No signal");
1204
1205 ImGui::TableNextColumn(); ImGui::TextUnformatted("1");
1206 ImGui::TableNextColumn(); ImGui::TextUnformatted("Searching signal");
1207
1208 ImGui::TableNextColumn(); ImGui::TextUnformatted("2");
1209 ImGui::TableNextColumn(); ImGui::TextUnformatted("Signal acquired");
1210
1211 ImGui::TableNextColumn(); ImGui::TextUnformatted("3");
1212 ImGui::TableNextColumn(); ImGui::TextUnformatted("Signal detected but unstable");
1213
1214 ImGui::TableNextColumn(); ImGui::TextUnformatted("4");
1215 ImGui::TableNextColumn(); ImGui::TextUnformatted("Code locked and time synchronized");
1216
1217 ImGui::TableNextColumn(); ImGui::TextUnformatted("5, 6, 7");
1218 ImGui::TableNextColumn(); ImGui::TextUnformatted("Code and carrier locked and time synchronized");
1219
1220 ImGui::EndTable();
1221 } },
1222 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; } },
1223 { /* 15 */ "RawMeas", vn::protocol::uart::GpsGroup::GPSGROUP_RAWMEAS,
1224 []() { ImGui::TextUnformatted("GNSS Raw Measurements.\n\nSatRaw Element:");
1225 if (ImGui::BeginTable("VectorNavSatRawTooltip", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
1226 {
1227 ImGui::TableSetupColumn("Name");
1228 ImGui::TableSetupColumn("Description");
1229 ImGui::TableHeadersRow();
1230
1231 ImGui::TableNextColumn(); ImGui::TextUnformatted("sys");
1232 ImGui::TableNextColumn(); ImGui::TextUnformatted("GNSS constellation indicator. See table below for details.");
1233
1234 ImGui::TableNextColumn(); ImGui::TextUnformatted("svId");
1235 ImGui::TableNextColumn(); ImGui::TextUnformatted("Space vehicle Id");
1236
1237 ImGui::TableNextColumn(); ImGui::TextUnformatted("freq");
1238 ImGui::TableNextColumn(); ImGui::TextUnformatted("Frequency indicator. See table below for details.");
1239
1240 ImGui::TableNextColumn(); ImGui::TextUnformatted("chan");
1241 ImGui::TableNextColumn(); ImGui::TextUnformatted("Channel Indicator. See table below for details.");
1242
1243 ImGui::TableNextColumn(); ImGui::TextUnformatted("slot");
1244 ImGui::TableNextColumn(); ImGui::TextUnformatted("Slot Id");
1245
1246 ImGui::TableNextColumn(); ImGui::TextUnformatted("cno");
1247 ImGui::TableNextColumn(); ImGui::TextUnformatted("Carrier-to-noise density ratio (signal strength) [dB-Hz]");
1248
1249 ImGui::TableNextColumn(); ImGui::TextUnformatted("flags");
1250 ImGui::TableNextColumn(); ImGui::TextUnformatted("Tracking info flags. See table below for details.");
1251
1252 ImGui::TableNextColumn(); ImGui::TextUnformatted("pr");
1253 ImGui::TableNextColumn(); ImGui::TextUnformatted("Pseudorange measurement in meters.");
1254
1255 ImGui::TableNextColumn(); ImGui::TextUnformatted("cp");
1256 ImGui::TableNextColumn(); ImGui::TextUnformatted("Carrier phase measurement in cycles.");
1257
1258 ImGui::TableNextColumn(); ImGui::TextUnformatted("dp");
1259 ImGui::TableNextColumn(); ImGui::TextUnformatted("Doppler measurement in Hz. Positive sign for approaching satellites.");
1260
1261 ImGui::EndTable();
1262 }
1263 ImGui::BeginChild("VectorNavSatRawTooltipGNSSConstelationChild", ImVec2(180, 217));
1264 ImGui::TextUnformatted("\nConstellation indicator:");
1265 if (ImGui::BeginTable("VectorNavSatRawTooltipGNSSConstelation", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
1266 {
1267 ImGui::TableSetupColumn("Value");
1268 ImGui::TableSetupColumn("Description");
1269 ImGui::TableHeadersRow();
1270
1271 ImGui::TableNextColumn(); ImGui::TextUnformatted("0");
1272 ImGui::TableNextColumn(); ImGui::TextUnformatted("GPS");
1273
1274 ImGui::TableNextColumn(); ImGui::TextUnformatted("1");
1275 ImGui::TableNextColumn(); ImGui::TextUnformatted("SBAS");
1276
1277 ImGui::TableNextColumn(); ImGui::TextUnformatted("2");
1278 ImGui::TableNextColumn(); ImGui::TextUnformatted("Galileo");
1279
1280 ImGui::TableNextColumn(); ImGui::TextUnformatted("3");
1281 ImGui::TableNextColumn(); ImGui::TextUnformatted("BeiDou");
1282
1283 ImGui::TableNextColumn(); ImGui::TextUnformatted("4");
1284 ImGui::TableNextColumn(); ImGui::TextUnformatted("IMES");
1285
1286 ImGui::TableNextColumn(); ImGui::TextUnformatted("5");
1287 ImGui::TableNextColumn(); ImGui::TextUnformatted("QZSS");
1288
1289 ImGui::TableNextColumn(); ImGui::TextUnformatted("6");
1290 ImGui::TableNextColumn(); ImGui::TextUnformatted("GLONASS");
1291
1292 ImGui::EndTable();
1293 }
1294 ImGui::EndChild();
1295 ImGui::SameLine();
1296 ImGui::BeginChild("VectorNavSatRawTooltipFreqChild", ImVec2(270, 235));
1297 ImGui::TextUnformatted("\nFrequency indicator:");
1298 if (ImGui::BeginTable("VectorNavSatRawTooltipFreq", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
1299 {
1300 ImGui::TableSetupColumn("Value");
1301 ImGui::TableSetupColumn("Description");
1302 ImGui::TableHeadersRow();
1303
1304 ImGui::TableNextColumn(); ImGui::TextUnformatted("0");
1305 ImGui::TableNextColumn(); ImGui::TextUnformatted("Rx Channel");
1306
1307 ImGui::TableNextColumn(); ImGui::TextUnformatted("1");
1308 ImGui::TableNextColumn(); ImGui::TextUnformatted("L1(GPS,QZSS,SBAS), G1(GLO),\nE2-L1-E1(GAL), B1(BDS)");
1309
1310 ImGui::TableNextColumn(); ImGui::TextUnformatted("2");
1311 ImGui::TableNextColumn(); ImGui::TextUnformatted("L2(GPS,QZSS), G2(GLO)");
1312
1313 ImGui::TableNextColumn(); ImGui::TextUnformatted("3");
1314 ImGui::TableNextColumn(); ImGui::TextUnformatted("L5(GPS,QZSS,SBAS), E5a(GAL)");
1315
1316 ImGui::TableNextColumn(); ImGui::TextUnformatted("4");
1317 ImGui::TableNextColumn(); ImGui::TextUnformatted("E6(GAL), LEX(QZSS), B3(BDS)");
1318
1319 ImGui::TableNextColumn(); ImGui::TextUnformatted("5");
1320 ImGui::TableNextColumn(); ImGui::TextUnformatted("E5b(GAL), B2(BDS)");
1321
1322 ImGui::TableNextColumn(); ImGui::TextUnformatted("6");
1323 ImGui::TableNextColumn(); ImGui::TextUnformatted("E5a+b(GAL)");
1324
1325 ImGui::EndTable();
1326 }
1327 ImGui::EndChild();
1328 ImGui::SameLine();
1329 ImGui::BeginChild("VectorNavSatRawTooltipFlagChild", ImVec2(255, 260));
1330 ImGui::TextUnformatted("\nTracking info flags:");
1331 if (ImGui::BeginTable("VectorNavSatRawTooltipFlags", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
1332 {
1333 ImGui::TableSetupColumn("Bit Offset");
1334 ImGui::TableSetupColumn("Description");
1335 ImGui::TableHeadersRow();
1336
1337 ImGui::TableNextColumn(); ImGui::TextUnformatted("0");
1338 ImGui::TableNextColumn(); ImGui::TextUnformatted("Searching");
1339
1340 ImGui::TableNextColumn(); ImGui::TextUnformatted("1");
1341 ImGui::TableNextColumn(); ImGui::TextUnformatted("Tracking");
1342
1343 ImGui::TableNextColumn(); ImGui::TextUnformatted("2");
1344 ImGui::TableNextColumn(); ImGui::TextUnformatted("Time Valid");
1345
1346 ImGui::TableNextColumn(); ImGui::TextUnformatted("3");
1347 ImGui::TableNextColumn(); ImGui::TextUnformatted("Code Lock");
1348
1349 ImGui::TableNextColumn(); ImGui::TextUnformatted("4");
1350 ImGui::TableNextColumn(); ImGui::TextUnformatted("Phase Lock");
1351
1352 ImGui::TableNextColumn(); ImGui::TextUnformatted("5");
1353 ImGui::TableNextColumn(); ImGui::TextUnformatted("Phase Half Ambiguity");
1354
1355 ImGui::TableNextColumn(); ImGui::TextUnformatted("6");
1356 ImGui::TableNextColumn(); ImGui::TextUnformatted("Phase Half Sub");
1357
1358 ImGui::TableNextColumn(); ImGui::TextUnformatted("7");
1359 ImGui::TableNextColumn(); ImGui::TextUnformatted("Phase Slip");
1360
1361 ImGui::TableNextColumn(); ImGui::TextUnformatted("8");
1362 ImGui::TableNextColumn(); ImGui::TextUnformatted("Pseudorange Smoothed");
1363
1364 ImGui::EndTable();
1365 }
1366 ImGui::EndChild();
1367 ImGui::TextUnformatted("\nChannel indicator:");
1368 if (ImGui::BeginTable("VectorNavSatRawTooltipChan", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
1369 {
1370 ImGui::TableSetupColumn("Value");
1371 ImGui::TableSetupColumn("Description");
1372 ImGui::TableHeadersRow();
1373
1374 ImGui::TableNextColumn(); ImGui::TextUnformatted("0");
1375 ImGui::TableNextColumn(); ImGui::TextUnformatted("P-code (GPS,GLO)");
1376
1377 ImGui::TableNextColumn(); ImGui::TextUnformatted("1");
1378 ImGui::TableNextColumn(); ImGui::TextUnformatted("C/A-code (GPS,GLO,SBAS,QZSS), C chan (GAL)");
1379
1380 ImGui::TableNextColumn(); ImGui::TextUnformatted("2");
1381 ImGui::TableNextColumn(); ImGui::TextUnformatted("semi-codeless (GPS)");
1382
1383 ImGui::TableNextColumn(); ImGui::TextUnformatted("3");
1384 ImGui::TableNextColumn(); ImGui::TextUnformatted("Y-code (GPS)");
1385
1386 ImGui::TableNextColumn(); ImGui::TextUnformatted("4");
1387 ImGui::TableNextColumn(); ImGui::TextUnformatted("M-code (GPS)");
1388
1389 ImGui::TableNextColumn(); ImGui::TextUnformatted("5");
1390 ImGui::TableNextColumn(); ImGui::TextUnformatted("codeless (GPS)");
1391
1392 ImGui::TableNextColumn(); ImGui::TextUnformatted("6");
1393 ImGui::TableNextColumn(); ImGui::TextUnformatted("A chan (GAL)");
1394
1395 ImGui::TableNextColumn(); ImGui::TextUnformatted("7");
1396 ImGui::TableNextColumn(); ImGui::TextUnformatted("B chan (GAL)");
1397
1398 ImGui::TableNextColumn(); ImGui::TextUnformatted("8");
1399 ImGui::TableNextColumn(); ImGui::TextUnformatted("I chan (GPS,GAL,QZSS,BDS)");
1400
1401 ImGui::TableNextColumn(); ImGui::TextUnformatted("9");
1402 ImGui::TableNextColumn(); ImGui::TextUnformatted("Q chan (GPS,GAL,QZSS,BDS)");
1403
1404 ImGui::TableNextColumn(); ImGui::TextUnformatted("10");
1405 ImGui::TableNextColumn(); ImGui::TextUnformatted("M chan (L2CGPS, L2CQZSS), D chan (GPS,QZSS)");
1406
1407 ImGui::TableNextColumn(); ImGui::TextUnformatted("11");
1408 ImGui::TableNextColumn(); ImGui::TextUnformatted("L chan (L2CGPS, L2CQZSS), P chan (GPS,QZSS)");
1409
1410 ImGui::TableNextColumn(); ImGui::TextUnformatted("12");
1411 ImGui::TableNextColumn(); ImGui::TextUnformatted("B+C chan (GAL), I+Q chan (GPS,GAL,QZSS,BDS),\nM+L chan (GPS,QZSS), D+P chan (GPS,QZSS)");
1412
1413 ImGui::TableNextColumn(); ImGui::TextUnformatted("13");
1414 ImGui::TableNextColumn(); ImGui::TextUnformatted("based on Z-tracking (GPS)");
1415
1416 ImGui::TableNextColumn(); ImGui::TextUnformatted("14");
1417 ImGui::TableNextColumn(); ImGui::TextUnformatted("A+B+C (GAL)");
1418
1419 ImGui::EndTable();
1420 } },
1421 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1422 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& /* bor */, uint32_t& binaryField) { (static_cast<vn::protocol::uart::GpsGroup>(binaryField) & vn::protocol::uart::GpsGroup::GPSGROUP_RAWMEAS) && (binaryField |= vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO); } },
1423} };
1424
1425const std::array<NAV::VectorNavSensor::BinaryGroupData, 9> NAV::VectorNavSensor::_binaryGroupAttitude{ {
1426 { /* 0 */ "VpeStatus", vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_VPESTATUS,
1427 []() { ImGui::TextUnformatted("VPE Status bitfield\n\n");
1428 if (ImGui::BeginTable("VectorNavSatRawTooltipChan", 4, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg))
1429 {
1430 ImGui::TableSetupColumn("Name", ImGuiTableColumnFlags_WidthFixed);
1431 ImGui::TableSetupColumn("Bit Offset", ImGuiTableColumnFlags_WidthFixed);
1432 ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed);
1433 ImGui::TableSetupColumn("Description", ImGuiTableColumnFlags_WidthFixed);
1434 ImGui::TableHeadersRow();
1435
1436 ImGui::TableNextColumn(); ImGui::TextUnformatted("AttitudeQuality");
1437 ImGui::TableNextColumn(); ImGui::TextUnformatted("0");
1438 ImGui::TableNextColumn(); ImGui::TextUnformatted("2 bits");
1439 ImGui::TableNextColumn(); ImGui::TextUnformatted("Provides an indication of the quality of the attitude solution.\n0 - Excellent\n1 - Good\n2 - Bad\n3 - Not tracking");
1440
1441 ImGui::TableNextColumn(); ImGui::TextUnformatted("GyroSaturation");
1442 ImGui::TableNextColumn(); ImGui::TextUnformatted("2");
1443 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 bits");
1444 ImGui::TableNextColumn(); ImGui::TextUnformatted("At least one gyro axis is currently saturated.");
1445
1446 ImGui::TableNextColumn(); ImGui::TextUnformatted("GyroSaturationRecovery");
1447 ImGui::TableNextColumn(); ImGui::TextUnformatted("3");
1448 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 bits");
1449 ImGui::TableNextColumn(); ImGui::TextUnformatted("Filter is in the process of recovering from a gyro\nsaturation event.");
1450
1451 ImGui::TableNextColumn(); ImGui::TextUnformatted("MagDisturbance");
1452 ImGui::TableNextColumn(); ImGui::TextUnformatted("4");
1453 ImGui::TableNextColumn(); ImGui::TextUnformatted("2 bits");
1454 ImGui::TableNextColumn(); ImGui::TextUnformatted("A magnetic DC disturbance has been detected.\n0 - No magnetic disturbance\n1 to 3 - Magnetic disturbance is present.");
1455
1456 ImGui::TableNextColumn(); ImGui::TextUnformatted("MagSaturation");
1457 ImGui::TableNextColumn(); ImGui::TextUnformatted("6");
1458 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 bits");
1459 ImGui::TableNextColumn(); ImGui::TextUnformatted("At least one magnetometer axis is currently saturated.");
1460
1461 ImGui::TableNextColumn(); ImGui::TextUnformatted("AccDisturbance");
1462 ImGui::TableNextColumn(); ImGui::TextUnformatted("7");
1463 ImGui::TableNextColumn(); ImGui::TextUnformatted("2 bits");
1464 ImGui::TableNextColumn(); ImGui::TextUnformatted("A strong acceleration disturbance has been detected.\n0 - No acceleration disturbance.\n1 to 3 - Acceleration disturbance has been detected.");
1465
1466 ImGui::TableNextColumn(); ImGui::TextUnformatted("AccSaturation");
1467 ImGui::TableNextColumn(); ImGui::TextUnformatted("9");
1468 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 bits");
1469 ImGui::TableNextColumn(); ImGui::TextUnformatted("At least one accelerometer axis is currently saturated.");
1470
1471 ImGui::TableNextColumn(); ImGui::TextUnformatted("Reserved");
1472 ImGui::TableNextColumn(); ImGui::TextUnformatted("10");
1473 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 bits");
1474 ImGui::TableNextColumn(); ImGui::TextUnformatted("Reserved for internal use. May change state at run- time.");
1475
1476 ImGui::TableNextColumn(); ImGui::TextUnformatted("KnownMagDisturbance");
1477 ImGui::TableNextColumn(); ImGui::TextUnformatted("11");
1478 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 bits");
1479 ImGui::TableNextColumn(); ImGui::TextUnformatted("A known magnetic disturbance has been reported by\nthe user and the magnetometer is currently tuned out.");
1480
1481 ImGui::TableNextColumn(); ImGui::TextUnformatted("KnownAccelDisturbance");
1482 ImGui::TableNextColumn(); ImGui::TextUnformatted("12");
1483 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 bits");
1484 ImGui::TableNextColumn(); ImGui::TextUnformatted("A known acceleration disturbance has been reported by\nthe user and the accelerometer is currently tuned out.");
1485
1486 ImGui::TableNextColumn(); ImGui::TextUnformatted("Reserved");
1487 ImGui::TableNextColumn(); ImGui::TextUnformatted("13");
1488 ImGui::TableNextColumn(); ImGui::TextUnformatted("3 bits");
1489 ImGui::TableNextColumn(); ImGui::TextUnformatted("Reserved for future use.");
1490
1491 ImGui::EndTable();
1492 } },
1493 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN100_VN110; } },
1494 { /* 1 */ "YawPitchRoll", vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YAWPITCHROLL,
1495 []() { ImGui::TextUnformatted("Yaw Pitch Roll\n\nThe estimated attitude Yaw, Pitch, and Roll angles measured in degrees. The attitude is given as a 3,2,1\nEuler angle sequence describing the body frame with respect to the local North East Down (NED) frame.\n\nYaw [+/- 180°]\nPitch [+/- 90°]\nRoll [+/- 180°]"); },
1496 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; },
1497 [](VectorNavSensor* sensor, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::AttitudeGroup>(binaryField) & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YAWPITCHROLL) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS)
1498 && ((sensor->_sensorModel == VectorNavModel::VN310) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW)); } },
1499 { /* 2 */ "Quaternion", vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_QUATERNION,
1500 []() { ImGui::TextUnformatted("Quaternion\n\nThe estimated attitude quaternion. The last term is the scalar value. The attitude is given as the body\nframe with respect to the local North East Down (NED) frame."); },
1501 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; },
1502 [](VectorNavSensor* sensor, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::AttitudeGroup>(binaryField) & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_QUATERNION) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS)
1503 && ((sensor->_sensorModel == VectorNavModel::VN310) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW)); } },
1504 { /* 3 */ "DCM", vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_DCM,
1505 []() { ImGui::TextUnformatted("Directional Cosine Matrix\n\nThe estimated attitude directional cosine matrix given in column major order. The DCM maps vectors\nfrom the North East Down (NED) frame into the body frame."); },
1506 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; },
1507 [](VectorNavSensor* sensor, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::AttitudeGroup>(binaryField) & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_DCM) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS)
1508 && ((sensor->_sensorModel == VectorNavModel::VN310) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW)); } },
1509 { /* 4 */ "MagNed", vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_MAGNED,
1510 []() { ImGui::TextUnformatted("Compensated magnetic (NED)\n\nThe current estimated magnetic field (Gauss), given in the North East Down (NED) frame. The current\nattitude solution is used to map the measurement from the measured body frame to the inertial (NED)\nframe. This measurement is compensated by both the static calibration (individual factory calibration\nstored in flash), and the dynamic calibration such as the user or onboard Hard/Soft Iron compensation\nregisters."); },
1511 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; },
1512 [](VectorNavSensor* sensor, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::AttitudeGroup>(binaryField) & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_MAGNED) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS)
1513 && ((sensor->_sensorModel == VectorNavModel::VN310) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW)); } },
1514 { /* 5 */ "AccelNed", vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_ACCELNED,
1515 []() { ImGui::TextUnformatted("Compensated acceleration (NED)\n\nThe estimated acceleration (with gravity) reported in m/s^2, given in the North East Down (NED) frame.\nThe acceleration measurement has been bias compensated by the onboard INS filter. This measurement\nis attitude dependent, since the attitude is used to map the measurement from the body frame into the\ninertial (NED) frame. If the device is stationary and the INS filter is tracking, the measurement should be\nnominally equivalent to the gravity reference vector in the inertial frame (NED)."); },
1516 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; },
1517 [](VectorNavSensor* sensor, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::AttitudeGroup>(binaryField) & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_ACCELNED) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS)
1518 && ((sensor->_sensorModel == VectorNavModel::VN310) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW)); } },
1519 { /* 6 */ "LinearAccelBody", vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY,
1520 []() { ImGui::TextUnformatted("Compensated linear acceleration (no gravity)\n\nThe estimated linear acceleration (without gravity) reported in m/s^2, and given in the body frame. The\nacceleration measurement has been bias compensated by the onboard INS filter, and the gravity\ncomponent has been removed using the current gravity reference vector model. This measurement is\nattitude dependent, since the attitude solution is required to map the gravity reference vector (known in\nthe inertial NED frame), into the body frame so that it can be removed from the measurement. If the\ndevice is stationary and the onboard INS filter is tracking, the measurement nominally will read 0 in all\nthree axes."); },
1521 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; },
1522 [](VectorNavSensor* sensor, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::AttitudeGroup>(binaryField) & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS)
1523 && ((sensor->_sensorModel == VectorNavModel::VN310) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW)); } },
1524 { /* 7 */ "LinearAccelNed", vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELNED,
1525 []() { ImGui::TextUnformatted("Compensated linear acceleration (no gravity) (NED)\n\nThe estimated linear acceleration (without gravity) reported in m/s^2, and given in the North East Down\n(NED) frame. This measurement is attitude dependent as the attitude solution is used to map the\nmeasurement from the body frame into the inertial (NED) frame. This acceleration measurement has\nbeen bias compensated by the onboard INS filter, and the gravity component has been removed using the\ncurrent gravity reference vector estimate. If the device is stationary and the onboard INS filter is tracking,\nthe measurement nominally will read 0 in all three axes."); },
1526 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; },
1527 [](VectorNavSensor* sensor, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::AttitudeGroup>(binaryField) & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELNED) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS)
1528 && ((sensor->_sensorModel == VectorNavModel::VN310) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW)); } },
1529 { /* 8 */ "YprU", vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YPRU,
1530 []() { ImGui::TextUnformatted("Yaw Pitch Roll uncertainty\n\nThe estimated attitude (Yaw, Pitch, Roll) uncertainty (1 Sigma), reported in degrees.\n\nThe estimated attitude (YprU) field is not valid when the INS Scenario mode in the INS Basic\nConfiguration register is set to AHRS mode. See the INS Basic Configuration Register in the INS\nsection for more details."); },
1531 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; },
1532 [](VectorNavSensor* sensor, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::AttitudeGroup>(binaryField) & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YPRU) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS)
1533 && ((sensor->_sensorModel == VectorNavModel::VN310) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW)); } },
1534} };
1535
1536const std::array<NAV::VectorNavSensor::BinaryGroupData, 11> NAV::VectorNavSensor::_binaryGroupINS{ {
1537 { /* 0 */ "InsStatus", vn::protocol::uart::InsGroup::INSGROUP_INSSTATUS,
1538 []() { ImGui::TextUnformatted("Ins Status bitfield:");
1539 if (ImGui::BeginTable("VectorNavInsStatusTooltip", 4, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg))
1540 {
1541 ImGui::TableSetupColumn("Name", ImGuiTableColumnFlags_WidthFixed);
1542 ImGui::TableSetupColumn("Bit Offset", ImGuiTableColumnFlags_WidthFixed);
1543 ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed);
1544 ImGui::TableSetupColumn("Description", ImGuiTableColumnFlags_WidthFixed);
1545 ImGui::TableHeadersRow();
1546
1547 ImGui::TableNextColumn(); ImGui::TextUnformatted("Mode");
1548 ImGui::TableNextColumn(); ImGui::TextUnformatted("0");
1549 ImGui::TableNextColumn(); ImGui::TextUnformatted("2 bits");
1550 ImGui::TableNextColumn(); ImGui::TextUnformatted("Indicates the current mode of the INS filter.\n\n0 = Not tracking. GNSS Compass is initializing. Output heading is based on\nmagnetometer measurements.\n1 = Aligning.\nINS Filter is dynamically aligning.\nFor a stationary startup: GNSS Compass has initialized and INS Filter is\naligning from the magnetic heading to the GNSS Compass heading.\nFor a dynamic startup: INS Filter has initialized and is dynamically aligning to\nTrue North heading.\nIn operation, if the INS Filter drops from INS Mode 2 back down to 1, the\nattitude uncertainty has increased above 2 degrees.\n2 = Tracking. The INS Filter is tracking and operating within specification.\n3 = Loss of GNSS. A GNSS outage has lasted more than 45 seconds. The INS\nFilter will no longer update the position and velocity outputs, but the attitude\nremains valid.");
1551
1552 ImGui::TableNextColumn(); ImGui::TextUnformatted("GpsFix");
1553 ImGui::TableNextColumn(); ImGui::TextUnformatted("2");
1554 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 bits");
1555 ImGui::TableNextColumn(); ImGui::TextUnformatted("Indicates whether the GNSS has a proper fix.");
1556
1557 ImGui::TableNextColumn(); ImGui::TextUnformatted("Error");
1558 ImGui::TableNextColumn(); ImGui::TextUnformatted("3");
1559 ImGui::TableNextColumn(); ImGui::TextUnformatted("4 bits");
1560 ImGui::TableNextColumn(); ImGui::TextUnformatted("Sensor measurement error code. See table below.\n0 = No errors detected.");
1561
1562 ImGui::TableNextColumn(); ImGui::TextUnformatted("Reserved");
1563 ImGui::TableNextColumn(); ImGui::TextUnformatted("7");
1564 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 bits");
1565 ImGui::TableNextColumn(); ImGui::TextUnformatted("Reserved for internal use. May toggle state during runtime and should be ignored.");
1566
1567 ImGui::TableNextColumn(); ImGui::TextUnformatted("GpsHeadingIns");
1568 ImGui::TableNextColumn(); ImGui::TextUnformatted("8");
1569 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 bits");
1570 ImGui::TableNextColumn(); ImGui::TextUnformatted("In stationary operation, if set the INS Filter has fully aligned to the GNSS\nCompass solution.\nIn dynamic operation, the GNSS Compass solution is currently aiding the INS\nFilter heading solution.");
1571
1572 ImGui::TableNextColumn(); ImGui::TextUnformatted("GpsCompass");
1573 ImGui::TableNextColumn(); ImGui::TextUnformatted("9");
1574 ImGui::TableNextColumn(); ImGui::TextUnformatted("1 bits");
1575 ImGui::TableNextColumn(); ImGui::TextUnformatted("Indicates if the GNSS compass is operational and reporting a heading\nsolution.");
1576
1577 ImGui::TableNextColumn(); ImGui::TextUnformatted("Reserved");
1578 ImGui::TableNextColumn(); ImGui::TextUnformatted("10");
1579 ImGui::TableNextColumn(); ImGui::TextUnformatted("6 bits");
1580 ImGui::TableNextColumn(); ImGui::TextUnformatted("Reserved for internal use. These bits will toggle state and should be ignored.");
1581
1582 ImGui::EndTable();
1583 }
1584 ImGui::TextUnformatted("\nError Bitfield:");
1585 if (ImGui::BeginTable("VectorNavInsStatusTooltipError", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
1586 {
1587 ImGui::TableSetupColumn("Name");
1588 ImGui::TableSetupColumn("Description");
1589 ImGui::TableHeadersRow();
1590
1591 ImGui::TableNextColumn(); ImGui::TextUnformatted("Reserved");
1592 ImGui::TableNextColumn(); ImGui::TextUnformatted("Reserved for future use and not currently used.");
1593
1594 ImGui::TableNextColumn(); ImGui::TextUnformatted("IMU Error");
1595 ImGui::TableNextColumn(); ImGui::TextUnformatted("High if IMU communication error is detected.");
1596
1597 ImGui::TableNextColumn(); ImGui::TextUnformatted("Mag/Pres Error");
1598 ImGui::TableNextColumn(); ImGui::TextUnformatted("High if Magnetometer or Pressure sensor error is detected.");
1599
1600 ImGui::TableNextColumn(); ImGui::TextUnformatted("GNSS Error");
1601 ImGui::TableNextColumn(); ImGui::TextUnformatted("High if GNSS communication error is detected.");
1602
1603 ImGui::EndTable();
1604 } },
1605 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1606 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::InsGroup>(binaryField) & vn::protocol::uart::InsGroup::INSGROUP_INSSTATUS) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW); } },
1607 { /* 1 */ "PosLla", vn::protocol::uart::InsGroup::INSGROUP_POSLLA,
1608 []() { ImGui::TextUnformatted("Ins Position (latitude, longitude, altitude)\n\nThe estimated position given as latitude, longitude, and altitude given in [deg, deg, m] respectively."); },
1609 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1610 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::InsGroup>(binaryField) & vn::protocol::uart::InsGroup::INSGROUP_POSLLA) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW); } },
1611 { /* 2 */ "PosEcef", vn::protocol::uart::InsGroup::INSGROUP_POSECEF,
1612 []() { ImGui::TextUnformatted("Ins Position (ECEF)\n\nThe estimated position given in the Earth centered Earth fixed (ECEF) frame, reported in meters."); },
1613 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1614 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::InsGroup>(binaryField) & vn::protocol::uart::InsGroup::INSGROUP_POSECEF) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW); } },
1615 { /* 3 */ "VelBody", vn::protocol::uart::InsGroup::INSGROUP_VELBODY,
1616 []() { ImGui::TextUnformatted("Ins Velocity (Body)\n\nThe estimated velocity in the body frame, given in m/s."); },
1617 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1618 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::InsGroup>(binaryField) & vn::protocol::uart::InsGroup::INSGROUP_VELBODY) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW); } },
1619 { /* 4 */ "VelNed", vn::protocol::uart::InsGroup::INSGROUP_VELNED,
1620 []() { ImGui::TextUnformatted("Ins Velocity (NED)\n\nThe estimated velocity in the North East Down (NED) frame, given in m/s."); },
1621 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1622 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::InsGroup>(binaryField) & vn::protocol::uart::InsGroup::INSGROUP_VELNED) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW); } },
1623 { /* 5 */ "VelEcef", vn::protocol::uart::InsGroup::INSGROUP_VELECEF,
1624 []() { ImGui::TextUnformatted("Ins Velocity (ECEF)\n\nThe estimated velocity in the Earth centered Earth fixed (ECEF) frame, given in m/s."); },
1625 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1626 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::InsGroup>(binaryField) & vn::protocol::uart::InsGroup::INSGROUP_VELECEF) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW); } },
1627 { /* 6 */ "MagEcef", vn::protocol::uart::InsGroup::INSGROUP_MAGECEF,
1628 []() { ImGui::TextUnformatted("Compensated magnetic (ECEF)\n\nThe compensated magnetic measurement in the Earth centered Earth fixed (ECEF) frame, given in Gauss."); },
1629 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1630 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::InsGroup>(binaryField) & vn::protocol::uart::InsGroup::INSGROUP_MAGECEF) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW); } },
1631 { /* 7 */ "AccelEcef", vn::protocol::uart::InsGroup::INSGROUP_ACCELECEF,
1632 []() { ImGui::TextUnformatted("Compensated acceleration (ECEF)\n\nThe estimated acceleration (with gravity) reported in m/s^2, given in the Earth centered Earth fixed (ECEF)\nframe. The acceleration measurement has been bias compensated by the onboard INS filter. This\nmeasurement is attitude dependent, since the attitude is used to map the measurement from the body frame\ninto the inertial (ECEF) frame. If the device is stationary and the INS filter is tracking, the measurement\nshould be nominally equivalent to the gravity reference vector in the inertial frame (ECEF)."); },
1633 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1634 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::InsGroup>(binaryField) & vn::protocol::uart::InsGroup::INSGROUP_ACCELECEF) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW); } },
1635 { /* 8 */ "LinearAccelEcef", vn::protocol::uart::InsGroup::INSGROUP_LINEARACCELECEF,
1636 []() { ImGui::TextUnformatted("Compensated linear acceleration (no gravity) (ECEF)\n\nThe estimated linear acceleration (without gravity) reported in m/s^2, and given in the Earth centered Earth\nfixed (ECEF) frame. This measurement is attitude dependent as the attitude solution is used to map the\nmeasurement from the body frame into the inertial (ECEF) frame. This acceleration measurement has been\nbias compensated by the onboard INS filter, and the gravity component has been removed using the current\ngravity reference vector estimate. If the device is stationary and the onboard INS filter is tracking, the\nmeasurement will nominally read 0 in all three axes."); },
1637 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1638 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::InsGroup>(binaryField) & vn::protocol::uart::InsGroup::INSGROUP_LINEARACCELECEF) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW); } },
1639 { /* 9 */ "PosU", vn::protocol::uart::InsGroup::INSGROUP_POSU,
1640 []() { ImGui::TextUnformatted("Ins Position Uncertainty\n\nThe estimated uncertainty (1 Sigma) in the current position estimate, given in meters."); },
1641 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1642 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::InsGroup>(binaryField) & vn::protocol::uart::InsGroup::INSGROUP_POSU) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW); } },
1643 { /* 10 */ "VelU", vn::protocol::uart::InsGroup::INSGROUP_VELU,
1644 []() { ImGui::TextUnformatted("Ins Velocity Uncertainty\n\nThe estimated uncertainty (1 Sigma) in the current velocity estimate, given in m/s."); },
1645 [](VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return sensorModel == VectorNavModel::VN310; },
1646 [](VectorNavSensor* /* sensor */, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField) { (static_cast<vn::protocol::uart::InsGroup>(binaryField) & vn::protocol::uart::InsGroup::INSGROUP_VELU) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK) && (bor.timeField |= vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW); } },
1647} };
1648
1649// NOLINTEND
1650
1651NAV::VectorNavSensor::VectorNavSensor()
1652 : Imu(typeStatic())
1653{
1654 LOG_TRACE("{}: called", name);
1655
1656 _onlyRealTime = true;
1657 _hasConfig = true;
1658 _guiConfigDefaultWindowSize = { 954, 783 };
1659
1660 CreateOutputPin("Ascii Output", Pin::Type::Flow, { NAV::StringObs::type() });
1661 CreateOutputPin("Binary Output 1", Pin::Type::Flow, { NAV::VectorNavBinaryOutput::type() });
1662 CreateOutputPin("Binary Output 2", Pin::Type::Flow, { NAV::VectorNavBinaryOutput::type() });
1663 CreateOutputPin("Binary Output 3", Pin::Type::Flow, { NAV::VectorNavBinaryOutput::type() });
1664
1665 _dividerFrequency = []() {
1666 std::map<int, int, std::greater<>> divFreq;
1667 for (int freq = 1; freq <= IMU_DEFAULT_FREQUENCY; freq++)
1668 {
1669 int divider = static_cast<int>(std::round(IMU_DEFAULT_FREQUENCY / freq));
1670
1671 if (!divFreq.contains(divider)
1672 || std::abs(divider - IMU_DEFAULT_FREQUENCY / freq) < std::abs(divider - IMU_DEFAULT_FREQUENCY / divFreq.at(divider)))
1673 {
1674 divFreq[divider] = freq;
1675 }
1676 }
1677 std::vector<uint16_t> divs;
1678 std::vector<std::string> freqs;
1679 for (auto& [divider, freq] : divFreq)
1680 {
1681 divs.push_back(static_cast<uint16_t>(divider));
1682 freqs.push_back(std::to_string(freq) + " Hz");
1683 LOG_DATA("VectorNavSensor: RateDivisor {} = {}", divs.back(), freqs.back());
1684 }
1685 return std::make_pair(divs, freqs);
1686 }();
1687}
1688
1689NAV::VectorNavSensor::~VectorNavSensor()
1690{
1691 LOG_TRACE("{}: called", nameId());
1692}
1693
1694std::string NAV::VectorNavSensor::typeStatic()
1695{
1696 return "VectorNavSensor";
1697}
1698
1699std::string NAV::VectorNavSensor::type() const
1700{
1701 return typeStatic();
1702}
1703
1704std::string NAV::VectorNavSensor::category()
1705{
1706 return "Data Provider";
1707}
1708
1709void NAV::VectorNavSensor::guiConfig()
1710{
1711 if (auto sensorModel = static_cast<int>(_sensorModel);
1712 ImGui::Combo("Sensor", &sensorModel, "VN-100 / VN-110\0VN-310\0\0"))
1713 {
1714 _sensorModel = static_cast<decltype(_sensorModel)>(sensorModel);
1715 LOG_DEBUG("{}: Sensor changed to {}", nameId(), fmt::underlying(_sensorModel));
1716 flow::ApplyChanges();
1717 doDeinitialize();
1718
1719 if (_sensorModel != VectorNavModel::VN310
1720 && (_asyncDataOutputType == vn::protocol::uart::AsciiAsync::VNGPS
1721 || _asyncDataOutputType == vn::protocol::uart::AsciiAsync::VNGPE
1722 || _asyncDataOutputType == vn::protocol::uart::AsciiAsync::VNINS
1723 || _asyncDataOutputType == vn::protocol::uart::AsciiAsync::VNINE
1724 || _asyncDataOutputType == vn::protocol::uart::AsciiAsync::VNISL
1725 || _asyncDataOutputType == vn::protocol::uart::AsciiAsync::VNISE
1726 || _asyncDataOutputType == vn::protocol::uart::AsciiAsync::VNG2S
1727 || _asyncDataOutputType == vn::protocol::uart::AsciiAsync::VNG2E))
1728 {
1729 _asyncDataOutputType = vn::protocol::uart::AsciiAsync::VNOFF;
1730 }
1731
1732 if (_sensorModel == VectorNavModel::VN310)
1733 {
1734 _communicationProtocolControlRegister.spiCount = vn::protocol::uart::CountMode::COUNTMODE_NONE;
1735 _communicationProtocolControlRegister.spiStatus = vn::protocol::uart::StatusMode::STATUSMODE_OFF;
1736 _communicationProtocolControlRegister.spiChecksum = vn::protocol::uart::ChecksumMode::CHECKSUMMODE_OFF;
1737 }
1738
1739 for (auto& binaryOutput : _binaryOutputRegister)
1740 {
1741 // for (const auto& item : _binaryGroupCommon)
1742 // {
1743 // if (!item.isEnabled(_sensorModel, binaryOutput, static_cast<uint32_t>(binaryOutput.commonField)))
1744 // {
1745 // binaryOutput.commonField &= ~vn::protocol::uart::CommonGroup(item.flagsValue);
1746 // }
1747 // }
1748 for (size_t i = 0; i < _binaryGroupTime.size(); i++)
1749 {
1750 const auto& item = _binaryGroupTime.at(i);
1751
1752 if (i == 0 /* TimeStartup */ || i == 9 /* TimeStatus */) { continue; } // Do not reset the field
1753
1754 // Check the fields if conditions are met
1755 if ((i == 2 /* GpsTow */ || i == 3 /* GpsWeek */)
1756 && _sensorModel == VectorNavModel::VN310
1757 && binaryOutput.timeField & vn::protocol::uart::TIMEGROUP_TIMESTARTUP
1758 && !_binaryGroupTime.at(0).isEnabled(_sensorModel, binaryOutput, static_cast<uint32_t>(binaryOutput.timeField)))
1759 {
1760 binaryOutput.timeField |= vn::protocol::uart::TimeGroup(item.flagsValue);
1761 continue;
1762 }
1763
1764 if (!item.isEnabled(_sensorModel, binaryOutput, static_cast<uint32_t>(binaryOutput.timeField)))
1765 {
1766 binaryOutput.timeField &= ~vn::protocol::uart::TimeGroup(item.flagsValue);
1767 }
1768 }
1769 for (const auto& item : _binaryGroupIMU)
1770 {
1771 if (!item.isEnabled(_sensorModel, binaryOutput, static_cast<uint32_t>(binaryOutput.imuField)))
1772 {
1773 binaryOutput.imuField &= ~vn::protocol::uart::ImuGroup(item.flagsValue);
1774 }
1775 }
1776 for (const auto& item : _binaryGroupGNSS)
1777 {
1778 if (!item.isEnabled(_sensorModel, binaryOutput, static_cast<uint32_t>(binaryOutput.gpsField)))
1779 {
1780 binaryOutput.gpsField &= ~vn::protocol::uart::GpsGroup(item.flagsValue);
1781 }
1782 if (!item.isEnabled(_sensorModel, binaryOutput, static_cast<uint32_t>(binaryOutput.gps2Field)))
1783 {
1784 binaryOutput.gps2Field &= ~vn::protocol::uart::GpsGroup(item.flagsValue);
1785 }
1786 }
1787 for (const auto& item : _binaryGroupAttitude)
1788 {
1789 if (!item.isEnabled(_sensorModel, binaryOutput, static_cast<uint32_t>(binaryOutput.attitudeField)))
1790 {
1791 binaryOutput.attitudeField &= ~vn::protocol::uart::AttitudeGroup(item.flagsValue);
1792 }
1793 }
1794 for (const auto& item : _binaryGroupINS)
1795 {
1796 if (!item.isEnabled(_sensorModel, binaryOutput, static_cast<uint32_t>(binaryOutput.insField)))
1797 {
1798 binaryOutput.insField &= ~vn::protocol::uart::InsGroup(item.flagsValue);
1799 }
1800 }
1801 }
1802 }
1803
1804 if (ImGui::InputTextWithHint("SensorPort", _connectedSensorPort.empty() ? "/dev/ttyUSB0" : _connectedSensorPort.c_str(), &_sensorPort))
1805 {
1806 LOG_DEBUG("{}: SensorPort changed to {}", nameId(), _sensorPort);
1807 flow::ApplyChanges();
1808 if (isInitialized())
1809 {
1810 doDeinitialize();
1811 }
1812 }
1813 ImGui::SameLine();
1814 gui::widgets::HelpMarker("COM port where the sensor is attached to\n"
1815 "- \"COM1\" (Windows format for physical and virtual (USB) serial port)\n"
1816 "- \"/dev/ttyS1\" (Linux format for physical serial port)\n"
1817 "- \"/dev/ttyUSB0\" (Linux format for virtual (USB) serial port)\n"
1818 "- \"/dev/tty.usbserial-FTXXXXXX\" (Mac OS X format for virtual (USB) serial port)\n"
1819 "- \"/dev/ttyS0\" (CYGWIN format. Usually the Windows COM port number minus 1. This would connect to COM1)");
1820
1821 bool isNodeInitialized = isInitialized();
1822 if (!isNodeInitialized)
1823 {
1824 ImGui::BeginDisabled();
1825 }
1826 if (ImGui::Button("Write settings"))
1827 {
1828 try
1829 {
1830 _vs.writeSettings();
1831 }
1832 catch (const std::exception& e)
1833 {
1834 LOG_ERROR("{}: Write settings threw an exception: {}", nameId(), e.what());
1835 }
1836 }
1837 if (!isNodeInitialized)
1838 {
1839 ImGui::EndDisabled();
1840 }
1841 if (ImGui::IsItemHovered())
1842 {
1843 ImGui::SetTooltip("This command will write the current register settings into non-volatile memory. Once the settings are stored\n"
1844 "in non-volatile (Flash) memory, the VN-310E module can be power cycled or reset, and the register will be\n"
1845 "reloaded from non-volatile memory.\n\n"
1846 "Due to limitations in the flash write speed the write settings command takes ~ 500ms to\n"
1847 "complete. Any commands that are sent to the sensor during this time will be responded to after\n"
1848 "the operation is complete.\n\n"
1849 "The sensor must be stationary when issuing a Write Settings Command otherwise a Reset\n"
1850 "command must also be issued to prevent the Kalman Filter from diverging during the write\n"
1851 "settings process.\n\n"
1852 "A write settings command is automatically send after initializing the node.");
1853 }
1854 ImGui::SameLine();
1855 if (!isNodeInitialized)
1856 {
1857 ImGui::BeginDisabled();
1858 }
1859 if (ImGui::Button("Restore factory settings"))
1860 {
1861 try
1862 {
1863 _vs.restoreFactorySettings();
1864 }
1865 catch (const std::exception& e)
1866 {
1867 LOG_ERROR("{}: Restore factory settings threw an exception: {}", nameId(), e.what());
1868 }
1869 }
1870 if (!isNodeInitialized)
1871 {
1872 ImGui::EndDisabled();
1873 }
1874 if (ImGui::IsItemHovered())
1875 {
1876 ImGui::SetTooltip("This command will restore the VN-310E module's factory default settings and will reset the module. There\n"
1877 "are no parameters for this command. The module will respond to this command before restoring the factory\n"
1878 "settings.");
1879 }
1880 ImGui::SameLine();
1881 if (!isNodeInitialized)
1882 {
1883 ImGui::BeginDisabled();
1884 }
1885 if (ImGui::Button("Reset sensor"))
1886 {
1887 try
1888 {
1889 _vs.reset();
1890 }
1891 catch (const std::exception& e)
1892 {
1893 LOG_ERROR("{}: Resetting threw an exception: {}", nameId(), e.what());
1894 }
1895 }
1896 if (!isNodeInitialized)
1897 {
1898 ImGui::EndDisabled();
1899 }
1900 if (ImGui::IsItemHovered())
1901 {
1902 ImGui::SetTooltip("This command will reset the module. There are no parameters required for this command. The module will\n"
1903 "first respond to the command and will then perform a reset. Upon a reset all registers will be reloaded with\n"
1904 "the values saved in non-volatile memory. If no values are stored in non-volatile memory, the device will default\n"
1905 "to factory settings. Also upon reset the VN-310E will re-initialize its Kalman filter, thus the filter will take a\n"
1906 "few seconds to completely converge on the correct attitude and correct for gyro bias.");
1907 }
1908
1909 // ###########################################################################################################
1910 // SYSTEM MODULE
1911 // ###########################################################################################################
1912
1913 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
1914 if (ImGui::CollapsingHeader(fmt::format("System Module##{}", size_t(id)).c_str()))
1915 {
1916 // ------------------------------------------- Serial Baud Rate ----------------------------------------------
1917
1918 std::array<const char*, 10> items = { "Fastest", "9600", "19200", "38400", "57600", "115200", "128000", "230400", "460800", "921600" };
1919 if (ImGui::Combo("Baudrate", &_selectedBaudrate, items.data(), items.size()))
1920 {
1921 LOG_DEBUG("{}: Baudrate changed to {}", nameId(), sensorBaudrate());
1922 flow::ApplyChanges();
1923 doDeinitialize();
1924 }
1925
1926 if (ImGui::TreeNode(fmt::format("Async Ascii Output##{}", size_t(id)).c_str()))
1927 {
1928 std::vector<std::pair<vn::protocol::uart::AsciiAsync, const char*>> asciiAsyncItems{
1929 { vn::protocol::uart::AsciiAsync::VNOFF, "Asynchronous output turned off" },
1930 { vn::protocol::uart::AsciiAsync::VNYPR, "Yaw, Pitch, Roll" },
1931 { vn::protocol::uart::AsciiAsync::VNQTN, "Quaternion" },
1932 { vn::protocol::uart::AsciiAsync::VNQMR, "Quaternion, Magnetic, Acceleration and Angular Rates" },
1933 { vn::protocol::uart::AsciiAsync::VNMAG, "Magnetic Measurements" },
1934 { vn::protocol::uart::AsciiAsync::VNACC, "Acceleration Measurements" },
1935 { vn::protocol::uart::AsciiAsync::VNGYR, "Angular Rate Measurements" },
1936 { vn::protocol::uart::AsciiAsync::VNMAR, "Magnetic, Acceleration, and Angular Rate Measurements" },
1937 { vn::protocol::uart::AsciiAsync::VNYMR, "Yaw, Pitch, Roll, Magnetic, Acceleration, and Angular Rate Measurements" },
1938 { vn::protocol::uart::AsciiAsync::VNYBA, "Yaw, Pitch, Roll, Body True Acceleration, and Angular Rates" },
1939 { vn::protocol::uart::AsciiAsync::VNYIA, "Yaw, Pitch, Roll, Inertial True Acceleration, and Angular Rates" },
1940 { vn::protocol::uart::AsciiAsync::VNIMU, "IMU Measurements" }
1941 };
1942 if (_sensorModel == VectorNavModel::VN310)
1943 {
1944 asciiAsyncItems.emplace_back(vn::protocol::uart::AsciiAsync::VNGPS, "GNSS LLA");
1945 asciiAsyncItems.emplace_back(vn::protocol::uart::AsciiAsync::VNGPE, "GNSS ECEF");
1946 asciiAsyncItems.emplace_back(vn::protocol::uart::AsciiAsync::VNINS, "INS LLA");
1947 asciiAsyncItems.emplace_back(vn::protocol::uart::AsciiAsync::VNINE, "INS ECEF");
1948 asciiAsyncItems.emplace_back(vn::protocol::uart::AsciiAsync::VNISL, "INS LLA 2");
1949 asciiAsyncItems.emplace_back(vn::protocol::uart::AsciiAsync::VNISE, "INS ECEF 2");
1950 }
1951 asciiAsyncItems.emplace_back(vn::protocol::uart::AsciiAsync::VNDTV, "Delta theta and delta velocity");
1952 if (_sensorModel == VectorNavModel::VN310)
1953 {
1954 asciiAsyncItems.emplace_back(vn::protocol::uart::AsciiAsync::VNG2S, "GNSS2 LLA");
1955 asciiAsyncItems.emplace_back(vn::protocol::uart::AsciiAsync::VNG2E, "GNSS2 ECEF");
1956 }
1957
1958 if (ImGui::BeginCombo(fmt::format("Async Ascii Output Type##{}", size_t(id)).c_str(), vn::protocol::uart::str(_asyncDataOutputType).c_str()))
1959 {
1960 for (const auto& asciiAsyncItem : asciiAsyncItems)
1961 {
1962 const bool isSelected = (_asyncDataOutputType == asciiAsyncItem.first);
1963 if (ImGui::Selectable(vn::protocol::uart::str(asciiAsyncItem.first).c_str(), isSelected))
1964 {
1965 _asyncDataOutputType = asciiAsyncItem.first;
1966 LOG_DEBUG("{}: _asyncDataOutputType changed to {}", nameId(), vn::protocol::uart::str(_asyncDataOutputType));
1967 flow::ApplyChanges();
1968 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
1969 {
1970 try
1971 {
1972 _vs.writeAsyncDataOutputType(_asyncDataOutputType);
1973 }
1974 catch (const std::exception& e)
1975 {
1976 LOG_ERROR("{}: Could not configure the asyncDataOutputType: {}", nameId(), e.what());
1977 doDeinitialize();
1978 }
1979 }
1980 else
1981 {
1982 doDeinitialize();
1983 }
1984 }
1985 if (ImGui::IsItemHovered())
1986 {
1987 ImGui::BeginTooltip();
1988 ImGui::TextUnformatted(asciiAsyncItem.second);
1989 ImGui::EndTooltip();
1990 }
1991
1992 // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
1993 if (isSelected)
1994 {
1995 ImGui::SetItemDefaultFocus();
1996 }
1997 }
1998 ImGui::EndCombo();
1999 }
2000 ImGui::SameLine();
2001 gui::widgets::HelpMarker("This register controls the type of data that will be asynchronously outputted by the module. With this "
2002 "register, the user can specify which data register will be automatically outputted when it gets updated "
2003 "with a new reading. The table below lists which registers can be set to asynchronously output, the value "
2004 "to specify which register to output, and the header of the asynchronous data packet. Asynchronous data "
2005 "output can be disabled by setting this register to zero. The asynchronous data output will be sent out "
2006 "automatically at a frequency specified by the Async Data Output Frequency Register.");
2007
2008 if (ImGui::SliderInt(fmt::format("Async Ascii Output Frequency##{}", size_t(id)).c_str(),
2009 &_asyncDataOutputFrequencySelected,
2010 0, _possibleAsyncDataOutputFrequency.size() - 1,
2011 fmt::format("{} Hz", _possibleAsyncDataOutputFrequency.at(static_cast<size_t>(_asyncDataOutputFrequencySelected))).c_str()))
2012 {
2013 _asyncDataOutputFrequency = static_cast<uint32_t>(_possibleAsyncDataOutputFrequency.at(static_cast<size_t>(_asyncDataOutputFrequencySelected)));
2014 LOG_DEBUG("{}: asyncDataOutputFrequency changed to {} Hz", nameId(), _asyncDataOutputFrequency);
2015 flow::ApplyChanges();
2016 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2017 {
2018 try
2019 {
2020 _vs.writeAsyncDataOutputFrequency(_asyncDataOutputFrequency);
2021 }
2022 catch (const std::exception& e)
2023 {
2024 LOG_ERROR("{}: Could not configure the asyncDataOutputFrequency: {}", nameId(), e.what());
2025 doDeinitialize();
2026 }
2027 }
2028 else
2029 {
2030 doDeinitialize();
2031 }
2032 }
2033 ImGui::SameLine();
2034 gui::widgets::HelpMarker("Asynchronous data output frequency.\nThe ADOF will be changed for the active serial port.");
2035
2036 if (ImGui::DragInt(fmt::format("Async Ascii Output buffer size##{}", size_t(id)).c_str(), &_asciiOutputBufferSize, 1.0F, 0, INT32_MAX / 2))
2037 {
2038 _asciiOutputBuffer.resize(static_cast<size_t>(_asciiOutputBufferSize));
2039 LOG_DEBUG("{}: asciiOutputBufferSize changed to {}", nameId(), _asciiOutputBufferSize);
2040 flow::ApplyChanges();
2041 }
2042
2043 std::string messages;
2044 for (const auto& msg : _asciiOutputBuffer)
2045 {
2046 messages.append(msg);
2047 }
2048 ImGui::TextUnformatted("Async Ascii Messages:");
2049 ImGui::BeginChild(fmt::format("##Ascii Mesages {}", size_t(id)).c_str(), ImVec2(0, 300), true);
2050 ImGui::PushTextWrapPos();
2051 ImGui::TextUnformatted(messages.c_str());
2052 ImGui::PopTextWrapPos();
2053 ImGui::EndChild();
2054
2055 ImGui::TreePop();
2056 }
2057
2058 if (ImGui::TreeNode(fmt::format("Synchronization Control##{}", size_t(id)).c_str()))
2059 {
2060 ImGui::TextUnformatted("Contains parameters which allow the timing of the VN-310E to be\n"
2061 "synchronized with external devices.");
2062
2063 if (ImGui::Checkbox(fmt::format("Show SyncIn Pin##{}", size_t(id)).c_str(), &_syncInPin))
2064 {
2065 LOG_DEBUG("{}: syncInPin changed to {}", nameId(), _syncInPin);
2066 if (_syncInPin)
2067 {
2068 _synchronizationControlRegister.syncInMode = vn::protocol::uart::SyncInMode::SYNCINMODE_COUNT;
2069 LOG_DEBUG("{}: synchronizationControlRegister.syncInMode changed to {}", nameId(), vn::protocol::uart::str(_synchronizationControlRegister.syncInMode));
2070 }
2071 flow::ApplyChanges();
2072 if (_syncInPin && inputPins.empty())
2073 {
2074 CreateInputPin("SyncIn", Pin::Type::Object, { "TimeSync" });
2075 }
2076 else if (!_syncInPin && !inputPins.empty())
2077 {
2078 DeleteInputPin(0);
2079 }
2080 }
2081
2082 static constexpr std::array<std::pair<vn::protocol::uart::SyncInMode, const char*>, 4> synchronizationControlSyncInModes = {
2083 { { vn::protocol::uart::SyncInMode::SYNCINMODE_COUNT, "Count number of trigger events on SYNC_IN" },
2084 { vn::protocol::uart::SyncInMode::SYNCINMODE_IMU, "Start IMU sampling on trigger of SYNC_IN" },
2085 { vn::protocol::uart::SyncInMode::SYNCINMODE_ASYNC, "Output asynchronous message on trigger of SYNC_IN" },
2086 { vn::protocol::uart::SyncInMode::SYNCINMODE_ASYNC3, "Output asynchronous or binary messages configured with a rate of 0 to output on trigger of SYNC_IN\n\n"
2087 "In ASYNC3 mode messages configured with an output rate = 0 are output each time the appropriate\n"
2088 "transistion edge of the SyncIn pin is captured according to the edge settings in the SyncInEdge field.\n"
2089 "Messages configured with output rate > 0 are not affected by the SyncIn pulse. This applies to the ASCII\n"
2090 "Async message set by the Async Data Output Register, the user configurate binary output messages set\n"
2091 "by the Binary Output Registers, as well as the NMEA messages configured by the NMEA Output Registers." } }
2092 };
2093 if (_syncInPin)
2094 {
2095 ImGui::BeginDisabled();
2096 }
2097
2098 if (ImGui::BeginCombo(fmt::format("SyncIn Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_synchronizationControlRegister.syncInMode).c_str()))
2099 {
2100 for (const auto& synchronizationControlSyncInMode : synchronizationControlSyncInModes)
2101 {
2102 const bool isSelected = (_synchronizationControlRegister.syncInMode == synchronizationControlSyncInMode.first);
2103 if (ImGui::Selectable(vn::protocol::uart::str(synchronizationControlSyncInMode.first).c_str(), isSelected))
2104 {
2105 _synchronizationControlRegister.syncInMode = synchronizationControlSyncInMode.first;
2106 LOG_DEBUG("{}: synchronizationControlRegister.syncInMode changed to {}", nameId(), vn::protocol::uart::str(_synchronizationControlRegister.syncInMode));
2107 flow::ApplyChanges();
2108 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2109 {
2110 try
2111 {
2112 _vs.writeSynchronizationControl(_synchronizationControlRegister);
2113 }
2114 catch (const std::exception& e)
2115 {
2116 LOG_ERROR("{}: Could not configure the synchronizationControlRegister: {}", nameId(), e.what());
2117 doDeinitialize();
2118 }
2119 }
2120 else
2121 {
2122 doDeinitialize();
2123 }
2124 }
2125 if (ImGui::IsItemHovered())
2126 {
2127 ImGui::BeginTooltip();
2128 ImGui::TextUnformatted(synchronizationControlSyncInMode.second);
2129 ImGui::EndTooltip();
2130 }
2131
2132 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
2133 {
2134 ImGui::SetItemDefaultFocus();
2135 }
2136 }
2137 ImGui::EndCombo();
2138 }
2139 ImGui::SameLine();
2140 gui::widgets::HelpMarker("The SyncInMode register controls the behavior of the SyncIn event. If the mode is set to COUNT then the "
2141 "internal clock will be used to control the IMU sampling. If SyncInMode is set to IMU then the IMU sampling "
2142 "loop will run on a SyncIn event. The relationship between the SyncIn event and a SyncIn trigger is defined "
2143 "by the SyncInEdge and SyncInSkipFactor parameters. If set to ASYNC then the VN-100 will output "
2144 "asynchronous serial messages upon each trigger event.");
2145 if (_syncInPin)
2146 {
2147 ImGui::EndDisabled();
2148 }
2149
2150 static constexpr std::array<std::pair<vn::protocol::uart::SyncInEdge, const char*>, 2> synchronizationControlSyncInEdges = {
2151 { { vn::protocol::uart::SyncInEdge::SYNCINEDGE_RISING, "Trigger on rising edge" },
2152 { vn::protocol::uart::SyncInEdge::SYNCINEDGE_FALLING, "Trigger on falling edge" } }
2153 };
2154 if (ImGui::BeginCombo(fmt::format("SyncIn Edge##{}", size_t(id)).c_str(), vn::protocol::uart::str(_synchronizationControlRegister.syncInEdge).c_str()))
2155 {
2156 for (const auto& synchronizationControlSyncInEdge : synchronizationControlSyncInEdges)
2157 {
2158 const bool isSelected = (_synchronizationControlRegister.syncInEdge == synchronizationControlSyncInEdge.first);
2159 if (ImGui::Selectable(vn::protocol::uart::str(synchronizationControlSyncInEdge.first).c_str(), isSelected))
2160 {
2161 _synchronizationControlRegister.syncInEdge = synchronizationControlSyncInEdge.first;
2162 LOG_DEBUG("{}: synchronizationControlRegister.syncInEdge changed to {}", nameId(), vn::protocol::uart::str(_synchronizationControlRegister.syncInEdge));
2163 flow::ApplyChanges();
2164 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2165 {
2166 try
2167 {
2168 _vs.writeSynchronizationControl(_synchronizationControlRegister);
2169 }
2170 catch (const std::exception& e)
2171 {
2172 LOG_ERROR("{}: Could not configure the synchronizationControlRegister: {}", nameId(), e.what());
2173 doDeinitialize();
2174 }
2175 }
2176 else
2177 {
2178 doDeinitialize();
2179 }
2180 }
2181 if (ImGui::IsItemHovered())
2182 {
2183 ImGui::BeginTooltip();
2184 ImGui::TextUnformatted(synchronizationControlSyncInEdge.second);
2185 ImGui::EndTooltip();
2186 }
2187
2188 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
2189 {
2190 ImGui::SetItemDefaultFocus();
2191 }
2192 }
2193 ImGui::EndCombo();
2194 }
2195 ImGui::SameLine();
2196 gui::widgets::HelpMarker("The SyncInEdge register controls the type of edge the signal is set to trigger on.\n"
2197 "The factory default state is to trigger on a rising edge.");
2198
2199 int syncInSkipFactor = _synchronizationControlRegister.syncInSkipFactor;
2200 if (ImGui::InputInt(fmt::format("SyncIn Skip Factor##{}", size_t(id)).c_str(), &syncInSkipFactor))
2201 {
2202 if (syncInSkipFactor < 0)
2203 {
2204 syncInSkipFactor = 0;
2205 }
2206 else if (syncInSkipFactor > std::numeric_limits<uint16_t>::max())
2207 {
2208 syncInSkipFactor = std::numeric_limits<uint16_t>::max();
2209 }
2210 _synchronizationControlRegister.syncInSkipFactor = static_cast<uint16_t>(syncInSkipFactor);
2211 LOG_DEBUG("{}: synchronizationControlRegister.syncInSkipFactor changed to {}", nameId(), _synchronizationControlRegister.syncInSkipFactor);
2212 flow::ApplyChanges();
2213 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2214 {
2215 try
2216 {
2217 _vs.writeSynchronizationControl(_synchronizationControlRegister);
2218 }
2219 catch (const std::exception& e)
2220 {
2221 LOG_ERROR("{}: Could not configure the synchronizationControlRegister: {}", nameId(), e.what());
2222 doDeinitialize();
2223 }
2224 }
2225 else
2226 {
2227 doDeinitialize();
2228 }
2229 }
2230 ImGui::SameLine();
2231 gui::widgets::HelpMarker("The SyncInSkipFactor defines how many times trigger edges defined by SyncInEdge should occur prior to "
2232 "triggering a SyncIn event. The action performed on a SyncIn event is determined by the SyncIn mode. As an "
2233 "example if the SyncInSkipFactor was set to 4 and a 1 kHz signal was attached to the SyncIn pin, then the "
2234 "SyncIn event would only occur at 200 Hz.");
2235
2236 static constexpr std::array<std::pair<vn::protocol::uart::SyncOutMode, const char*>, 5> synchronizationControlSyncOutModes = {
2237 { { vn::protocol::uart::SyncOutMode::SYNCOUTMODE_NONE, "None" },
2238 { vn::protocol::uart::SyncOutMode::SYNCOUTMODE_IMUSTART, "Trigger at start of IMU sampling" },
2239 { vn::protocol::uart::SyncOutMode::SYNCOUTMODE_IMUREADY, "Trigger when IMU measurements are available" },
2240 { vn::protocol::uart::SyncOutMode::SYNCOUTMODE_INS, "Trigger when attitude measurements are available" },
2241 { vn::protocol::uart::SyncOutMode::SYNCOUTMODE_GPSPPS, "Trigger on a GPS PPS event (1 Hz) when a 3D fix is valid." } }
2242 };
2243 if (ImGui::BeginCombo(fmt::format("SyncOut Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_synchronizationControlRegister.syncOutMode).c_str()))
2244 {
2245 for (const auto& synchronizationControlSyncOutMode : synchronizationControlSyncOutModes)
2246 {
2247 const bool isSelected = (_synchronizationControlRegister.syncOutMode == synchronizationControlSyncOutMode.first);
2248 if (ImGui::Selectable(vn::protocol::uart::str(synchronizationControlSyncOutMode.first).c_str(), isSelected))
2249 {
2250 _synchronizationControlRegister.syncOutMode = synchronizationControlSyncOutMode.first;
2251 LOG_DEBUG("{}: synchronizationControlRegister.syncOutMode changed to {}", nameId(), vn::protocol::uart::str(_synchronizationControlRegister.syncOutMode));
2252
2253 if (_synchronizationControlRegister.syncOutMode == vn::protocol::uart::SyncOutMode::SYNCOUTMODE_GPSPPS
2254 && outputPins.size() <= 4)
2255 {
2256 CreateOutputPin("SyncOut", Pin::Type::Object, { "TimeSync" }, &_timeSyncOut);
2257 }
2258 else if (_synchronizationControlRegister.syncOutMode != vn::protocol::uart::SyncOutMode::SYNCOUTMODE_GPSPPS
2259 && outputPins.size() == 5)
2260 {
2261 DeleteOutputPin(4);
2262 }
2263
2264 flow::ApplyChanges();
2265 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2266 {
2267 try
2268 {
2269 _vs.writeSynchronizationControl(_synchronizationControlRegister);
2270 }
2271 catch (const std::exception& e)
2272 {
2273 LOG_ERROR("{}: Could not configure the synchronizationControlRegister: {}", nameId(), e.what());
2274 doDeinitialize();
2275 }
2276 }
2277 else
2278 {
2279 doDeinitialize();
2280 }
2281 }
2282 if (ImGui::IsItemHovered())
2283 {
2284 ImGui::BeginTooltip();
2285 ImGui::TextUnformatted(synchronizationControlSyncOutMode.second);
2286 ImGui::EndTooltip();
2287 }
2288
2289 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
2290 {
2291 ImGui::SetItemDefaultFocus();
2292 }
2293 }
2294 ImGui::EndCombo();
2295 }
2296 ImGui::SameLine();
2297 gui::widgets::HelpMarker("The SyncOutMode register controls the behavior of the SyncOut pin. If this is set to IMU then the SyncOut "
2298 "will start the pulse when the internal IMU sample loop starts. This mode is used to make a sensor the Master "
2299 "in a multi-sensor network array. If this is set to IMU_READY mode then the pulse will start when IMU "
2300 "measurements become available. If this is set to INS mode then the pulse will start when attitude "
2301 "measurements are made available. Changes to this register take effect immediately.");
2302
2303 static constexpr std::array<std::pair<vn::protocol::uart::SyncOutPolarity, const char*>, 2> synchronizationControlSyncOutPolarities = {
2304 { { vn::protocol::uart::SyncOutPolarity::SYNCOUTPOLARITY_NEGATIVE, "Negative Pulse" },
2305 { vn::protocol::uart::SyncOutPolarity::SYNCOUTPOLARITY_POSITIVE, "Positive Pulse" } }
2306 };
2307 if (ImGui::BeginCombo(fmt::format("SyncOut Polarity##{}", size_t(id)).c_str(), vn::protocol::uart::str(_synchronizationControlRegister.syncOutPolarity).c_str()))
2308 {
2309 for (const auto& synchronizationControlSyncOutPolarity : synchronizationControlSyncOutPolarities)
2310 {
2311 const bool isSelected = (_synchronizationControlRegister.syncOutPolarity == synchronizationControlSyncOutPolarity.first);
2312 if (ImGui::Selectable(vn::protocol::uart::str(synchronizationControlSyncOutPolarity.first).c_str(), isSelected))
2313 {
2314 _synchronizationControlRegister.syncOutPolarity = synchronizationControlSyncOutPolarity.first;
2315 LOG_DEBUG("{}: synchronizationControlRegister.syncOutPolarity changed to {}", nameId(), vn::protocol::uart::str(_synchronizationControlRegister.syncOutPolarity));
2316 flow::ApplyChanges();
2317 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2318 {
2319 try
2320 {
2321 _vs.writeSynchronizationControl(_synchronizationControlRegister);
2322 }
2323 catch (const std::exception& e)
2324 {
2325 LOG_ERROR("{}: Could not configure the synchronizationControlRegister: {}", nameId(), e.what());
2326 doDeinitialize();
2327 }
2328 }
2329 else
2330 {
2331 doDeinitialize();
2332 }
2333 }
2334 if (ImGui::IsItemHovered())
2335 {
2336 ImGui::BeginTooltip();
2337 ImGui::TextUnformatted(synchronizationControlSyncOutPolarity.second);
2338 ImGui::EndTooltip();
2339 }
2340
2341 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
2342 {
2343 ImGui::SetItemDefaultFocus();
2344 }
2345 }
2346 ImGui::EndCombo();
2347 }
2348 ImGui::SameLine();
2349 gui::widgets::HelpMarker("The SyncOutPolarity register controls the polarity of the output pulse on the SyncOut pin.\n"
2350 "Changes to this register take effect immediately.");
2351
2352 int syncOutSkipFactor = _synchronizationControlRegister.syncOutSkipFactor;
2353 if (ImGui::InputInt(fmt::format("SyncOut Skip Factor##{}", size_t(id)).c_str(), &syncOutSkipFactor))
2354 {
2355 if (syncOutSkipFactor < 0)
2356 {
2357 syncOutSkipFactor = 0;
2358 }
2359 else if (syncOutSkipFactor > std::numeric_limits<uint16_t>::max())
2360 {
2361 syncOutSkipFactor = std::numeric_limits<uint16_t>::max();
2362 }
2363 _synchronizationControlRegister.syncOutSkipFactor = static_cast<uint16_t>(syncOutSkipFactor);
2364 LOG_DEBUG("{}: synchronizationControlRegister.syncOutSkipFactor changed to {}", nameId(), _synchronizationControlRegister.syncOutSkipFactor);
2365 flow::ApplyChanges();
2366 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2367 {
2368 try
2369 {
2370 _vs.writeSynchronizationControl(_synchronizationControlRegister);
2371 }
2372 catch (const std::exception& e)
2373 {
2374 LOG_ERROR("{}: Could not configure the synchronizationControlRegister: {}", nameId(), e.what());
2375 doDeinitialize();
2376 }
2377 }
2378 else
2379 {
2380 doDeinitialize();
2381 }
2382 }
2383 ImGui::SameLine();
2384 gui::widgets::HelpMarker("The SyncOutSkipFactor defines how many times the sync out event should be skipped before actually triggering the SyncOut pin.");
2385
2386 int syncOutPulseWidth = static_cast<int>(_synchronizationControlRegister.syncOutPulseWidth);
2387 if (ImGui::InputInt(fmt::format("SyncOut Pulse Width##{}", size_t(id)).c_str(), &syncOutPulseWidth))
2388 {
2389 syncOutPulseWidth = std::max(syncOutPulseWidth, 0);
2390 _synchronizationControlRegister.syncOutPulseWidth = static_cast<uint32_t>(syncOutPulseWidth);
2391 LOG_DEBUG("{}: synchronizationControlRegister.syncOutPulseWidth changed to {}", nameId(), _synchronizationControlRegister.syncOutPulseWidth);
2392 flow::ApplyChanges();
2393 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2394 {
2395 try
2396 {
2397 _vs.writeSynchronizationControl(_synchronizationControlRegister);
2398 }
2399 catch (const std::exception& e)
2400 {
2401 LOG_ERROR("{}: Could not configure the synchronizationControlRegister: {}", nameId(), e.what());
2402 doDeinitialize();
2403 }
2404 }
2405 else
2406 {
2407 doDeinitialize();
2408 }
2409 }
2410 ImGui::SameLine();
2411 gui::widgets::HelpMarker("The SyncOutPulseWidth field controls the desired width of the SyncOut pulse.\n"
2412 "The default value is 100,000,000 (100 ms).");
2413
2414 ImGui::TreePop();
2415 }
2416
2417 if (ImGui::TreeNode(fmt::format("Communication Protocol Control##{}", size_t(id)).c_str()))
2418 {
2419 ImGui::TextUnformatted("Contains parameters that controls the communication protocol used by the sensor.");
2420
2421 static constexpr std::array<std::pair<vn::protocol::uart::CountMode, const char*>, 5> communicationProtocolControlSerialCounts = {
2422 { { vn::protocol::uart::CountMode::COUNTMODE_NONE, "OFF" },
2423 { vn::protocol::uart::CountMode::COUNTMODE_SYNCINCOUNT, "SyncIn Counter" },
2424 { vn::protocol::uart::CountMode::COUNTMODE_SYNCINTIME, "SyncIn Time" },
2425 { vn::protocol::uart::CountMode::COUNTMODE_SYNCOUTCOUNTER, "SyncOut Counter" },
2426 { vn::protocol::uart::CountMode::COUNTMODE_GPSPPS, "Gps Pps Time" } }
2427 };
2428 if (ImGui::BeginCombo(fmt::format("Serial Count Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_communicationProtocolControlRegister.serialCount).c_str()))
2429 {
2430 for (const auto& communicationProtocolControlSerialCount : communicationProtocolControlSerialCounts)
2431 {
2432 const bool isSelected = (_communicationProtocolControlRegister.serialCount == communicationProtocolControlSerialCount.first);
2433 if (ImGui::Selectable(vn::protocol::uart::str(communicationProtocolControlSerialCount.first).c_str(), isSelected))
2434 {
2435 _communicationProtocolControlRegister.serialCount = communicationProtocolControlSerialCount.first;
2436 LOG_DEBUG("{}: communicationProtocolControlRegister.serialCount changed to {}", nameId(), vn::protocol::uart::str(_communicationProtocolControlRegister.serialCount));
2437 flow::ApplyChanges();
2438 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2439 {
2440 try
2441 {
2442 _vs.writeCommunicationProtocolControl(_communicationProtocolControlRegister);
2443 }
2444 catch (const std::exception& e)
2445 {
2446 LOG_ERROR("{}: Could not configure the communicationProtocolControlRegister: {}", nameId(), e.what());
2447 doDeinitialize();
2448 }
2449 }
2450 else
2451 {
2452 doDeinitialize();
2453 }
2454 }
2455 if (ImGui::IsItemHovered())
2456 {
2457 ImGui::BeginTooltip();
2458 ImGui::TextUnformatted(communicationProtocolControlSerialCount.second);
2459 ImGui::EndTooltip();
2460 }
2461
2462 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
2463 {
2464 ImGui::SetItemDefaultFocus();
2465 }
2466 }
2467 ImGui::EndCombo();
2468 }
2469 ImGui::SameLine();
2470 gui::widgets::HelpMarker("The SerialCount field provides a means of appending a time or counter to the end of all asynchronous "
2471 "communication messages transmitted on the serial interface. The values for each of these counters come "
2472 "directly from the Synchronization Status Register in the System subsystem.\n\n"
2473 "With the SerialCount field set to OFF a typical serial asynchronous message would appear as the following:\n"
2474 "$VNYPR,+010.071,+000.278,-002.026*60\n\n"
2475 "With the SerialCount field set to one of the non-zero values the same asynchronous message would appear "
2476 "instead as:\n"
2477 "$VNYPR,+010.071,+000.278,-002.026,T1162704*2F\n\n"
2478 "When the SerialCount field is enabled the counter will always be appended to the end of the message just "
2479 "prior to the checksum. The counter will be preceded by the T character to distinguish it from the status field.");
2480
2481 static constexpr std::array<std::pair<vn::protocol::uart::StatusMode, const char*>, 3> communicationProtocolControlSerialStatuses = {
2482 { { vn::protocol::uart::StatusMode::STATUSMODE_OFF, "OFF" },
2483 { vn::protocol::uart::StatusMode::STATUSMODE_VPESTATUS, "VPE Status" },
2484 { vn::protocol::uart::StatusMode::STATUSMODE_INSSTATUS, "INS Status" } }
2485 };
2486 if (ImGui::BeginCombo(fmt::format("Serial Status##{}", size_t(id)).c_str(), vn::protocol::uart::str(_communicationProtocolControlRegister.serialStatus).c_str()))
2487 {
2488 for (const auto& communicationProtocolControlSerialStatus : communicationProtocolControlSerialStatuses)
2489 {
2490 const bool isSelected = (_communicationProtocolControlRegister.serialStatus == communicationProtocolControlSerialStatus.first);
2491 if (ImGui::Selectable(vn::protocol::uart::str(communicationProtocolControlSerialStatus.first).c_str(), isSelected))
2492 {
2493 _communicationProtocolControlRegister.serialStatus = communicationProtocolControlSerialStatus.first;
2494 LOG_DEBUG("{}: communicationProtocolControlRegister.serialStatus changed to {}", nameId(), vn::protocol::uart::str(_communicationProtocolControlRegister.serialStatus));
2495 flow::ApplyChanges();
2496 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2497 {
2498 try
2499 {
2500 _vs.writeCommunicationProtocolControl(_communicationProtocolControlRegister);
2501 }
2502 catch (const std::exception& e)
2503 {
2504 LOG_ERROR("{}: Could not configure the communicationProtocolControlRegister: {}", nameId(), e.what());
2505 doDeinitialize();
2506 }
2507 }
2508 else
2509 {
2510 doDeinitialize();
2511 }
2512 }
2513 if (ImGui::IsItemHovered())
2514 {
2515 ImGui::BeginTooltip();
2516 ImGui::TextUnformatted(communicationProtocolControlSerialStatus.second);
2517 ImGui::EndTooltip();
2518 }
2519
2520 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
2521 {
2522 ImGui::SetItemDefaultFocus();
2523 }
2524 }
2525 ImGui::EndCombo();
2526 }
2527 ImGui::SameLine();
2528 gui::widgets::HelpMarker("The SerialStatus field provides a means of tracking real-time status information pertain to the overall state "
2529 "of the sensor measurements and onboard filtering algorithm. As with the SerialCount, a typical serial "
2530 "asynchronous message would appear as the following:\n"
2531 "$VNYPR,+010.071,+000.278,-002.026*60\n\n"
2532 "With the SerialStatus field set to one of the non-zero values, the same asynchronous message would appear "
2533 "instead as:\n"
2534 "$VNYPR,+010.071,+000.278,-002.026,S0000*1F\n\n"
2535 "When the SerialStatus field is enabled the status will always be appended to the end of the message just "
2536 "prior to the checksum. If both the SerialCount and SerialStatus are enabled then the SerialStatus will be "
2537 "displayed first. The counter will be preceded by the S character to distinguish it from the counter field. The "
2538 "status consists of 4 hexadecimal characters.");
2539
2540 if (_sensorModel == VectorNavModel::VN100_VN110)
2541 {
2542 if (ImGui::BeginCombo(fmt::format("SPI Count##{}", size_t(id)).c_str(), vn::protocol::uart::str(_communicationProtocolControlRegister.spiCount).c_str()))
2543 {
2544 for (const auto& communicationProtocolControlSpiCount : communicationProtocolControlSerialCounts)
2545 {
2546 const bool isSelected = (_communicationProtocolControlRegister.spiCount == communicationProtocolControlSpiCount.first);
2547 if (ImGui::Selectable(vn::protocol::uart::str(communicationProtocolControlSpiCount.first).c_str(), isSelected))
2548 {
2549 _communicationProtocolControlRegister.spiCount = communicationProtocolControlSpiCount.first;
2550 LOG_DEBUG("{}: communicationProtocolControlRegister.spiCount changed to {}", nameId(), vn::protocol::uart::str(_communicationProtocolControlRegister.spiCount));
2551 flow::ApplyChanges();
2552 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2553 {
2554 try
2555 {
2556 _vs.writeCommunicationProtocolControl(_communicationProtocolControlRegister);
2557 }
2558 catch (const std::exception& e)
2559 {
2560 LOG_ERROR("{}: Could not configure the communicationProtocolControlRegister: {}", nameId(), e.what());
2561 doDeinitialize();
2562 }
2563 }
2564 else
2565 {
2566 doDeinitialize();
2567 }
2568 }
2569 if (ImGui::IsItemHovered())
2570 {
2571 ImGui::BeginTooltip();
2572 ImGui::TextUnformatted(communicationProtocolControlSpiCount.second);
2573 ImGui::EndTooltip();
2574 }
2575
2576 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
2577 {
2578 ImGui::SetItemDefaultFocus();
2579 }
2580 }
2581 ImGui::EndCombo();
2582 }
2583 ImGui::SameLine();
2584 gui::widgets::HelpMarker("The SPICount field provides a means of appending a time or counter to the end of all SPI packets. The "
2585 "values for each of these counters come directly from the Synchronization Status Register.");
2586 }
2587
2588 if (_sensorModel == VectorNavModel::VN100_VN110)
2589 {
2590 if (ImGui::BeginCombo(fmt::format("SPI Status##{}", size_t(id)).c_str(), vn::protocol::uart::str(_communicationProtocolControlRegister.spiStatus).c_str()))
2591 {
2592 for (const auto& communicationProtocolControlSpiStatus : communicationProtocolControlSerialStatuses)
2593 {
2594 const bool isSelected = (_communicationProtocolControlRegister.spiStatus == communicationProtocolControlSpiStatus.first);
2595 if (ImGui::Selectable(vn::protocol::uart::str(communicationProtocolControlSpiStatus.first).c_str(), isSelected))
2596 {
2597 _communicationProtocolControlRegister.spiStatus = communicationProtocolControlSpiStatus.first;
2598 LOG_DEBUG("{}: communicationProtocolControlRegister.spiStatus changed to {}", nameId(), vn::protocol::uart::str(_communicationProtocolControlRegister.spiStatus));
2599 flow::ApplyChanges();
2600 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2601 {
2602 try
2603 {
2604 _vs.writeCommunicationProtocolControl(_communicationProtocolControlRegister);
2605 }
2606 catch (const std::exception& e)
2607 {
2608 LOG_ERROR("{}: Could not configure the communicationProtocolControlRegister: {}", nameId(), e.what());
2609 doDeinitialize();
2610 }
2611 }
2612 else
2613 {
2614 doDeinitialize();
2615 }
2616 }
2617 if (ImGui::IsItemHovered())
2618 {
2619 ImGui::BeginTooltip();
2620 ImGui::TextUnformatted(communicationProtocolControlSpiStatus.second);
2621 ImGui::EndTooltip();
2622 }
2623
2624 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
2625 {
2626 ImGui::SetItemDefaultFocus();
2627 }
2628 }
2629 ImGui::EndCombo();
2630 }
2631 ImGui::SameLine();
2632 gui::widgets::HelpMarker("The AsyncStatus field provides a means of tracking real-time status information pertaining to the overall "
2633 "state of the sensor measurements and onboard filtering algorithm. This information is very useful in "
2634 "situations where action must be taken when certain crucial events happen such as the detection of gyro "
2635 "saturation or magnetic interference.");
2636 }
2637
2638 static constexpr std::array<std::pair<vn::protocol::uart::ChecksumMode, const char*>, 2> communicationProtocolControlSerialChecksums = {
2639 { { vn::protocol::uart::ChecksumMode::CHECKSUMMODE_CHECKSUM, "8-Bit Checksum" },
2640 { vn::protocol::uart::ChecksumMode::CHECKSUMMODE_CRC, "16-Bit CRC" } }
2641 };
2642 if (ImGui::BeginCombo(fmt::format("Serial Checksum##{}", size_t(id)).c_str(), vn::protocol::uart::str(_communicationProtocolControlRegister.serialChecksum).c_str()))
2643 {
2644 for (const auto& communicationProtocolControlSerialChecksum : communicationProtocolControlSerialChecksums)
2645 {
2646 const bool isSelected = (_communicationProtocolControlRegister.serialChecksum == communicationProtocolControlSerialChecksum.first);
2647 if (ImGui::Selectable(vn::protocol::uart::str(communicationProtocolControlSerialChecksum.first).c_str(), isSelected))
2648 {
2649 _communicationProtocolControlRegister.serialChecksum = communicationProtocolControlSerialChecksum.first;
2650 LOG_DEBUG("{}: communicationProtocolControlRegister.serialChecksum changed to {}", nameId(), vn::protocol::uart::str(_communicationProtocolControlRegister.serialChecksum));
2651 flow::ApplyChanges();
2652 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2653 {
2654 try
2655 {
2656 _vs.writeCommunicationProtocolControl(_communicationProtocolControlRegister);
2657 }
2658 catch (const std::exception& e)
2659 {
2660 LOG_ERROR("{}: Could not configure the communicationProtocolControlRegister: {}", nameId(), e.what());
2661 doDeinitialize();
2662 }
2663 }
2664 else
2665 {
2666 doDeinitialize();
2667 }
2668 }
2669 if (ImGui::IsItemHovered())
2670 {
2671 ImGui::BeginTooltip();
2672 ImGui::TextUnformatted(communicationProtocolControlSerialChecksum.second);
2673 ImGui::EndTooltip();
2674 }
2675
2676 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
2677 {
2678 ImGui::SetItemDefaultFocus();
2679 }
2680 }
2681 ImGui::EndCombo();
2682 }
2683 ImGui::SameLine();
2684 gui::widgets::HelpMarker("This field controls the type of checksum used for the serial communications. Normally the VN-310E uses an "
2685 "8-bit checksum identical to the type used for normal GPS NMEA packets. This form of checksum however "
2686 "offers only a limited means of error checking. As an alternative a full 16-bit CRC (CRC16-CCITT with "
2687 "polynomial = 0x07) is also offered. The 2-byte CRC value is printed using 4 hexadecimal digits.");
2688
2689 if (_sensorModel == VectorNavModel::VN100_VN110)
2690 {
2691 static constexpr std::array<std::pair<vn::protocol::uart::ChecksumMode, const char*>, 3> communicationProtocolControlSpiChecksums = {
2692 { { vn::protocol::uart::ChecksumMode::CHECKSUMMODE_OFF, "OFF" },
2693 { vn::protocol::uart::ChecksumMode::CHECKSUMMODE_CHECKSUM, "8-Bit Checksum" },
2694 { vn::protocol::uart::ChecksumMode::CHECKSUMMODE_CRC, "16-Bit CRC" } }
2695 };
2696 if (ImGui::BeginCombo(fmt::format("SPI Checksum##{}", size_t(id)).c_str(), vn::protocol::uart::str(_communicationProtocolControlRegister.spiChecksum).c_str()))
2697 {
2698 for (const auto& communicationProtocolControlSpiChecksum : communicationProtocolControlSpiChecksums)
2699 {
2700 const bool isSelected = (_communicationProtocolControlRegister.spiChecksum == communicationProtocolControlSpiChecksum.first);
2701 if (ImGui::Selectable(vn::protocol::uart::str(communicationProtocolControlSpiChecksum.first).c_str(), isSelected))
2702 {
2703 _communicationProtocolControlRegister.spiChecksum = communicationProtocolControlSpiChecksum.first;
2704 LOG_DEBUG("{}: communicationProtocolControlRegister.spiChecksum changed to {}", nameId(), vn::protocol::uart::str(_communicationProtocolControlRegister.spiChecksum));
2705 flow::ApplyChanges();
2706 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2707 {
2708 try
2709 {
2710 _vs.writeCommunicationProtocolControl(_communicationProtocolControlRegister);
2711 }
2712 catch (const std::exception& e)
2713 {
2714 LOG_ERROR("{}: Could not configure the communicationProtocolControlRegister: {}", nameId(), e.what());
2715 doDeinitialize();
2716 }
2717 }
2718 else
2719 {
2720 doDeinitialize();
2721 }
2722 }
2723 if (ImGui::IsItemHovered())
2724 {
2725 ImGui::BeginTooltip();
2726 ImGui::TextUnformatted(communicationProtocolControlSpiChecksum.second);
2727 ImGui::EndTooltip();
2728 }
2729
2730 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
2731 {
2732 ImGui::SetItemDefaultFocus();
2733 }
2734 }
2735 ImGui::EndCombo();
2736 }
2737 ImGui::SameLine();
2738 gui::widgets::HelpMarker("This field controls the type of checksum used for the SPI communications. The checksum is appended to "
2739 "the end of the binary data packet. The 16-bit CRC is identical to the one described above for the "
2740 "SerialChecksum.");
2741 }
2742
2743 static constexpr std::array<std::pair<vn::protocol::uart::ErrorMode, const char*>, 3> communicationProtocolControlErrorModes = {
2744 { { vn::protocol::uart::ErrorMode::ERRORMODE_IGNORE, "Ignore Error" },
2745 { vn::protocol::uart::ErrorMode::ERRORMODE_SEND, "Send Error" },
2746 { vn::protocol::uart::ErrorMode::ERRORMODE_SENDANDOFF, "Send Error and set ADOR register to OFF" } }
2747 };
2748 if (ImGui::BeginCombo(fmt::format("Error Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_communicationProtocolControlRegister.errorMode).c_str()))
2749 {
2750 for (const auto& communicationProtocolControlErrorMode : communicationProtocolControlErrorModes)
2751 {
2752 const bool isSelected = (_communicationProtocolControlRegister.errorMode == communicationProtocolControlErrorMode.first);
2753 if (ImGui::Selectable(vn::protocol::uart::str(communicationProtocolControlErrorMode.first).c_str(), isSelected))
2754 {
2755 _communicationProtocolControlRegister.errorMode = communicationProtocolControlErrorMode.first;
2756 LOG_DEBUG("{}: communicationProtocolControlRegister.errorMode changed to {}", nameId(), vn::protocol::uart::str(_communicationProtocolControlRegister.errorMode));
2757 flow::ApplyChanges();
2758 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2759 {
2760 try
2761 {
2762 _vs.writeCommunicationProtocolControl(_communicationProtocolControlRegister);
2763 }
2764 catch (const std::exception& e)
2765 {
2766 LOG_ERROR("{}: Could not configure the communicationProtocolControlRegister: {}", nameId(), e.what());
2767 doDeinitialize();
2768 }
2769 }
2770 else
2771 {
2772 doDeinitialize();
2773 }
2774 }
2775 if (ImGui::IsItemHovered())
2776 {
2777 ImGui::BeginTooltip();
2778 ImGui::TextUnformatted(communicationProtocolControlErrorMode.second);
2779 ImGui::EndTooltip();
2780 }
2781
2782 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
2783 {
2784 ImGui::SetItemDefaultFocus();
2785 }
2786 }
2787 ImGui::EndCombo();
2788 }
2789 ImGui::SameLine();
2790 gui::widgets::HelpMarker("This field controls the type of action taken by the VectorNav when an error event occurs. If the send error "
2791 "mode is enabled then a message similar to the one shown below will be sent on the serial bus when an error "
2792 "event occurs.\n\n"
2793 "$VNERR,03*72\n\n"
2794 "Regardless of the state of the ErrorMode, the number of error events is always recorded and is made available "
2795 "in the SysErrors field of the Communication Protocol Status Register in the System subsystem.");
2796
2797 if (ImGui::TreeNode(fmt::format("Example Async Messages##{}", size_t(id)).c_str()))
2798 {
2799 ImGui::TextUnformatted("The following table shows example asynchronous messages with the\nAsyncCount and the AsyncStatus values appended to the end.");
2800
2801 if (ImGui::BeginTable("Example Async Messages Table", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
2802 {
2803 ImGui::TableSetupColumn("Example Type");
2804 ImGui::TableSetupColumn("Message");
2805 ImGui::TableHeadersRow();
2806
2807 ImGui::TableNextColumn();
2808 ImGui::TextUnformatted("Async Message with\nAsyncCount Enabled");
2809 ImGui::TableNextColumn();
2810 ImGui::TextUnformatted("$VNYPR,+010.071,+000.278,-002.026,T1162704*2F");
2811
2812 ImGui::TableNextColumn();
2813 ImGui::TextUnformatted("Async Message with\nAsyncStatus Enabled");
2814 ImGui::TableNextColumn();
2815 ImGui::TextUnformatted("$VNYPR,+010.071,+000.278,-002.026,S0000*1F");
2816
2817 ImGui::TableNextColumn();
2818 ImGui::TextUnformatted("Async Message with\nAsyncCount and\nAsyncStatus Enabled");
2819 ImGui::TableNextColumn();
2820 ImGui::TextUnformatted("$VNYPR,+010.071,+000.278,-002.026,T1162704,S0000*50");
2821
2822 ImGui::EndTable();
2823 }
2824
2825 ImGui::TreePop();
2826 }
2827
2828 ImGui::TreePop();
2829 }
2830
2831 for (size_t b = 0; b < _binaryOutputRegister.size(); b++)
2832 {
2833 if (ImGui::TreeNode(fmt::format("Binary Output {}##{}", b + 1, size_t(id)).c_str()))
2834 {
2835 static constexpr std::array<std::pair<vn::protocol::uart::AsyncMode, const char*>, 4> asyncModes = {
2836 { { vn::protocol::uart::AsyncMode::ASYNCMODE_NONE, " User message is not automatically sent out either serial port" },
2837 { vn::protocol::uart::AsyncMode::ASYNCMODE_PORT1, "Message is sent out serial port 1 at a fixed rate" },
2838 { vn::protocol::uart::AsyncMode::ASYNCMODE_PORT2, "Message is sent out serial port 2 at a fixed rate" },
2839 { vn::protocol::uart::AsyncMode::ASYNCMODE_BOTH, "Message is sent out both serial ports at a fixed rate" } }
2840 };
2841 if ((_binaryOutputRegisterMerge == BinaryRegisterMerge::Output1_Output2 && b == 1)
2842 || (_binaryOutputRegisterMerge == BinaryRegisterMerge::Output1_Output3 && b == 2)
2843 || (_binaryOutputRegisterMerge == BinaryRegisterMerge::Output2_Output3 && b == 2))
2844 {
2845 ImGui::BeginDisabled();
2846 }
2847
2848 if (ImGui::BeginCombo(fmt::format("Async Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_binaryOutputRegister.at(b).asyncMode).c_str()))
2849 {
2850 for (const auto& asyncMode : asyncModes)
2851 {
2852 const bool isSelected = (_binaryOutputRegister.at(b).asyncMode == asyncMode.first);
2853 if (ImGui::Selectable(vn::protocol::uart::str(asyncMode.first).c_str(), isSelected))
2854 {
2855 _binaryOutputRegister.at(b).asyncMode = asyncMode.first;
2856 LOG_DEBUG("{}: binaryOutputRegister.at({}).asyncMode changed to {}", nameId(), b, vn::protocol::uart::str(_binaryOutputRegister.at(b).asyncMode));
2857
2858 std::vector<size_t> binaryOutputRegistersToUpdate;
2859 binaryOutputRegistersToUpdate.push_back(b);
2860 if (_binaryOutputRegisterMerge == BinaryRegisterMerge::Output1_Output2 && b == 0)
2861 {
2862 _binaryOutputRegister.at(1).asyncMode = asyncMode.first;
2863 LOG_DEBUG("{}: binaryOutputRegister.at({}).asyncMode changed to {}", nameId(), 1, vn::protocol::uart::str(_binaryOutputRegister.at(1).asyncMode));
2864 binaryOutputRegistersToUpdate.push_back(1);
2865 }
2866 else if ((_binaryOutputRegisterMerge == BinaryRegisterMerge::Output1_Output3 && b == 0)
2867 || (_binaryOutputRegisterMerge == BinaryRegisterMerge::Output2_Output3 && b == 1))
2868 {
2869 _binaryOutputRegister.at(2).asyncMode = asyncMode.first;
2870 LOG_DEBUG("{}: binaryOutputRegister.at({}).asyncMode changed to {}", nameId(), 2, vn::protocol::uart::str(_binaryOutputRegister.at(2).asyncMode));
2871 binaryOutputRegistersToUpdate.push_back(2);
2872 }
2873 flow::ApplyChanges();
2874
2875 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2876 {
2877 for (const auto& binUpdate : binaryOutputRegistersToUpdate)
2878 {
2879 try
2880 {
2881 switch (binUpdate)
2882 {
2883 case 0:
2884 _vs.writeBinaryOutput1(_binaryOutputRegister.at(binUpdate));
2885 break;
2886 case 1:
2887 _vs.writeBinaryOutput2(_binaryOutputRegister.at(binUpdate));
2888 break;
2889 case 2:
2890 _vs.writeBinaryOutput3(_binaryOutputRegister.at(binUpdate));
2891 break;
2892 default:
2893 break;
2894 }
2895 }
2896 catch (const std::exception& e)
2897 {
2898 LOG_ERROR("{}: Could not configure the binaryOutputRegister {}: {}", nameId(), binUpdate + 1, e.what());
2899 doDeinitialize();
2900 }
2901 }
2902 }
2903 else
2904 {
2905 doDeinitialize();
2906 }
2907 }
2908 if (ImGui::IsItemHovered())
2909 {
2910 ImGui::BeginTooltip();
2911 ImGui::TextUnformatted(asyncMode.second);
2912 ImGui::EndTooltip();
2913 }
2914
2915 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
2916 {
2917 ImGui::SetItemDefaultFocus();
2918 }
2919 }
2920 ImGui::EndCombo();
2921 }
2922 ImGui::SameLine();
2923 gui::widgets::HelpMarker("Selects whether the output message should be sent "
2924 "out on the serial port(s) at a fixed rate.");
2925
2926 const char* frequencyText = _binaryOutputSelectedFrequency.at(b) < _dividerFrequency.first.size()
2927 ? _dividerFrequency.second.at(_binaryOutputSelectedFrequency.at(b)).c_str()
2928 : "Unknown";
2929 if (auto freq = static_cast<int>(_binaryOutputSelectedFrequency.at(b));
2930 ImGui::SliderInt(fmt::format("Frequency##{} {}", size_t(id), b).c_str(),
2931 &freq,
2932 0, static_cast<int>(_dividerFrequency.second.size()) - 1,
2933 frequencyText))
2934 {
2935 _binaryOutputSelectedFrequency.at(b) = static_cast<size_t>(freq);
2936 _binaryOutputRegister.at(b).rateDivisor = _dividerFrequency.first.at(_binaryOutputSelectedFrequency.at(b));
2937 LOG_DEBUG("{}: Frequency of Binary Group {} changed to {}", nameId(), b + 1, _dividerFrequency.second.at(_binaryOutputSelectedFrequency.at(b)));
2938 if (_coupleImuRateOutput)
2939 {
2940 if (_binaryOutputRegister.at(b).imuField & (vn::protocol::uart::IMUGROUP_ACCEL | vn::protocol::uart::IMUGROUP_UNCOMPACCEL))
2941 {
2942 _imuFilteringConfigurationRegister.accelWindowSize = _binaryOutputRegister.at(b).rateDivisor;
2943 }
2944 if (_binaryOutputRegister.at(b).imuField & (vn::protocol::uart::IMUGROUP_ANGULARRATE | vn::protocol::uart::IMUGROUP_UNCOMPGYRO))
2945 {
2946 _imuFilteringConfigurationRegister.gyroWindowSize = _binaryOutputRegister.at(b).rateDivisor;
2947 }
2948 if (_binaryOutputRegister.at(b).imuField & (vn::protocol::uart::IMUGROUP_MAG | vn::protocol::uart::IMUGROUP_UNCOMPMAG))
2949 {
2950 _imuFilteringConfigurationRegister.magWindowSize = _binaryOutputRegister.at(b).rateDivisor;
2951 }
2952 if (_binaryOutputRegister.at(b).imuField & (vn::protocol::uart::IMUGROUP_MAG | vn::protocol::uart::IMUGROUP_UNCOMPMAG))
2953 {
2954 _imuFilteringConfigurationRegister.magWindowSize = _binaryOutputRegister.at(b).rateDivisor;
2955 }
2956 if (_binaryOutputRegister.at(b).imuField & vn::protocol::uart::IMUGROUP_TEMP)
2957 {
2958 _imuFilteringConfigurationRegister.tempWindowSize = _binaryOutputRegister.at(b).rateDivisor;
2959 }
2960 if (_binaryOutputRegister.at(b).imuField & vn::protocol::uart::IMUGROUP_PRES)
2961 {
2962 _imuFilteringConfigurationRegister.presWindowSize = _binaryOutputRegister.at(b).rateDivisor;
2963 }
2964 LOG_DATA("{}: Frequency of ImuFilter matches output frequency (coupled 'rateDivisor' and 'windowSize' of moving average filter.)", nameId());
2965
2966 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
2967 {
2968 try
2969 {
2970 _vs.writeImuFilteringConfiguration(_imuFilteringConfigurationRegister);
2971 }
2972 catch (const std::exception& e)
2973 {
2974 LOG_ERROR("{}: Could not configure the imuFilteringConfigurationRegister: {}", nameId(), e.what());
2975 doDeinitialize();
2976 }
2977 }
2978 else
2979 {
2980 doDeinitialize();
2981 }
2982 }
2983
2984 std::vector<size_t> binaryOutputRegistersToUpdate;
2985 binaryOutputRegistersToUpdate.push_back(b);
2986 if (_binaryOutputRegisterMerge == BinaryRegisterMerge::Output1_Output2 && b == 0)
2987 {
2988 _binaryOutputSelectedFrequency.at(1) = _binaryOutputSelectedFrequency.at(b);
2989 _binaryOutputRegister.at(1).rateDivisor = _dividerFrequency.first.at(_binaryOutputSelectedFrequency.at(1));
2990 LOG_DEBUG("{}: Frequency of Binary Group {} changed to {}", nameId(), 1 + 1, _dividerFrequency.second.at(_binaryOutputSelectedFrequency.at(1)));
2991 binaryOutputRegistersToUpdate.push_back(1);
2992 }
2993 else if ((_binaryOutputRegisterMerge == BinaryRegisterMerge::Output1_Output3 && b == 0)
2994 || (_binaryOutputRegisterMerge == BinaryRegisterMerge::Output2_Output3 && b == 1))
2995 {
2996 _binaryOutputSelectedFrequency.at(2) = _binaryOutputSelectedFrequency.at(b);
2997 _binaryOutputRegister.at(2).rateDivisor = _dividerFrequency.first.at(_binaryOutputSelectedFrequency.at(2));
2998 LOG_DEBUG("{}: Frequency of Binary Group {} changed to {}", nameId(), 2 + 1, _dividerFrequency.second.at(_binaryOutputSelectedFrequency.at(2)));
2999 binaryOutputRegistersToUpdate.push_back(2);
3000 }
3001
3002 flow::ApplyChanges();
3003 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3004 {
3005 for (const auto& binUpdate : binaryOutputRegistersToUpdate)
3006 {
3007 try
3008 {
3009 switch (binUpdate)
3010 {
3011 case 0:
3012 _vs.writeBinaryOutput1(_binaryOutputRegister.at(binUpdate));
3013 break;
3014 case 1:
3015 _vs.writeBinaryOutput2(_binaryOutputRegister.at(binUpdate));
3016 break;
3017 case 2:
3018 _vs.writeBinaryOutput3(_binaryOutputRegister.at(binUpdate));
3019 break;
3020 default:
3021 break;
3022 }
3023 }
3024 catch (const std::exception& e)
3025 {
3026 LOG_ERROR("{}: Could not configure the binaryOutputRegister {}: {}", nameId(), binUpdate + 1, e.what());
3027 doDeinitialize();
3028 }
3029 }
3030 }
3031 else
3032 {
3033 doDeinitialize();
3034 }
3035 }
3036 if ((_binaryOutputRegisterMerge == BinaryRegisterMerge::Output1_Output2 && b == 1)
3037 || (_binaryOutputRegisterMerge == BinaryRegisterMerge::Output1_Output3 && b == 2)
3038 || (_binaryOutputRegisterMerge == BinaryRegisterMerge::Output2_Output3 && b == 2))
3039 {
3040 ImGui::EndDisabled();
3041 }
3042
3043 if (ImGui::BeginTable(fmt::format("##VectorNavSensorConfig ({})", size_t(id)).c_str(), 6,
3044 ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg))
3045 {
3046 // ImGui::TableSetupColumn("Common", ImGuiTableColumnFlags_WidthFixed);
3047 ImGui::TableSetupColumn("Time", ImGuiTableColumnFlags_WidthFixed);
3048 ImGui::TableSetupColumn("IMU", ImGuiTableColumnFlags_WidthFixed);
3049 ImGui::TableSetupColumn("GNSS1", ImGuiTableColumnFlags_WidthFixed);
3050 ImGui::TableSetupColumn("Attitude", ImGuiTableColumnFlags_WidthFixed);
3051 ImGui::TableSetupColumn("INS", ImGuiTableColumnFlags_WidthFixed);
3052 ImGui::TableSetupColumn("GNSS2", ImGuiTableColumnFlags_WidthFixed);
3053 ImGui::TableHeadersRow();
3054
3055 auto CheckboxFlags = [&, this](int index, const char* label, int* flags, int flags_value, bool enabled, void (*toggleFields)(VectorNavSensor*, vn::sensors::BinaryOutputRegister&, uint32_t&)) {
3056 ImGui::TableSetColumnIndex(index);
3057
3058 if (!enabled)
3059 {
3060 ImGui::BeginDisabled();
3061 }
3062
3063 if (ImGui::CheckboxFlags(label, flags, flags_value))
3064 {
3065 LOG_DEBUG("{}: Field '{}' of Binary Group {} is now {}", nameId(), std::string(label).substr(0, std::string(label).find('#')), b + 1, (*flags & flags_value) ? "checked" : "unchecked");
3066 if (toggleFields)
3067 {
3068 toggleFields(this, _binaryOutputRegister.at(b), *reinterpret_cast<uint32_t*>(flags));
3069 }
3070 flow::ApplyChanges();
3071 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3072 {
3073 try
3074 {
3075 if (b == 0)
3076 {
3077 _vs.writeBinaryOutput1(_binaryOutputRegister.at(b));
3078 }
3079 else if (b == 1)
3080 {
3081 _vs.writeBinaryOutput2(_binaryOutputRegister.at(b));
3082 }
3083 else if (b == 2)
3084 {
3085 _vs.writeBinaryOutput3(_binaryOutputRegister.at(b));
3086 }
3087
3088 for (auto& link : outputPins.at(b + 2).links)
3089 {
3090 if (auto* connectedPin = link.getConnectedPin())
3091 {
3092 outputPins.at(b + 2).recreateLink(*connectedPin);
3093 }
3094 }
3095 }
3096 catch (const std::exception& e)
3097 {
3098 LOG_ERROR("{}: Could not configure the binaryOutputRegister {}: {}", nameId(), b + 1, e.what());
3099 doDeinitialize();
3100 }
3101 }
3102 else
3103 {
3104 doDeinitialize();
3105 }
3106 }
3107
3108 if (!enabled)
3109 {
3110 ImGui::EndDisabled();
3111 }
3112 };
3113
3114 for (size_t i = 0; i < 16; i++)
3115 {
3116 if (i < std::max({ /* _binaryGroupCommon.size(), */ _binaryGroupTime.size(), _binaryGroupIMU.size(),
3117 _binaryGroupGNSS.size(), _binaryGroupAttitude.size(), _binaryGroupINS.size() }))
3118 {
3119 ImGui::TableNextRow();
3120 }
3121 // if (i < _binaryGroupCommon.size())
3122 // {
3123 // const auto& binaryGroupItem = _binaryGroupCommon.at(i);
3124 // CheckboxFlags(0, fmt::format("{}##Common {} {}", binaryGroupItem.name, size_t(id), b).c_str(),
3125 // reinterpret_cast<int*>(&_binaryOutputRegister.at(b).commonField),
3126 // binaryGroupItem.flagsValue,
3127 // binaryGroupItem.isEnabled(_sensorModel, _binaryOutputRegister.at(b), static_cast<uint32_t>(_binaryOutputRegister.at(b).commonField)),
3128 // binaryGroupItem.toggleFields);
3129 // if (ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled) && binaryGroupItem.tooltip != nullptr)
3130 // {
3131 // ImGui::BeginTooltip();
3132 // binaryGroupItem.tooltip();
3133 // ImGui::EndTooltip();
3134 // }
3135 // }
3136 if (i < _binaryGroupTime.size())
3137 {
3138 const auto& binaryGroupItem = _binaryGroupTime.at(i);
3139 CheckboxFlags(0, fmt::format("{}##Time {} {}", binaryGroupItem.name, size_t(id), b).c_str(),
3140 reinterpret_cast<int*>(&_binaryOutputRegister.at(b).timeField),
3141 binaryGroupItem.flagsValue,
3142 binaryGroupItem.isEnabled(_sensorModel, _binaryOutputRegister.at(b), static_cast<uint32_t>(_binaryOutputRegister.at(b).timeField)),
3143 binaryGroupItem.toggleFields);
3144 if (ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled) && binaryGroupItem.tooltip != nullptr)
3145 {
3146 ImGui::BeginTooltip();
3147 binaryGroupItem.tooltip();
3148 ImGui::EndTooltip();
3149 }
3150 }
3151 if (i < _binaryGroupIMU.size())
3152 {
3153 const auto& binaryGroupItem = _binaryGroupIMU.at(i);
3154 CheckboxFlags(1, fmt::format("{}##IMU {} {}", binaryGroupItem.name, size_t(id), b).c_str(),
3155 reinterpret_cast<int*>(&_binaryOutputRegister.at(b).imuField),
3156 binaryGroupItem.flagsValue,
3157 binaryGroupItem.isEnabled(_sensorModel, _binaryOutputRegister.at(b), static_cast<uint32_t>(_binaryOutputRegister.at(b).imuField)),
3158 binaryGroupItem.toggleFields);
3159 if (ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled) && binaryGroupItem.tooltip != nullptr)
3160 {
3161 ImGui::BeginTooltip();
3162 binaryGroupItem.tooltip();
3163 ImGui::EndTooltip();
3164 }
3165 }
3166 if (i < _binaryGroupGNSS.size())
3167 {
3168 const auto& binaryGroupItem = _binaryGroupGNSS.at(i);
3169 CheckboxFlags(2, fmt::format("{}##GNSS1 {} {}", binaryGroupItem.name, size_t(id), b).c_str(),
3170 reinterpret_cast<int*>(&_binaryOutputRegister.at(b).gpsField),
3171 binaryGroupItem.flagsValue,
3172 binaryGroupItem.isEnabled(_sensorModel, _binaryOutputRegister.at(b), static_cast<uint32_t>(_binaryOutputRegister.at(b).gpsField)),
3173 binaryGroupItem.toggleFields);
3174 if (ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled) && binaryGroupItem.tooltip != nullptr)
3175 {
3176 ImGui::BeginTooltip();
3177 binaryGroupItem.tooltip();
3178 ImGui::EndTooltip();
3179 }
3180 }
3181 if (i < _binaryGroupAttitude.size())
3182 {
3183 const auto& binaryGroupItem = _binaryGroupAttitude.at(i);
3184 CheckboxFlags(3, fmt::format("{}##Attitude {} {}", binaryGroupItem.name, size_t(id), b).c_str(),
3185 reinterpret_cast<int*>(&_binaryOutputRegister.at(b).attitudeField),
3186 binaryGroupItem.flagsValue,
3187 binaryGroupItem.isEnabled(_sensorModel, _binaryOutputRegister.at(b), static_cast<uint32_t>(_binaryOutputRegister.at(b).attitudeField)),
3188 binaryGroupItem.toggleFields);
3189 if (ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled) && binaryGroupItem.tooltip != nullptr)
3190 {
3191 ImGui::BeginTooltip();
3192 binaryGroupItem.tooltip();
3193 ImGui::EndTooltip();
3194 }
3195 }
3196 if (i < _binaryGroupINS.size())
3197 {
3198 const auto& binaryGroupItem = _binaryGroupINS.at(i);
3199 CheckboxFlags(4, fmt::format("{}##INS {} {}", binaryGroupItem.name, size_t(id), b).c_str(),
3200 reinterpret_cast<int*>(&_binaryOutputRegister.at(b).insField),
3201 binaryGroupItem.flagsValue,
3202 binaryGroupItem.isEnabled(_sensorModel, _binaryOutputRegister.at(b), static_cast<uint32_t>(_binaryOutputRegister.at(b).insField)),
3203 binaryGroupItem.toggleFields);
3204 if (ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled) && binaryGroupItem.tooltip != nullptr)
3205 {
3206 ImGui::BeginTooltip();
3207 binaryGroupItem.tooltip();
3208 ImGui::EndTooltip();
3209 }
3210 }
3211 if (i < _binaryGroupGNSS.size())
3212 {
3213 const auto& binaryGroupItem = _binaryGroupGNSS.at(i);
3214 CheckboxFlags(5, fmt::format("{}##GNSS2 {} {}", binaryGroupItem.name, size_t(id), b).c_str(),
3215 reinterpret_cast<int*>(&_binaryOutputRegister.at(b).gps2Field),
3216 binaryGroupItem.flagsValue,
3217 binaryGroupItem.isEnabled(_sensorModel, _binaryOutputRegister.at(b), static_cast<uint32_t>(_binaryOutputRegister.at(b).gps2Field)),
3218 binaryGroupItem.toggleFields);
3219 if (ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled) && binaryGroupItem.tooltip != nullptr)
3220 {
3221 ImGui::BeginTooltip();
3222 binaryGroupItem.tooltip();
3223 ImGui::EndTooltip();
3224 }
3225 }
3226 }
3227
3228 ImGui::EndTable();
3229 }
3230
3231 ImGui::TreePop();
3232 }
3233 }
3234
3235 if (auto binaryOutputRegisterMerge = static_cast<int>(_binaryOutputRegisterMerge);
3236 ImGui::Combo(fmt::format("Merge Binary outputs").c_str(), &binaryOutputRegisterMerge, "None\0"
3237 "1 <-> 2\0"
3238 "1 <-> 3\0"
3239 "2 <-> 3\0\0"))
3240 {
3241 _binaryOutputRegisterMerge = static_cast<decltype(_binaryOutputRegisterMerge)>(binaryOutputRegisterMerge);
3242 flow::ApplyChanges();
3243 switch (_binaryOutputRegisterMerge)
3244 {
3245 case BinaryRegisterMerge::Output1_Output2:
3246 _binaryOutputRegister.at(1).rateDivisor = _binaryOutputRegister.at(0).rateDivisor;
3247 _binaryOutputSelectedFrequency.at(1) = _binaryOutputSelectedFrequency.at(0);
3248 _binaryOutputRegister.at(1).asyncMode = _binaryOutputRegister.at(0).asyncMode;
3249 outputPins.at(2).deleteLinks();
3250 outputPins.at(1).type = Pin::Type::Flow;
3251 outputPins.at(2).type = Pin::Type::None;
3252 outputPins.at(3).type = Pin::Type::Flow;
3253 break;
3254 case BinaryRegisterMerge::Output1_Output3:
3255 _binaryOutputRegister.at(2).rateDivisor = _binaryOutputRegister.at(0).rateDivisor;
3256 _binaryOutputSelectedFrequency.at(2) = _binaryOutputSelectedFrequency.at(0);
3257 _binaryOutputRegister.at(2).asyncMode = _binaryOutputRegister.at(0).asyncMode;
3258 outputPins.at(3).deleteLinks();
3259 outputPins.at(1).type = Pin::Type::Flow;
3260 outputPins.at(2).type = Pin::Type::Flow;
3261 outputPins.at(3).type = Pin::Type::None;
3262 break;
3263 case BinaryRegisterMerge::Output2_Output3:
3264 _binaryOutputRegister.at(2).rateDivisor = _binaryOutputRegister.at(1).rateDivisor;
3265 _binaryOutputSelectedFrequency.at(2) = _binaryOutputSelectedFrequency.at(1);
3266 _binaryOutputRegister.at(2).asyncMode = _binaryOutputRegister.at(1).asyncMode;
3267 outputPins.at(3).deleteLinks();
3268 outputPins.at(1).type = Pin::Type::Flow;
3269 outputPins.at(2).type = Pin::Type::Flow;
3270 outputPins.at(3).type = Pin::Type::None;
3271 break;
3272 default:
3273 break;
3274 }
3275 }
3276 ImGui::SameLine();
3277 NAV::gui::widgets::HelpMarker("VectorNav sensors have a buffer overflow if packages get too big (SatInfo/RawMeas selected and a lot of satellites). "
3278 "A workaround is, to split the data into different Binary Outputs. "
3279 "This option merges the outputs into a single observation.");
3280
3281 // TODO: Add Gui Config for NMEA output - User manual VN-310 - 8.2.14 (p 103)
3282 }
3283
3284 // ###########################################################################################################
3285 // IMU SUBSYSTEM
3286 // ###########################################################################################################
3287
3288 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
3289 if (ImGui::CollapsingHeader(fmt::format("IMU Subsystem##{}", size_t(id)).c_str()))
3290 {
3291 if (ImGui::TreeNode(fmt::format("Reference Frame Rotation##{}", size_t(id)).c_str()))
3292 {
3293 ImGui::TextUnformatted("Allows the measurements of the VN-310E to be rotated into a different reference frame.");
3294 ImGui::SameLine();
3295 gui::widgets::HelpMarker("This register contains a transformation matrix that allows for the transformation of measured acceleration, "
3296 "magnetic, and angular rates from the body frame of the VN-310E to any other arbitrary frame of reference. "
3297 "The use of this register allows for the sensor to be placed in any arbitrary orientation with respect to the "
3298 "user's desired body coordinate frame. This register can also be used to correct for any orientation errors due "
3299 "to mounting the VN-310E on the user's vehicle or platform.\n\n"
3300 "(X Y Z)_U = C * (X Y Z)_B\n\n"
3301 "The variables (X Y Z)_B are a measured parameter such as acceleration in the body reference frame with "
3302 "respect to the VectorNav. The variables (X Y Z)_U are a measured parameter such as acceleration in the user's "
3303 "frame of reference. The reference frame rotation register Thus needs to be loaded with the transformation "
3304 "matrix that will transform measurements from the body reference frame of the VectorNav to the desired user "
3305 "frame of reference.");
3306 if (_sensorModel == VectorNavModel::VN310)
3307 {
3308 ImGui::SameLine();
3309 gui::widgets::HelpMarker("The reference frame rotation is performed on all vector measurements prior to entering the INS "
3310 "filter. As such, changing this register while the attitude filter is running will lead to unexpected "
3311 "behavior in the INS output. To prevent this, the register is cached on startup and changes will "
3312 "not take effect during runtime. After setting the reference frame rotation register to its new value, "
3313 "send a write settings command and then reset the VN-310E. This will allow the INS filter to "
3314 "startup with the newly set reference frame rotation.",
3315 "(!)");
3316 }
3317 ImGui::SameLine();
3318 gui::widgets::HelpMarker("The matrix C in the Reference Frame Rotation Register must be an orthonormal, right-handed"
3319 " matrix. The sensor will output an error if the tolerance is not within 1e-5. The sensor will also"
3320 " report an error if any of the parameters are greater than 1 or less than -1.",
3321 "(!)");
3322
3323 ImGui::TextUnformatted("Rotation Angles [deg]");
3324 ImGui::SameLine();
3325 gui::widgets::HelpMarker("The rotation happens in ZYX Order");
3326 ImGui::Indent();
3327
3328 Eigen::Matrix3d dcm;
3329 dcm << _referenceFrameRotationMatrix.e00, _referenceFrameRotationMatrix.e01, _referenceFrameRotationMatrix.e02,
3330 _referenceFrameRotationMatrix.e10, _referenceFrameRotationMatrix.e11, _referenceFrameRotationMatrix.e12,
3331 _referenceFrameRotationMatrix.e20, _referenceFrameRotationMatrix.e21, _referenceFrameRotationMatrix.e22;
3332 auto b_Quat_p = Eigen::Quaterniond(dcm);
3333
3334 Eigen::Vector3d eulerRot = rad2deg(trafo::quat2eulerZYX(b_Quat_p.conjugate()));
3335 std::array<float, 3> imuRot = { static_cast<float>(eulerRot.x()), static_cast<float>(eulerRot.y()), static_cast<float>(eulerRot.z()) };
3336 if (ImGui::InputFloat3(fmt::format("##rotationAngles{}", size_t(id)).c_str(), imuRot.data()))
3337 {
3338 // (-180:180] x (-90:90] x (-180:180]
3339 imuRot.at(0) = std::max(imuRot.at(0), -179.9999F);
3340 imuRot.at(0) = std::min(imuRot.at(0), 180.0F);
3341 imuRot.at(1) = std::max(imuRot.at(1), -89.9999F);
3342 imuRot.at(1) = std::min(imuRot.at(1), 90.0F);
3343 imuRot.at(2) = std::max(imuRot.at(2), -179.9999F);
3344 imuRot.at(2) = std::min(imuRot.at(2), 180.0F);
3345
3346 Eigen::Matrix3f dcmf = trafo::b_Quat_p(deg2rad(imuRot.at(0)), deg2rad(imuRot.at(1)), deg2rad(imuRot.at(2))).toRotationMatrix().cast<float>();
3347 _referenceFrameRotationMatrix = vn::math::mat3f(dcmf(0, 0), dcmf(0, 1), dcmf(0, 2),
3348 dcmf(1, 0), dcmf(1, 1), dcmf(1, 2),
3349 dcmf(2, 0), dcmf(2, 1), dcmf(2, 2));
3350 LOG_DEBUG("{}: referenceFrameRotationMatrix changed to {}", nameId(), _referenceFrameRotationMatrix);
3351 flow::ApplyChanges();
3352 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3353 {
3354 try
3355 {
3356 _vs.writeReferenceFrameRotation(_referenceFrameRotationMatrix);
3357 }
3358 catch (const std::exception& e)
3359 {
3360 LOG_ERROR("{}: Could not configure the referenceFrameRotationMatrix: {}", nameId(), e.what());
3361 doDeinitialize();
3362 }
3363 }
3364 else
3365 {
3366 doDeinitialize();
3367 }
3368 }
3369
3370 ImGui::Unindent();
3371 ImGui::TextUnformatted("Rotation Matrix C");
3372 ImGui::Indent();
3373
3374 std::array<float, 3> row = { _referenceFrameRotationMatrix.e00, _referenceFrameRotationMatrix.e01, _referenceFrameRotationMatrix.e02 };
3375 if (ImGui::InputFloat3(fmt::format("##referenceFrameRotationMatrix row 0 {}", size_t(id)).c_str(), row.data(), "%.2f"))
3376 {
3377 _referenceFrameRotationMatrix.e00 = row.at(0);
3378 _referenceFrameRotationMatrix.e01 = row.at(1);
3379 _referenceFrameRotationMatrix.e02 = row.at(2);
3380 LOG_DEBUG("{}: referenceFrameRotationMatrix changed to {}", nameId(), _referenceFrameRotationMatrix);
3381 flow::ApplyChanges();
3382 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3383 {
3384 try
3385 {
3386 _vs.writeReferenceFrameRotation(_referenceFrameRotationMatrix);
3387 }
3388 catch (const std::exception& e)
3389 {
3390 LOG_ERROR("{}: Could not configure the referenceFrameRotationMatrix: {}", nameId(), e.what());
3391 doDeinitialize();
3392 }
3393 }
3394 else
3395 {
3396 doDeinitialize();
3397 }
3398 }
3399 row = { _referenceFrameRotationMatrix.e10, _referenceFrameRotationMatrix.e11, _referenceFrameRotationMatrix.e12 };
3400 if (ImGui::InputFloat3(fmt::format("##referenceFrameRotationMatrix row 1 {}", size_t(id)).c_str(), row.data(), "%.2f"))
3401 {
3402 _referenceFrameRotationMatrix.e10 = row.at(0);
3403 _referenceFrameRotationMatrix.e11 = row.at(1);
3404 _referenceFrameRotationMatrix.e12 = row.at(2);
3405 LOG_DEBUG("{}: referenceFrameRotationMatrix changed to {}", nameId(), _referenceFrameRotationMatrix);
3406 flow::ApplyChanges();
3407 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3408 {
3409 try
3410 {
3411 _vs.writeReferenceFrameRotation(_referenceFrameRotationMatrix);
3412 }
3413 catch (const std::exception& e)
3414 {
3415 LOG_ERROR("{}: Could not configure the referenceFrameRotationMatrix: {}", nameId(), e.what());
3416 doDeinitialize();
3417 }
3418 }
3419 else
3420 {
3421 doDeinitialize();
3422 }
3423 }
3424 row = { _referenceFrameRotationMatrix.e20, _referenceFrameRotationMatrix.e21, _referenceFrameRotationMatrix.e22 };
3425 if (ImGui::InputFloat3(fmt::format("##referenceFrameRotationMatrix row 2 {}", size_t(id)).c_str(), row.data(), "%.2f"))
3426 {
3427 _referenceFrameRotationMatrix.e20 = row.at(0);
3428 _referenceFrameRotationMatrix.e21 = row.at(1);
3429 _referenceFrameRotationMatrix.e22 = row.at(2);
3430 LOG_DEBUG("{}: referenceFrameRotationMatrix changed to {}", nameId(), _referenceFrameRotationMatrix);
3431 flow::ApplyChanges();
3432 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3433 {
3434 try
3435 {
3436 _vs.writeReferenceFrameRotation(_referenceFrameRotationMatrix);
3437 }
3438 catch (const std::exception& e)
3439 {
3440 LOG_ERROR("{}: Could not configure the referenceFrameRotationMatrix: {}", nameId(), e.what());
3441 doDeinitialize();
3442 }
3443 }
3444 else
3445 {
3446 doDeinitialize();
3447 }
3448 }
3449
3450 ImGui::Unindent();
3451 ImGui::TreePop();
3452 }
3453
3454 if (ImGui::TreeNode(fmt::format("IMU Filtering Configuration##{}", size_t(id)).c_str()))
3455 {
3456 ImGui::TextUnformatted("This register allows the user to configure the FIR filtering which is applied to the IMU measurements.\n"
3457 "The filter is a uniformly-weighted moving window (boxcar) filter, also known as a 'moving-average' filter.\n"
3458 "Its window size can be adjusted with respect to the internal IMU frequency (800 Hz).");
3459
3460 if (ImGui::Checkbox(fmt::format("Couple the Imu-Filter's rate to the output rate##{}", size_t(id)).c_str(), &_coupleImuRateOutput))
3461 {
3462 LOG_DEBUG("{}: coupleImuRateOutput changed to {}", nameId(), _coupleImuRateOutput);
3463 flow::ApplyChanges();
3464 }
3465 ImGui::SameLine();
3466 gui::widgets::HelpMarker("If the window-size of the IMU's moving-average filter is smaller than the output rate "
3467 "divisor, some samples are lost. This results in a low output rate that still contains "
3468 "noise due to high frequencies. Therefore, it is recommended to couple the ImuFilter's "
3469 "rate to the output rate.");
3470
3471 int magWindowSize = _imuFilteringConfigurationRegister.magWindowSize;
3472 if (_coupleImuRateOutput)
3473 {
3474 ImGui::BeginDisabled();
3475 }
3476 if (ImGui::InputInt(fmt::format("Mag Window Size##{}", size_t(id)).c_str(), &magWindowSize))
3477 {
3478 if (magWindowSize < 0)
3479 {
3480 magWindowSize = 0;
3481 }
3482 else if (magWindowSize > std::numeric_limits<uint16_t>::max())
3483 {
3484 magWindowSize = std::numeric_limits<uint16_t>::max();
3485 }
3486 _imuFilteringConfigurationRegister.magWindowSize = static_cast<uint16_t>(magWindowSize);
3487 LOG_DEBUG("{}: imuFilteringConfigurationRegister.magWindowSize changed to {}", nameId(), _imuFilteringConfigurationRegister.magWindowSize);
3488 flow::ApplyChanges();
3489 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3490 {
3491 try
3492 {
3493 _vs.writeImuFilteringConfiguration(_imuFilteringConfigurationRegister);
3494 }
3495 catch (const std::exception& e)
3496 {
3497 LOG_ERROR("{}: Could not configure the imuFilteringConfigurationRegister: {}", nameId(), e.what());
3498 doDeinitialize();
3499 }
3500 }
3501 else
3502 {
3503 doDeinitialize();
3504 }
3505 }
3506 ImGui::SameLine();
3507 gui::widgets::HelpMarker("The WindowSize parameters for each sensor define the number of samples at the IMU rate (default 800Hz) "
3508 "which will be averaged for each output measurement.");
3509
3510 int accelWindowSize = _imuFilteringConfigurationRegister.accelWindowSize;
3511 if (ImGui::InputInt(fmt::format("Accel Window Size##{}", size_t(id)).c_str(), &accelWindowSize))
3512 {
3513 if (accelWindowSize < 0)
3514 {
3515 accelWindowSize = 0;
3516 }
3517 else if (accelWindowSize > std::numeric_limits<uint16_t>::max())
3518 {
3519 accelWindowSize = std::numeric_limits<uint16_t>::max();
3520 }
3521 _imuFilteringConfigurationRegister.accelWindowSize = static_cast<uint16_t>(accelWindowSize);
3522 LOG_DEBUG("{}: imuFilteringConfigurationRegister.accelWindowSize changed to {}", nameId(), _imuFilteringConfigurationRegister.accelWindowSize);
3523 flow::ApplyChanges();
3524 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3525 {
3526 try
3527 {
3528 _vs.writeImuFilteringConfiguration(_imuFilteringConfigurationRegister);
3529 }
3530 catch (const std::exception& e)
3531 {
3532 LOG_ERROR("{}: Could not configure the imuFilteringConfigurationRegister: {}", nameId(), e.what());
3533 doDeinitialize();
3534 }
3535 }
3536 else
3537 {
3538 doDeinitialize();
3539 }
3540 }
3541 ImGui::SameLine();
3542 gui::widgets::HelpMarker("The WindowSize parameters for each sensor define the number of samples at the IMU rate (default 800Hz) "
3543 "which will be averaged for each output measurement.");
3544
3545 int gyroWindowSize = _imuFilteringConfigurationRegister.gyroWindowSize;
3546 if (ImGui::InputInt(fmt::format("Gyro Window Size##{}", size_t(id)).c_str(), &gyroWindowSize))
3547 {
3548 if (gyroWindowSize < 0)
3549 {
3550 gyroWindowSize = 0;
3551 }
3552 else if (gyroWindowSize > std::numeric_limits<uint16_t>::max())
3553 {
3554 gyroWindowSize = std::numeric_limits<uint16_t>::max();
3555 }
3556 _imuFilteringConfigurationRegister.gyroWindowSize = static_cast<uint16_t>(gyroWindowSize);
3557 LOG_DEBUG("{}: imuFilteringConfigurationRegister.gyroWindowSize changed to {}", nameId(), _imuFilteringConfigurationRegister.gyroWindowSize);
3558 flow::ApplyChanges();
3559 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3560 {
3561 try
3562 {
3563 _vs.writeImuFilteringConfiguration(_imuFilteringConfigurationRegister);
3564 }
3565 catch (const std::exception& e)
3566 {
3567 LOG_ERROR("{}: Could not configure the imuFilteringConfigurationRegister: {}", nameId(), e.what());
3568 doDeinitialize();
3569 }
3570 }
3571 else
3572 {
3573 doDeinitialize();
3574 }
3575 }
3576 ImGui::SameLine();
3577 gui::widgets::HelpMarker("The WindowSize parameters for each sensor define the number of samples at the IMU rate (default 800Hz) "
3578 "which will be averaged for each output measurement.");
3579
3580 int tempWindowSize = _imuFilteringConfigurationRegister.tempWindowSize;
3581 if (ImGui::InputInt(fmt::format("Temp Window Size##{}", size_t(id)).c_str(), &tempWindowSize))
3582 {
3583 if (tempWindowSize < 0)
3584 {
3585 tempWindowSize = 0;
3586 }
3587 else if (tempWindowSize > std::numeric_limits<uint16_t>::max())
3588 {
3589 tempWindowSize = std::numeric_limits<uint16_t>::max();
3590 }
3591 _imuFilteringConfigurationRegister.tempWindowSize = static_cast<uint16_t>(tempWindowSize);
3592 LOG_DEBUG("{}: imuFilteringConfigurationRegister.tempWindowSize changed to {}", nameId(), _imuFilteringConfigurationRegister.tempWindowSize);
3593 flow::ApplyChanges();
3594 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3595 {
3596 try
3597 {
3598 _vs.writeImuFilteringConfiguration(_imuFilteringConfigurationRegister);
3599 }
3600 catch (const std::exception& e)
3601 {
3602 LOG_ERROR("{}: Could not configure the imuFilteringConfigurationRegister: {}", nameId(), e.what());
3603 doDeinitialize();
3604 }
3605 }
3606 else
3607 {
3608 doDeinitialize();
3609 }
3610 }
3611 ImGui::SameLine();
3612 gui::widgets::HelpMarker("The WindowSize parameters for each sensor define the number of samples at the IMU rate (default 800Hz) "
3613 "which will be averaged for each output measurement.");
3614
3615 int presWindowSize = _imuFilteringConfigurationRegister.presWindowSize;
3616 if (ImGui::InputInt(fmt::format("Pres Window Size##{}", size_t(id)).c_str(), &presWindowSize))
3617 {
3618 if (presWindowSize < 0)
3619 {
3620 presWindowSize = 0;
3621 }
3622 else if (presWindowSize > std::numeric_limits<uint16_t>::max())
3623 {
3624 presWindowSize = std::numeric_limits<uint16_t>::max();
3625 }
3626 _imuFilteringConfigurationRegister.presWindowSize = static_cast<uint16_t>(presWindowSize);
3627 LOG_DEBUG("{}: imuFilteringConfigurationRegister.presWindowSize changed to {}", nameId(), _imuFilteringConfigurationRegister.presWindowSize);
3628 flow::ApplyChanges();
3629 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3630 {
3631 try
3632 {
3633 _vs.writeImuFilteringConfiguration(_imuFilteringConfigurationRegister);
3634 }
3635 catch (const std::exception& e)
3636 {
3637 LOG_ERROR("{}: Could not configure the imuFilteringConfigurationRegister: {}", nameId(), e.what());
3638 doDeinitialize();
3639 }
3640 }
3641 else
3642 {
3643 doDeinitialize();
3644 }
3645 }
3646 ImGui::SameLine();
3647 gui::widgets::HelpMarker("The WindowSize parameters for each sensor define the number of samples at the IMU rate (default 800Hz) "
3648 "which will be averaged for each output measurement.");
3649 if (_coupleImuRateOutput)
3650 {
3651 ImGui::EndDisabled();
3652 }
3653
3654 static constexpr std::array<std::pair<vn::protocol::uart::FilterMode, const char*>, 4> imuFilteringConfigurationFilterModes = {
3655 { { vn::protocol::uart::FilterMode::FILTERMODE_NOFILTERING, "No Filtering" },
3656 { vn::protocol::uart::FilterMode::FILTERMODE_ONLYRAW, "Filtering performed only on raw uncompensated IMU measurements." },
3657 { vn::protocol::uart::FilterMode::FILTERMODE_ONLYCOMPENSATED, "Filtering performed only on compensated IMU measurements." },
3658 { vn::protocol::uart::FilterMode::FILTERMODE_BOTH, "Filtering performed on both uncompensated and compensated IMU measurements." } }
3659 };
3660 if (ImGui::BeginCombo(fmt::format("Mag Filter Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_imuFilteringConfigurationRegister.magFilterMode).c_str()))
3661 {
3662 for (const auto& imuFilteringConfigurationFilterMode : imuFilteringConfigurationFilterModes)
3663 {
3664 const bool isSelected = (_imuFilteringConfigurationRegister.magFilterMode == imuFilteringConfigurationFilterMode.first);
3665 if (ImGui::Selectable(vn::protocol::uart::str(imuFilteringConfigurationFilterMode.first).c_str(), isSelected))
3666 {
3667 _imuFilteringConfigurationRegister.magFilterMode = imuFilteringConfigurationFilterMode.first;
3668 LOG_DEBUG("{}: imuFilteringConfigurationRegister.magFilterMode changed to {}", nameId(), vn::protocol::uart::str(_imuFilteringConfigurationRegister.magFilterMode));
3669 flow::ApplyChanges();
3670 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3671 {
3672 try
3673 {
3674 _vs.writeImuFilteringConfiguration(_imuFilteringConfigurationRegister);
3675 }
3676 catch (const std::exception& e)
3677 {
3678 LOG_ERROR("{}: Could not configure the imuFilteringConfigurationRegister: {}", nameId(), e.what());
3679 doDeinitialize();
3680 }
3681 }
3682 else
3683 {
3684 doDeinitialize();
3685 }
3686 }
3687 if (ImGui::IsItemHovered())
3688 {
3689 ImGui::BeginTooltip();
3690 ImGui::TextUnformatted(imuFilteringConfigurationFilterMode.second);
3691 ImGui::EndTooltip();
3692 }
3693
3694 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
3695 {
3696 ImGui::SetItemDefaultFocus();
3697 }
3698 }
3699 ImGui::EndCombo();
3700 }
3701 ImGui::SameLine();
3702 gui::widgets::HelpMarker("The FilterMode parameters for each sensor select which output quantities the filtering should be applied to. "
3703 "Filtering can be applied to either the uncompensated IMU measurements, compensated (HSI and biases "
3704 "compensated by onboard filters, if applicable), or both.");
3705
3706 if (ImGui::BeginCombo(fmt::format("Accel Filter Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_imuFilteringConfigurationRegister.accelFilterMode).c_str()))
3707 {
3708 for (const auto& imuFilteringConfigurationFilterMode : imuFilteringConfigurationFilterModes)
3709 {
3710 const bool isSelected = (_imuFilteringConfigurationRegister.accelFilterMode == imuFilteringConfigurationFilterMode.first);
3711 if (ImGui::Selectable(vn::protocol::uart::str(imuFilteringConfigurationFilterMode.first).c_str(), isSelected))
3712 {
3713 _imuFilteringConfigurationRegister.accelFilterMode = imuFilteringConfigurationFilterMode.first;
3714 LOG_DEBUG("{}: imuFilteringConfigurationRegister.accelFilterMode changed to {}", nameId(), vn::protocol::uart::str(_imuFilteringConfigurationRegister.accelFilterMode));
3715 flow::ApplyChanges();
3716 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3717 {
3718 try
3719 {
3720 _vs.writeImuFilteringConfiguration(_imuFilteringConfigurationRegister);
3721 }
3722 catch (const std::exception& e)
3723 {
3724 LOG_ERROR("{}: Could not configure the imuFilteringConfigurationRegister: {}", nameId(), e.what());
3725 doDeinitialize();
3726 }
3727 }
3728 else
3729 {
3730 doDeinitialize();
3731 }
3732 }
3733 if (ImGui::IsItemHovered())
3734 {
3735 ImGui::BeginTooltip();
3736 ImGui::TextUnformatted(imuFilteringConfigurationFilterMode.second);
3737 ImGui::EndTooltip();
3738 }
3739
3740 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
3741 {
3742 ImGui::SetItemDefaultFocus();
3743 }
3744 }
3745 ImGui::EndCombo();
3746 }
3747 ImGui::SameLine();
3748 gui::widgets::HelpMarker("The FilterMode parameters for each sensor select which output quantities the filtering should be applied to. "
3749 "Filtering can be applied to either the uncompensated IMU measurements, compensated (HSI and biases "
3750 "compensated by onboard filters, if applicable), or both.");
3751
3752 if (ImGui::BeginCombo(fmt::format("Gyro Filter Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_imuFilteringConfigurationRegister.gyroFilterMode).c_str()))
3753 {
3754 for (const auto& imuFilteringConfigurationFilterMode : imuFilteringConfigurationFilterModes)
3755 {
3756 const bool isSelected = (_imuFilteringConfigurationRegister.gyroFilterMode == imuFilteringConfigurationFilterMode.first);
3757 if (ImGui::Selectable(vn::protocol::uart::str(imuFilteringConfigurationFilterMode.first).c_str(), isSelected))
3758 {
3759 _imuFilteringConfigurationRegister.gyroFilterMode = imuFilteringConfigurationFilterMode.first;
3760 LOG_DEBUG("{}: imuFilteringConfigurationRegister.gyroFilterMode changed to {}", nameId(), vn::protocol::uart::str(_imuFilteringConfigurationRegister.gyroFilterMode));
3761 flow::ApplyChanges();
3762 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3763 {
3764 try
3765 {
3766 _vs.writeImuFilteringConfiguration(_imuFilteringConfigurationRegister);
3767 }
3768 catch (const std::exception& e)
3769 {
3770 LOG_ERROR("{}: Could not configure the imuFilteringConfigurationRegister: {}", nameId(), e.what());
3771 doDeinitialize();
3772 }
3773 }
3774 else
3775 {
3776 doDeinitialize();
3777 }
3778 }
3779 if (ImGui::IsItemHovered())
3780 {
3781 ImGui::BeginTooltip();
3782 ImGui::TextUnformatted(imuFilteringConfigurationFilterMode.second);
3783 ImGui::EndTooltip();
3784 }
3785
3786 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
3787 {
3788 ImGui::SetItemDefaultFocus();
3789 }
3790 }
3791 ImGui::EndCombo();
3792 }
3793 ImGui::SameLine();
3794 gui::widgets::HelpMarker("The FilterMode parameters for each sensor select which output quantities the filtering should be applied to. "
3795 "Filtering can be applied to either the uncompensated IMU measurements, compensated (HSI and biases "
3796 "compensated by onboard filters, if applicable), or both.");
3797
3798 if (ImGui::BeginCombo(fmt::format("Temp Filter Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_imuFilteringConfigurationRegister.tempFilterMode).c_str()))
3799 {
3800 for (const auto& imuFilteringConfigurationFilterMode : imuFilteringConfigurationFilterModes)
3801 {
3802 const bool isSelected = (_imuFilteringConfigurationRegister.tempFilterMode == imuFilteringConfigurationFilterMode.first);
3803 if (ImGui::Selectable(vn::protocol::uart::str(imuFilteringConfigurationFilterMode.first).c_str(), isSelected))
3804 {
3805 _imuFilteringConfigurationRegister.tempFilterMode = imuFilteringConfigurationFilterMode.first;
3806 LOG_DEBUG("{}: imuFilteringConfigurationRegister.tempFilterMode changed to {}", nameId(), vn::protocol::uart::str(_imuFilteringConfigurationRegister.tempFilterMode));
3807 flow::ApplyChanges();
3808 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3809 {
3810 try
3811 {
3812 _vs.writeImuFilteringConfiguration(_imuFilteringConfigurationRegister);
3813 }
3814 catch (const std::exception& e)
3815 {
3816 LOG_ERROR("{}: Could not configure the imuFilteringConfigurationRegister: {}", nameId(), e.what());
3817 doDeinitialize();
3818 }
3819 }
3820 else
3821 {
3822 doDeinitialize();
3823 }
3824 }
3825 if (ImGui::IsItemHovered())
3826 {
3827 ImGui::BeginTooltip();
3828 ImGui::TextUnformatted(imuFilteringConfigurationFilterMode.second);
3829 ImGui::EndTooltip();
3830 }
3831
3832 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
3833 {
3834 ImGui::SetItemDefaultFocus();
3835 }
3836 }
3837 ImGui::EndCombo();
3838 }
3839 ImGui::SameLine();
3840 gui::widgets::HelpMarker("The FilterMode parameters for each sensor select which output quantities the filtering should be applied to. "
3841 "Filtering can be applied to either the uncompensated IMU measurements, compensated (HSI and biases "
3842 "compensated by onboard filters, if applicable), or both.");
3843
3844 if (ImGui::BeginCombo(fmt::format("Pres Filter Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_imuFilteringConfigurationRegister.presFilterMode).c_str()))
3845 {
3846 for (const auto& imuFilteringConfigurationFilterMode : imuFilteringConfigurationFilterModes)
3847 {
3848 const bool isSelected = (_imuFilteringConfigurationRegister.presFilterMode == imuFilteringConfigurationFilterMode.first);
3849 if (ImGui::Selectable(vn::protocol::uart::str(imuFilteringConfigurationFilterMode.first).c_str(), isSelected))
3850 {
3851 _imuFilteringConfigurationRegister.presFilterMode = imuFilteringConfigurationFilterMode.first;
3852 LOG_DEBUG("{}: imuFilteringConfigurationRegister.presFilterMode changed to {}", nameId(), vn::protocol::uart::str(_imuFilteringConfigurationRegister.presFilterMode));
3853 flow::ApplyChanges();
3854 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3855 {
3856 try
3857 {
3858 _vs.writeImuFilteringConfiguration(_imuFilteringConfigurationRegister);
3859 }
3860 catch (const std::exception& e)
3861 {
3862 LOG_ERROR("{}: Could not configure the imuFilteringConfigurationRegister: {}", nameId(), e.what());
3863 doDeinitialize();
3864 }
3865 }
3866 else
3867 {
3868 doDeinitialize();
3869 }
3870 }
3871 if (ImGui::IsItemHovered())
3872 {
3873 ImGui::BeginTooltip();
3874 ImGui::TextUnformatted(imuFilteringConfigurationFilterMode.second);
3875 ImGui::EndTooltip();
3876 }
3877
3878 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
3879 {
3880 ImGui::SetItemDefaultFocus();
3881 }
3882 }
3883 ImGui::EndCombo();
3884 }
3885 ImGui::SameLine();
3886 gui::widgets::HelpMarker("The FilterMode parameters for each sensor select which output quantities the filtering should be applied to. "
3887 "Filtering can be applied to either the uncompensated IMU measurements, compensated (HSI and biases "
3888 "compensated by onboard filters, if applicable), or both.");
3889
3890 ImGui::TreePop();
3891 }
3892
3893 if (ImGui::TreeNode(fmt::format("Delta Theta and Delta Velocity Configuration##{}", size_t(id)).c_str()))
3894 {
3895 ImGui::TextUnformatted("The Delta Theta and Delta Velocity Configuration register allows configuration of the onboard coning and\n"
3896 "sculling used to generate integrated motion values from the angular rate and acceleration IMU quantities.\n"
3897 "The fully-coupled coning and sculling integrals are computed at the IMU sample rate (nominal 400 Hz).");
3898
3899 static constexpr std::array<std::pair<vn::protocol::uart::IntegrationFrame, const char*>, 2> deltaThetaAndDeltaVelocityConfigurationIntegrationFrames = {
3900 { { vn::protocol::uart::IntegrationFrame::INTEGRATIONFRAME_BODY, "Body frame" },
3901 { vn::protocol::uart::IntegrationFrame::INTEGRATIONFRAME_NED, "NED frame" } }
3902 };
3903 if (ImGui::BeginCombo(fmt::format("Integration Frame##{}", size_t(id)).c_str(), vn::protocol::uart::str(_deltaThetaAndDeltaVelocityConfigurationRegister.integrationFrame).c_str()))
3904 {
3905 for (const auto& deltaThetaAndDeltaVelocityConfigurationIntegrationFrame : deltaThetaAndDeltaVelocityConfigurationIntegrationFrames)
3906 {
3907 const bool isSelected = (_deltaThetaAndDeltaVelocityConfigurationRegister.integrationFrame == deltaThetaAndDeltaVelocityConfigurationIntegrationFrame.first);
3908 if (ImGui::Selectable(vn::protocol::uart::str(deltaThetaAndDeltaVelocityConfigurationIntegrationFrame.first).c_str(), isSelected))
3909 {
3910 _deltaThetaAndDeltaVelocityConfigurationRegister.integrationFrame = deltaThetaAndDeltaVelocityConfigurationIntegrationFrame.first;
3911 LOG_DEBUG("{}: deltaThetaAndDeltaVelocityConfigurationRegister.integrationFrame changed to {}", nameId(), vn::protocol::uart::str(_deltaThetaAndDeltaVelocityConfigurationRegister.integrationFrame));
3912 flow::ApplyChanges();
3913 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3914 {
3915 try
3916 {
3917 _vs.writeDeltaThetaAndDeltaVelocityConfiguration(_deltaThetaAndDeltaVelocityConfigurationRegister);
3918 }
3919 catch (const std::exception& e)
3920 {
3921 LOG_ERROR("{}: Could not configure the deltaThetaAndDeltaVelocityConfigurationRegister: {}", nameId(), e.what());
3922 doDeinitialize();
3923 }
3924 }
3925 else
3926 {
3927 doDeinitialize();
3928 }
3929 }
3930 if (ImGui::IsItemHovered())
3931 {
3932 ImGui::BeginTooltip();
3933 ImGui::TextUnformatted(deltaThetaAndDeltaVelocityConfigurationIntegrationFrame.second);
3934 ImGui::EndTooltip();
3935 }
3936
3937 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
3938 {
3939 ImGui::SetItemDefaultFocus();
3940 }
3941 }
3942 ImGui::EndCombo();
3943 }
3944 ImGui::SameLine();
3945 gui::widgets::HelpMarker("The IntegrationFrame register setting selects the reference frame used for coning and sculling. Note that "
3946 "using any frame other than the body frame will rely on the onboard Kalman filter's attitude estimate. The "
3947 "factory default state is to integrate in the sensor body frame.");
3948
3949 static constexpr std::array<std::pair<vn::protocol::uart::CompensationMode, const char*>, 2> deltaThetaAndDeltaVelocityConfigurationGyroCompensationModes = {
3950 { { vn::protocol::uart::CompensationMode::COMPENSATIONMODE_NONE, "None" },
3951 { vn::protocol::uart::CompensationMode::COMPENSATIONMODE_BIAS, "Bias" } }
3952 };
3953 if (ImGui::BeginCombo(fmt::format("Gyro Compensation##{}", size_t(id)).c_str(), vn::protocol::uart::str(_deltaThetaAndDeltaVelocityConfigurationRegister.gyroCompensation).c_str()))
3954 {
3955 for (const auto& deltaThetaAndDeltaVelocityConfigurationGyroCompensationMode : deltaThetaAndDeltaVelocityConfigurationGyroCompensationModes)
3956 {
3957 const bool isSelected = (_deltaThetaAndDeltaVelocityConfigurationRegister.gyroCompensation == deltaThetaAndDeltaVelocityConfigurationGyroCompensationMode.first);
3958 if (ImGui::Selectable(vn::protocol::uart::str(deltaThetaAndDeltaVelocityConfigurationGyroCompensationMode.first).c_str(), isSelected))
3959 {
3960 _deltaThetaAndDeltaVelocityConfigurationRegister.gyroCompensation = deltaThetaAndDeltaVelocityConfigurationGyroCompensationMode.first;
3961 LOG_DEBUG("{}: deltaThetaAndDeltaVelocityConfigurationRegister.gyroCompensation changed to {}", nameId(), vn::protocol::uart::str(_deltaThetaAndDeltaVelocityConfigurationRegister.gyroCompensation));
3962 flow::ApplyChanges();
3963 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
3964 {
3965 try
3966 {
3967 _vs.writeDeltaThetaAndDeltaVelocityConfiguration(_deltaThetaAndDeltaVelocityConfigurationRegister);
3968 }
3969 catch (const std::exception& e)
3970 {
3971 LOG_ERROR("{}: Could not configure the deltaThetaAndDeltaVelocityConfigurationRegister: {}", nameId(), e.what());
3972 doDeinitialize();
3973 }
3974 }
3975 else
3976 {
3977 doDeinitialize();
3978 }
3979 }
3980 if (ImGui::IsItemHovered())
3981 {
3982 ImGui::BeginTooltip();
3983 ImGui::TextUnformatted(deltaThetaAndDeltaVelocityConfigurationGyroCompensationMode.second);
3984 ImGui::EndTooltip();
3985 }
3986
3987 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
3988 {
3989 ImGui::SetItemDefaultFocus();
3990 }
3991 }
3992 ImGui::EndCombo();
3993 }
3994 ImGui::SameLine();
3995 gui::widgets::HelpMarker("The GyroCompensation register setting selects the compensation to be applied to the angular rate "
3996 "measurements before integration. If bias compensation is selected, the onboard Kalman filter’s real-time "
3997 "estimate of the gyro biases will be used to compensate the IMU measurements before integration. The "
3998 "factory default state is to integrate the uncompensated angular rates from the IMU.");
3999
4000 static constexpr std::array<std::pair<vn::protocol::uart::AccCompensationMode, const char*>, 2> deltaThetaAndDeltaVelocityConfigurationAccelCompensationModes = {
4001 { { vn::protocol::uart::AccCompensationMode::ACCCOMPENSATIONMODE_NONE, "None" },
4002 { vn::protocol::uart::AccCompensationMode::ACCCOMPENSATIONMODE_BIAS, "Bias" } }
4003 };
4004 if (ImGui::BeginCombo(fmt::format("Accel Compensation##{}", size_t(id)).c_str(), vn::protocol::uart::str(_deltaThetaAndDeltaVelocityConfigurationRegister.accelCompensation).c_str()))
4005 {
4006 for (const auto& deltaThetaAndDeltaVelocityConfigurationAccelCompensationMode : deltaThetaAndDeltaVelocityConfigurationAccelCompensationModes)
4007 {
4008 const bool isSelected = (_deltaThetaAndDeltaVelocityConfigurationRegister.accelCompensation == deltaThetaAndDeltaVelocityConfigurationAccelCompensationMode.first);
4009 if (ImGui::Selectable(vn::protocol::uart::str(deltaThetaAndDeltaVelocityConfigurationAccelCompensationMode.first).c_str(), isSelected))
4010 {
4011 _deltaThetaAndDeltaVelocityConfigurationRegister.accelCompensation = deltaThetaAndDeltaVelocityConfigurationAccelCompensationMode.first;
4012 LOG_DEBUG("{}: deltaThetaAndDeltaVelocityConfigurationRegister.accelCompensation changed to {}", nameId(), vn::protocol::uart::str(_deltaThetaAndDeltaVelocityConfigurationRegister.accelCompensation));
4013 flow::ApplyChanges();
4014 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4015 {
4016 try
4017 {
4018 _vs.writeDeltaThetaAndDeltaVelocityConfiguration(_deltaThetaAndDeltaVelocityConfigurationRegister);
4019 }
4020 catch (const std::exception& e)
4021 {
4022 LOG_ERROR("{}: Could not configure the deltaThetaAndDeltaVelocityConfigurationRegister: {}", nameId(), e.what());
4023 doDeinitialize();
4024 }
4025 }
4026 else
4027 {
4028 doDeinitialize();
4029 }
4030 }
4031 if (ImGui::IsItemHovered())
4032 {
4033 ImGui::BeginTooltip();
4034 ImGui::TextUnformatted(deltaThetaAndDeltaVelocityConfigurationAccelCompensationMode.second);
4035 ImGui::EndTooltip();
4036 }
4037
4038 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
4039 {
4040 ImGui::SetItemDefaultFocus();
4041 }
4042 }
4043 ImGui::EndCombo();
4044 }
4045 ImGui::SameLine();
4046 gui::widgets::HelpMarker("The AccelCompensation register setting selects the compensation to be applied to the acceleration "
4047 "measurements before integration. If bias compensation is selected, the onboard Kalman filter’s real-time "
4048 "estimate of the accel biases will be used to compensate the IMU measurements before integration. The "
4049 "factory default state is to integrate the uncompensated acceleration from the IMU.");
4050
4051 ImGui::TreePop();
4052 }
4053 }
4054
4055 // ###########################################################################################################
4056 // GNSS SUBSYSTEM
4057 // ###########################################################################################################
4058
4059 if (_sensorModel == VectorNavModel::VN310)
4060 {
4061 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
4062 if (ImGui::CollapsingHeader(fmt::format("GNSS Subsystem##{}", size_t(id)).c_str()))
4063 {
4064 if (ImGui::TreeNode(fmt::format("GNSS Configuration##{}", size_t(id)).c_str()))
4065 {
4066 static constexpr std::array<std::pair<vn::protocol::uart::GpsMode, const char*>, 3> gpsConfigurationModes = {
4067 { { vn::protocol::uart::GpsMode::GPSMODE_ONBOARDGPS, "Use onboard GNSS" },
4068 { vn::protocol::uart::GpsMode::GPSMODE_EXTERNALGPS, "Use external GNSS" },
4069 { vn::protocol::uart::GpsMode::GPSMODE_EXTERNALVN200GPS, "Use external VectorNav sensor as the GNSS" } }
4070 };
4071 if (ImGui::BeginCombo(fmt::format("Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_gpsConfigurationRegister.mode).c_str()))
4072 {
4073 for (const auto& gpsConfigurationMode : gpsConfigurationModes)
4074 {
4075 const bool isSelected = (_gpsConfigurationRegister.mode == gpsConfigurationMode.first);
4076 if (ImGui::Selectable(vn::protocol::uart::str(gpsConfigurationMode.first).c_str(), isSelected))
4077 {
4078 _gpsConfigurationRegister.mode = gpsConfigurationMode.first;
4079 LOG_DEBUG("{}: gpsConfigurationRegister.mode changed to {}", nameId(), vn::protocol::uart::str(_gpsConfigurationRegister.mode));
4080 flow::ApplyChanges();
4081 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4082 {
4083 try
4084 {
4085 _vs.writeGpsConfiguration(_gpsConfigurationRegister);
4086 }
4087 catch (const std::exception& e)
4088 {
4089 LOG_ERROR("{}: Could not configure the gpsConfigurationRegister: {}", nameId(), e.what());
4090 doDeinitialize();
4091 }
4092 }
4093 else
4094 {
4095 doDeinitialize();
4096 }
4097 }
4098 if (ImGui::IsItemHovered())
4099 {
4100 ImGui::BeginTooltip();
4101 ImGui::TextUnformatted(gpsConfigurationMode.second);
4102 ImGui::EndTooltip();
4103 }
4104
4105 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
4106 {
4107 ImGui::SetItemDefaultFocus();
4108 }
4109 }
4110 ImGui::EndCombo();
4111 }
4112
4113 static constexpr std::array<std::pair<vn::protocol::uart::PpsSource, const char*>, 4> gpsConfigurationPpsSources = {
4114 { { vn::protocol::uart::PpsSource::PPSSOURCE_GPSPPSRISING, "GNSS PPS signal is present on the GNSS_PPS pin (pin 24) and should trigger on a rising edge." },
4115 { vn::protocol::uart::PpsSource::PPSSOURCE_GPSPPSFALLING, "GNSS PPS signal is present on the GNSS_PPS pin (pin 24) and should trigger on a falling edge" },
4116 { vn::protocol::uart::PpsSource::PPSSOURCE_SYNCINRISING, "GNSS PPS signal is present on the SyncIn pin (pin 22) and should trigger on a rising edge" },
4117 { vn::protocol::uart::PpsSource::PPSSOURCE_SYNCINFALLING, "GNSS PPS signal is present on the SyncIn pin (pin 22) and should trigger on a falling edge" } }
4118 };
4119 if (ImGui::BeginCombo(fmt::format("PPS Source##{}", size_t(id)).c_str(), vn::protocol::uart::str(_gpsConfigurationRegister.ppsSource).c_str()))
4120 {
4121 for (const auto& gpsConfigurationPpsSource : gpsConfigurationPpsSources)
4122 {
4123 const bool isSelected = (_gpsConfigurationRegister.ppsSource == gpsConfigurationPpsSource.first);
4124 if (ImGui::Selectable(vn::protocol::uart::str(gpsConfigurationPpsSource.first).c_str(), isSelected))
4125 {
4126 _gpsConfigurationRegister.ppsSource = gpsConfigurationPpsSource.first;
4127 LOG_DEBUG("{}: gpsConfigurationRegister.ppsSource changed to {}", nameId(), vn::protocol::uart::str(_gpsConfigurationRegister.ppsSource));
4128 flow::ApplyChanges();
4129 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4130 {
4131 try
4132 {
4133 _vs.writeGpsConfiguration(_gpsConfigurationRegister);
4134 }
4135 catch (const std::exception& e)
4136 {
4137 LOG_ERROR("{}: Could not configure the gpsConfigurationRegister: {}", nameId(), e.what());
4138 doDeinitialize();
4139 }
4140 }
4141 else
4142 {
4143 doDeinitialize();
4144 }
4145 }
4146 if (ImGui::IsItemHovered())
4147 {
4148 ImGui::BeginTooltip();
4149 ImGui::TextUnformatted(gpsConfigurationPpsSource.second);
4150 ImGui::EndTooltip();
4151 }
4152
4153 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
4154 {
4155 ImGui::SetItemDefaultFocus();
4156 }
4157 }
4158 ImGui::EndCombo();
4159 }
4160
4161 static constexpr std::array<vn::protocol::uart::GpsRate, 1> gpsConfigurationRates = {
4162 { /* vn::protocol::uart::GpsRate::GPSRATE_1HZ, */
4163 vn::protocol::uart::GpsRate::GPSRATE_5HZ }
4164 };
4165 if (ImGui::BeginCombo(fmt::format("Rate##{}", size_t(id)).c_str(), vn::protocol::uart::str(_gpsConfigurationRegister.rate).c_str()))
4166 {
4167 for (const auto& gpsConfigurationRate : gpsConfigurationRates)
4168 {
4169 const bool isSelected = (_gpsConfigurationRegister.rate == gpsConfigurationRate);
4170 if (ImGui::Selectable(vn::protocol::uart::str(gpsConfigurationRate).c_str(), isSelected))
4171 {
4172 _gpsConfigurationRegister.rate = gpsConfigurationRate;
4173 LOG_DEBUG("{}: gpsConfigurationRegister.rate changed to {}", nameId(), vn::protocol::uart::str(_gpsConfigurationRegister.rate));
4174 flow::ApplyChanges();
4175 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4176 {
4177 try
4178 {
4179 _vs.writeGpsConfiguration(_gpsConfigurationRegister);
4180 }
4181 catch (const std::exception& e)
4182 {
4183 LOG_ERROR("{}: Could not configure the gpsConfigurationRegister: {}", nameId(), e.what());
4184 doDeinitialize();
4185 }
4186 }
4187 else
4188 {
4189 doDeinitialize();
4190 }
4191 }
4192
4193 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
4194 {
4195 ImGui::SetItemDefaultFocus();
4196 }
4197 }
4198 ImGui::EndCombo();
4199 }
4200 ImGui::SameLine();
4201 gui::widgets::HelpMarker("GNSS navigation rate. Value must be set to 5.");
4202
4203 static constexpr std::array<std::pair<vn::protocol::uart::AntPower, const char*>, 3> gpsConfigurationAntPowers = {
4204 { { vn::protocol::uart::AntPower::ANTPOWER_OFFRESV, "Disable antenna power supply." },
4205 { vn::protocol::uart::AntPower::ANTPOWER_INTERNAL, "Use internal antenna power supply (3V, 50mA combined)." },
4206 { vn::protocol::uart::AntPower::ANTPOWER_EXTERNAL, "Use external antenna power supply (VANT pin, up to 5V and 100mA combined)" } }
4207 };
4208 if (ImGui::BeginCombo(fmt::format("Ant Power##{}", size_t(id)).c_str(), vn::protocol::uart::str(_gpsConfigurationRegister.antPow).c_str()))
4209 {
4210 for (const auto& gpsConfigurationAntPower : gpsConfigurationAntPowers)
4211 {
4212 const bool isSelected = (_gpsConfigurationRegister.antPow == gpsConfigurationAntPower.first);
4213 if (ImGui::Selectable(vn::protocol::uart::str(gpsConfigurationAntPower.first).c_str(), isSelected))
4214 {
4215 _gpsConfigurationRegister.antPow = gpsConfigurationAntPower.first;
4216 LOG_DEBUG("{}: gpsConfigurationRegister.antPow changed to {}", nameId(), vn::protocol::uart::str(_gpsConfigurationRegister.antPow));
4217 flow::ApplyChanges();
4218 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4219 {
4220 try
4221 {
4222 _vs.writeGpsConfiguration(_gpsConfigurationRegister);
4223 }
4224 catch (const std::exception& e)
4225 {
4226 LOG_ERROR("{}: Could not configure the gpsConfigurationRegister: {}", nameId(), e.what());
4227 doDeinitialize();
4228 }
4229 }
4230 else
4231 {
4232 doDeinitialize();
4233 }
4234 }
4235 if (ImGui::IsItemHovered())
4236 {
4237 ImGui::BeginTooltip();
4238 ImGui::TextUnformatted(gpsConfigurationAntPower.second);
4239 ImGui::EndTooltip();
4240 }
4241
4242 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
4243 {
4244 ImGui::SetItemDefaultFocus();
4245 }
4246 }
4247 ImGui::EndCombo();
4248 }
4249 ImGui::SameLine();
4250 gui::widgets::HelpMarker("GNSS navigation rate. Value must be set to 5.");
4251
4252 ImGui::TreePop();
4253 }
4254
4255 if (ImGui::TreeNode(fmt::format("GNSS Antenna A Offset##{}", size_t(id)).c_str()))
4256 {
4257 ImGui::TextUnformatted("The position of the GNSS antenna A relative to the sensor in the vehicle coordinate frame\n"
4258 "also referred to as the GNSS antenna lever arm.");
4259
4260 if (ImGui::InputFloat3(fmt::format("##gpsAntennaOffset {}", size_t(id)).c_str(), _gpsAntennaOffset.c, "%.6f"))
4261 {
4262 LOG_DEBUG("{}: gpsAntennaOffset changed to {}", nameId(), _gpsAntennaOffset);
4263 flow::ApplyChanges();
4264 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4265 {
4266 try
4267 {
4268 _vs.writeGpsAntennaOffset(_gpsAntennaOffset);
4269 }
4270 catch (const std::exception& e)
4271 {
4272 LOG_ERROR("{}: Could not configure the gpsAntennaOffset: {}", nameId(), e.what());
4273 doDeinitialize();
4274 }
4275 }
4276 else
4277 {
4278 doDeinitialize();
4279 }
4280 }
4281
4282 ImGui::TreePop();
4283 }
4284
4285 if (ImGui::TreeNode(fmt::format("GNSS Compass Baseline##{}", size_t(id)).c_str()))
4286 {
4287 ImGui::TextUnformatted("Configures the position offset and measurement uncertainty of the second GNSS\n"
4288 "antenna relative to the first GNSS antenna in the vehicle reference frame.");
4289
4290 if (ImGui::InputFloat3(fmt::format("Position [m]##{}", size_t(id)).c_str(), _gpsCompassBaselineRegister.position.c, "%.6f"))
4291 {
4292 LOG_DEBUG("{}: gpsCompassBaselineRegister.position changed to {}", nameId(), _gpsCompassBaselineRegister.position);
4293 flow::ApplyChanges();
4294 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4295 {
4296 try
4297 {
4298 _vs.writeGpsCompassBaseline(_gpsCompassBaselineRegister);
4299 }
4300 catch (const std::exception& e)
4301 {
4302 LOG_ERROR("{}: Could not configure the gpsCompassBaselineRegister: {}", nameId(), e.what());
4303 doDeinitialize();
4304 }
4305 }
4306 else
4307 {
4308 doDeinitialize();
4309 }
4310 }
4311 ImGui::SameLine();
4312 gui::widgets::HelpMarker("HEADING ACCURACY\n\n"
4313 "The accuracy of the estimated heading is dependent upon the accuracy of the measured baseline "
4314 "between the two GNSS antennas. The factory default baseline is {1.0m, 0.0m, 0.0m}. If any other "
4315 "baseline is used, it is extremely important that the user acurately measures this baseline to ensure "
4316 "accurate heading estimates.\n"
4317 "The heading accuracy is linearly proportional to the measurement accuracy of the position of "
4318 "GNSS antenna B with respect to GNSS antenna A, and inversely proportional to the baseline "
4319 "length.\n\n"
4320 "Heading Error [deg] ~= 0.57 * (Baseline Error [cm]) / (Baseline Length [m])\n\n"
4321 "On a 1 meter baseline, a 1 cm measurement error equates to heading error of 0.6 degrees.",
4322 "(!)");
4323
4324 if (ImGui::InputFloat3(fmt::format("Uncertainty [m]##{}", size_t(id)).c_str(), _gpsCompassBaselineRegister.uncertainty.c, "%.3f"))
4325 {
4326 LOG_DEBUG("{}: gpsCompassBaselineRegister.uncertainty changed to {}", nameId(), _gpsCompassBaselineRegister.uncertainty);
4327 flow::ApplyChanges();
4328 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4329 {
4330 try
4331 {
4332 _vs.writeGpsCompassBaseline(_gpsCompassBaselineRegister);
4333 }
4334 catch (const std::exception& e)
4335 {
4336 LOG_ERROR("{}: Could not configure the gpsCompassBaselineRegister: {}", nameId(), e.what());
4337 doDeinitialize();
4338 }
4339 }
4340 else
4341 {
4342 doDeinitialize();
4343 }
4344 }
4345 ImGui::SameLine();
4346 gui::widgets::HelpMarker("MEASUREMENT UNCERTAINTY\n\n"
4347 "For the VN-310E to function properly it is very important that the user supplies a reasonable "
4348 "measurement uncertainty that is greater than the actual uncertainty in the baseline measurement. "
4349 "The VN-310E uses the uncertainty supplied by the user to validate measurements that it receives "
4350 "from the GNSS receivers. If the user inputs an uncertainty that is lower than the actual error in "
4351 "the baseline measurement between the two antennas, the VN-310E will no longer be able to derive "
4352 "heading estimates from the GNSS.\n\n"
4353 "It is recommended that you set the uncertainty equal to twice what you expect the worst case "
4354 "error to be in your baseline measurements. In many applications it is easier to measure more "
4355 "accurately in one direction than another. It is recommended that you set each of the X, Y, & Z "
4356 "uncertainties seperately to reflect this, as opposed to using a single large value.",
4357 "(!)");
4358
4359 ImGui::TreePop();
4360 }
4361 }
4362 }
4363
4364 // ###########################################################################################################
4365 // ATTITUDE SUBSYSTEM
4366 // ###########################################################################################################
4367
4368 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
4369 if (ImGui::CollapsingHeader(fmt::format("Attitude Subsystem##{}", size_t(id)).c_str()))
4370 {
4371 if (ImGui::TreeNode(fmt::format("VPE Basic Control##{}", size_t(id)).c_str()))
4372 {
4373 ImGui::TextUnformatted("Provides control over various features relating to the onboard attitude filtering algorithm.");
4374
4375 static constexpr std::array<vn::protocol::uart::VpeEnable, 2> vpeBasicControlEnables = {
4376 { vn::protocol::uart::VpeEnable::VPEENABLE_DISABLE,
4377 vn::protocol::uart::VpeEnable::VPEENABLE_ENABLE }
4378 };
4379 if (ImGui::BeginCombo(fmt::format("Enable##{}", size_t(id)).c_str(), vn::protocol::uart::str(_vpeBasicControlRegister.enable).c_str()))
4380 {
4381 for (const auto& vpeBasicControlEnable : vpeBasicControlEnables)
4382 {
4383 const bool isSelected = (_vpeBasicControlRegister.enable == vpeBasicControlEnable);
4384 if (ImGui::Selectable(vn::protocol::uart::str(vpeBasicControlEnable).c_str(), isSelected))
4385 {
4386 _vpeBasicControlRegister.enable = vpeBasicControlEnable;
4387 LOG_DEBUG("{}: vpeBasicControlRegister.enable changed to {}", nameId(), vn::protocol::uart::str(_vpeBasicControlRegister.enable));
4388 flow::ApplyChanges();
4389 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4390 {
4391 try
4392 {
4393 _vs.writeVpeBasicControl(_vpeBasicControlRegister);
4394 }
4395 catch (const std::exception& e)
4396 {
4397 LOG_ERROR("{}: Could not configure the vpeBasicControlRegister: {}", nameId(), e.what());
4398 doDeinitialize();
4399 }
4400 }
4401 else
4402 {
4403 doDeinitialize();
4404 }
4405 }
4406
4407 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
4408 {
4409 ImGui::SetItemDefaultFocus();
4410 }
4411 }
4412 ImGui::EndCombo();
4413 }
4414
4415 static constexpr std::array<vn::protocol::uart::HeadingMode, 3> vpeBasicControlHeadingModes = {
4416 { vn::protocol::uart::HeadingMode::HEADINGMODE_ABSOLUTE,
4417 vn::protocol::uart::HeadingMode::HEADINGMODE_RELATIVE,
4418 vn::protocol::uart::HeadingMode::HEADINGMODE_INDOOR }
4419 };
4420 if (ImGui::BeginCombo(fmt::format("Heading Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_vpeBasicControlRegister.headingMode).c_str()))
4421 {
4422 for (const auto& vpeBasicControlHeadingMode : vpeBasicControlHeadingModes)
4423 {
4424 const bool isSelected = (_vpeBasicControlRegister.headingMode == vpeBasicControlHeadingMode);
4425 if (ImGui::Selectable(vn::protocol::uart::str(vpeBasicControlHeadingMode).c_str(), isSelected))
4426 {
4427 _vpeBasicControlRegister.headingMode = vpeBasicControlHeadingMode;
4428 LOG_DEBUG("{}: vpeBasicControlRegister.headingMode changed to {}", nameId(), vn::protocol::uart::str(_vpeBasicControlRegister.headingMode));
4429 flow::ApplyChanges();
4430 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4431 {
4432 try
4433 {
4434 _vs.writeVpeBasicControl(_vpeBasicControlRegister);
4435 }
4436 catch (const std::exception& e)
4437 {
4438 LOG_ERROR("{}: Could not configure the vpeBasicControlRegister: {}", nameId(), e.what());
4439 doDeinitialize();
4440 }
4441 }
4442 else
4443 {
4444 doDeinitialize();
4445 }
4446 }
4447
4448 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
4449 {
4450 ImGui::SetItemDefaultFocus();
4451 }
4452 }
4453 ImGui::EndCombo();
4454 }
4455
4456 static constexpr std::array<vn::protocol::uart::VpeMode, 2> vpeBasicControlModes = {
4457 { vn::protocol::uart::VpeMode::VPEMODE_OFF,
4458 vn::protocol::uart::VpeMode::VPEMODE_MODE1 }
4459 };
4460 if (ImGui::BeginCombo(fmt::format("Filtering Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_vpeBasicControlRegister.filteringMode).c_str()))
4461 {
4462 for (const auto& vpeBasicControlMode : vpeBasicControlModes)
4463 {
4464 const bool isSelected = (_vpeBasicControlRegister.filteringMode == vpeBasicControlMode);
4465 if (ImGui::Selectable(vn::protocol::uart::str(vpeBasicControlMode).c_str(), isSelected))
4466 {
4467 _vpeBasicControlRegister.filteringMode = vpeBasicControlMode;
4468 LOG_DEBUG("{}: vpeBasicControlRegister.filteringMode changed to {}", nameId(), vn::protocol::uart::str(_vpeBasicControlRegister.filteringMode));
4469 flow::ApplyChanges();
4470 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4471 {
4472 try
4473 {
4474 _vs.writeVpeBasicControl(_vpeBasicControlRegister);
4475 }
4476 catch (const std::exception& e)
4477 {
4478 LOG_ERROR("{}: Could not configure the vpeBasicControlRegister: {}", nameId(), e.what());
4479 doDeinitialize();
4480 }
4481 }
4482 else
4483 {
4484 doDeinitialize();
4485 }
4486 }
4487
4488 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
4489 {
4490 ImGui::SetItemDefaultFocus();
4491 }
4492 }
4493 ImGui::EndCombo();
4494 }
4495
4496 if (ImGui::BeginCombo(fmt::format("Tuning Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_vpeBasicControlRegister.tuningMode).c_str()))
4497 {
4498 for (const auto& vpeBasicControlMode : vpeBasicControlModes)
4499 {
4500 const bool isSelected = (_vpeBasicControlRegister.tuningMode == vpeBasicControlMode);
4501 if (ImGui::Selectable(vn::protocol::uart::str(vpeBasicControlMode).c_str(), isSelected))
4502 {
4503 _vpeBasicControlRegister.tuningMode = vpeBasicControlMode;
4504 LOG_DEBUG("{}: vpeBasicControlRegister.tuningMode changed to {}", nameId(), vn::protocol::uart::str(_vpeBasicControlRegister.tuningMode));
4505 flow::ApplyChanges();
4506 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4507 {
4508 try
4509 {
4510 _vs.writeVpeBasicControl(_vpeBasicControlRegister);
4511 }
4512 catch (const std::exception& e)
4513 {
4514 LOG_ERROR("{}: Could not configure the vpeBasicControlRegister: {}", nameId(), e.what());
4515 doDeinitialize();
4516 }
4517 }
4518 else
4519 {
4520 doDeinitialize();
4521 }
4522 }
4523
4524 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
4525 {
4526 ImGui::SetItemDefaultFocus();
4527 }
4528 }
4529 ImGui::EndCombo();
4530 }
4531
4532 ImGui::TreePop();
4533 }
4534
4535 if (_sensorModel == VectorNavModel::VN100_VN110)
4536 {
4537 if (ImGui::TreeNode(fmt::format("VPE Magnetometer Basic Tuning##{}", size_t(id)).c_str()))
4538 {
4539 ImGui::TextUnformatted("Provides basic control of the adaptive filtering and tuning for the magnetometer.");
4540
4541 if (ImGui::DragFloat3(fmt::format("Base Tuning Level##{}", size_t(id)).c_str(), _vpeMagnetometerBasicTuningRegister.baseTuning.c, 0.1F, 0.0F, 10.0F, "%.1f"))
4542 {
4543 LOG_DEBUG("{}: vpeMagnetometerBasicTuningRegister.baseTuning changed to {}", nameId(), _vpeMagnetometerBasicTuningRegister.baseTuning);
4544 flow::ApplyChanges();
4545 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4546 {
4547 try
4548 {
4549 _vs.writeVpeMagnetometerBasicTuning(_vpeMagnetometerBasicTuningRegister);
4550 }
4551 catch (const std::exception& e)
4552 {
4553 LOG_ERROR("{}: Could not configure the vpeMagnetometerBasicTuningRegister: {}", nameId(), e.what());
4554 doDeinitialize();
4555 }
4556 }
4557 else
4558 {
4559 doDeinitialize();
4560 }
4561 }
4562 ImGui::SameLine();
4563 gui::widgets::HelpMarker("This sets the level of confidence placed in the magnetometer when no disturbances are present. "
4564 "A larger number provides better heading accuracy, but with more sensitivity to magnetic interference.");
4565
4566 if (ImGui::DragFloat3(fmt::format("Adaptive Tuning Level##{}", size_t(id)).c_str(), _vpeMagnetometerBasicTuningRegister.adaptiveTuning.c, 0.1F, 0.0F, 10.0F, "%.1f"))
4567 {
4568 LOG_DEBUG("{}: vpeMagnetometerBasicTuningRegister.adaptiveTuning changed to {}", nameId(), _vpeMagnetometerBasicTuningRegister.adaptiveTuning);
4569 flow::ApplyChanges();
4570 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4571 {
4572 try
4573 {
4574 _vs.writeVpeMagnetometerBasicTuning(_vpeMagnetometerBasicTuningRegister);
4575 }
4576 catch (const std::exception& e)
4577 {
4578 LOG_ERROR("{}: Could not configure the vpeMagnetometerBasicTuningRegister: {}", nameId(), e.what());
4579 doDeinitialize();
4580 }
4581 }
4582 else
4583 {
4584 doDeinitialize();
4585 }
4586 }
4587
4588 if (ImGui::DragFloat3(fmt::format("Adaptive Filtering Level##{}", size_t(id)).c_str(), _vpeMagnetometerBasicTuningRegister.adaptiveFiltering.c, 0.1F, 0.0F, 10.0F, "%.1f"))
4589 {
4590 LOG_DEBUG("{}: vpeMagnetometerBasicTuningRegister.adaptiveFiltering changed to {}", nameId(), _vpeMagnetometerBasicTuningRegister.adaptiveFiltering);
4591 flow::ApplyChanges();
4592 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4593 {
4594 try
4595 {
4596 _vs.writeVpeMagnetometerBasicTuning(_vpeMagnetometerBasicTuningRegister);
4597 }
4598 catch (const std::exception& e)
4599 {
4600 LOG_ERROR("{}: Could not configure the vpeMagnetometerBasicTuningRegister: {}", nameId(), e.what());
4601 doDeinitialize();
4602 }
4603 }
4604 else
4605 {
4606 doDeinitialize();
4607 }
4608 }
4609
4610 ImGui::TreePop();
4611 }
4612
4613 if (ImGui::TreeNode(fmt::format("VPE Accelerometer Basic Tuning##{}", size_t(id)).c_str()))
4614 {
4615 ImGui::TextUnformatted("Provides basic control of the adaptive filtering and tuning for the accelerometer.");
4616
4617 if (ImGui::DragFloat3(fmt::format("Base Tuning Level##{}", size_t(id)).c_str(), _vpeAccelerometerBasicTuningRegister.baseTuning.c, 0.1F, 0.0F, 10.0F, "%.1f"))
4618 {
4619 LOG_DEBUG("{}: vpeAccelerometerBasicTuningRegister.baseTuning changed to {}", nameId(), _vpeAccelerometerBasicTuningRegister.baseTuning);
4620 flow::ApplyChanges();
4621 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4622 {
4623 try
4624 {
4625 _vs.writeVpeAccelerometerBasicTuning(_vpeAccelerometerBasicTuningRegister);
4626 }
4627 catch (const std::exception& e)
4628 {
4629 LOG_ERROR("{}: Could not configure the vpeAccelerometerBasicTuningRegister: {}", nameId(), e.what());
4630 doDeinitialize();
4631 }
4632 }
4633 else
4634 {
4635 doDeinitialize();
4636 }
4637 }
4638 ImGui::SameLine();
4639 gui::widgets::HelpMarker("This sets the level of confidence placed in the accelerometer when no disturbances are present. "
4640 "A larger number provides better pitch/roll heading accuracy, but with more sensitivity to acceleration interference.");
4641
4642 if (ImGui::DragFloat3(fmt::format("Adaptive Tuning Level##{}", size_t(id)).c_str(), _vpeAccelerometerBasicTuningRegister.adaptiveTuning.c, 0.1F, 0.0F, 10.0F, "%.1f"))
4643 {
4644 LOG_DEBUG("{}: vpeAccelerometerBasicTuningRegister.adaptiveTuning changed to {}", nameId(), _vpeAccelerometerBasicTuningRegister.adaptiveTuning);
4645 flow::ApplyChanges();
4646 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4647 {
4648 try
4649 {
4650 _vs.writeVpeAccelerometerBasicTuning(_vpeAccelerometerBasicTuningRegister);
4651 }
4652 catch (const std::exception& e)
4653 {
4654 LOG_ERROR("{}: Could not configure the vpeAccelerometerBasicTuningRegister: {}", nameId(), e.what());
4655 doDeinitialize();
4656 }
4657 }
4658 else
4659 {
4660 doDeinitialize();
4661 }
4662 }
4663
4664 if (ImGui::DragFloat3(fmt::format("Adaptive Filtering Level##{}", size_t(id)).c_str(), _vpeAccelerometerBasicTuningRegister.adaptiveFiltering.c, 0.1F, 0.0F, 10.0F, "%.1f"))
4665 {
4666 LOG_DEBUG("{}: vpeAccelerometerBasicTuningRegister.adaptiveFiltering changed to {}", nameId(), _vpeAccelerometerBasicTuningRegister.adaptiveFiltering);
4667 flow::ApplyChanges();
4668 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4669 {
4670 try
4671 {
4672 _vs.writeVpeAccelerometerBasicTuning(_vpeAccelerometerBasicTuningRegister);
4673 }
4674 catch (const std::exception& e)
4675 {
4676 LOG_ERROR("{}: Could not configure the vpeAccelerometerBasicTuningRegister: {}", nameId(), e.what());
4677 doDeinitialize();
4678 }
4679 }
4680 else
4681 {
4682 doDeinitialize();
4683 }
4684 }
4685
4686 ImGui::TreePop();
4687 }
4688
4689 if (ImGui::TreeNode(fmt::format("VPE Gyro Basic Tuning##{}", size_t(id)).c_str()))
4690 {
4691 ImGui::TextUnformatted("Provides basic control of the adaptive filtering and tuning for the gyroscope.");
4692
4693 if (ImGui::DragFloat3(fmt::format("Base Tuning Level##{}", size_t(id)).c_str(), _vpeGyroBasicTuningRegister.baseTuning.c, 0.1F, 0.0F, 10.0F, "%.1f"))
4694 {
4695 LOG_DEBUG("{}: vpeGyroBasicTuningRegister.baseTuning changed to {}", nameId(), _vpeGyroBasicTuningRegister.baseTuning);
4696 flow::ApplyChanges();
4697 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4698 {
4699 try
4700 {
4701 _vs.writeVpeGyroBasicTuning(_vpeGyroBasicTuningRegister);
4702 }
4703 catch (const std::exception& e)
4704 {
4705 LOG_ERROR("{}: Could not configure the vpeGyroBasicTuningRegister: {}", nameId(), e.what());
4706 doDeinitialize();
4707 }
4708 }
4709 else
4710 {
4711 doDeinitialize();
4712 }
4713 }
4714 ImGui::SameLine();
4715 gui::widgets::HelpMarker("This sets the level of confidence placed in the gyro axes.");
4716
4717 if (ImGui::DragFloat3(fmt::format("Adaptive Tuning Level##{}", size_t(id)).c_str(), _vpeGyroBasicTuningRegister.adaptiveTuning.c, 0.1F, 0.0F, 10.0F, "%.1f"))
4718 {
4719 LOG_DEBUG("{}: vpeGyroBasicTuningRegister.adaptiveTuning changed to {}", nameId(), _vpeGyroBasicTuningRegister.adaptiveTuning);
4720 flow::ApplyChanges();
4721 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4722 {
4723 try
4724 {
4725 _vs.writeVpeGyroBasicTuning(_vpeGyroBasicTuningRegister);
4726 }
4727 catch (const std::exception& e)
4728 {
4729 LOG_ERROR("{}: Could not configure the vpeGyroBasicTuningRegister: {}", nameId(), e.what());
4730 doDeinitialize();
4731 }
4732 }
4733 else
4734 {
4735 doDeinitialize();
4736 }
4737 }
4738
4739 if (ImGui::DragFloat3(fmt::format("Variance - Angular Walk##{}", size_t(id)).c_str(), _vpeGyroBasicTuningRegister.angularWalkVariance.c, 0.1F, 0.0F, 10.0F, "%.1f"))
4740 {
4741 LOG_DEBUG("{}: vpeGyroBasicTuningRegister.angularWalkVariance changed to {}", nameId(), _vpeGyroBasicTuningRegister.angularWalkVariance);
4742 flow::ApplyChanges();
4743 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4744 {
4745 try
4746 {
4747 _vs.writeVpeGyroBasicTuning(_vpeGyroBasicTuningRegister);
4748 }
4749 catch (const std::exception& e)
4750 {
4751 LOG_ERROR("{}: Could not configure the vpeGyroBasicTuningRegister: {}", nameId(), e.what());
4752 doDeinitialize();
4753 }
4754 }
4755 else
4756 {
4757 doDeinitialize();
4758 }
4759 }
4760
4761 ImGui::TreePop();
4762 }
4763
4764 if (ImGui::TreeNode(fmt::format("Filter Startup Gyro Bias##{}", size_t(id)).c_str()))
4765 {
4766 ImGui::TextUnformatted("The filter gyro bias estimate used at startup [rad/s].");
4767
4768 if (ImGui::InputFloat3(fmt::format("## FilterStartupGyroBias {}", size_t(id)).c_str(), _filterStartupGyroBias.c, "%.1f"))
4769 {
4770 LOG_DEBUG("{}: filterStartupGyroBias changed to {}", nameId(), _filterStartupGyroBias);
4771 flow::ApplyChanges();
4772 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4773 {
4774 try
4775 {
4776 _vs.writeFilterStartupGyroBias(_filterStartupGyroBias);
4777 }
4778 catch (const std::exception& e)
4779 {
4780 LOG_ERROR("{}: Could not configure the filterStartupGyroBias: {}", nameId(), e.what());
4781 doDeinitialize();
4782 }
4783 }
4784 else
4785 {
4786 doDeinitialize();
4787 }
4788 }
4789
4790 ImGui::TreePop();
4791 }
4792 }
4793 }
4794
4795 // ###########################################################################################################
4796 // INS SUBSYSTEM
4797 // ###########################################################################################################
4798
4799 if (_sensorModel == VectorNavModel::VN310)
4800 {
4801 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
4802 if (ImGui::CollapsingHeader(fmt::format("INS Subsystem##{}", size_t(id)).c_str()))
4803 {
4804 if (ImGui::TreeNode(fmt::format("INS Basic Configuration##{}", size_t(id)).c_str()))
4805 {
4806 static constexpr std::array<std::pair<vn::protocol::uart::Scenario, const char*>, 3> insBasicConfigurationRegisterVn300Scenarios = {
4807 { { vn::protocol::uart::Scenario::SCENARIO_INSWITHPRESSURE, "General purpose INS with barometric pressure sensor" },
4808 { vn::protocol::uart::Scenario::SCENARIO_INSWITHOUTPRESSURE, "General purpose INS without barometric pressure sensor" },
4809 { vn::protocol::uart::Scenario::SCENARIO_GPSMOVINGBASELINEDYNAMIC, "GNSS moving baseline for dynamic applications" } }
4810 };
4811 if (ImGui::BeginCombo(fmt::format("Scenario##{}", size_t(id)).c_str(), vn::protocol::uart::str(_insBasicConfigurationRegisterVn300.scenario).c_str()))
4812 {
4813 for (const auto& insBasicConfigurationRegisterVn300Scenario : insBasicConfigurationRegisterVn300Scenarios)
4814 {
4815 const bool isSelected = (_insBasicConfigurationRegisterVn300.scenario == insBasicConfigurationRegisterVn300Scenario.first);
4816 if (ImGui::Selectable(vn::protocol::uart::str(insBasicConfigurationRegisterVn300Scenario.first).c_str(), isSelected))
4817 {
4818 _insBasicConfigurationRegisterVn300.scenario = insBasicConfigurationRegisterVn300Scenario.first;
4819 LOG_DEBUG("{}: insBasicConfigurationRegisterVn300.scenario changed to {}", nameId(), vn::protocol::uart::str(_insBasicConfigurationRegisterVn300.scenario));
4820 flow::ApplyChanges();
4821 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4822 {
4823 try
4824 {
4825 _vs.writeInsBasicConfigurationVn300(_insBasicConfigurationRegisterVn300);
4826 }
4827 catch (const std::exception& e)
4828 {
4829 LOG_ERROR("{}: Could not configure the insBasicConfigurationRegisterVn300: {}", nameId(), e.what());
4830 doDeinitialize();
4831 }
4832 }
4833 else
4834 {
4835 doDeinitialize();
4836 }
4837 }
4838 if (ImGui::IsItemHovered())
4839 {
4840 ImGui::BeginTooltip();
4841 ImGui::TextUnformatted(insBasicConfigurationRegisterVn300Scenario.second);
4842 ImGui::EndTooltip();
4843 }
4844
4845 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
4846 {
4847 ImGui::SetItemDefaultFocus();
4848 }
4849 }
4850 ImGui::EndCombo();
4851 }
4852
4853 if (ImGui::Checkbox(fmt::format("Ahrs Aiding##{}", size_t(id)).c_str(), &_insBasicConfigurationRegisterVn300.ahrsAiding))
4854 {
4855 LOG_DEBUG("{}: insBasicConfigurationRegisterVn300.ahrsAiding changed to {}", nameId(), _insBasicConfigurationRegisterVn300.ahrsAiding);
4856 flow::ApplyChanges();
4857 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4858 {
4859 try
4860 {
4861 _vs.writeInsBasicConfigurationVn300(_insBasicConfigurationRegisterVn300);
4862 }
4863 catch (const std::exception& e)
4864 {
4865 LOG_ERROR("{}: Could not configure the insBasicConfigurationRegisterVn300: {}", nameId(), e.what());
4866 doDeinitialize();
4867 }
4868 }
4869 else
4870 {
4871 doDeinitialize();
4872 }
4873 }
4874 ImGui::SameLine();
4875 gui::widgets::HelpMarker("Enables AHRS attitude aiding. AHRS aiding provides "
4876 "the ability to switch to using the magnetometer to "
4877 "stabilize heading during times when the device is "
4878 "stationary and the GNSS compass is not available. "
4879 "AHRS aiding also helps to eliminate large updates in "
4880 "the attitude solution during times when heading is "
4881 "weakly observable, such as at startup.");
4882
4883 if (ImGui::Checkbox(fmt::format("Estimate Baseline##{}", size_t(id)).c_str(), &_insBasicConfigurationRegisterVn300.estBaseline))
4884 {
4885 LOG_DEBUG("{}: insBasicConfigurationRegisterVn300.estBaseline changed to {}", nameId(), _insBasicConfigurationRegisterVn300.estBaseline);
4886 flow::ApplyChanges();
4887 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4888 {
4889 try
4890 {
4891 _vs.writeInsBasicConfigurationVn300(_insBasicConfigurationRegisterVn300);
4892 }
4893 catch (const std::exception& e)
4894 {
4895 LOG_ERROR("{}: Could not configure the insBasicConfigurationRegisterVn300: {}", nameId(), e.what());
4896 doDeinitialize();
4897 }
4898 }
4899 else
4900 {
4901 doDeinitialize();
4902 }
4903 }
4904 ImGui::SameLine();
4905 gui::widgets::HelpMarker("Enables GNSS compass baseline estimation by INS");
4906
4907 ImGui::TreePop();
4908 }
4909
4910 if (ImGui::TreeNode(fmt::format("Startup Filter Bias Estimate##{}", size_t(id)).c_str()))
4911 {
4912 ImGui::TextUnformatted("Sets the initial estimate for the filter bias states");
4913
4914 if (ImGui::InputFloat3(fmt::format("Gyro Bias [rad/s]##{}", size_t(id)).c_str(), _startupFilterBiasEstimateRegister.gyroBias.c, "%.1f"))
4915 {
4916 LOG_DEBUG("{}: startupFilterBiasEstimateRegister.gyroBias changed to {}", nameId(), _startupFilterBiasEstimateRegister.gyroBias);
4917 flow::ApplyChanges();
4918 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4919 {
4920 try
4921 {
4922 _vs.writeStartupFilterBiasEstimate(_startupFilterBiasEstimateRegister);
4923 }
4924 catch (const std::exception& e)
4925 {
4926 LOG_ERROR("{}: Could not configure the startupFilterBiasEstimateRegister: {}", nameId(), e.what());
4927 doDeinitialize();
4928 }
4929 }
4930 else
4931 {
4932 doDeinitialize();
4933 }
4934 }
4935
4936 if (ImGui::InputFloat3(fmt::format("Accel Bias [m/s^2]##{}", size_t(id)).c_str(), _startupFilterBiasEstimateRegister.accelBias.c, "%.1f"))
4937 {
4938 LOG_DEBUG("{}: startupFilterBiasEstimateRegister.accelBias changed to {}", nameId(), _startupFilterBiasEstimateRegister.accelBias);
4939 flow::ApplyChanges();
4940 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4941 {
4942 try
4943 {
4944 _vs.writeStartupFilterBiasEstimate(_startupFilterBiasEstimateRegister);
4945 }
4946 catch (const std::exception& e)
4947 {
4948 LOG_ERROR("{}: Could not configure the startupFilterBiasEstimateRegister: {}", nameId(), e.what());
4949 doDeinitialize();
4950 }
4951 }
4952 else
4953 {
4954 doDeinitialize();
4955 }
4956 }
4957
4958 if (ImGui::InputFloat(fmt::format("Pressure Bias [m]##{}", size_t(id)).c_str(), &_startupFilterBiasEstimateRegister.pressureBias))
4959 {
4960 LOG_DEBUG("{}: startupFilterBiasEstimateRegister.pressureBias changed to {}", nameId(), _startupFilterBiasEstimateRegister.pressureBias);
4961 flow::ApplyChanges();
4962 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
4963 {
4964 try
4965 {
4966 _vs.writeStartupFilterBiasEstimate(_startupFilterBiasEstimateRegister);
4967 }
4968 catch (const std::exception& e)
4969 {
4970 LOG_ERROR("{}: Could not configure the startupFilterBiasEstimateRegister: {}", nameId(), e.what());
4971 doDeinitialize();
4972 }
4973 }
4974 else
4975 {
4976 doDeinitialize();
4977 }
4978 }
4979
4980 ImGui::TreePop();
4981 }
4982 }
4983 }
4984
4985 // ###########################################################################################################
4986 // HARD/SOFT IRON ESTIMATOR SUBSYSTEM
4987 // ###########################################################################################################
4988
4989 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
4990 if (ImGui::CollapsingHeader(fmt::format("Hard/Soft Iron Estimator Subsystem##{}", size_t(id)).c_str()))
4991 {
4992 if (ImGui::TreeNode(fmt::format("Magnetometer Calibration Control##{}", size_t(id)).c_str()))
4993 {
4994 ImGui::TextUnformatted("Controls the magnetometer real-time calibration algorithm.");
4995 if (_sensorModel == VectorNavModel::VN310)
4996 {
4997 ImGui::SameLine();
4998 gui::widgets::HelpMarker("On the PRODUCT the magnetometer is only used to provide a coarse heading estimate at startup "
4999 "and is completely tuned out of the INS filter during normal operation. A Hard/Soft Iron calibration "
5000 "may be performed to try and improve the startup magnetic heading, but is not required for nominal "
5001 "operaiton.",
5002 "(!)");
5003 }
5004
5005 static constexpr std::array<std::pair<vn::protocol::uart::HsiMode, const char*>, 3> magnetometerCalibrationControlHsiModes = {
5006 { { vn::protocol::uart::HsiMode::HSIMODE_OFF, "Real-time hard/soft iron calibration algorithm is turned off" },
5007 { vn::protocol::uart::HsiMode::HSIMODE_RUN, "Runs the real-time hard/soft iron calibration. The algorithm will continue using its existing solution.\n"
5008 "The algorithm can be started and stopped at any time by switching between the HSI_OFF and HSI_RUN state." },
5009 { vn::protocol::uart::HsiMode::HSIMODE_RESET, "Resets the real-time hard/soft iron solution" } }
5010 };
5011 if (ImGui::BeginCombo(fmt::format("HSI Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_magnetometerCalibrationControlRegister.hsiMode).c_str()))
5012 {
5013 for (const auto& magnetometerCalibrationControlHsiMode : magnetometerCalibrationControlHsiModes)
5014 {
5015 const bool isSelected = (_magnetometerCalibrationControlRegister.hsiMode == magnetometerCalibrationControlHsiMode.first);
5016 if (ImGui::Selectable(vn::protocol::uart::str(magnetometerCalibrationControlHsiMode.first).c_str(), isSelected))
5017 {
5018 _magnetometerCalibrationControlRegister.hsiMode = magnetometerCalibrationControlHsiMode.first;
5019 LOG_DEBUG("{}: magnetometerCalibrationControlRegister.hsiMode changed to {}", nameId(), vn::protocol::uart::str(_magnetometerCalibrationControlRegister.hsiMode));
5020 flow::ApplyChanges();
5021 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5022 {
5023 try
5024 {
5025 _vs.writeMagnetometerCalibrationControl(_magnetometerCalibrationControlRegister);
5026 }
5027 catch (const std::exception& e)
5028 {
5029 LOG_ERROR("{}: Could not configure the magnetometerCalibrationControlRegister: {}", nameId(), e.what());
5030 doDeinitialize();
5031 }
5032 }
5033 else
5034 {
5035 doDeinitialize();
5036 }
5037 }
5038 if (ImGui::IsItemHovered())
5039 {
5040 ImGui::BeginTooltip();
5041 ImGui::TextUnformatted(magnetometerCalibrationControlHsiMode.second);
5042 ImGui::EndTooltip();
5043 }
5044
5045 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
5046 {
5047 ImGui::SetItemDefaultFocus();
5048 }
5049 }
5050 ImGui::EndCombo();
5051 }
5052 ImGui::SameLine();
5053 gui::widgets::HelpMarker("Controls the mode of operation for the onboard real-time magnetometer hard/soft iron compensation algorithm.");
5054
5055 static constexpr std::array<std::pair<vn::protocol::uart::HsiOutput, const char*>, 2> magnetometerCalibrationControlHsiOutputs = {
5056 { { vn::protocol::uart::HsiOutput::HSIOUTPUT_NOONBOARD, "Onboard HSI is not applied to the magnetic measurements" },
5057 { vn::protocol::uart::HsiOutput::HSIOUTPUT_USEONBOARD, "Onboard HSI is applied to the magnetic measurements" } }
5058 };
5059 if (ImGui::BeginCombo(fmt::format("HSI Output##{}", size_t(id)).c_str(), vn::protocol::uart::str(_magnetometerCalibrationControlRegister.hsiOutput).c_str()))
5060 {
5061 for (const auto& [magnetometerCalibrationControlHsiOutputMode, magnetometerCalibrationControlHsiOutputDescription] : magnetometerCalibrationControlHsiOutputs)
5062 {
5063 const bool isSelected = (_magnetometerCalibrationControlRegister.hsiOutput == magnetometerCalibrationControlHsiOutputMode);
5064 if (ImGui::Selectable(vn::protocol::uart::str(magnetometerCalibrationControlHsiOutputMode).c_str(), isSelected))
5065 {
5066 _magnetometerCalibrationControlRegister.hsiOutput = magnetometerCalibrationControlHsiOutputMode;
5067 LOG_DEBUG("{}: magnetometerCalibrationControlRegister.hsiOutput changed to {}", nameId(), vn::protocol::uart::str(_magnetometerCalibrationControlRegister.hsiOutput));
5068 flow::ApplyChanges();
5069 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5070 {
5071 try
5072 {
5073 _vs.writeMagnetometerCalibrationControl(_magnetometerCalibrationControlRegister);
5074 }
5075 catch (const std::exception& e)
5076 {
5077 LOG_ERROR("{}: Could not configure the magnetometerCalibrationControlRegister: {}", nameId(), e.what());
5078 doDeinitialize();
5079 }
5080 }
5081 else
5082 {
5083 doDeinitialize();
5084 }
5085 }
5086 if (ImGui::IsItemHovered())
5087 {
5088 ImGui::BeginTooltip();
5089 ImGui::TextUnformatted(magnetometerCalibrationControlHsiOutputDescription);
5090 ImGui::EndTooltip();
5091 }
5092
5093 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
5094 {
5095 ImGui::SetItemDefaultFocus();
5096 }
5097 }
5098 ImGui::EndCombo();
5099 }
5100 ImGui::SameLine();
5101 gui::widgets::HelpMarker("Controls the type of measurements that are provided as "
5102 "outputs from the magnetometer sensor and also "
5103 "subsequently used in the attitude filter.");
5104
5105 int convergeRate = _magnetometerCalibrationControlRegister.convergeRate;
5106 if (ImGui::SliderInt(fmt::format("Converge Rate##{}", size_t(id)).c_str(), &convergeRate, 0, 5))
5107 {
5108 _magnetometerCalibrationControlRegister.convergeRate = static_cast<uint8_t>(convergeRate);
5109 LOG_DEBUG("{}: magnetometerCalibrationControlRegister.convergeRate changed to {}", nameId(), _magnetometerCalibrationControlRegister.convergeRate);
5110 flow::ApplyChanges();
5111 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5112 {
5113 try
5114 {
5115 _vs.writeMagnetometerCalibrationControl(_magnetometerCalibrationControlRegister);
5116 }
5117 catch (const std::exception& e)
5118 {
5119 LOG_ERROR("{}: Could not configure the magnetometerCalibrationControlRegister: {}", nameId(), e.what());
5120 doDeinitialize();
5121 }
5122 }
5123 else
5124 {
5125 doDeinitialize();
5126 }
5127 }
5128 ImGui::SameLine();
5129 gui::widgets::HelpMarker("Controls how quickly the hard/soft iron solution is allowed "
5130 "to converge onto a new solution. The slower the "
5131 "convergence the more accurate the estimate of the "
5132 "hard/soft iron solution. A quicker convergence will provide "
5133 "a less accurate estimate of the hard/soft iron parameters, "
5134 "but for applications where the hard/soft iron changes "
5135 "rapidly may provide a more accurate attitude estimate.\n\n"
5136 "Range: 1 to 5\n"
5137 "1 = Solution converges slowly over approximately 60-90 seconds.\n"
5138 "5 = Solution converges rapidly over approximately 15-20 seconds.");
5139
5140 ImGui::TreePop();
5141 }
5142 }
5143
5144 // ###########################################################################################################
5145 // WORLD MAGNETIC & GRAVITY MODULE
5146 // ###########################################################################################################
5147
5148 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
5149 if (ImGui::CollapsingHeader(fmt::format("World Magnetic & Gravity Module##{}", size_t(id)).c_str()))
5150 {
5151 if (ImGui::TreeNode(fmt::format("Magnetic and Gravity Reference Vectors##{}", size_t(id)).c_str()))
5152 {
5153 ImGui::TextUnformatted("Magnetic and gravity reference vectors");
5154 ImGui::SameLine();
5155 gui::widgets::HelpMarker("This register contains the reference vectors for the magnetic and gravitational fields as used by the "
5156 "onboard filter. The values map to either the user-set values or the results of calculations of the onboard "
5157 "reference models (see the Reference Vector Configuration Register in the IMU subsystem). When the "
5158 "reference values come from the onboard model(s), those values are read-only. When the reference models "
5159 "are disabled, the values reflect the user reference vectors and will be writable. For example, if the onboard "
5160 "World Magnetic Model is enabled and the onboard Gravitational Model is disabled, only the gravity "
5161 "reference values will be modified on a register write. Note that the user reference vectors will not be "
5162 "overwritten by the onboard models, but will retain their previous values for when the onboard models are "
5163 "disabled.");
5164
5165 if (ImGui::InputFloat3(fmt::format("Magnetic Reference [Gauss]##{}", size_t(id)).c_str(), _magneticAndGravityReferenceVectorsRegister.magRef.c, "%.3f"))
5166 {
5167 LOG_DEBUG("{}: magneticAndGravityReferenceVectorsRegister.magRef changed to {}", nameId(), _magneticAndGravityReferenceVectorsRegister.magRef);
5168 flow::ApplyChanges();
5169 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5170 {
5171 try
5172 {
5173 _vs.writeMagneticAndGravityReferenceVectors(_magneticAndGravityReferenceVectorsRegister);
5174 }
5175 catch (const std::exception& e)
5176 {
5177 LOG_ERROR("{}: Could not configure the magneticAndGravityReferenceVectorsRegister: {}", nameId(), e.what());
5178 doDeinitialize();
5179 }
5180 }
5181 else
5182 {
5183 doDeinitialize();
5184 }
5185 }
5186
5187 if (ImGui::InputFloat3(fmt::format("Gravity Reference [m/s^2]##{}", size_t(id)).c_str(), _magneticAndGravityReferenceVectorsRegister.accRef.c, "%.6f"))
5188 {
5189 LOG_DEBUG("{}: magneticAndGravityReferenceVectorsRegister.accRef changed to {}", nameId(), _magneticAndGravityReferenceVectorsRegister.accRef);
5190 flow::ApplyChanges();
5191 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5192 {
5193 try
5194 {
5195 _vs.writeMagneticAndGravityReferenceVectors(_magneticAndGravityReferenceVectorsRegister);
5196 }
5197 catch (const std::exception& e)
5198 {
5199 LOG_ERROR("{}: Could not configure the magneticAndGravityReferenceVectorsRegister: {}", nameId(), e.what());
5200 doDeinitialize();
5201 }
5202 }
5203 else
5204 {
5205 doDeinitialize();
5206 }
5207 }
5208
5209 ImGui::TreePop();
5210 }
5211
5212 if (ImGui::TreeNode(fmt::format("Reference Vector Configuration##{}", size_t(id)).c_str()))
5213 {
5214 ImGui::TextUnformatted("Control register for both the onboard world magnetic and gravity model corrections");
5215 ImGui::SameLine();
5216 gui::widgets::HelpMarker("This register allows configuration of the onboard spherical harmonic models used to calculate the local "
5217 "magnetic and gravitational reference values. Having accurate magnetic reference values improves the "
5218 "accuracy of heading when using the magnetometer and accounts for magnetic declination. Having accurate "
5219 "gravitational reference values improves accuracy by allowing the INS filter to more accurately estimate the "
5220 "accelerometer biases. The VectorNav currently includes the EGM96 gravitational model and the WMM2010 "
5221 "magnetic model. The models are upgradable to allow updating to future models when available.\n\n"
5222 "The magnetic and gravity models can be individually enabled or disabled using the UseMagModel and "
5223 "UseGravityModel parameters, respectively. When disabled, the corresponding values set by the user in the "
5224 "Reference Vector Register in the IMU subsystem will be used instead of values calculated by the onboard "
5225 "model.\n\n"
5226 "The VectorNav starts up with the user configured reference vector values. Shortly after startup (and if the "
5227 "models are enabled), the location and time set in this register will be used to update the reference vectors. "
5228 "When a 3D GNSS fix is available, the location and time reported by the GNSS will be used to update the model. "
5229 "If GNSS is lost, the reference vectors will hold their last valid values. The model values will be recalculated "
5230 "whenever the current position has changed by the RecaclThreshold or the date has changed by more than "
5231 "approximately 8 hours, whichever comes first.");
5232
5233 if (ImGui::Checkbox(fmt::format("Use Mag Model##{}", size_t(id)).c_str(), &_referenceVectorConfigurationRegister.useMagModel))
5234 {
5235 LOG_DEBUG("{}: referenceVectorConfigurationRegister.useMagModel changed to {}", nameId(), _referenceVectorConfigurationRegister.useMagModel);
5236 flow::ApplyChanges();
5237 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5238 {
5239 try
5240 {
5241 _vs.writeReferenceVectorConfiguration(_referenceVectorConfigurationRegister);
5242 }
5243 catch (const std::exception& e)
5244 {
5245 LOG_ERROR("{}: Could not configure the referenceVectorConfigurationRegister: {}", nameId(), e.what());
5246 doDeinitialize();
5247 }
5248 }
5249 else
5250 {
5251 doDeinitialize();
5252 }
5253 }
5254
5255 if (ImGui::Checkbox(fmt::format("Use Gravity Model##{}", size_t(id)).c_str(), &_referenceVectorConfigurationRegister.useGravityModel))
5256 {
5257 LOG_DEBUG("{}: referenceVectorConfigurationRegister.useGravityModel changed to {}", nameId(), _referenceVectorConfigurationRegister.useGravityModel);
5258 flow::ApplyChanges();
5259 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5260 {
5261 try
5262 {
5263 _vs.writeReferenceVectorConfiguration(_referenceVectorConfigurationRegister);
5264 }
5265 catch (const std::exception& e)
5266 {
5267 LOG_ERROR("{}: Could not configure the referenceVectorConfigurationRegister: {}", nameId(), e.what());
5268 doDeinitialize();
5269 }
5270 }
5271 else
5272 {
5273 doDeinitialize();
5274 }
5275 }
5276
5277 auto recalcThreshold = static_cast<int>(_referenceVectorConfigurationRegister.recalcThreshold);
5278 if (ImGui::InputInt(fmt::format("Recalc Threshold [m]##{}", size_t(id)).c_str(), &recalcThreshold))
5279 {
5280 _referenceVectorConfigurationRegister.recalcThreshold = static_cast<uint32_t>(recalcThreshold);
5281 LOG_DEBUG("{}: referenceVectorConfigurationRegister.recalcThreshold changed to {}", nameId(), _referenceVectorConfigurationRegister.recalcThreshold);
5282 flow::ApplyChanges();
5283 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5284 {
5285 try
5286 {
5287 _vs.writeReferenceVectorConfiguration(_referenceVectorConfigurationRegister);
5288 }
5289 catch (const std::exception& e)
5290 {
5291 LOG_ERROR("{}: Could not configure the referenceVectorConfigurationRegister: {}", nameId(), e.what());
5292 doDeinitialize();
5293 }
5294 }
5295 else
5296 {
5297 doDeinitialize();
5298 }
5299 }
5300 ImGui::SameLine();
5301 gui::widgets::HelpMarker("Maximum distance traveled before magnetic and gravity models are recalculated for the new position.");
5302
5303 if (ImGui::InputFloat(fmt::format("Year##{}", size_t(id)).c_str(), &_referenceVectorConfigurationRegister.year))
5304 {
5305 LOG_DEBUG("{}: referenceVectorConfigurationRegister.year changed to {}", nameId(), _referenceVectorConfigurationRegister.year);
5306 flow::ApplyChanges();
5307 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5308 {
5309 try
5310 {
5311 _vs.writeReferenceVectorConfiguration(_referenceVectorConfigurationRegister);
5312 }
5313 catch (const std::exception& e)
5314 {
5315 LOG_ERROR("{}: Could not configure the referenceVectorConfigurationRegister: {}", nameId(), e.what());
5316 doDeinitialize();
5317 }
5318 }
5319 else
5320 {
5321 doDeinitialize();
5322 }
5323 }
5324 ImGui::SameLine();
5325 gui::widgets::HelpMarker("The reference date expressed as a decimal year. Used for both the magnetic and gravity models.");
5326
5327 if (ImGui::InputDouble(fmt::format("Latitude [deg]##{}", size_t(id)).c_str(), &_referenceVectorConfigurationRegister.position[0]))
5328 {
5329 LOG_DEBUG("{}: referenceVectorConfigurationRegister.position.latitude changed to {}", nameId(), _referenceVectorConfigurationRegister.position[0]);
5330 flow::ApplyChanges();
5331 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5332 {
5333 try
5334 {
5335 _vs.writeReferenceVectorConfiguration(_referenceVectorConfigurationRegister);
5336 }
5337 catch (const std::exception& e)
5338 {
5339 LOG_ERROR("{}: Could not configure the referenceVectorConfigurationRegister: {}", nameId(), e.what());
5340 doDeinitialize();
5341 }
5342 }
5343 else
5344 {
5345 doDeinitialize();
5346 }
5347 }
5348 ImGui::SameLine();
5349 gui::widgets::HelpMarker("The reference latitude position in degrees.");
5350
5351 if (ImGui::InputDouble(fmt::format("Longitude [deg]##{}", size_t(id)).c_str(), &_referenceVectorConfigurationRegister.position[1]))
5352 {
5353 LOG_DEBUG("{}: referenceVectorConfigurationRegister.position.longitude changed to {}", nameId(), _referenceVectorConfigurationRegister.position[1]);
5354 flow::ApplyChanges();
5355 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5356 {
5357 try
5358 {
5359 _vs.writeReferenceVectorConfiguration(_referenceVectorConfigurationRegister);
5360 }
5361 catch (const std::exception& e)
5362 {
5363 LOG_ERROR("{}: Could not configure the referenceVectorConfigurationRegister: {}", nameId(), e.what());
5364 doDeinitialize();
5365 }
5366 }
5367 else
5368 {
5369 doDeinitialize();
5370 }
5371 }
5372 ImGui::SameLine();
5373 gui::widgets::HelpMarker("The reference longitude position in degrees.");
5374
5375 if (ImGui::InputDouble(fmt::format("Altitude [m]##{}", size_t(id)).c_str(), &_referenceVectorConfigurationRegister.position[2]))
5376 {
5377 LOG_DEBUG("{}: referenceVectorConfigurationRegister.position.altitude changed to {}", nameId(), _referenceVectorConfigurationRegister.position[2]);
5378 flow::ApplyChanges();
5379 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5380 {
5381 try
5382 {
5383 _vs.writeReferenceVectorConfiguration(_referenceVectorConfigurationRegister);
5384 }
5385 catch (const std::exception& e)
5386 {
5387 LOG_ERROR("{}: Could not configure the referenceVectorConfigurationRegister: {}", nameId(), e.what());
5388 doDeinitialize();
5389 }
5390 }
5391 else
5392 {
5393 doDeinitialize();
5394 }
5395 }
5396 ImGui::SameLine();
5397 gui::widgets::HelpMarker("The reference altitude above the reference ellipsoid in meters.");
5398
5399 ImGui::TreePop();
5400 }
5401 }
5402
5403 // ###########################################################################################################
5404 // VELOCITY AIDING
5405 // ###########################################################################################################
5406
5407 if (_sensorModel == VectorNavModel::VN100_VN110)
5408 {
5409 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
5410 if (ImGui::CollapsingHeader(fmt::format("Velocity Aiding##{}", size_t(id)).c_str()))
5411 {
5412 if (ImGui::TreeNode(fmt::format("Velocity Compensation Control##{}", size_t(id)).c_str()))
5413 {
5414 ImGui::TextUnformatted("Provides control over the velocity compensation feature for the attitude filter.");
5415
5416 static constexpr std::array<vn::protocol::uart::VelocityCompensationMode, 2> velocityCompensationControlModes = {
5417 { vn::protocol::uart::VelocityCompensationMode::VELOCITYCOMPENSATIONMODE_DISABLED,
5418 vn::protocol::uart::VelocityCompensationMode::VELOCITYCOMPENSATIONMODE_BODYMEASUREMENT }
5419 };
5420 if (ImGui::BeginCombo(fmt::format("Mode##{}", size_t(id)).c_str(), vn::protocol::uart::str(_velocityCompensationControlRegister.mode).c_str()))
5421 {
5422 for (const auto& velocityCompensationControlMode : velocityCompensationControlModes)
5423 {
5424 const bool isSelected = (_velocityCompensationControlRegister.mode == velocityCompensationControlMode);
5425 if (ImGui::Selectable(vn::protocol::uart::str(velocityCompensationControlMode).c_str(), isSelected))
5426 {
5427 _velocityCompensationControlRegister.mode = velocityCompensationControlMode;
5428 LOG_DEBUG("{}: velocityCompensationControlRegister.mode changed to {}", nameId(), vn::protocol::uart::str(_velocityCompensationControlRegister.mode));
5429 flow::ApplyChanges();
5430 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5431 {
5432 try
5433 {
5434 _vs.writeVelocityCompensationControl(_velocityCompensationControlRegister);
5435 }
5436 catch (const std::exception& e)
5437 {
5438 LOG_ERROR("{}: Could not configure the velocityCompensationControlRegister: {}", nameId(), e.what());
5439 doDeinitialize();
5440 }
5441 }
5442 else
5443 {
5444 doDeinitialize();
5445 }
5446 }
5447
5448 if (isSelected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
5449 {
5450 ImGui::SetItemDefaultFocus();
5451 }
5452 }
5453 ImGui::EndCombo();
5454 }
5455 ImGui::SameLine();
5456 gui::widgets::HelpMarker("Selects the type of velocity compensation performed by the VPE");
5457
5458 if (ImGui::InputFloat(fmt::format("Velocity Tuning##{}", size_t(id)).c_str(), &_velocityCompensationControlRegister.velocityTuning))
5459 {
5460 LOG_DEBUG("{}: velocityCompensationControlRegister.velocityTuning changed to {}", nameId(), _velocityCompensationControlRegister.velocityTuning);
5461 flow::ApplyChanges();
5462 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5463 {
5464 try
5465 {
5466 _vs.writeVelocityCompensationControl(_velocityCompensationControlRegister);
5467 }
5468 catch (const std::exception& e)
5469 {
5470 LOG_ERROR("{}: Could not configure the velocityCompensationControlRegister: {}", nameId(), e.what());
5471 doDeinitialize();
5472 }
5473 }
5474 else
5475 {
5476 doDeinitialize();
5477 }
5478 }
5479 ImGui::SameLine();
5480 gui::widgets::HelpMarker("Tuning parameter for the velocity measurement");
5481
5482 if (ImGui::InputFloat(fmt::format("Rate Tuning##{}", size_t(id)).c_str(), &_velocityCompensationControlRegister.rateTuning))
5483 {
5484 LOG_DEBUG("{}: velocityCompensationControlRegister.rateTuning changed to {}", nameId(), _velocityCompensationControlRegister.rateTuning);
5485 flow::ApplyChanges();
5486 if (isInitialized() && _vs.isConnected() && _vs.verifySensorConnectivity())
5487 {
5488 try
5489 {
5490 _vs.writeVelocityCompensationControl(_velocityCompensationControlRegister);
5491 }
5492 catch (const std::exception& e)
5493 {
5494 LOG_ERROR("{}: Could not configure the velocityCompensationControlRegister: {}", nameId(), e.what());
5495 doDeinitialize();
5496 }
5497 }
5498 else
5499 {
5500 doDeinitialize();
5501 }
5502 }
5503 ImGui::SameLine();
5504 gui::widgets::HelpMarker("Tuning parameter for the angular rate measurement");
5505
5506 ImGui::TreePop();
5507 }
5508 }
5509 }
5510}
5511
5512[[nodiscard]] json NAV::VectorNavSensor::save() const
5513{
5514 LOG_TRACE("{}: called", nameId());
5515
5516 json j;
5517
5518 j["UartSensor"] = UartSensor::save();
5519 j["sensorModel"] = _sensorModel;
5520
5521 // ###########################################################################################################
5522 // SYSTEM MODULE
5523 // ###########################################################################################################
5524
5525 j["asyncDataOutputType"] = _asyncDataOutputType;
5526 j["asyncDataOutputFrequency"] = _asyncDataOutputFrequency;
5527 j["asciiOutputBufferSize"] = _asciiOutputBufferSize;
5528 j["syncInPin"] = _syncInPin;
5529 j["synchronizationControlRegister"] = _synchronizationControlRegister;
5530 j["communicationProtocolControlRegister"] = _communicationProtocolControlRegister;
5531 j["binaryOutputRegisterMerge"] = _binaryOutputRegisterMerge;
5532 for (size_t b = 0; b < 3; b++)
5533 {
5534 j[fmt::format("binaryOutputRegister{}", b + 1)] = _binaryOutputRegister.at(b);
5535 j[fmt::format("binaryOutputRegister{}-Frequency", b + 1)] = _dividerFrequency.second.at(static_cast<size_t>(_binaryOutputSelectedFrequency.at(b)));
5536 }
5537
5538 // ###########################################################################################################
5539 // IMU SUBSYSTEM
5540 // ###########################################################################################################
5541
5542 j["referenceFrameRotationMatrix"] = _referenceFrameRotationMatrix;
5543 j["imuFilteringConfigurationRegister"] = _imuFilteringConfigurationRegister;
5544 j["deltaThetaAndDeltaVelocityConfigurationRegister"] = _deltaThetaAndDeltaVelocityConfigurationRegister;
5545 j["coupleImuRateOutput"] = _coupleImuRateOutput;
5546
5547 // ###########################################################################################################
5548 // GNSS SUBSYSTEM
5549 // ###########################################################################################################
5550
5551 j["gpsConfigurationRegister"] = _gpsConfigurationRegister;
5552 j["gpsAntennaOffset"] = _gpsAntennaOffset;
5553 j["gpsCompassBaselineRegister"] = _gpsCompassBaselineRegister;
5554
5555 // ###########################################################################################################
5556 // ATTITUDE SUBSYSTEM
5557 // ###########################################################################################################
5558
5559 j["vpeBasicControlRegister"] = _vpeBasicControlRegister;
5560 j["vpeMagnetometerBasicTuningRegister"] = _vpeMagnetometerBasicTuningRegister;
5561 j["vpeAccelerometerBasicTuningRegister"] = _vpeAccelerometerBasicTuningRegister;
5562 j["vpeGyroBasicTuningRegister"] = _vpeGyroBasicTuningRegister;
5563 j["filterStartupGyroBias"] = _filterStartupGyroBias;
5564
5565 // ###########################################################################################################
5566 // INS SUBSYSTEM
5567 // ###########################################################################################################
5568
5569 j["insBasicConfigurationRegisterVn300"] = _insBasicConfigurationRegisterVn300;
5570 j["startupFilterBiasEstimateRegister"] = _startupFilterBiasEstimateRegister;
5571
5572 // ###########################################################################################################
5573 // HARD/SOFT IRON ESTIMATOR SUBSYSTEM
5574 // ###########################################################################################################
5575
5576 j["magnetometerCalibrationControlRegister"] = _magnetometerCalibrationControlRegister;
5577
5578 // ###########################################################################################################
5579 // WORLD MAGNETIC & GRAVITY MODULE
5580 // ###########################################################################################################
5581
5582 j["magneticAndGravityReferenceVectorsRegister"] = _magneticAndGravityReferenceVectorsRegister;
5583 j["referenceVectorConfigurationRegister"] = _referenceVectorConfigurationRegister;
5584
5585 // ###########################################################################################################
5586 // VELOCITY AIDING
5587 // ###########################################################################################################
5588
5589 j["velocityCompensationControlRegister"] = _velocityCompensationControlRegister;
5590
5591 return j;
5592}
5593
5594void NAV::VectorNavSensor::restore(json const& j)
5595{
5596 LOG_TRACE("{}: called", nameId());
5597
5598 if (j.contains("UartSensor"))
5599 {
5600 UartSensor::restore(j.at("UartSensor"));
5601 }
5602 if (j.contains("sensorModel"))
5603 {
5604 j.at("sensorModel").get_to(_sensorModel);
5605 }
5606
5607 // ###########################################################################################################
5608 // SYSTEM MODULE
5609 // ###########################################################################################################
5610
5611 if (j.contains("asyncDataOutputType"))
5612 {
5613 j.at("asyncDataOutputType").get_to(_asyncDataOutputType);
5614 }
5615 if (j.contains("asyncDataOutputFrequency"))
5616 {
5617 j.at("asyncDataOutputFrequency").get_to(_asyncDataOutputFrequency);
5618 _asyncDataOutputFrequencySelected = static_cast<int>(std::ranges::find(_possibleAsyncDataOutputFrequency, static_cast<int>(_asyncDataOutputFrequency))
5619 - _possibleAsyncDataOutputFrequency.begin());
5620 }
5621 if (j.contains("asciiOutputBufferSize"))
5622 {
5623 j.at("asciiOutputBufferSize").get_to(_asciiOutputBufferSize);
5624 _asciiOutputBuffer.resize(static_cast<size_t>(_asciiOutputBufferSize));
5625 }
5626 if (j.contains("syncInPin"))
5627 {
5628 j.at("syncInPin").get_to(_syncInPin);
5629 if (_syncInPin && inputPins.empty())
5630 {
5631 CreateInputPin("SyncIn", Pin::Type::Object, { "TimeSync" });
5632 }
5633 }
5634 if (j.contains("synchronizationControlRegister"))
5635 {
5636 j.at("synchronizationControlRegister").get_to(_synchronizationControlRegister);
5637 if (_synchronizationControlRegister.syncOutMode == vn::protocol::uart::SyncOutMode::SYNCOUTMODE_GPSPPS
5638 && outputPins.size() <= 4)
5639 {
5640 CreateOutputPin("SyncOut", Pin::Type::Object, { "TimeSync" }, &_timeSyncOut);
5641 }
5642 }
5643 if (j.contains("communicationProtocolControlRegister"))
5644 {
5645 j.at("communicationProtocolControlRegister").get_to(_communicationProtocolControlRegister);
5646 }
5647 if (j.contains("binaryOutputRegisterMerge"))
5648 {
5649 j.at("binaryOutputRegisterMerge").get_to(_binaryOutputRegisterMerge);
5650 if (_binaryOutputRegisterMerge == BinaryRegisterMerge::Output1_Output2)
5651 {
5652 outputPins.at(1).type = Pin::Type::Flow;
5653 outputPins.at(2).type = Pin::Type::None;
5654 outputPins.at(3).type = Pin::Type::Flow;
5655 }
5656 else if (_binaryOutputRegisterMerge == BinaryRegisterMerge::Output1_Output3
5657 || _binaryOutputRegisterMerge == BinaryRegisterMerge::Output2_Output3)
5658 {
5659 outputPins.at(1).type = Pin::Type::Flow;
5660 outputPins.at(2).type = Pin::Type::Flow;
5661 outputPins.at(3).type = Pin::Type::None;
5662 }
5663 }
5664 for (size_t b = 0; b < 3; b++)
5665 {
5666 if (j.contains(fmt::format("binaryOutputRegister{}", b + 1)))
5667 {
5668 j.at(fmt::format("binaryOutputRegister{}", b + 1)).get_to(_binaryOutputRegister.at(b));
5669 }
5670 if (j.contains(fmt::format("binaryOutputRegister{}-Frequency", b + 1)))
5671 {
5672 std::string frequency;
5673 j.at(fmt::format("binaryOutputRegister{}-Frequency", b + 1)).get_to(frequency);
5674 for (size_t i = 0; i < _dividerFrequency.second.size(); i++)
5675 {
5676 if (_dividerFrequency.second.at(i) == frequency)
5677 {
5678 _binaryOutputSelectedFrequency.at(b) = i;
5679 break;
5680 }
5681 }
5682 }
5683 }
5684
5685 // ###########################################################################################################
5686 // IMU SUBSYSTEM
5687 // ###########################################################################################################
5688
5689 if (j.contains("referenceFrameRotationMatrix"))
5690 {
5691 j.at("referenceFrameRotationMatrix").get_to(_referenceFrameRotationMatrix);
5692 }
5693 if (j.contains("imuFilteringConfigurationRegister"))
5694 {
5695 j.at("imuFilteringConfigurationRegister").get_to(_imuFilteringConfigurationRegister);
5696 }
5697 if (j.contains("deltaThetaAndDeltaVelocityConfigurationRegister"))
5698 {
5699 j.at("deltaThetaAndDeltaVelocityConfigurationRegister").get_to(_deltaThetaAndDeltaVelocityConfigurationRegister);
5700 }
5701 if (j.contains("coupleImuRateOutput"))
5702 {
5703 j.at("coupleImuRateOutput").get_to(_coupleImuRateOutput);
5704 }
5705
5706 // ###########################################################################################################
5707 // GNSS SUBSYSTEM
5708 // ###########################################################################################################
5709
5710 if (j.contains("gpsConfigurationRegister"))
5711 {
5712 j.at("gpsConfigurationRegister").get_to(_gpsConfigurationRegister);
5713 }
5714 if (j.contains("gpsAntennaOffset"))
5715 {
5716 j.at("gpsAntennaOffset").get_to(_gpsAntennaOffset);
5717 }
5718 if (j.contains("gpsCompassBaselineRegister"))
5719 {
5720 j.at("gpsCompassBaselineRegister").get_to(_gpsCompassBaselineRegister);
5721 }
5722
5723 // ###########################################################################################################
5724 // ATTITUDE SUBSYSTEM
5725 // ###########################################################################################################
5726
5727 if (j.contains("vpeBasicControlRegister"))
5728 {
5729 j.at("vpeBasicControlRegister").get_to(_vpeBasicControlRegister);
5730 }
5731 if (j.contains("vpeMagnetometerBasicTuningRegister"))
5732 {
5733 j.at("vpeMagnetometerBasicTuningRegister").get_to(_vpeMagnetometerBasicTuningRegister);
5734 }
5735 if (j.contains("vpeAccelerometerBasicTuningRegister"))
5736 {
5737 j.at("vpeAccelerometerBasicTuningRegister").get_to(_vpeAccelerometerBasicTuningRegister);
5738 }
5739 if (j.contains("vpeGyroBasicTuningRegister"))
5740 {
5741 j.at("vpeGyroBasicTuningRegister").get_to(_vpeGyroBasicTuningRegister);
5742 }
5743 if (j.contains("filterStartupGyroBias"))
5744 {
5745 j.at("filterStartupGyroBias").get_to(_filterStartupGyroBias);
5746 }
5747
5748 // ###########################################################################################################
5749 // INS SUBSYSTEM
5750 // ###########################################################################################################
5751
5752 if (j.contains("insBasicConfigurationRegisterVn300"))
5753 {
5754 j.at("insBasicConfigurationRegisterVn300").get_to(_insBasicConfigurationRegisterVn300);
5755 }
5756 if (j.contains("startupFilterBiasEstimateRegister"))
5757 {
5758 j.at("startupFilterBiasEstimateRegister").get_to(_startupFilterBiasEstimateRegister);
5759 }
5760
5761 // ###########################################################################################################
5762 // HARD/SOFT IRON ESTIMATOR SUBSYSTEM
5763 // ###########################################################################################################
5764
5765 if (j.contains("magnetometerCalibrationControlRegister"))
5766 {
5767 j.at("magnetometerCalibrationControlRegister").get_to(_magnetometerCalibrationControlRegister);
5768 }
5769
5770 // ###########################################################################################################
5771 // WORLD MAGNETIC & GRAVITY MODULE
5772 // ###########################################################################################################
5773
5774 if (j.contains("magneticAndGravityReferenceVectorsRegister"))
5775 {
5776 j.at("magneticAndGravityReferenceVectorsRegister").get_to(_magneticAndGravityReferenceVectorsRegister);
5777 }
5778 if (j.contains("referenceVectorConfigurationRegister"))
5779 {
5780 j.at("referenceVectorConfigurationRegister").get_to(_referenceVectorConfigurationRegister);
5781 }
5782
5783 // ###########################################################################################################
5784 // VELOCITY AIDING
5785 // ###########################################################################################################
5786
5787 if (j.contains("velocityCompensationControlRegister"))
5788 {
5789 j.at("velocityCompensationControlRegister").get_to(_velocityCompensationControlRegister);
5790 }
5791}
5792
5793bool NAV::VectorNavSensor::resetNode()
5794{
5795 std::ranges::for_each(_lastMessageTime, [](InsTime& insTime) { insTime.reset(); });
5796 std::ranges::for_each(_lastMessageTimeSinceStartup, [](uint64_t& value) { value = 0; });
5797 _gnssTimeCounter = {};
5798
5799 _lastSyncInCnt = 0;
5800 _lastSyncOutCnt = 0;
5801 _syncCntOffset = 0;
5802
5803 _timeSyncOut.ppsTime.reset();
5804 _timeSyncOut.syncOutCnt = 0;
5805 _binaryOutputRegisterMergeObservation = nullptr;
5806
5807 return true;
5808}
5809
5810bool NAV::VectorNavSensor::initialize()
5811{
5812 LOG_TRACE("{}: called", nameId());
5813
5814 // ###########################################################################################################
5815 // Connecting
5816 // ###########################################################################################################
5817
5818 // Choose baudrate
5819 Baudrate targetBaudrate = sensorBaudrate() == BAUDRATE_FASTEST
5820 ? static_cast<Baudrate>(vn::sensors::VnSensor::supportedBaudrates()[vn::sensors::VnSensor::supportedBaudrates().size() - 1])
5821 : sensorBaudrate();
5822
5823 Baudrate connectedBaudrate{};
5824 _connectedSensorPort.clear();
5825
5826 // Search for the VectorNav Sensor
5827 size_t maxTries = std::filesystem::exists(_sensorPort) ? 5 : 0;
5828 for (size_t i = 0; i < maxTries; ++i)
5829 {
5830 if (int32_t foundBaudrate = 0;
5831 vn::sensors::Searcher::search(_sensorPort, &foundBaudrate))
5832 {
5833 // Sensor was found at specified port with the baudrate 'foundBaudrate'
5834 connectedBaudrate = static_cast<Baudrate>(foundBaudrate);
5835 _connectedSensorPort = _sensorPort;
5836 LOG_DEBUG("{}: Found VectorNav sensor on specified port '{}' with baudrate {} (it took {} attempts to connect)",
5837 nameId(), _connectedSensorPort, foundBaudrate, i + 1);
5838 break;
5839 }
5840 }
5841 if (_connectedSensorPort.empty())
5842 {
5843 if (std::filesystem::exists(_sensorPort))
5844 {
5845 LOG_ERROR("{}: Could not connect to VectorNav sensor on specified port '{}', but port exists.", nameId(), _sensorPort);
5846 return false;
5847 }
5848
5849 for (size_t i = 0; i < 5; ++i)
5850 {
5851 if (!_connectedSensorPort.empty())
5852 {
5853 break;
5854 }
5855
5856 if (std::vector<std::pair<std::string, uint32_t>> foundSensors = vn::sensors::Searcher::search();
5857 !foundSensors.empty())
5858 {
5859 for (const auto& [port, baudrate] : foundSensors)
5860 {
5861 _vs.connect(port, baudrate);
5862 std::string modelNumber = _vs.readModelNumber();
5863 _vs.disconnect();
5864
5865 LOG_DEBUG("{}: Found VectorNav sensor '{}' on port '{}' with baudrate {} ({} VN sensors connected in total)",
5866 nameId(), modelNumber, port, baudrate, foundSensors.size());
5867
5868 // Identify the sensor by its model number
5869 const std::string searchString = _sensorModel == VectorNavModel::VN100_VN110 ? "VN-1" : "VN-3";
5870 if (modelNumber.find(searchString) != std::string::npos)
5871 {
5872 _connectedSensorPort = port;
5873 connectedBaudrate = static_cast<Baudrate>(baudrate);
5874
5875 LOG_DEBUG("{}: Selected VectorNav sensor '{}' on port '{}' (it took {} attempts to connect)",
5876 nameId(), modelNumber, port, i + 1);
5877 break;
5878 }
5879 }
5880 }
5881 }
5882 if (_connectedSensorPort.empty())
5883 {
5884 LOG_ERROR("{}: Could not connect. Is the sensor connected and do you have read permissions?", nameId());
5885 return false;
5886 }
5887 }
5888
5889 try
5890 {
5891 // Connect to the sensor (vs.verifySensorConnectivity does not have to be called as sensor is already tested)
5892 _vs.connect(_connectedSensorPort, connectedBaudrate);
5893 }
5894 catch (const std::exception& e)
5895 {
5896 LOG_ERROR("{}: Failed to connect to sensor on port '{}' with baudrate {} with error: {}", nameId(),
5897 _connectedSensorPort, connectedBaudrate, e.what());
5898 return false;
5899 }
5900
5901 try
5902 {
5903 if (!_vs.verifySensorConnectivity())
5904 {
5905 LOG_ERROR("{}: Connected to sensor on port '{}' with baudrate {} but sensor does not answer", nameId(),
5906 _connectedSensorPort, connectedBaudrate);
5907 return false;
5908 }
5909 }
5910 catch (const std::exception& e)
5911 {
5912 LOG_ERROR("{}: Connected to sensor on port '{}' with baudrate {} but sensor threw an exception: {}", nameId(),
5913 _connectedSensorPort, connectedBaudrate, e.what());
5914 return false;
5915 }
5916
5917 // Query the sensor's model number
5918 auto modelNumber = _vs.readModelNumber();
5919 LOG_DEBUG("{}: {} connected on port '{}' with baudrate {}", nameId(), _vs.readModelNumber(), _connectedSensorPort, connectedBaudrate);
5920
5921 if (_sensorModel == VectorNavModel::VN100_VN110 && (!modelNumber.starts_with("VN-100") && !modelNumber.starts_with("VN-110")))
5922 {
5923 LOG_ERROR("{}: VN100/VN110 configured, but sensor identifies as {}", nameId(), modelNumber);
5924 doDeinitialize();
5925 return false;
5926 }
5927 if (_sensorModel == VectorNavModel::VN310 && !modelNumber.starts_with("VN-310"))
5928 {
5929 LOG_ERROR("{}: VN310 configured, but sensor identifies as {}", nameId(), modelNumber);
5930 doDeinitialize();
5931 return false;
5932 }
5933
5934 // ###########################################################################################################
5935 // SYSTEM MODULE
5936 // ###########################################################################################################
5937
5938 // Change Connection Baudrate
5939 if (targetBaudrate != connectedBaudrate)
5940 {
5941 auto suppBaud = vn::sensors::VnSensor::supportedBaudrates();
5942 if (std::ranges::find(suppBaud, targetBaudrate) != suppBaud.end())
5943 {
5944 _vs.changeBaudRate(targetBaudrate);
5945 LOG_DEBUG("{}: Baudrate changed to {}", nameId(), static_cast<size_t>(targetBaudrate));
5946 }
5947 else
5948 {
5949 LOG_ERROR("{}: Does not support baudrate {}", nameId(), static_cast<size_t>(targetBaudrate));
5950 return false;
5951 }
5952 }
5953 if (_vs.readSerialBaudRate() != targetBaudrate)
5954 {
5955 LOG_ERROR("{}: Changing the baudrate from {} to {} was not successfull", nameId(), _vs.readSerialBaudRate(), targetBaudrate);
5956 doDeinitialize();
5957 return false;
5958 }
5959
5960 _vs.writeSynchronizationControl(_synchronizationControlRegister);
5961 if (auto vnSynchronizationControlRegister = _vs.readSynchronizationControl();
5962 vnSynchronizationControlRegister.syncInMode != _synchronizationControlRegister.syncInMode
5963 || vnSynchronizationControlRegister.syncInEdge != _synchronizationControlRegister.syncInEdge
5964 || vnSynchronizationControlRegister.syncInSkipFactor != _synchronizationControlRegister.syncInSkipFactor
5965 || vnSynchronizationControlRegister.syncOutMode != _synchronizationControlRegister.syncOutMode
5966 || vnSynchronizationControlRegister.syncOutPolarity != _synchronizationControlRegister.syncOutPolarity
5967 || vnSynchronizationControlRegister.syncOutSkipFactor != _synchronizationControlRegister.syncOutSkipFactor
5968 || vnSynchronizationControlRegister.syncOutPulseWidth != _synchronizationControlRegister.syncOutPulseWidth)
5969 {
5970 LOG_ERROR("{}: Writing the synchronizationControlRegister was not successfull.\n"
5971 "Target: syncInMode = {}, syncInEdge = {}, syncInSkipFactor = {}, syncOutMode = {}, syncOutPolarity = {}, syncOutSkipFactor = {}, syncOutPulseWidth = {}\n"
5972 "Sensor: syncInMode = {}, syncInEdge = {}, syncInSkipFactor = {}, syncOutMode = {}, syncOutPolarity = {}, syncOutSkipFactor = {}, syncOutPulseWidth = {}",
5973 nameId(),
5974 _synchronizationControlRegister.syncInMode, _synchronizationControlRegister.syncInEdge, _synchronizationControlRegister.syncInSkipFactor, _synchronizationControlRegister.syncOutMode, _synchronizationControlRegister.syncOutPolarity, _synchronizationControlRegister.syncOutSkipFactor, _synchronizationControlRegister.syncOutPulseWidth,
5975 vnSynchronizationControlRegister.syncInMode, vnSynchronizationControlRegister.syncInEdge, vnSynchronizationControlRegister.syncInSkipFactor, vnSynchronizationControlRegister.syncOutMode, vnSynchronizationControlRegister.syncOutPolarity, vnSynchronizationControlRegister.syncOutSkipFactor, vnSynchronizationControlRegister.syncOutPulseWidth);
5976 doDeinitialize();
5977 return false;
5978 }
5979
5980 _vs.writeCommunicationProtocolControl(_communicationProtocolControlRegister);
5981 if (auto vnCommunicationProtocolControlRegister = _vs.readCommunicationProtocolControl();
5982 vnCommunicationProtocolControlRegister.serialCount != _communicationProtocolControlRegister.serialCount
5983 || vnCommunicationProtocolControlRegister.serialStatus != _communicationProtocolControlRegister.serialStatus
5984 || vnCommunicationProtocolControlRegister.spiCount != _communicationProtocolControlRegister.spiCount
5985 || vnCommunicationProtocolControlRegister.spiStatus != _communicationProtocolControlRegister.spiStatus
5986 || vnCommunicationProtocolControlRegister.serialChecksum != _communicationProtocolControlRegister.serialChecksum
5987 || vnCommunicationProtocolControlRegister.spiChecksum != _communicationProtocolControlRegister.spiChecksum
5988 || vnCommunicationProtocolControlRegister.errorMode != _communicationProtocolControlRegister.errorMode)
5989 {
5990 LOG_ERROR("{}: Writing the communicationProtocolControlRegister was not successfull.\n"
5991 "Target: serialCount = {}, serialStatus = {}, spiCount = {}, spiStatus = {}, serialChecksum = {}, spiChecksum = {}, errorMode = {}\n"
5992 "Sensor: serialCount = {}, serialStatus = {}, spiCount = {}, spiStatus = {}, serialChecksum = {}, spiChecksum = {}, errorMode = {}",
5993 nameId(),
5994 _communicationProtocolControlRegister.serialCount, _communicationProtocolControlRegister.serialStatus, _communicationProtocolControlRegister.spiCount, _communicationProtocolControlRegister.spiStatus, _communicationProtocolControlRegister.serialChecksum, _communicationProtocolControlRegister.spiChecksum, _communicationProtocolControlRegister.errorMode,
5995 vnCommunicationProtocolControlRegister.serialCount, vnCommunicationProtocolControlRegister.serialStatus, vnCommunicationProtocolControlRegister.spiCount, vnCommunicationProtocolControlRegister.spiStatus, vnCommunicationProtocolControlRegister.serialChecksum, vnCommunicationProtocolControlRegister.spiChecksum, vnCommunicationProtocolControlRegister.errorMode);
5996 doDeinitialize();
5997 return false;
5998 }
5999
6000 // _vs.writeSynchronizationStatus(vn::sensors::SynchronizationStatusRegister); // User manual VN-310 - 8.3.1 (p 105) / VN-100 - 5.3.1 (p 76)
6001
6002 // ###########################################################################################################
6003 // IMU SUBSYSTEM
6004 // ###########################################################################################################
6005
6006 // _vs.writeMagnetometerCompensation(vn::sensors::MagnetometerCompensationRegister()); // User manual VN-310 - 9.2.1 (p 111) / VN-100 - 6.2.1 (p 82)
6007 // _vs.writeAccelerationCompensation(vn::sensors::AccelerationCompensationRegister()); // User manual VN-310 - 9.2.2 (p 112) / VN-100 - 6.2.2 (p 83)
6008 // _vs.writeGyroCompensation(vn::sensors::GyroCompensationRegister()); // User manual VN-310 - 9.2.3 (p 113) / VN-100 - 6.2.3 (p 84)
6009
6010 _vs.writeReferenceFrameRotation(_referenceFrameRotationMatrix);
6011 if (auto vnReferenceFrameRotationMatrix = _vs.readReferenceFrameRotation();
6012 vnReferenceFrameRotationMatrix != _referenceFrameRotationMatrix)
6013 {
6014 LOG_ERROR("{}: Writing the referenceFrameRotationMatrix was not successfull.\n"
6015 "Target: DCM = {}\n"
6016 "Sensor: DCM = {}",
6017 nameId(), _referenceFrameRotationMatrix, vnReferenceFrameRotationMatrix);
6018 doDeinitialize();
6019 return false;
6020 }
6021
6022 _vs.writeImuFilteringConfiguration(_imuFilteringConfigurationRegister);
6023 if (auto vnImuFilteringConfigurationRegister = _vs.readImuFilteringConfiguration();
6024 vnImuFilteringConfigurationRegister.magWindowSize != _imuFilteringConfigurationRegister.magWindowSize
6025 || vnImuFilteringConfigurationRegister.accelWindowSize != _imuFilteringConfigurationRegister.accelWindowSize
6026 || vnImuFilteringConfigurationRegister.gyroWindowSize != _imuFilteringConfigurationRegister.gyroWindowSize
6027 || vnImuFilteringConfigurationRegister.tempWindowSize != _imuFilteringConfigurationRegister.tempWindowSize
6028 || vnImuFilteringConfigurationRegister.presWindowSize != _imuFilteringConfigurationRegister.presWindowSize
6029 || vnImuFilteringConfigurationRegister.magFilterMode != _imuFilteringConfigurationRegister.magFilterMode
6030 || vnImuFilteringConfigurationRegister.accelFilterMode != _imuFilteringConfigurationRegister.accelFilterMode
6031 || vnImuFilteringConfigurationRegister.gyroFilterMode != _imuFilteringConfigurationRegister.gyroFilterMode
6032 || vnImuFilteringConfigurationRegister.tempFilterMode != _imuFilteringConfigurationRegister.tempFilterMode
6033 || vnImuFilteringConfigurationRegister.presFilterMode != _imuFilteringConfigurationRegister.presFilterMode)
6034 {
6035 LOG_ERROR("{}: Writing the imuFilteringConfigurationRegister was not successfull.\n"
6036 "Target: magWindowSize = {}, accelWindowSize = {}, gyroWindowSize = {}, tempWindowSize = {}, presWindowSize = {}, magFilterMode = {}, accelFilterMode = {}, gyroFilterMode = {}, tempFilterMode = {}, presFilterMode = {}\n"
6037 "Sensor: magWindowSize = {}, accelWindowSize = {}, gyroWindowSize = {}, tempWindowSize = {}, presWindowSize = {}, magFilterMode = {}, accelFilterMode = {}, gyroFilterMode = {}, tempFilterMode = {}, presFilterMode = {}",
6038 nameId(),
6039 _imuFilteringConfigurationRegister.magWindowSize, _imuFilteringConfigurationRegister.accelWindowSize, _imuFilteringConfigurationRegister.gyroWindowSize, _imuFilteringConfigurationRegister.tempWindowSize, _imuFilteringConfigurationRegister.presWindowSize, _imuFilteringConfigurationRegister.magFilterMode, _imuFilteringConfigurationRegister.accelFilterMode, _imuFilteringConfigurationRegister.gyroFilterMode, _imuFilteringConfigurationRegister.tempFilterMode, _imuFilteringConfigurationRegister.presFilterMode,
6040 vnImuFilteringConfigurationRegister.magWindowSize, vnImuFilteringConfigurationRegister.accelWindowSize, vnImuFilteringConfigurationRegister.gyroWindowSize, vnImuFilteringConfigurationRegister.tempWindowSize, vnImuFilteringConfigurationRegister.presWindowSize, vnImuFilteringConfigurationRegister.magFilterMode, vnImuFilteringConfigurationRegister.accelFilterMode, vnImuFilteringConfigurationRegister.gyroFilterMode, vnImuFilteringConfigurationRegister.tempFilterMode, vnImuFilteringConfigurationRegister.presFilterMode);
6041 doDeinitialize();
6042 return false;
6043 }
6044
6045 _vs.writeDeltaThetaAndDeltaVelocityConfiguration(_deltaThetaAndDeltaVelocityConfigurationRegister);
6046 if (auto vnDeltaThetaAndDeltaVelocityConfigurationRegister = _vs.readDeltaThetaAndDeltaVelocityConfiguration();
6047 vnDeltaThetaAndDeltaVelocityConfigurationRegister.integrationFrame != _deltaThetaAndDeltaVelocityConfigurationRegister.integrationFrame
6048 || vnDeltaThetaAndDeltaVelocityConfigurationRegister.gyroCompensation != _deltaThetaAndDeltaVelocityConfigurationRegister.gyroCompensation
6049 || vnDeltaThetaAndDeltaVelocityConfigurationRegister.accelCompensation != _deltaThetaAndDeltaVelocityConfigurationRegister.accelCompensation
6050 || vnDeltaThetaAndDeltaVelocityConfigurationRegister.earthRateCorrection != _deltaThetaAndDeltaVelocityConfigurationRegister.earthRateCorrection)
6051 {
6052 LOG_ERROR("{}: Writing the deltaThetaAndDeltaVelocityConfigurationRegister was not successfull.\n"
6053 "Target: integrationFrame = {}, gyroCompensation = {}, accelCompensation = {}, earthRateCorrection = {}\n"
6054 "Sensor: integrationFrame = {}, gyroCompensation = {}, accelCompensation = {}, earthRateCorrection = {}",
6055 nameId(),
6056 _deltaThetaAndDeltaVelocityConfigurationRegister.integrationFrame, _deltaThetaAndDeltaVelocityConfigurationRegister.gyroCompensation, _deltaThetaAndDeltaVelocityConfigurationRegister.accelCompensation, _deltaThetaAndDeltaVelocityConfigurationRegister.earthRateCorrection,
6057 vnDeltaThetaAndDeltaVelocityConfigurationRegister.integrationFrame, vnDeltaThetaAndDeltaVelocityConfigurationRegister.gyroCompensation, vnDeltaThetaAndDeltaVelocityConfigurationRegister.accelCompensation, vnDeltaThetaAndDeltaVelocityConfigurationRegister.earthRateCorrection);
6058 doDeinitialize();
6059 return false;
6060 }
6061
6062 // ###########################################################################################################
6063 // GNSS SUBSYSTEM
6064 // ###########################################################################################################
6065
6066 if (_sensorModel == VectorNavModel::VN310)
6067 {
6068 _vs.writeGpsConfiguration(_gpsConfigurationRegister);
6069 if (auto vnGpsConfigurationRegister = _vs.readGpsConfiguration();
6070 vnGpsConfigurationRegister.mode != _gpsConfigurationRegister.mode
6071 || vnGpsConfigurationRegister.ppsSource != _gpsConfigurationRegister.ppsSource
6072 || vnGpsConfigurationRegister.rate != _gpsConfigurationRegister.rate
6073 || vnGpsConfigurationRegister.antPow != _gpsConfigurationRegister.antPow)
6074 {
6075 LOG_ERROR("{}: Writing the gpsConfigurationRegister was not successfull.\n"
6076 "Target: mode = {}, ppsSource = {}, rate = {}, antPow = {}\n"
6077 "Sensor: mode = {}, ppsSource = {}, rate = {}, antPow = {}",
6078 nameId(),
6079 _gpsConfigurationRegister.mode, _gpsConfigurationRegister.ppsSource, _gpsConfigurationRegister.rate, _gpsConfigurationRegister.antPow,
6080 vnGpsConfigurationRegister.mode, vnGpsConfigurationRegister.ppsSource, vnGpsConfigurationRegister.rate, vnGpsConfigurationRegister.antPow);
6081 doDeinitialize();
6082 return false;
6083 }
6084
6085 _vs.writeGpsAntennaOffset(_gpsAntennaOffset);
6086 if (auto vnGpsAntennaOffset = _vs.readGpsAntennaOffset();
6087 vnGpsAntennaOffset != _gpsAntennaOffset)
6088 {
6089 LOG_ERROR("{}: Writing the gpsAntennaOffset was not successfull.\n"
6090 "Target: {}\n"
6091 "Sensor: {}",
6092 nameId(),
6093 _gpsAntennaOffset,
6094 vnGpsAntennaOffset);
6095 doDeinitialize();
6096 return false;
6097 }
6098
6099 _vs.writeGpsCompassBaseline(_gpsCompassBaselineRegister);
6100 if (auto vnGpsCompassBaselineRegister = _vs.readGpsCompassBaseline();
6101 vnGpsCompassBaselineRegister.position != _gpsCompassBaselineRegister.position
6102 || vnGpsCompassBaselineRegister.uncertainty != _gpsCompassBaselineRegister.uncertainty)
6103 {
6104 LOG_ERROR("{}: Writing the gpsCompassBaselineRegister was not successfull.\n"
6105 "Target: position = {}, uncertainty = {}\n"
6106 "Sensor: position = {}, uncertainty = {}",
6107 nameId(),
6108 _gpsCompassBaselineRegister.position, _gpsCompassBaselineRegister.uncertainty,
6109 vnGpsCompassBaselineRegister.position, vnGpsCompassBaselineRegister.uncertainty);
6110 doDeinitialize();
6111 return false;
6112 }
6113 }
6114
6115 // ###########################################################################################################
6116 // ATTITUDE SUBSYSTEM
6117 // ###########################################################################################################
6118 if (_sensorModel == VectorNavModel::VN100_VN110)
6119 {
6120 // _vs.tare(); // User manual VN-100 - 7.1.1 (p 92)
6121 // _vs.magneticDisturbancePresent(bool); // User manual VN-100 - 7.1.2 (p 92)
6122 // _vs.accelerationDisturbancePresent(true); // User manual VN-100 - 7.1.3 (p 92f)
6123 }
6124 // _vs.setGyroBias(); // User manual VN-310 - 11.1.1 (p 148) / VN-100 - 7.1.4 (p 93)
6125
6126 // TODO: Implement in vnproglib: _vs.setInitialHeading() - User manual VN-310 - 11.1.2 (p 148)
6127
6128 _vs.writeVpeBasicControl(_vpeBasicControlRegister);
6129 if (auto vnVpeBasicControlRegister = _vs.readVpeBasicControl();
6130 vnVpeBasicControlRegister.enable != _vpeBasicControlRegister.enable
6131 || vnVpeBasicControlRegister.headingMode != _vpeBasicControlRegister.headingMode
6132 || vnVpeBasicControlRegister.filteringMode != _vpeBasicControlRegister.filteringMode
6133 || vnVpeBasicControlRegister.tuningMode != _vpeBasicControlRegister.tuningMode)
6134 {
6135 LOG_ERROR("{}: Writing the vpeBasicControlRegister was not successfull.\n"
6136 "Target: enable = {}, headingMode = {}, filteringMode = {}, tuningMode = {}\n"
6137 "Sensor: enable = {}, headingMode = {}, filteringMode = {}, tuningMode = {}",
6138 nameId(),
6139 _vpeBasicControlRegister.enable, _vpeBasicControlRegister.headingMode, _vpeBasicControlRegister.filteringMode, _vpeBasicControlRegister.tuningMode,
6140 vnVpeBasicControlRegister.enable, vnVpeBasicControlRegister.headingMode, vnVpeBasicControlRegister.filteringMode, vnVpeBasicControlRegister.tuningMode);
6141 doDeinitialize();
6142 return false;
6143 }
6144
6145 if (_sensorModel == VectorNavModel::VN100_VN110)
6146 {
6147 _vs.writeVpeMagnetometerBasicTuning(_vpeMagnetometerBasicTuningRegister);
6148 if (auto vnVpeMagnetometerBasicTuningRegister = _vs.readVpeMagnetometerBasicTuning();
6149 vnVpeMagnetometerBasicTuningRegister.baseTuning != _vpeMagnetometerBasicTuningRegister.baseTuning
6150 || vnVpeMagnetometerBasicTuningRegister.adaptiveTuning != _vpeMagnetometerBasicTuningRegister.adaptiveTuning
6151 || vnVpeMagnetometerBasicTuningRegister.adaptiveFiltering != _vpeMagnetometerBasicTuningRegister.adaptiveFiltering)
6152 {
6153 LOG_ERROR("{}: Writing the vpeMagnetometerBasicTuningRegister was not successfull.\n"
6154 "Target: baseTuning = {}, adaptiveTuning = {}, adaptiveFiltering = {}\n"
6155 "Sensor: baseTuning = {}, adaptiveTuning = {}, adaptiveFiltering = {}",
6156 nameId(),
6157 _vpeMagnetometerBasicTuningRegister.baseTuning, _vpeMagnetometerBasicTuningRegister.adaptiveTuning, _vpeMagnetometerBasicTuningRegister.adaptiveFiltering,
6158 vnVpeMagnetometerBasicTuningRegister.baseTuning, vnVpeMagnetometerBasicTuningRegister.adaptiveTuning, vnVpeMagnetometerBasicTuningRegister.adaptiveFiltering);
6159 doDeinitialize();
6160 return false;
6161 }
6162
6163 _vs.writeVpeAccelerometerBasicTuning(_vpeAccelerometerBasicTuningRegister);
6164 if (auto vnVpeAccelerometerBasicTuningRegister = _vs.readVpeAccelerometerBasicTuning();
6165 vnVpeAccelerometerBasicTuningRegister.baseTuning != _vpeAccelerometerBasicTuningRegister.baseTuning
6166 || vnVpeAccelerometerBasicTuningRegister.adaptiveTuning != _vpeAccelerometerBasicTuningRegister.adaptiveTuning
6167 || vnVpeAccelerometerBasicTuningRegister.adaptiveFiltering != _vpeAccelerometerBasicTuningRegister.adaptiveFiltering)
6168 {
6169 LOG_ERROR("{}: Writing the vpeAccelerometerBasicTuningRegister was not successfull.\n"
6170 "Target: baseTuning = {}, adaptiveTuning = {}, adaptiveFiltering = {}\n"
6171 "Sensor: baseTuning = {}, adaptiveTuning = {}, adaptiveFiltering = {}",
6172 nameId(),
6173 _vpeAccelerometerBasicTuningRegister.baseTuning, _vpeAccelerometerBasicTuningRegister.adaptiveTuning, _vpeAccelerometerBasicTuningRegister.adaptiveFiltering,
6174 vnVpeAccelerometerBasicTuningRegister.baseTuning, vnVpeAccelerometerBasicTuningRegister.adaptiveTuning, vnVpeAccelerometerBasicTuningRegister.adaptiveFiltering);
6175 doDeinitialize();
6176 return false;
6177 }
6178
6179 _vs.writeFilterStartupGyroBias(_filterStartupGyroBias);
6180 if (auto vnFilterStartupGyroBias = _vs.readFilterStartupGyroBias();
6181 vnFilterStartupGyroBias != _filterStartupGyroBias)
6182 {
6183 LOG_ERROR("{}: Writing the filterStartupGyroBias was not successfull.\n"
6184 "Target: {}\n"
6185 "Sensor: {}",
6186 nameId(),
6187 _filterStartupGyroBias,
6188 vnFilterStartupGyroBias);
6189 doDeinitialize();
6190 return false;
6191 }
6192
6193 _vs.writeVpeGyroBasicTuning(_vpeGyroBasicTuningRegister);
6194 if (auto vnVpeGyroBasicTuningRegister = _vs.readVpeGyroBasicTuning();
6195 vnVpeGyroBasicTuningRegister.angularWalkVariance != _vpeGyroBasicTuningRegister.angularWalkVariance
6196 || vnVpeGyroBasicTuningRegister.baseTuning != _vpeGyroBasicTuningRegister.baseTuning
6197 || vnVpeGyroBasicTuningRegister.adaptiveTuning != _vpeGyroBasicTuningRegister.adaptiveTuning)
6198 {
6199 LOG_ERROR("{}: Writing the vpeGyroBasicTuningRegister was not successfull.\n"
6200 "Target: angularWalkVariance = {}, baseTuning = {}, adaptiveTuning = {}\n"
6201 "Sensor: angularWalkVariance = {}, baseTuning = {}, adaptiveTuning = {}",
6202 nameId(),
6203 _vpeGyroBasicTuningRegister.angularWalkVariance, _vpeGyroBasicTuningRegister.baseTuning, _vpeGyroBasicTuningRegister.adaptiveTuning,
6204 vnVpeGyroBasicTuningRegister.angularWalkVariance, vnVpeGyroBasicTuningRegister.baseTuning, vnVpeGyroBasicTuningRegister.adaptiveTuning);
6205 doDeinitialize();
6206 return false;
6207 }
6208 }
6209
6210 // ###########################################################################################################
6211 // INS SUBSYSTEM
6212 // ###########################################################################################################
6213
6214 if (_sensorModel == VectorNavModel::VN310)
6215 {
6216 _vs.writeInsBasicConfigurationVn300(_insBasicConfigurationRegisterVn300);
6217 if (auto vnInsBasicConfigurationRegisterVn300 = _vs.readInsBasicConfigurationVn300();
6218 vnInsBasicConfigurationRegisterVn300.scenario != _insBasicConfigurationRegisterVn300.scenario
6219 || vnInsBasicConfigurationRegisterVn300.ahrsAiding != _insBasicConfigurationRegisterVn300.ahrsAiding
6220 || vnInsBasicConfigurationRegisterVn300.estBaseline != _insBasicConfigurationRegisterVn300.estBaseline)
6221 {
6222 LOG_ERROR("{}: Writing the insBasicConfigurationRegisterVn300 was not successfull.\n"
6223 "Target: scenario = {}, ahrsAiding = {}, estBaseline = {}\n"
6224 "Sensor: scenario = {}, ahrsAiding = {}, estBaseline = {}",
6225 nameId(),
6226 _insBasicConfigurationRegisterVn300.scenario, _insBasicConfigurationRegisterVn300.ahrsAiding, _insBasicConfigurationRegisterVn300.estBaseline,
6227 vnInsBasicConfigurationRegisterVn300.scenario, vnInsBasicConfigurationRegisterVn300.ahrsAiding, vnInsBasicConfigurationRegisterVn300.estBaseline);
6228 doDeinitialize();
6229 return false;
6230 }
6231
6232 _vs.writeStartupFilterBiasEstimate(_startupFilterBiasEstimateRegister);
6233 if (auto vnStartupFilterBiasEstimateRegister = _vs.readStartupFilterBiasEstimate();
6234 vnStartupFilterBiasEstimateRegister.gyroBias != _startupFilterBiasEstimateRegister.gyroBias
6235 || vnStartupFilterBiasEstimateRegister.accelBias != _startupFilterBiasEstimateRegister.accelBias
6236 || vnStartupFilterBiasEstimateRegister.pressureBias != _startupFilterBiasEstimateRegister.pressureBias)
6237 {
6238 LOG_ERROR("{}: Writing the startupFilterBiasEstimateRegister was not successfull.\n"
6239 "Target: gyroBias = {}, accelBias = {}, pressureBias = {}\n"
6240 "Sensor: gyroBias = {}, accelBias = {}, pressureBias = {}",
6241 nameId(),
6242 _startupFilterBiasEstimateRegister.gyroBias, _startupFilterBiasEstimateRegister.accelBias, _startupFilterBiasEstimateRegister.pressureBias,
6243 vnStartupFilterBiasEstimateRegister.gyroBias, vnStartupFilterBiasEstimateRegister.accelBias, vnStartupFilterBiasEstimateRegister.pressureBias);
6244 doDeinitialize();
6245 return false;
6246 }
6247 }
6248
6249 // ###########################################################################################################
6250 // HARD/SOFT IRON ESTIMATOR SUBSYSTEM
6251 // ###########################################################################################################
6252
6253 _vs.writeMagnetometerCalibrationControl(_magnetometerCalibrationControlRegister);
6254 if (auto vnMagnetometerCalibrationControlRegister = _vs.readMagnetometerCalibrationControl();
6255 vnMagnetometerCalibrationControlRegister.hsiMode != _magnetometerCalibrationControlRegister.hsiMode
6256 || vnMagnetometerCalibrationControlRegister.hsiOutput != _magnetometerCalibrationControlRegister.hsiOutput
6257 || vnMagnetometerCalibrationControlRegister.convergeRate != _magnetometerCalibrationControlRegister.convergeRate)
6258 {
6259 LOG_ERROR("{}: Writing the magnetometerCalibrationControlRegister was not successfull.\n"
6260 "Target: hsiMode = {}, hsiOutput = {}, convergeRate = {}\n"
6261 "Sensor: hsiMode = {}, hsiOutput = {}, convergeRate = {}",
6262 nameId(),
6263 _magnetometerCalibrationControlRegister.hsiMode, _magnetometerCalibrationControlRegister.hsiOutput, _magnetometerCalibrationControlRegister.convergeRate,
6264 vnMagnetometerCalibrationControlRegister.hsiMode, vnMagnetometerCalibrationControlRegister.hsiOutput, vnMagnetometerCalibrationControlRegister.convergeRate);
6265 doDeinitialize();
6266 return false;
6267 }
6268
6269 // ###########################################################################################################
6270 // WORLD MAGNETIC & GRAVITY MODULE
6271 // ###########################################################################################################
6272
6273 _vs.writeMagneticAndGravityReferenceVectors(_magneticAndGravityReferenceVectorsRegister);
6274 if (auto vnMagneticAndGravityReferenceVectorsRegister = _vs.readMagneticAndGravityReferenceVectors();
6275 (!_referenceVectorConfigurationRegister.useMagModel && vnMagneticAndGravityReferenceVectorsRegister.magRef != _magneticAndGravityReferenceVectorsRegister.magRef)
6276 || (!_referenceVectorConfigurationRegister.useGravityModel && vnMagneticAndGravityReferenceVectorsRegister.accRef != _magneticAndGravityReferenceVectorsRegister.accRef))
6277 {
6278 LOG_ERROR("{}: Writing the magneticAndGravityReferenceVectorsRegister was not successfull.\n"
6279 "Target: magRef = {}, accRef = {}\n"
6280 "Sensor: magRef = {}, accRef = {}",
6281 nameId(),
6282 _magneticAndGravityReferenceVectorsRegister.magRef, _magneticAndGravityReferenceVectorsRegister.accRef,
6283 vnMagneticAndGravityReferenceVectorsRegister.magRef, vnMagneticAndGravityReferenceVectorsRegister.accRef);
6284 doDeinitialize();
6285 return false;
6286 }
6287
6288 _vs.writeReferenceVectorConfiguration(_referenceVectorConfigurationRegister);
6289 if (auto vnReferenceVectorConfigurationRegister = _vs.readReferenceVectorConfiguration();
6290 vnReferenceVectorConfigurationRegister.useMagModel != _referenceVectorConfigurationRegister.useMagModel
6291 || vnReferenceVectorConfigurationRegister.useGravityModel != _referenceVectorConfigurationRegister.useGravityModel
6292 || vnReferenceVectorConfigurationRegister.recalcThreshold != _referenceVectorConfigurationRegister.recalcThreshold
6293 || vnReferenceVectorConfigurationRegister.year != _referenceVectorConfigurationRegister.year
6294 || vnReferenceVectorConfigurationRegister.position != _referenceVectorConfigurationRegister.position)
6295 {
6296 LOG_ERROR("{}: Writing the referenceVectorConfigurationRegister was not successfull.\n"
6297 "Target: useMagModel = {}, useGravityModel = {}, recalcThreshold = {}, year = {}, position = {}\n"
6298 "Sensor: useMagModel = {}, useGravityModel = {}, recalcThreshold = {}, year = {}, position = {}",
6299 nameId(),
6300 _referenceVectorConfigurationRegister.useMagModel, _referenceVectorConfigurationRegister.useGravityModel, _referenceVectorConfigurationRegister.recalcThreshold, _referenceVectorConfigurationRegister.year, _referenceVectorConfigurationRegister.position,
6301 vnReferenceVectorConfigurationRegister.useMagModel, vnReferenceVectorConfigurationRegister.useGravityModel, vnReferenceVectorConfigurationRegister.recalcThreshold, vnReferenceVectorConfigurationRegister.year, vnReferenceVectorConfigurationRegister.position);
6302 doDeinitialize();
6303 return false;
6304 }
6305
6306 // ###########################################################################################################
6307 // Velocity Aiding
6308 // ###########################################################################################################
6309
6310 if (_sensorModel == VectorNavModel::VN100_VN110)
6311 {
6312 _vs.writeVelocityCompensationControl(_velocityCompensationControlRegister);
6313 if (auto vnVelocityCompensationControlRegister = _vs.readVelocityCompensationControl();
6314 vnVelocityCompensationControlRegister.mode != _velocityCompensationControlRegister.mode
6315 || vnVelocityCompensationControlRegister.velocityTuning != _velocityCompensationControlRegister.velocityTuning
6316 || vnVelocityCompensationControlRegister.rateTuning != _velocityCompensationControlRegister.rateTuning)
6317 {
6318 LOG_ERROR("{}: Writing the velocityCompensationControlRegister was not successfull.\n"
6319 "Target: mode = {}, velocityTuning = {}, rateTuning = {}\n"
6320 "Sensor: mode = {}, velocityTuning = {}, rateTuning = {}",
6321 nameId(),
6322 _velocityCompensationControlRegister.mode, _velocityCompensationControlRegister.velocityTuning, _velocityCompensationControlRegister.rateTuning,
6323 vnVelocityCompensationControlRegister.mode, vnVelocityCompensationControlRegister.velocityTuning, vnVelocityCompensationControlRegister.rateTuning);
6324 doDeinitialize();
6325 return false;
6326 }
6327
6328 // _vs.writeVelocityCompensationMeasurement(vn::math::vec3f); // User manual VN-100 - 10.3.1 (p 124)
6329 }
6330
6331 // ###########################################################################################################
6332 // Outputs
6333 // ###########################################################################################################
6334
6335 _vs.writeAsyncDataOutputType(_asyncDataOutputType);
6336 if (auto vnAsyncDataOutputType = _vs.readAsyncDataOutputType();
6337 vnAsyncDataOutputType != _asyncDataOutputType)
6338 {
6339 LOG_ERROR("{}: Writing the asyncDataOutputType was not successfull.\n"
6340 "Target: {}\n"
6341 "Sensor: {}",
6342 nameId(),
6343 _asyncDataOutputType,
6344 vnAsyncDataOutputType);
6345 doDeinitialize();
6346 return false;
6347 }
6348
6349 if (_asyncDataOutputType != vn::protocol::uart::AsciiAsync::VNOFF)
6350 {
6351 _vs.writeAsyncDataOutputFrequency(_asyncDataOutputFrequency);
6352 if (auto vnAsyncDataOutputFrequency = _vs.readAsyncDataOutputType();
6353 vnAsyncDataOutputFrequency != _asyncDataOutputFrequency)
6354 {
6355 LOG_ERROR("{}: Writing the asyncDataOutputFrequency was not successfull.\n"
6356 "Target: {}\n"
6357 "Sensor: {}",
6358 nameId(),
6359 _asyncDataOutputFrequency,
6360 vnAsyncDataOutputFrequency);
6361 doDeinitialize();
6362 return false;
6363 }
6364 }
6365
6366 [[maybe_unused]] size_t binaryOutputRegisterCounter = 1; // To give a proper error message
6367 try
6368 {
6369 // The sensor does somehow report wrong flags here, so we can't check if it actually worked
6370
6371 // auto checkBinaryRegister = [&](const vn::sensors::BinaryOutputRegister& current, const vn::sensors::BinaryOutputRegister& target) {
6372 // if (current.asyncMode != target.asyncMode
6373 // || current.rateDivisor != target.rateDivisor
6374 // || current.commonField != target.commonField
6375 // || current.timeField != target.timeField
6376 // || current.imuField != target.imuField
6377 // || current.gpsField != target.gpsField
6378 // || current.attitudeField != target.attitudeField
6379 // || current.insField != target.insField
6380 // || current.gps2Field != target.gps2Field)
6381 // {
6382 // LOG_ERROR("{}: Writing the binaryOutputRegister {} was not successfull.\n"
6383 // "Target: asyncMode = {}, rateDivisor = {}, commonField = {}, timeField = {}, imuField = {}, gpsField = {}, attitudeField = {}, insField = {}, gps2Field = {}\n"
6384 // "Sensor: asyncMode = {}, rateDivisor = {}, commonField = {}, timeField = {}, imuField = {}, gpsField = {}, attitudeField = {}, insField = {}, gps2Field = {}",
6385 // nameId(), binaryOutputRegisterCounter,
6386 // target.asyncMode, target.rateDivisor, target.commonField, target.timeField, target.imuField, target.gpsField, target.attitudeField, target.insField, target.gps2Field,
6387 // current.asyncMode, current.rateDivisor, current.commonField, current.timeField, current.imuField, current.gpsField, current.attitudeField, current.insField, current.gps2Field);
6388 // return false;
6389 // }
6390 // return true;
6391 // };
6392
6393 _vs.writeBinaryOutput1(_binaryOutputRegister.at(0));
6394 // if (!checkBinaryRegister(_vs.readBinaryOutput1(), _binaryOutputRegister.at(0)))
6395 // {
6396 // DoDeinitialize();;
6397 // return false;
6398 // }
6399
6400 binaryOutputRegisterCounter++;
6401 _vs.writeBinaryOutput2(_binaryOutputRegister.at(1));
6402 // if (!checkBinaryRegister(_vs.readBinaryOutput2(), _binaryOutputRegister.at(1)))
6403 // {
6404 // DoDeinitialize();;
6405 // return false;
6406 // }
6407
6408 binaryOutputRegisterCounter++;
6409 _vs.writeBinaryOutput3(_binaryOutputRegister.at(2));
6410 // if (!checkBinaryRegister(_vs.readBinaryOutput3(), _binaryOutputRegister.at(2)))
6411 // {
6412 // DoDeinitialize();;
6413 // return false;
6414 // }
6415 }
6416 catch (const std::exception& e)
6417 {
6418 LOG_ERROR("{}: Could not configure binary output register {}: {}", nameId(), binaryOutputRegisterCounter, e.what());
6419 doDeinitialize();
6420 return false;
6421 }
6422
6423 // Some changes need to be set at startup, therefore write the settings and reset the device
6424 LOG_DEBUG("{}: writing settings", nameId());
6425 _vs.writeSettings();
6426
6427 LOG_DEBUG("{}: Resetting device to apply permenent settings", nameId());
6428 _vs.reset();
6429
6430 // TODO: Implement in vnproglib: _vs.writeNmeaOutput1(...) - User manual VN-310 - 8.2.14 (p 103)
6431 // TODO: Implement in vnproglib: _vs.writeNmeaOutput2(...) - User manual VN-310 - 8.2.15 (p 104)
6432
6433 _vs.registerAsyncPacketReceivedHandler(this, asciiOrBinaryAsyncMessageReceived);
6434
6435 LOG_DEBUG("{}: successfully initialized", nameId());
6436
6437 return true;
6438}
6439
6440void NAV::VectorNavSensor::deinitialize()
6441{
6442 LOG_TRACE("{}: called", nameId());
6443
6444 try
6445 {
6446 if (_vs.isConnected())
6447 {
6448 try
6449 {
6450 _vs.unregisterAsyncPacketReceivedHandler();
6451 }
6452 catch (const std::exception& e)
6453 {
6454 LOG_WARN("{}: Could not unregisterAsyncPacketReceivedHandler ({})", nameId(), e.what());
6455 }
6456
6457 try
6458 {
6459 vn::sensors::BinaryOutputRegister bor{
6460 vn::protocol::uart::AsyncMode::ASYNCMODE_NONE, // AsyncMode
6461 800, // RateDivisor
6462 vn::protocol::uart::CommonGroup::COMMONGROUP_NONE, // CommonGroup
6463 vn::protocol::uart::TimeGroup::TIMEGROUP_NONE, // TimeGroup
6464 vn::protocol::uart::ImuGroup::IMUGROUP_NONE, // IMUGroup
6465 vn::protocol::uart::GpsGroup::GPSGROUP_NONE, // GNSS1Group
6466 vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_NONE, // AttitudeGroup
6467 vn::protocol::uart::InsGroup::INSGROUP_NONE, // INSGroup
6468 vn::protocol::uart::GpsGroup::GPSGROUP_NONE // GNSS2Group
6469 };
6470
6471 _vs.writeBinaryOutput1(bor);
6472 _vs.writeBinaryOutput2(bor);
6473 _vs.writeBinaryOutput3(bor);
6474 _vs.writeAsyncDataOutputType(vn::protocol::uart::AsciiAsync::VNOFF);
6475 _vs.changeBaudRate(NAV::UartSensor::Baudrate::BAUDRATE_115200);
6476 _vs.writeSettings();
6477 LOG_DEBUG("{}: Sensor output turned off", nameId());
6478 }
6479 catch (const std::exception& e)
6480 {
6481 LOG_WARN("{}: Could not turn off sensor output ({})", nameId(), e.what());
6482 }
6483
6484 try
6485 {
6486 _vs.disconnect();
6487 LOG_DEBUG("{}: Sensor disconnected", nameId());
6488 }
6489 catch (const std::exception& e)
6490 {
6491 LOG_WARN("{}: Could not disconnect ({})", nameId(), e.what());
6492 }
6493 }
6494 }
6495 catch (const std::exception& e)
6496 {
6497 LOG_WARN("{}: Unhandled exception while deinitializing sensor ({})", nameId(), e.what());
6498 }
6499}
6500
6501void NAV::VectorNavSensor::mergeVectorNavBinaryObservations(const std::shared_ptr<VectorNavBinaryOutput>& target, const std::shared_ptr<VectorNavBinaryOutput>& source)
6502{
6503 target->insTime = !target->insTime.empty() ? target->insTime : source->insTime;
6504 // Group 2 (Time)
6505 if (!target->timeOutputs && source->timeOutputs)
6506 {
6507 target->timeOutputs = source->timeOutputs;
6508 }
6509 else if (target->timeOutputs && source->timeOutputs)
6510 {
6511 target->timeOutputs->timeStartup = target->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP ? target->timeOutputs->timeStartup : source->timeOutputs->timeStartup;
6512 target->timeOutputs->timeGps = target->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEGPS ? target->timeOutputs->timeGps : source->timeOutputs->timeGps;
6513 target->timeOutputs->gpsTow = target->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW ? target->timeOutputs->gpsTow : source->timeOutputs->gpsTow;
6514 target->timeOutputs->gpsWeek = target->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK ? target->timeOutputs->gpsWeek : source->timeOutputs->gpsWeek;
6515 target->timeOutputs->timeSyncIn = target->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESYNCIN ? target->timeOutputs->timeSyncIn : source->timeOutputs->timeSyncIn;
6516 target->timeOutputs->timePPS = target->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEGPSPPS ? target->timeOutputs->timePPS : source->timeOutputs->timePPS;
6517 // target->timeOutputs->timeUtc = target->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEUTC ? target->timeOutputs->timeUtc : source->timeOutputs->timeUtc;
6518 target->timeOutputs->syncInCnt = target->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_SYNCINCNT ? target->timeOutputs->syncInCnt : source->timeOutputs->syncInCnt;
6519 target->timeOutputs->syncOutCnt = target->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_SYNCOUTCNT ? target->timeOutputs->syncOutCnt : source->timeOutputs->syncOutCnt;
6520 target->timeOutputs->timeStatus = target->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS ? target->timeOutputs->timeStatus : source->timeOutputs->timeStatus;
6521
6522 target->timeOutputs->timeField |= source->timeOutputs->timeField;
6523 }
6524 // Group 3 (IMU)
6525 if (!target->imuOutputs && source->imuOutputs)
6526 {
6527 target->imuOutputs = source->imuOutputs;
6528 }
6529 else if (target->imuOutputs && source->imuOutputs)
6530 {
6531 target->imuOutputs->imuStatus = target->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_IMUSTATUS ? target->imuOutputs->imuStatus : source->imuOutputs->imuStatus;
6532 target->imuOutputs->uncompMag = target->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPMAG ? target->imuOutputs->uncompMag : source->imuOutputs->uncompMag;
6533 target->imuOutputs->uncompAccel = target->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPACCEL ? target->imuOutputs->uncompAccel : source->imuOutputs->uncompAccel;
6534 target->imuOutputs->uncompGyro = target->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPGYRO ? target->imuOutputs->uncompGyro : source->imuOutputs->uncompGyro;
6535 target->imuOutputs->temp = target->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_TEMP ? target->imuOutputs->temp : source->imuOutputs->temp;
6536 target->imuOutputs->pres = target->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_PRES ? target->imuOutputs->pres : source->imuOutputs->pres;
6537 target->imuOutputs->deltaTime = target->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTATHETA ? target->imuOutputs->deltaTime : source->imuOutputs->deltaTime;
6538 target->imuOutputs->deltaTheta = target->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTATHETA ? target->imuOutputs->deltaTheta : source->imuOutputs->deltaTheta;
6539 target->imuOutputs->deltaV = target->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTAVEL ? target->imuOutputs->deltaV : source->imuOutputs->deltaV;
6540 target->imuOutputs->mag = target->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_MAG ? target->imuOutputs->mag : source->imuOutputs->mag;
6541 target->imuOutputs->accel = target->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_ACCEL ? target->imuOutputs->accel : source->imuOutputs->accel;
6542 target->imuOutputs->angularRate = target->imuOutputs->imuField & vn::protocol::uart::ImuGroup::IMUGROUP_ANGULARRATE ? target->imuOutputs->angularRate : source->imuOutputs->angularRate;
6543
6544 target->imuOutputs->imuField |= source->imuOutputs->imuField;
6545 }
6546 // Group 4 (GNSS1)
6547 if (!target->gnss1Outputs && source->gnss1Outputs)
6548 {
6549 target->gnss1Outputs = source->gnss1Outputs;
6550 }
6551 else if (target->gnss1Outputs && source->gnss1Outputs)
6552 {
6553 // target->gnss1Outputs->timeUtc = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_UTC ? target->gnss1Outputs->timeUtc : source->gnss1Outputs->timeUtc;
6554 target->gnss1Outputs->tow = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_TOW ? target->gnss1Outputs->tow : source->gnss1Outputs->tow;
6555 target->gnss1Outputs->week = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_WEEK ? target->gnss1Outputs->week : source->gnss1Outputs->week;
6556 target->gnss1Outputs->numSats = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_NUMSATS ? target->gnss1Outputs->numSats : source->gnss1Outputs->numSats;
6557 target->gnss1Outputs->fix = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_FIX ? target->gnss1Outputs->fix : source->gnss1Outputs->fix;
6558 target->gnss1Outputs->posLla = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_POSLLA ? target->gnss1Outputs->posLla : source->gnss1Outputs->posLla;
6559 target->gnss1Outputs->posEcef = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_POSECEF ? target->gnss1Outputs->posEcef : source->gnss1Outputs->posEcef;
6560 target->gnss1Outputs->velNed = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_VELNED ? target->gnss1Outputs->velNed : source->gnss1Outputs->velNed;
6561 target->gnss1Outputs->velEcef = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_VELECEF ? target->gnss1Outputs->velEcef : source->gnss1Outputs->velEcef;
6562 target->gnss1Outputs->posU = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_POSU ? target->gnss1Outputs->posU : source->gnss1Outputs->posU;
6563 target->gnss1Outputs->velU = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_VELU ? target->gnss1Outputs->velU : source->gnss1Outputs->velU;
6564 target->gnss1Outputs->timeU = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_TIMEU ? target->gnss1Outputs->timeU : source->gnss1Outputs->timeU;
6565 target->gnss1Outputs->timeInfo = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO ? target->gnss1Outputs->timeInfo : source->gnss1Outputs->timeInfo;
6566 target->gnss1Outputs->dop = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_DOP ? target->gnss1Outputs->dop : source->gnss1Outputs->dop;
6567 target->gnss1Outputs->satInfo = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_SATINFO ? target->gnss1Outputs->satInfo : source->gnss1Outputs->satInfo;
6568 target->gnss1Outputs->raw = target->gnss1Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_RAWMEAS ? target->gnss1Outputs->raw : source->gnss1Outputs->raw;
6569
6570 target->gnss1Outputs->gnssField |= source->gnss1Outputs->gnssField;
6571 }
6572 // Group 5 (Attitude)
6573 if (!target->attitudeOutputs && source->attitudeOutputs)
6574 {
6575 target->attitudeOutputs = source->attitudeOutputs;
6576 }
6577 else if (target->attitudeOutputs && source->attitudeOutputs)
6578 {
6579 target->attitudeOutputs->vpeStatus = target->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_VPESTATUS ? target->attitudeOutputs->vpeStatus : source->attitudeOutputs->vpeStatus;
6580 target->attitudeOutputs->ypr = target->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YAWPITCHROLL ? target->attitudeOutputs->ypr : source->attitudeOutputs->ypr;
6581 target->attitudeOutputs->qtn = target->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_QUATERNION ? target->attitudeOutputs->qtn : source->attitudeOutputs->qtn;
6582 target->attitudeOutputs->dcm = target->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_DCM ? target->attitudeOutputs->dcm : source->attitudeOutputs->dcm;
6583 target->attitudeOutputs->magNed = target->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_MAGNED ? target->attitudeOutputs->magNed : source->attitudeOutputs->magNed;
6584 target->attitudeOutputs->accelNed = target->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_ACCELNED ? target->attitudeOutputs->accelNed : source->attitudeOutputs->accelNed;
6585 target->attitudeOutputs->linearAccelBody = target->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY ? target->attitudeOutputs->linearAccelBody : source->attitudeOutputs->linearAccelBody;
6586 target->attitudeOutputs->linearAccelNed = target->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELNED ? target->attitudeOutputs->linearAccelNed : source->attitudeOutputs->linearAccelNed;
6587 target->attitudeOutputs->yprU = target->attitudeOutputs->attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YPRU ? target->attitudeOutputs->yprU : source->attitudeOutputs->yprU;
6588
6589 target->attitudeOutputs->attitudeField |= source->attitudeOutputs->attitudeField;
6590 }
6591 // Group 6 (INS)
6592 if (!target->insOutputs && source->insOutputs)
6593 {
6594 target->insOutputs = source->insOutputs;
6595 }
6596 else if (target->insOutputs && source->insOutputs)
6597 {
6598 target->insOutputs->insStatus = target->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_INSSTATUS ? target->insOutputs->insStatus : source->insOutputs->insStatus;
6599 target->insOutputs->posLla = target->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_POSLLA ? target->insOutputs->posLla : source->insOutputs->posLla;
6600 target->insOutputs->posEcef = target->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_POSECEF ? target->insOutputs->posEcef : source->insOutputs->posEcef;
6601 target->insOutputs->velBody = target->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_VELBODY ? target->insOutputs->velBody : source->insOutputs->velBody;
6602 target->insOutputs->velNed = target->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_VELNED ? target->insOutputs->velNed : source->insOutputs->velNed;
6603 target->insOutputs->velEcef = target->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_VELECEF ? target->insOutputs->velEcef : source->insOutputs->velEcef;
6604 target->insOutputs->magEcef = target->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_MAGECEF ? target->insOutputs->magEcef : source->insOutputs->magEcef;
6605 target->insOutputs->accelEcef = target->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_ACCELECEF ? target->insOutputs->accelEcef : source->insOutputs->accelEcef;
6606 target->insOutputs->linearAccelEcef = target->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_LINEARACCELECEF ? target->insOutputs->linearAccelEcef : source->insOutputs->linearAccelEcef;
6607 target->insOutputs->posU = target->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_POSU ? target->insOutputs->posU : source->insOutputs->posU;
6608 target->insOutputs->velU = target->insOutputs->insField & vn::protocol::uart::InsGroup::INSGROUP_VELU ? target->insOutputs->velU : source->insOutputs->velU;
6609
6610 target->insOutputs->insField |= source->insOutputs->insField;
6611 }
6612 // Group 7 (GNSS2)
6613 if (!target->gnss2Outputs && source->gnss2Outputs)
6614 {
6615 target->gnss2Outputs = source->gnss2Outputs;
6616 }
6617 else if (target->gnss2Outputs && source->gnss2Outputs)
6618 {
6619 // target->gnss2Outputs->timeUtc = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_UTC ? target->gnss2Outputs->timeUtc : source->gnss2Outputs->timeUtc;
6620 target->gnss2Outputs->tow = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_TOW ? target->gnss2Outputs->tow : source->gnss2Outputs->tow;
6621 target->gnss2Outputs->week = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_WEEK ? target->gnss2Outputs->week : source->gnss2Outputs->week;
6622 target->gnss2Outputs->numSats = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_NUMSATS ? target->gnss2Outputs->numSats : source->gnss2Outputs->numSats;
6623 target->gnss2Outputs->fix = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_FIX ? target->gnss2Outputs->fix : source->gnss2Outputs->fix;
6624 target->gnss2Outputs->posLla = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_POSLLA ? target->gnss2Outputs->posLla : source->gnss2Outputs->posLla;
6625 target->gnss2Outputs->posEcef = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_POSECEF ? target->gnss2Outputs->posEcef : source->gnss2Outputs->posEcef;
6626 target->gnss2Outputs->velNed = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_VELNED ? target->gnss2Outputs->velNed : source->gnss2Outputs->velNed;
6627 target->gnss2Outputs->velEcef = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_VELECEF ? target->gnss2Outputs->velEcef : source->gnss2Outputs->velEcef;
6628 target->gnss2Outputs->posU = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_POSU ? target->gnss2Outputs->posU : source->gnss2Outputs->posU;
6629 target->gnss2Outputs->velU = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_VELU ? target->gnss2Outputs->velU : source->gnss2Outputs->velU;
6630 target->gnss2Outputs->timeU = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_TIMEU ? target->gnss2Outputs->timeU : source->gnss2Outputs->timeU;
6631 target->gnss2Outputs->timeInfo = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO ? target->gnss2Outputs->timeInfo : source->gnss2Outputs->timeInfo;
6632 target->gnss2Outputs->dop = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_DOP ? target->gnss2Outputs->dop : source->gnss2Outputs->dop;
6633 target->gnss2Outputs->satInfo = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_SATINFO ? target->gnss2Outputs->satInfo : source->gnss2Outputs->satInfo;
6634 target->gnss2Outputs->raw = target->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_RAWMEAS ? target->gnss2Outputs->raw : source->gnss2Outputs->raw;
6635
6636 target->gnss2Outputs->gnssField |= source->gnss2Outputs->gnssField;
6637 }
6638}
6639
6640void NAV::VectorNavSensor::asciiOrBinaryAsyncMessageReceived(void* userData, vn::protocol::uart::Packet& p, [[maybe_unused]] size_t index)
6641{
6642 auto* vnSensor = static_cast<VectorNavSensor*>(userData);
6643
6644 LOG_DATA("{}: Received message", vnSensor->nameId());
6645
6646 if (p.getPacketLength() > 2500)
6647 {
6648 LOG_ERROR("{} Packet size is {} bytes. VectorNav internal buffer overflows happen if the size is > 2550 bytes. "
6649 "You potentially already lost packages without noticing. "
6650 "Consider splitting the packet to different Binary outputs.",
6651 vnSensor->nameId(), p.getPacketLength());
6652 }
6653 else if (p.getPacketLength() > 2200)
6654 {
6655 LOG_WARN("{} Packet size is {} bytes. VectorNav internal buffer overflows happen if the size is > 2550 bytes. "
6656 "Consider splitting the packet to different Binary outputs.",
6657 vnSensor->nameId(), p.getPacketLength());
6658 }
6659
6660 if (p.type() == vn::protocol::uart::Packet::TYPE_BINARY)
6661 {
6662 for (size_t b = 0; b < 3; b++)
6663 {
6664 // Make sure that the binary packet is from the type we expect
6665 if (p.isCompatible(vnSensor->_binaryOutputRegister.at(b).commonField,
6666 vnSensor->_binaryOutputRegister.at(b).timeField,
6667 vnSensor->_binaryOutputRegister.at(b).imuField,
6668 vnSensor->_binaryOutputRegister.at(b).gpsField,
6669 vnSensor->_binaryOutputRegister.at(b).attitudeField,
6670 vnSensor->_binaryOutputRegister.at(b).insField,
6671 vnSensor->_binaryOutputRegister.at(b).gps2Field))
6672 {
6673 auto obs = std::make_shared<VectorNavBinaryOutput>(vnSensor->_imuPos);
6674
6675 // Group 2 (Time)
6676 if (vnSensor->_binaryOutputRegister.at(b).timeField != vn::protocol::uart::TimeGroup::TIMEGROUP_NONE)
6677 {
6678 if (!obs->timeOutputs)
6679 {
6680 obs->timeOutputs = std::make_shared<NAV::vendor::vectornav::TimeOutputs>();
6681 obs->timeOutputs->timeField |= vnSensor->_binaryOutputRegister.at(b).timeField;
6682 }
6683
6684 if (vnSensor->_binaryOutputRegister.at(b).timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP)
6685 {
6686 obs->timeOutputs->timeStartup = p.extractUint64();
6687 }
6688 if (vnSensor->_binaryOutputRegister.at(b).timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEGPS)
6689 {
6690 obs->timeOutputs->timeGps = p.extractUint64();
6691 }
6692 if (vnSensor->_binaryOutputRegister.at(b).timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW)
6693 {
6694 obs->timeOutputs->gpsTow = p.extractUint64();
6695 }
6696 if (vnSensor->_binaryOutputRegister.at(b).timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK)
6697 {
6698 obs->timeOutputs->gpsWeek = p.extractUint16();
6699 }
6700 if (vnSensor->_binaryOutputRegister.at(b).timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESYNCIN)
6701 {
6702 obs->timeOutputs->timeSyncIn = p.extractUint64();
6703 }
6704 if (vnSensor->_binaryOutputRegister.at(b).timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEGPSPPS)
6705 {
6706 obs->timeOutputs->timePPS = p.extractUint64();
6707 }
6708 // if (vnSensor->_binaryOutputRegister.at(b).timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEUTC)
6709 // {
6710 // obs->timeOutputs->timeUtc.year = p.extractInt8();
6711 // obs->timeOutputs->timeUtc.month = p.extractUint8();
6712 // obs->timeOutputs->timeUtc.day = p.extractUint8();
6713 // obs->timeOutputs->timeUtc.hour = p.extractUint8();
6714 // obs->timeOutputs->timeUtc.min = p.extractUint8();
6715 // obs->timeOutputs->timeUtc.sec = p.extractUint8();
6716 // obs->timeOutputs->timeUtc.ms = p.extractUint16();
6717 // }
6718 if (vnSensor->_binaryOutputRegister.at(b).timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_SYNCINCNT)
6719 {
6720 obs->timeOutputs->syncInCnt = p.extractUint32();
6721 }
6722 if (vnSensor->_binaryOutputRegister.at(b).timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_SYNCOUTCNT)
6723 {
6724 obs->timeOutputs->syncOutCnt = p.extractUint32();
6725 }
6726 if (vnSensor->_binaryOutputRegister.at(b).timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS)
6727 {
6728 obs->timeOutputs->timeStatus = p.extractUint8();
6729 }
6730 }
6731 // Group 3 (IMU)
6732 if (vnSensor->_binaryOutputRegister.at(b).imuField != vn::protocol::uart::ImuGroup::IMUGROUP_NONE)
6733 {
6734 if (!obs->imuOutputs)
6735 {
6736 obs->imuOutputs = std::make_shared<NAV::vendor::vectornav::ImuOutputs>();
6737 obs->imuOutputs->imuField |= vnSensor->_binaryOutputRegister.at(b).imuField;
6738 }
6739
6740 if (vnSensor->_binaryOutputRegister.at(b).imuField & vn::protocol::uart::ImuGroup::IMUGROUP_IMUSTATUS)
6741 {
6742 obs->imuOutputs->imuStatus = p.extractUint16();
6743 }
6744 if (vnSensor->_binaryOutputRegister.at(b).imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPMAG)
6745 {
6746 auto vec = p.extractVec3f();
6747 obs->imuOutputs->uncompMag = { vec.x, vec.y, vec.z };
6748 }
6749 if (vnSensor->_binaryOutputRegister.at(b).imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPACCEL)
6750 {
6751 auto vec = p.extractVec3f();
6752 obs->imuOutputs->uncompAccel = { vec.x, vec.y, vec.z };
6753 }
6754 if (vnSensor->_binaryOutputRegister.at(b).imuField & vn::protocol::uart::ImuGroup::IMUGROUP_UNCOMPGYRO)
6755 {
6756 auto vec = p.extractVec3f();
6757 obs->imuOutputs->uncompGyro = { vec.x, vec.y, vec.z };
6758 }
6759 if (vnSensor->_binaryOutputRegister.at(b).imuField & vn::protocol::uart::ImuGroup::IMUGROUP_TEMP)
6760 {
6761 obs->imuOutputs->temp = p.extractFloat();
6762 }
6763 if (vnSensor->_binaryOutputRegister.at(b).imuField & vn::protocol::uart::ImuGroup::IMUGROUP_PRES)
6764 {
6765 obs->imuOutputs->pres = p.extractFloat();
6766 }
6767 if (vnSensor->_binaryOutputRegister.at(b).imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTATHETA)
6768 {
6769 obs->imuOutputs->deltaTime = p.extractFloat();
6770 auto vec = p.extractVec3f();
6771 obs->imuOutputs->deltaTheta = { vec.x, vec.y, vec.z };
6772 }
6773 if (vnSensor->_binaryOutputRegister.at(b).imuField & vn::protocol::uart::ImuGroup::IMUGROUP_DELTAVEL)
6774 {
6775 auto vec = p.extractVec3f();
6776 obs->imuOutputs->deltaV = { vec.x, vec.y, vec.z };
6777 }
6778 if (vnSensor->_binaryOutputRegister.at(b).imuField & vn::protocol::uart::ImuGroup::IMUGROUP_MAG)
6779 {
6780 auto vec = p.extractVec3f();
6781 obs->imuOutputs->mag = { vec.x, vec.y, vec.z };
6782 }
6783 if (vnSensor->_binaryOutputRegister.at(b).imuField & vn::protocol::uart::ImuGroup::IMUGROUP_ACCEL)
6784 {
6785 auto vec = p.extractVec3f();
6786 obs->imuOutputs->accel = { vec.x, vec.y, vec.z };
6787 }
6788 if (vnSensor->_binaryOutputRegister.at(b).imuField & vn::protocol::uart::ImuGroup::IMUGROUP_ANGULARRATE)
6789 {
6790 auto vec = p.extractVec3f();
6791 obs->imuOutputs->angularRate = { vec.x, vec.y, vec.z };
6792 }
6793 }
6794 // Group 4 (GNSS1)
6795 if (vnSensor->_binaryOutputRegister.at(b).gpsField != vn::protocol::uart::GpsGroup::GPSGROUP_NONE)
6796 {
6797 if (!obs->gnss1Outputs)
6798 {
6799 obs->gnss1Outputs = std::make_shared<NAV::vendor::vectornav::GnssOutputs>();
6800 obs->gnss1Outputs->gnssField |= vnSensor->_binaryOutputRegister.at(b).gpsField;
6801 }
6802
6803 // if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_UTC)
6804 // {
6805 // obs->gnss1Outputs->timeUtc.year = p.extractInt8();
6806 // obs->gnss1Outputs->timeUtc.month = p.extractUint8();
6807 // obs->gnss1Outputs->timeUtc.day = p.extractUint8();
6808 // obs->gnss1Outputs->timeUtc.hour = p.extractUint8();
6809 // obs->gnss1Outputs->timeUtc.min = p.extractUint8();
6810 // obs->gnss1Outputs->timeUtc.sec = p.extractUint8();
6811 // obs->gnss1Outputs->timeUtc.ms = p.extractUint16();
6812 // }
6813 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_TOW)
6814 {
6815 obs->gnss1Outputs->tow = p.extractUint64();
6816 }
6817 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_WEEK)
6818 {
6819 obs->gnss1Outputs->week = p.extractUint16();
6820 }
6821 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_NUMSATS)
6822 {
6823 obs->gnss1Outputs->numSats = p.extractUint8();
6824 }
6825 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_FIX)
6826 {
6827 obs->gnss1Outputs->fix = p.extractUint8();
6828 }
6829 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_POSLLA)
6830 {
6831 auto vec = p.extractVec3d();
6832 obs->gnss1Outputs->posLla = { vec.x, vec.y, vec.z };
6833 }
6834 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_POSECEF)
6835 {
6836 auto vec = p.extractVec3d();
6837 obs->gnss1Outputs->posEcef = { vec.x, vec.y, vec.z };
6838 }
6839 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_VELNED)
6840 {
6841 auto vec = p.extractVec3f();
6842 obs->gnss1Outputs->velNed = { vec.x, vec.y, vec.z };
6843 }
6844 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_VELECEF)
6845 {
6846 auto vec = p.extractVec3f();
6847 obs->gnss1Outputs->velEcef = { vec.x, vec.y, vec.z };
6848 }
6849 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_POSU)
6850 {
6851 auto vec = p.extractVec3f();
6852 obs->gnss1Outputs->posU = { vec.x, vec.y, vec.z };
6853 }
6854 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_VELU)
6855 {
6856 obs->gnss1Outputs->velU = p.extractFloat();
6857 }
6858 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_TIMEU)
6859 {
6860 obs->gnss1Outputs->timeU = p.extractFloat();
6861 }
6862 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO)
6863 {
6864 obs->gnss1Outputs->timeInfo.status = p.extractUint8();
6865 obs->gnss1Outputs->timeInfo.leapSeconds = p.extractInt8();
6866 }
6867 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_DOP)
6868 {
6869 obs->gnss1Outputs->dop.gDop = p.extractFloat();
6870 obs->gnss1Outputs->dop.pDop = p.extractFloat();
6871 obs->gnss1Outputs->dop.tDop = p.extractFloat();
6872 obs->gnss1Outputs->dop.vDop = p.extractFloat();
6873 obs->gnss1Outputs->dop.hDop = p.extractFloat();
6874 obs->gnss1Outputs->dop.nDop = p.extractFloat();
6875 obs->gnss1Outputs->dop.eDop = p.extractFloat();
6876 }
6877 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_SATINFO)
6878 {
6879 obs->gnss1Outputs->satInfo.numSats = p.extractUint8();
6880 p.extractUint8(); // Reserved for future use
6881
6882 LOG_DATA("{}: SatInfo: numSats {}", vnSensor->nameId(), obs->gnss1Outputs->satInfo.numSats);
6883 for (size_t i = 0; i < obs->gnss1Outputs->satInfo.numSats; i++)
6884 {
6885 auto sys = p.extractInt8();
6886 auto svId = p.extractUint8();
6887 auto flags = p.extractUint8();
6888 auto cno = p.extractUint8();
6889 auto qi = p.extractUint8();
6890 auto el = p.extractInt8();
6891 auto az = p.extractInt16();
6892 obs->gnss1Outputs->satInfo.satellites.emplace_back(sys, svId, flags, cno, qi, el, az);
6893 LOG_DATA("{}: SatInfo: sys {}, svId {}, flags {}, cno {}, qi {}, el {}, az {}", vnSensor->nameId(),
6894 sys, svId, flags, cno, qi, el, az);
6895 }
6896 }
6897 if (vnSensor->_binaryOutputRegister.at(b).gpsField & vn::protocol::uart::GpsGroup::GPSGROUP_RAWMEAS)
6898 {
6899 obs->gnss1Outputs->raw.tow = p.extractDouble();
6900 obs->gnss1Outputs->raw.week = p.extractUint16();
6901 obs->gnss1Outputs->raw.numSats = p.extractUint8();
6902 p.extractUint8(); // Reserved for future use
6903 LOG_DATA("{}: RawMeas: tow {}, week {}, numSats {}", vnSensor->nameId(),
6904 obs->gnss1Outputs->raw.tow, obs->gnss1Outputs->raw.week, obs->gnss1Outputs->raw.numSats);
6905
6906 for (size_t i = 0; i < obs->gnss1Outputs->raw.numSats; i++)
6907 {
6908 auto sys = p.extractUint8();
6909 auto svId = p.extractUint8();
6910 auto freq = p.extractUint8();
6911 auto chan = p.extractUint8();
6912 auto slot = p.extractInt8();
6913 auto cno = p.extractUint8();
6914 auto flags = p.extractUint16();
6915 auto pr = p.extractDouble();
6916 auto cp = p.extractDouble();
6917 auto dp = p.extractFloat();
6918 obs->gnss1Outputs->raw.satellites.emplace_back(sys, svId, freq, chan, slot, cno, flags, pr, cp, dp);
6919 LOG_DATA("{}: RawMeas: sys {}, svId {}, freq {}, chan {}, slot {}, cno {}, flags {}, pr {}, cp {}, dp {}",
6920 vnSensor->nameId(), static_cast<int>(sys), static_cast<int>(svId), static_cast<int>(freq), static_cast<int>(chan),
6921 static_cast<int>(slot), static_cast<int>(cno), static_cast<int>(flags), pr, cp, dp);
6922 }
6923 }
6924 }
6925 // Group 5 (Attitude)
6926 if (vnSensor->_binaryOutputRegister.at(b).attitudeField != vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_NONE)
6927 {
6928 if (!obs->attitudeOutputs)
6929 {
6930 obs->attitudeOutputs = std::make_shared<NAV::vendor::vectornav::AttitudeOutputs>();
6931 obs->attitudeOutputs->attitudeField |= vnSensor->_binaryOutputRegister.at(b).attitudeField;
6932 }
6933
6934 if (vnSensor->_binaryOutputRegister.at(b).attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_VPESTATUS)
6935 {
6936 obs->attitudeOutputs->vpeStatus = p.extractUint16();
6937 }
6938 if (vnSensor->_binaryOutputRegister.at(b).attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YAWPITCHROLL)
6939 {
6940 auto vec = p.extractVec3f();
6941 obs->attitudeOutputs->ypr = { vec.x, vec.y, vec.z };
6942 }
6943 if (vnSensor->_binaryOutputRegister.at(b).attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_QUATERNION)
6944 {
6945 auto vec = p.extractVec4f();
6946 obs->attitudeOutputs->qtn = { vec.w, vec.x, vec.y, vec.z };
6947 }
6948 if (vnSensor->_binaryOutputRegister.at(b).attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_DCM)
6949 {
6950 auto col0 = p.extractVec3f();
6951 auto col1 = p.extractVec3f();
6952 auto col2 = p.extractVec3f();
6953 obs->attitudeOutputs->dcm << col0.x, col1.x, col2.x,
6954 col0.y, col1.y, col2.y,
6955 col0.z, col1.z, col2.z;
6956 }
6957 if (vnSensor->_binaryOutputRegister.at(b).attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_MAGNED)
6958 {
6959 auto vec = p.extractVec3f();
6960 obs->attitudeOutputs->magNed = { vec.x, vec.y, vec.z };
6961 }
6962 if (vnSensor->_binaryOutputRegister.at(b).attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_ACCELNED)
6963 {
6964 auto vec = p.extractVec3f();
6965 obs->attitudeOutputs->accelNed = { vec.x, vec.y, vec.z };
6966 }
6967 if (vnSensor->_binaryOutputRegister.at(b).attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY)
6968 {
6969 auto vec = p.extractVec3f();
6970 obs->attitudeOutputs->linearAccelBody = { vec.x, vec.y, vec.z };
6971 }
6972 if (vnSensor->_binaryOutputRegister.at(b).attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_LINEARACCELNED)
6973 {
6974 auto vec = p.extractVec3f();
6975 obs->attitudeOutputs->linearAccelNed = { vec.x, vec.y, vec.z };
6976 }
6977 if (vnSensor->_binaryOutputRegister.at(b).attitudeField & vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_YPRU)
6978 {
6979 auto vec = p.extractVec3f();
6980 obs->attitudeOutputs->yprU = { vec.x, vec.y, vec.z };
6981 }
6982 }
6983 // Group 6 (INS)
6984 if (vnSensor->_binaryOutputRegister.at(b).insField != vn::protocol::uart::InsGroup::INSGROUP_NONE)
6985 {
6986 if (!obs->insOutputs)
6987 {
6988 obs->insOutputs = std::make_shared<NAV::vendor::vectornav::InsOutputs>();
6989 obs->insOutputs->insField |= vnSensor->_binaryOutputRegister.at(b).insField;
6990 }
6991
6992 if (vnSensor->_binaryOutputRegister.at(b).insField & vn::protocol::uart::InsGroup::INSGROUP_INSSTATUS)
6993 {
6994 obs->insOutputs->insStatus = p.extractUint16();
6995 }
6996 if (vnSensor->_binaryOutputRegister.at(b).insField & vn::protocol::uart::InsGroup::INSGROUP_POSLLA)
6997 {
6998 auto vec = p.extractVec3d();
6999 obs->insOutputs->posLla = { vec.x, vec.y, vec.z };
7000 }
7001 if (vnSensor->_binaryOutputRegister.at(b).insField & vn::protocol::uart::InsGroup::INSGROUP_POSECEF)
7002 {
7003 auto vec = p.extractVec3d();
7004 obs->insOutputs->posEcef = { vec.x, vec.y, vec.z };
7005 }
7006 if (vnSensor->_binaryOutputRegister.at(b).insField & vn::protocol::uart::InsGroup::INSGROUP_VELBODY)
7007 {
7008 auto vec = p.extractVec3f();
7009 obs->insOutputs->velBody = { vec.x, vec.y, vec.z };
7010 }
7011 if (vnSensor->_binaryOutputRegister.at(b).insField & vn::protocol::uart::InsGroup::INSGROUP_VELNED)
7012 {
7013 auto vec = p.extractVec3f();
7014 obs->insOutputs->velNed = { vec.x, vec.y, vec.z };
7015 }
7016 if (vnSensor->_binaryOutputRegister.at(b).insField & vn::protocol::uart::InsGroup::INSGROUP_VELECEF)
7017 {
7018 auto vec = p.extractVec3f();
7019 obs->insOutputs->velEcef = { vec.x, vec.y, vec.z };
7020 }
7021 if (vnSensor->_binaryOutputRegister.at(b).insField & vn::protocol::uart::InsGroup::INSGROUP_MAGECEF)
7022 {
7023 auto vec = p.extractVec3f();
7024 obs->insOutputs->magEcef = { vec.x, vec.y, vec.z };
7025 }
7026 if (vnSensor->_binaryOutputRegister.at(b).insField & vn::protocol::uart::InsGroup::INSGROUP_ACCELECEF)
7027 {
7028 auto vec = p.extractVec3f();
7029 obs->insOutputs->accelEcef = { vec.x, vec.y, vec.z };
7030 }
7031 if (vnSensor->_binaryOutputRegister.at(b).insField & vn::protocol::uart::InsGroup::INSGROUP_LINEARACCELECEF)
7032 {
7033 auto vec = p.extractVec3f();
7034 obs->insOutputs->linearAccelEcef = { vec.x, vec.y, vec.z };
7035 }
7036 if (vnSensor->_binaryOutputRegister.at(b).insField & vn::protocol::uart::InsGroup::INSGROUP_POSU)
7037 {
7038 obs->insOutputs->posU = p.extractFloat();
7039 }
7040 if (vnSensor->_binaryOutputRegister.at(b).insField & vn::protocol::uart::InsGroup::INSGROUP_VELU)
7041 {
7042 obs->insOutputs->velU = p.extractFloat();
7043 }
7044 }
7045 // Group 7 (GNSS2)
7046 if (vnSensor->_binaryOutputRegister.at(b).gps2Field != vn::protocol::uart::GpsGroup::GPSGROUP_NONE)
7047 {
7048 if (!obs->gnss2Outputs)
7049 {
7050 obs->gnss2Outputs = std::make_shared<NAV::vendor::vectornav::GnssOutputs>();
7051 obs->gnss2Outputs->gnssField |= vnSensor->_binaryOutputRegister.at(b).gps2Field;
7052 }
7053
7054 // if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_UTC)
7055 // {
7056 // obs->gnss2Outputs->timeUtc.year = p.extractInt8();
7057 // obs->gnss2Outputs->timeUtc.month = p.extractUint8();
7058 // obs->gnss2Outputs->timeUtc.day = p.extractUint8();
7059 // obs->gnss2Outputs->timeUtc.hour = p.extractUint8();
7060 // obs->gnss2Outputs->timeUtc.min = p.extractUint8();
7061 // obs->gnss2Outputs->timeUtc.sec = p.extractUint8();
7062 // obs->gnss2Outputs->timeUtc.ms = p.extractUint16();
7063 // }
7064 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_TOW)
7065 {
7066 obs->gnss2Outputs->tow = p.extractUint64();
7067 }
7068 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_WEEK)
7069 {
7070 obs->gnss2Outputs->week = p.extractUint16();
7071 }
7072 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_NUMSATS)
7073 {
7074 obs->gnss2Outputs->numSats = p.extractUint8();
7075 }
7076 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_FIX)
7077 {
7078 obs->gnss2Outputs->fix = p.extractUint8();
7079 }
7080 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_POSLLA)
7081 {
7082 auto vec = p.extractVec3d();
7083 obs->gnss2Outputs->posLla = { vec.x, vec.y, vec.z };
7084 }
7085 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_POSECEF)
7086 {
7087 auto vec = p.extractVec3d();
7088 obs->gnss2Outputs->posEcef = { vec.x, vec.y, vec.z };
7089 }
7090 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_VELNED)
7091 {
7092 auto vec = p.extractVec3f();
7093 obs->gnss2Outputs->velNed = { vec.x, vec.y, vec.z };
7094 }
7095 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_VELECEF)
7096 {
7097 auto vec = p.extractVec3f();
7098 obs->gnss2Outputs->velEcef = { vec.x, vec.y, vec.z };
7099 }
7100 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_POSU)
7101 {
7102 auto vec = p.extractVec3f();
7103 obs->gnss2Outputs->posU = { vec.x, vec.y, vec.z };
7104 }
7105 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_VELU)
7106 {
7107 obs->gnss2Outputs->velU = p.extractFloat();
7108 }
7109 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_TIMEU)
7110 {
7111 obs->gnss2Outputs->timeU = p.extractFloat();
7112 }
7113 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_TIMEINFO)
7114 {
7115 obs->gnss2Outputs->timeInfo.status = p.extractUint8();
7116 obs->gnss2Outputs->timeInfo.leapSeconds = p.extractInt8();
7117 }
7118 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_DOP)
7119 {
7120 obs->gnss2Outputs->dop.gDop = p.extractFloat();
7121 obs->gnss2Outputs->dop.pDop = p.extractFloat();
7122 obs->gnss2Outputs->dop.tDop = p.extractFloat();
7123 obs->gnss2Outputs->dop.vDop = p.extractFloat();
7124 obs->gnss2Outputs->dop.hDop = p.extractFloat();
7125 obs->gnss2Outputs->dop.nDop = p.extractFloat();
7126 obs->gnss2Outputs->dop.eDop = p.extractFloat();
7127 }
7128 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_SATINFO)
7129 {
7130 obs->gnss2Outputs->satInfo.numSats = p.extractUint8();
7131 p.extractUint8(); // Reserved for future use
7132 for (size_t i = 0; i < obs->gnss2Outputs->satInfo.numSats; i++)
7133 {
7134 auto sys = p.extractInt8();
7135 auto svId = p.extractUint8();
7136 auto flags = p.extractUint8();
7137 auto cno = p.extractUint8();
7138 auto qi = p.extractUint8();
7139 auto el = p.extractInt8();
7140 auto az = p.extractInt16();
7141 obs->gnss2Outputs->satInfo.satellites.emplace_back(sys, svId, flags, cno, qi, el, az);
7142 }
7143 }
7144 if (vnSensor->_binaryOutputRegister.at(b).gps2Field & vn::protocol::uart::GpsGroup::GPSGROUP_RAWMEAS)
7145 {
7146 obs->gnss2Outputs->raw.tow = p.extractDouble();
7147 obs->gnss2Outputs->raw.week = p.extractUint16();
7148 obs->gnss2Outputs->raw.numSats = p.extractUint8();
7149 p.extractUint8(); // Reserved for future use
7150 for (size_t i = 0; i < obs->gnss2Outputs->raw.numSats; i++)
7151 {
7152 auto sys = p.extractUint8();
7153 auto svId = p.extractUint8();
7154 auto freq = p.extractUint8();
7155 auto chan = p.extractUint8();
7156 auto slot = p.extractInt8();
7157 auto cno = p.extractUint8();
7158 auto flags = p.extractUint16();
7159 auto pr = p.extractDouble();
7160 auto cp = p.extractDouble();
7161 auto dp = p.extractFloat();
7162 obs->gnss2Outputs->raw.satellites.emplace_back(sys, svId, freq, chan, slot, cno, flags, pr, cp, dp);
7163 }
7164 }
7165 }
7166
7167 if (p.getCurExtractLoc() != p.getPacketLength() - 2) // 2 Bytes CRC should be left
7168 {
7169 LOG_DEBUG("{}: Only {} of {} bytes were extracted from the Binary Output {}", vnSensor->nameId(), p.getCurExtractLoc(), p.getPacketLength(), b + 1);
7170 }
7171
7172 // --------------------------------------------- Fetch InsTime -----------------------------------------------
7173 auto updateSyncOut = [vnSensor, obs](const InsTime& ppsTime) {
7174 if (vnSensor->_synchronizationControlRegister.syncOutMode == vn::protocol::uart::SYNCOUTMODE_GPSPPS
7175 && (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_SYNCOUTCNT))
7176 {
7177 if (obs->timeOutputs->syncOutCnt > 0)
7178 {
7179 if (vnSensor->_timeSyncOut.syncOutCnt == 0)
7180 {
7181 LOG_INFO("{}: Found PPS time {} and is providing it to its connected nodes", vnSensor->nameId(), ppsTime.toYMDHMS(GPST));
7182 }
7183 if (vnSensor->_timeSyncOut.ppsTime != ppsTime || vnSensor->_timeSyncOut.syncOutCnt != obs->timeOutputs->syncOutCnt)
7184 {
7185 LOG_DATA("{}: SyncOut time {}, pps {}, syncOutCnt {}",
7186 vnSensor->nameId(), obs->insTime.toYMDHMS(GPST),
7187 ppsTime.toYMDHMS(GPST), obs->timeOutputs->syncOutCnt);
7188 }
7189 vnSensor->_timeSyncOut.ppsTime = ppsTime;
7190 vnSensor->_timeSyncOut.syncOutCnt = obs->timeOutputs->syncOutCnt;
7191 }
7192 }
7193 };
7194
7195 // Group 2 (Time)
7196 if (obs->timeOutputs && (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS))
7197 {
7198 if (obs->timeOutputs->timeStatus.dateOk())
7199 {
7200 if (obs->timeOutputs->timeStatus.timeOk()
7201 && (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW)
7202 && (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK))
7203 {
7204 obs->insTime = InsTime(InsTime_GPSweekTow(0, obs->timeOutputs->gpsWeek, obs->timeOutputs->gpsTow * 1e-9L));
7205 updateSyncOut(InsTime(0, obs->timeOutputs->gpsWeek, std::floor(obs->timeOutputs->gpsTow * 1e-9L)));
7206 }
7207 else if (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMEGPS)
7208 {
7209 auto secondsSinceEpoche = static_cast<long double>(obs->timeOutputs->timeGps) * 1e-9L;
7210 auto week = static_cast<uint16_t>(secondsSinceEpoche / static_cast<long double>(InsTimeUtil::SECONDS_PER_DAY * InsTimeUtil::DAYS_PER_WEEK));
7211 auto tow = secondsSinceEpoche - week * InsTimeUtil::SECONDS_PER_DAY * InsTimeUtil::DAYS_PER_WEEK;
7212
7213 obs->insTime = InsTime(InsTime_GPSweekTow(0, week, tow));
7214 updateSyncOut(InsTime(0, week, std::floor(tow)));
7215 }
7216 }
7217 }
7218
7219 if (obs->insTime.empty() // Look for master to give GNSS time
7220 && (obs->timeOutputs->timeField & vn::protocol::uart::TIMEGROUP_SYNCINCNT) // We need syncin cnt for this
7221 && (obs->timeOutputs->timeField & vn::protocol::uart::TIMEGROUP_TIMESYNCIN) // We need syncin time for this
7222 && vnSensor->_syncInPin && vnSensor->inputPins.front().isPinLinked()) // Try to get a sync from the master
7223 {
7224 if (auto timeSyncMaster = vnSensor->getInputValue<TimeSync>(0);
7225 timeSyncMaster && !timeSyncMaster->v->ppsTime.empty())
7226 {
7227 // This can have the following values
7228 // - -1: PPS -> VN310 message -> VN100 message (which happened before the VN310 message)
7229 // - 0: PPS -> VN310 message -> VN100 message
7230 // - 1: PPS -> VN100 message -> VN310 message
7231 int64_t syncCntDiff = obs->timeOutputs->syncInCnt - timeSyncMaster->v->syncOutCnt - vnSensor->_syncCntOffset;
7232
7233 if (std::abs(syncCntDiff) > 1) // Counters did not start at the same time
7234 {
7235 vnSensor->_syncCntOffset = obs->timeOutputs->syncInCnt - timeSyncMaster->v->syncOutCnt;
7236 syncCntDiff -= vnSensor->_syncCntOffset;
7237 LOG_DEBUG("{}: Counters did not start at the same time (_syncCntOffset = {})", vnSensor->nameId(), vnSensor->_syncCntOffset);
7238 }
7239 vnSensor->_lastSyncOutCnt = timeSyncMaster->v->syncOutCnt;
7240 vnSensor->_lastSyncInCnt = obs->timeOutputs->syncInCnt;
7241
7242 obs->insTime = timeSyncMaster->v->ppsTime + std::chrono::nanoseconds(obs->timeOutputs->timeSyncIn)
7243 + std::chrono::seconds(syncCntDiff);
7244 LOG_DATA("{}: SyncIn time {}, pps {}, syncOutCnt {}, syncInCnt {}, syncCntDiff {}",
7245 vnSensor->nameId(), obs->insTime.toYMDHMS(GPST), timeSyncMaster->v->ppsTime.toYMDHMS(GPST),
7246 timeSyncMaster->v->syncOutCnt, obs->timeOutputs->syncInCnt, syncCntDiff);
7247 }
7248 else
7249 {
7250 return;
7251 }
7252 }
7253
7254 if (obs->insTime.empty()
7255 && !vnSensor->_lastMessageTime.at(b).empty()
7256 && obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP)
7257 {
7258 obs->insTime = vnSensor->_lastMessageTime.at(b) + std::chrono::nanoseconds(obs->timeOutputs->timeStartup - vnSensor->_lastMessageTimeSinceStartup.at(b));
7259 }
7260
7261 if (obs->insTime.empty() // Look if other sensors have set a global time
7262 && !(obs->timeOutputs // but only if we did not request the time from the sensor via GNSS
7263 && (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTATUS)
7264 && (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_GPSTOW)
7265 && (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_GPSWEEK)))
7266 {
7267 if (InsTime currentTime = util::time::GetCurrentInsTime();
7268 !currentTime.empty())
7269 {
7270 obs->insTime = currentTime;
7271 }
7272 }
7273
7274 if (!obs->insTime.empty())
7275 {
7276 if (!vnSensor->_lastMessageTime.at(b).empty())
7277 {
7278 // FIXME: This seems like a bug in clang-tidy. Check if it is working in future versions of clang-tidy
7279 // NOLINTNEXTLINE(hicpp-use-nullptr, modernize-use-nullptr)
7280 if (obs->insTime - vnSensor->_lastMessageTime.at(b) >= std::chrono::duration<double>(1.5 * (vnSensor->_binaryOutputRegister.at(b).rateDivisor / IMU_DEFAULT_FREQUENCY)))
7281 {
7282 LOG_WARN("{}: Potentially lost a message. dt = {:.4} s, expect {} s. (Previous message at [{}], current message [{}])", vnSensor->nameId(),
7283 static_cast<double>((obs->insTime - vnSensor->_lastMessageTime.at(b)).count()),
7284 1. / IMU_DEFAULT_FREQUENCY * vnSensor->_binaryOutputRegister.at(b).rateDivisor,
7285 vnSensor->_lastMessageTime.at(b), obs->insTime);
7286 }
7287 }
7288 vnSensor->_lastMessageTime.at(b) = obs->insTime;
7289 if (obs->timeOutputs->timeField & vn::protocol::uart::TimeGroup::TIMEGROUP_TIMESTARTUP)
7290 {
7291 vnSensor->_lastMessageTimeSinceStartup.at(b) = obs->timeOutputs->timeStartup;
7292 }
7293 }
7294
7295 if ((vnSensor->_binaryOutputRegisterMerge == BinaryRegisterMerge::Output1_Output2 && (b == 0 || b == 1))
7296 || (vnSensor->_binaryOutputRegisterMerge == BinaryRegisterMerge::Output1_Output3 && (b == 0 || b == 2))
7297 || (vnSensor->_binaryOutputRegisterMerge == BinaryRegisterMerge::Output2_Output3 && (b == 1 || b == 2)))
7298 {
7299 if (vnSensor->_binaryOutputRegisterMergeObservation == nullptr)
7300 {
7301 std::stringstream sstream;
7302 if (!obs->insTime.empty())
7303 {
7304 sstream << obs->insTime;
7305 }
7306 else
7307 {
7308 sstream << obs->timeOutputs->timeStartup;
7309 }
7310 LOG_DATA("{}: {} - Storing message with time {}", vnSensor->nameId(), b, sstream.str());
7311
7312 vnSensor->_binaryOutputRegisterMergeObservation = obs;
7313 vnSensor->_binaryOutputRegisterMergeIndex = b;
7314 }
7315 else
7316 {
7317 auto allowedTimeDiff = std::chrono::microseconds(static_cast<int>(0.5 * (vnSensor->_binaryOutputRegister.at(b).rateDivisor / IMU_DEFAULT_FREQUENCY) * 1e6));
7318 if (vnSensor->_binaryOutputRegisterMergeIndex != b
7319 && ((!obs->insTime.empty() && !vnSensor->_binaryOutputRegisterMergeObservation->insTime.empty()
7320 && (obs->insTime - vnSensor->_binaryOutputRegisterMergeObservation->insTime < allowedTimeDiff)) // NOLINT(hicpp-use-nullptr, modernize-use-nullptr)
7321 || (obs->timeOutputs != nullptr && vnSensor->_binaryOutputRegisterMergeObservation->timeOutputs != nullptr
7322 && obs->timeOutputs->timeField & vn::protocol::uart::TIMEGROUP_TIMESTARTUP
7323 && vnSensor->_binaryOutputRegisterMergeObservation->timeOutputs->timeField & vn::protocol::uart::TIMEGROUP_TIMESTARTUP
7324 && (std::chrono::nanoseconds(obs->timeOutputs->timeStartup - vnSensor->_binaryOutputRegisterMergeObservation->timeOutputs->timeStartup) < allowedTimeDiff)))) // NOLINT(hicpp-use-nullptr, modernize-use-nullptr)
7325 {
7326 // Stored and new observation have same time, so merge them
7327 mergeVectorNavBinaryObservations(obs, vnSensor->_binaryOutputRegisterMergeObservation);
7328 vnSensor->_binaryOutputRegisterMergeObservation = nullptr;
7329
7330 std::stringstream sstream;
7331 if (!obs->insTime.empty())
7332 {
7333 sstream << obs->insTime;
7334 }
7335 else
7336 {
7337 sstream << obs->timeOutputs->timeStartup;
7338 }
7339 LOG_DATA("{}: {} - Merged message with time {}", vnSensor->nameId(), b, sstream.str());
7340
7341 vnSensor->invokeCallbacks(std::min(b, vnSensor->_binaryOutputRegisterMergeIndex) + 1, obs);
7342 }
7343 else
7344 {
7345 [[maybe_unused]] long double timeDiff = 0.0L;
7346 std::stringstream sstreamOld;
7347 std::stringstream sstreamNew;
7348 if (!obs->insTime.empty() && !vnSensor->_binaryOutputRegisterMergeObservation->insTime.empty())
7349 {
7350 timeDiff = (obs->insTime - vnSensor->_binaryOutputRegisterMergeObservation->insTime).count();
7351 sstreamOld << vnSensor->_binaryOutputRegisterMergeObservation->insTime;
7352 sstreamNew << obs->insTime;
7353 }
7354 else
7355 {
7356 sstreamOld << vnSensor->_binaryOutputRegisterMergeObservation->timeOutputs->timeStartup;
7357 sstreamNew << obs->timeOutputs->timeStartup;
7358 timeDiff = static_cast<double>(obs->timeOutputs->timeStartup
7359 - vnSensor->_binaryOutputRegisterMergeObservation->timeOutputs->timeStartup)
7360 * 1e-9;
7361 }
7362
7363 LOG_WARN("{}: Merging failed, because timestamps do not match. Potentially lost a message (diff {} sec, old ({}) {}, new ({}) {})",
7364 vnSensor->nameId(), timeDiff, vnSensor->_binaryOutputRegisterMergeIndex, sstreamOld.str(), b, sstreamNew.str());
7365
7366 // Send out old message and save new one
7367 vnSensor->invokeCallbacks(vnSensor->_binaryOutputRegisterMergeIndex + 1, vnSensor->_binaryOutputRegisterMergeObservation);
7368 vnSensor->_binaryOutputRegisterMergeObservation = obs;
7369 vnSensor->_binaryOutputRegisterMergeIndex = b;
7370 }
7371 }
7372 }
7373 else
7374 {
7375 if (!obs->insTime.empty())
7376 {
7377 LOG_DATA("{}: {} - Normal message with time {}", vnSensor->nameId(), b, obs->insTime);
7378 }
7379 // Calls all the callbacks
7380 vnSensor->invokeCallbacks(b + 1, obs);
7381 }
7382 }
7383 }
7384 }
7385 else if (p.type() == vn::protocol::uart::Packet::TYPE_ASCII)
7386 {
7387 LOG_DATA("{} received an ASCII Async message: {}", vnSensor->nameId(), p.datastr());
7388 vnSensor->_asciiOutputBuffer.push_back(p.datastr());
7389
7390 auto obs = std::make_shared<StringObs>(p.datastr());
7391
7392 if (InsTime currentTime = util::time::GetCurrentInsTime();
7393 !currentTime.empty())
7394 {
7395 obs->insTime = currentTime;
7396 }
7397 // Calls all the callbacks
7398 vnSensor->invokeCallbacks(VectorNavSensor::OUTPUT_PORT_INDEX_ASCII_OUTPUT, obs);
7399 }
7400}
CoordinateFrames.hpp
Transformation collection.
FlowManager.hpp
Save/Load the Nodes.
json
nlohmann::json json
json namespace
Definition FlowManager.hpp:23
HelpMarker.hpp
Text Help Marker (?) with Tooltip.
Logger.hpp
Utility class for logging to console and file.
LOG_DEBUG
#define LOG_DEBUG
Debug information. Should not be called on functions which receive observations (spamming)
Definition Logger.hpp:67
LOG_DATA
#define LOG_DATA
All output which occurs repeatedly every time observations are received.
Definition Logger.hpp:29
LOG_ERROR
#define LOG_ERROR
Error occurred, which stops part of the program to work, but not everything.
Definition Logger.hpp:73
LOG_WARN
#define LOG_WARN
Error occurred, but a fallback option exists and program continues to work normally.
Definition Logger.hpp:71
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
StringObs.hpp
Wrapper for String Messages.
TimeBase.hpp
Keeps track of the current real/simulation time.
VectorNavSensor.hpp
Vector Nav Sensors.
VectorNavTypes.hpp
Type Definitions for VectorNav messages.
NAV::Imu::Imu
Imu(const Imu &)=delete
Copy constructor.
NAV::InsTime
The class is responsible for all time-related tasks.
Definition InsTime.hpp:710
NAV::InsTime::empty
constexpr bool empty() const
Checks if the Time object has a value.
Definition InsTime.hpp:1089
NAV::InsTime::reset
void reset()
Resets the InsTime object.
Definition InsTime.hpp:1095
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::outputPins
std::vector< OutputPin > outputPins
List of output pins.
Definition Node.hpp:511
NAV::Node::DeleteOutputPin
bool DeleteOutputPin(size_t pinIndex)
Deletes the output pin. Invalidates the pin reference given.
Definition Node.cpp:298
NAV::Node::inputPins
std::vector< InputPin > inputPins
List of input pins.
Definition Node.hpp:509
NAV::Node::CreateOutputPin
OutputPin * CreateOutputPin(const char *name, Pin::Type pinType, const std::vector< std::string > &dataIdentifier, OutputPin::PinData data=static_cast< void * >(nullptr), int idx=-1)
Create an Output Pin object.
Definition Node.cpp:278
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::_onlyRealTime
bool _onlyRealTime
Whether the node can run in post-processing or only real-time.
Definition Node.hpp:531
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::DeleteInputPin
bool DeleteInputPin(size_t pinIndex)
Deletes the input pin. Invalidates the pin reference given.
Definition Node.cpp:310
NAV::Node::_hasConfig
bool _hasConfig
Flag if the config window should be shown.
Definition Node.hpp:525
NAV::StringObs::type
static std::string type()
Returns the type of the data class.
Definition StringObs.hpp:33
NAV::UartSensor::_selectedBaudrate
int _selectedBaudrate
Baudrate for the sensor.
Definition UartSensor.hpp:83
NAV::UartSensor::Baudrate
Baudrate
Available Baudrates.
Definition UartSensor.hpp:30
NAV::UartSensor::BAUDRATE_115200
@ BAUDRATE_115200
Baudrate with 115200 symbols per second [Baud].
Definition UartSensor.hpp:36
NAV::UartSensor::BAUDRATE_FASTEST
@ BAUDRATE_FASTEST
Fastest possible Baudrate will be automatically chosen.
Definition UartSensor.hpp:31
NAV::UartSensor::_sensorPort
std::string _sensorPort
Definition UartSensor.hpp:80
NAV::UartSensor::restore
void restore(const json &j)
Restores the node from a json object.
Definition UartSensor.cpp:25
NAV::UartSensor::sensorBaudrate
Baudrate sensorBaudrate() const
Returns the Baudrate for the element Selected by the GUI.
Definition UartSensor.cpp:41
NAV::UartSensor::save
json save() const
Saves the node into a json object.
Definition UartSensor.cpp:13
NAV::VectorNavBinaryOutput::type
static std::string type()
Returns the type of the data class.
Definition VectorNavBinaryOutput.hpp:48
NAV::VectorNavSensor
Vector Nav Sensor Class.
Definition VectorNavSensor.hpp:44
NAV::VectorNavSensor::OUTPUT_PORT_INDEX_ASCII_OUTPUT
static constexpr size_t OUTPUT_PORT_INDEX_ASCII_OUTPUT
Flow (StringObs)
Definition VectorNavSensor.hpp:90
NAV::VectorNavSensor::_binaryOutputRegister
std::array< vn::sensors::BinaryOutputRegister, 3 > _binaryOutputRegister
Binary Output Register 1 - 3.
Definition VectorNavSensor.hpp:245
NAV::VectorNavSensor::_lastSyncOutCnt
int64_t _lastSyncOutCnt
Last received syncOutCnt.
Definition VectorNavSensor.hpp:195
NAV::VectorNavSensor::_magnetometerCalibrationControlRegister
vn::sensors::MagnetometerCalibrationControlRegister _magnetometerCalibrationControlRegister
Magnetometer Calibration Control.
Definition VectorNavSensor.hpp:435
NAV::VectorNavSensor::typeStatic
static std::string typeStatic()
String representation of the Class Type.
Definition VectorNavSensor.cpp:1694
NAV::VectorNavSensor::_lastMessageTimeSinceStartup
std::array< uint64_t, 3 > _lastMessageTimeSinceStartup
Stores the time of the last received message.
Definition VectorNavSensor.hpp:138
NAV::VectorNavSensor::category
static std::string category()
String representation of the Class Category.
Definition VectorNavSensor.cpp:1704
NAV::VectorNavSensor::_vpeAccelerometerBasicTuningRegister
vn::sensors::VpeAccelerometerBasicTuningRegister _vpeAccelerometerBasicTuningRegister
VPE Accelerometer Basic Tuning.
Definition VectorNavSensor.hpp:381
NAV::VectorNavSensor::save
json save() const override
Saves the node into a json object.
Definition VectorNavSensor.cpp:5512
NAV::VectorNavSensor::_magneticAndGravityReferenceVectorsRegister
vn::sensors::MagneticAndGravityReferenceVectorsRegister _magneticAndGravityReferenceVectorsRegister
Magnetic and Gravity Reference Vectors.
Definition VectorNavSensor.hpp:449
NAV::VectorNavSensor::_possibleAsyncDataOutputFrequency
static constexpr std::array _possibleAsyncDataOutputFrequency
Possible values for the Async Data Output Frequency Register.
Definition VectorNavSensor.hpp:157
NAV::VectorNavSensor::_asyncDataOutputFrequencySelected
int _asyncDataOutputFrequencySelected
Selected Frequency of the Async Ascii Output in the GUI.
Definition VectorNavSensor.hpp:163
NAV::VectorNavSensor::_lastMessageTime
std::array< InsTime, 3 > _lastMessageTime
Stores the time of the last received message.
Definition VectorNavSensor.hpp:135
NAV::VectorNavSensor::_binaryGroupTime
static const std::array< BinaryGroupData, 10 > _binaryGroupTime
Binary group 1 contains a wide assortment of commonly used data required for most applications.
Definition VectorNavSensor.hpp:729
NAV::VectorNavSensor::_filterStartupGyroBias
vn::math::vec3f _filterStartupGyroBias
Filter Startup Gyro Bias.
Definition VectorNavSensor.hpp:401
NAV::VectorNavSensor::type
std::string type() const override
String representation of the Class Type.
Definition VectorNavSensor.cpp:1699
NAV::VectorNavSensor::_binaryGroupIMU
static const std::array< BinaryGroupData, 11 > _binaryGroupIMU
Binary group 3 provides all outputs which are dependent upon the measurements collected from the onbo...
Definition VectorNavSensor.hpp:893
NAV::VectorNavSensor::_sensorModel
VectorNavModel _sensorModel
The sensor model which is selected in the GUI.
Definition VectorNavSensor.hpp:120
NAV::VectorNavSensor::_coupleImuRateOutput
bool _coupleImuRateOutput
Couple the ImuFilter's rate (window size of moving-average filter) to the output rate (rateDivisor)
Definition VectorNavSensor.hpp:201
NAV::VectorNavSensor::coupleImuFilterRates
static void coupleImuFilterRates(NAV::VectorNavSensor *sensor, vn::sensors::BinaryOutputRegister &bor, uint32_t &binaryField)
Updates the ImuFilter's rate when pressing the checkbox button.
Definition VectorNavSensor.cpp:627
NAV::VectorNavSensor::_deltaThetaAndDeltaVelocityConfigurationRegister
vn::sensors::DeltaThetaAndDeltaVelocityConfigurationRegister _deltaThetaAndDeltaVelocityConfigurationRegister
Delta Theta and Delta Velocity Configuration.
Definition VectorNavSensor.hpp:314
NAV::VectorNavSensor::_binaryOutputRegisterMergeObservation
std::shared_ptr< VectorNavBinaryOutput > _binaryOutputRegisterMergeObservation
First observation received, which should be merged together.
Definition VectorNavSensor.hpp:236
NAV::VectorNavSensor::guiConfig
void guiConfig() override
ImGui config window which is shown on double click.
Definition VectorNavSensor.cpp:1709
NAV::VectorNavSensor::_vs
vn::sensors::VnSensor _vs
VnSensor Object.
Definition VectorNavSensor.hpp:123
NAV::VectorNavSensor::_connectedSensorPort
std::string _connectedSensorPort
Connected sensor port.
Definition VectorNavSensor.hpp:126
NAV::VectorNavSensor::~VectorNavSensor
~VectorNavSensor() override
Destructor.
Definition VectorNavSensor.cpp:1689
NAV::VectorNavSensor::_asciiOutputBuffer
ScrollingBuffer< std::string > _asciiOutputBuffer
Buffer to store Ascii Output Messages.
Definition VectorNavSensor.hpp:169
NAV::VectorNavSensor::_vpeGyroBasicTuningRegister
vn::sensors::VpeGyroBasicTuningRegister _vpeGyroBasicTuningRegister
VPE Gyro Basic Tuning.
Definition VectorNavSensor.hpp:391
NAV::VectorNavSensor::_binaryOutputSelectedFrequency
std::array< size_t, 3 > _binaryOutputSelectedFrequency
Selected Frequency of the Binary Outputs in the GUI.
Definition VectorNavSensor.hpp:279
NAV::VectorNavSensor::_referenceFrameRotationMatrix
vn::math::mat3f _referenceFrameRotationMatrix
Reference Frame Rotation.
Definition VectorNavSensor.hpp:289
NAV::VectorNavSensor::_asyncDataOutputType
vn::protocol::uart::AsciiAsync _asyncDataOutputType
Async Data Output Type Register.
Definition VectorNavSensor.hpp:153
NAV::VectorNavSensor::_imuFilteringConfigurationRegister
vn::sensors::ImuFilteringConfigurationRegister _imuFilteringConfigurationRegister
IMU Filtering Configuration.
Definition VectorNavSensor.hpp:297
NAV::VectorNavSensor::_gpsCompassBaselineRegister
vn::sensors::GpsCompassBaselineRegister _gpsCompassBaselineRegister
GNSS Compass Baseline.
Definition VectorNavSensor.hpp:347
NAV::VectorNavSensor::_startupFilterBiasEstimateRegister
vn::sensors::StartupFilterBiasEstimateRegister _startupFilterBiasEstimateRegister
Startup Filter Bias Estimate.
Definition VectorNavSensor.hpp:421
NAV::VectorNavSensor::_gpsConfigurationRegister
vn::sensors::GpsConfigurationRegister _gpsConfigurationRegister
GNSS Configuration.
Definition VectorNavSensor.hpp:327
NAV::VectorNavSensor::_gnssTimeCounter
struct NAV::VectorNavSensor::@061131263266062172247357061343017357216211246313 _gnssTimeCounter
Last received GNSS time.
NAV::VectorNavSensor::VectorNavSensor
VectorNavSensor()
Default constructor.
Definition VectorNavSensor.cpp:1651
NAV::VectorNavSensor::restore
void restore(const json &j) override
Restores the node from a json object.
Definition VectorNavSensor.cpp:5594
NAV::VectorNavSensor::_binaryGroupINS
static const std::array< BinaryGroupData, 11 > _binaryGroupINS
Binary group 6 provides all estimated outputs which are dependent upon the onboard INS state solution...
Definition VectorNavSensor.hpp:1536
NAV::VectorNavSensor::_binaryGroupAttitude
static const std::array< BinaryGroupData, 9 > _binaryGroupAttitude
Binary group 5 provides all estimated outputs which are dependent upon the estimated attitude solutio...
Definition VectorNavSensor.hpp:1425
NAV::VectorNavSensor::VectorNavModel::VN310
@ VN310
VN-310/E (Tactical-Grade GNSS/INS with Integrated GNSS-Compass)
Definition VectorNavSensor.hpp:116
NAV::VectorNavSensor::_vpeMagnetometerBasicTuningRegister
vn::sensors::VpeMagnetometerBasicTuningRegister _vpeMagnetometerBasicTuningRegister
VPE Magnetometer Basic Tuning.
Definition VectorNavSensor.hpp:371
NAV::VectorNavSensor::initialize
bool initialize() override
Initialize the node.
Definition VectorNavSensor.cpp:5810
NAV::VectorNavSensor::_asciiOutputBufferSize
int _asciiOutputBufferSize
Max size of the Ascii Output.
Definition VectorNavSensor.hpp:166
NAV::VectorNavSensor::_gpsAntennaOffset
vn::math::vec3f _gpsAntennaOffset
GNSS Antenna A Offset.
Definition VectorNavSensor.hpp:338
NAV::VectorNavSensor::IMU_DEFAULT_FREQUENCY
static constexpr double IMU_DEFAULT_FREQUENCY
Internal Frequency of the Sensor.
Definition VectorNavSensor.hpp:129
NAV::VectorNavSensor::_syncCntOffset
int64_t _syncCntOffset
Offset between syncIn and syncOut.
Definition VectorNavSensor.hpp:198
NAV::VectorNavSensor::_synchronizationControlRegister
vn::sensors::SynchronizationControlRegister _synchronizationControlRegister
Synchronization Control.
Definition VectorNavSensor.hpp:175
NAV::VectorNavSensor::_vpeBasicControlRegister
vn::sensors::VpeBasicControlRegister _vpeBasicControlRegister
VPE Basic Control.
Definition VectorNavSensor.hpp:360
NAV::VectorNavSensor::deinitialize
void deinitialize() override
Deinitialize the node.
Definition VectorNavSensor.cpp:6440
NAV::VectorNavSensor::asciiOrBinaryAsyncMessageReceived
static void asciiOrBinaryAsyncMessageReceived(void *userData, vn::protocol::uart::Packet &p, size_t index)
Callback handler for notifications of new asynchronous data packets received.
Definition VectorNavSensor.cpp:6640
NAV::VectorNavSensor::_communicationProtocolControlRegister
vn::sensors::CommunicationProtocolControlRegister _communicationProtocolControlRegister
Communication Protocol Control.
Definition VectorNavSensor.hpp:213
NAV::VectorNavSensor::_timeSyncOut
TimeSync _timeSyncOut
Time synchronization for master sensors.
Definition VectorNavSensor.hpp:186
NAV::VectorNavSensor::_asyncDataOutputFrequency
uint32_t _asyncDataOutputFrequency
Async Data Output Frequency Register.
Definition VectorNavSensor.hpp:161
NAV::VectorNavSensor::_binaryOutputRegisterMerge
BinaryRegisterMerge _binaryOutputRegisterMerge
Merge binary output registers together. This has to be done because VectorNav sensors have a buffer o...
Definition VectorNavSensor.hpp:233
NAV::VectorNavSensor::_referenceVectorConfigurationRegister
vn::sensors::ReferenceVectorConfigurationRegister _referenceVectorConfigurationRegister
Reference Vector Configuration.
Definition VectorNavSensor.hpp:458
NAV::VectorNavSensor::_syncInPin
bool _syncInPin
Show the SyncIn Pin.
Definition VectorNavSensor.hpp:189
NAV::VectorNavSensor::_insBasicConfigurationRegisterVn300
vn::sensors::InsBasicConfigurationRegisterVn300 _insBasicConfigurationRegisterVn300
INS Basic Configuration.
Definition VectorNavSensor.hpp:411
NAV::VectorNavSensor::_dividerFrequency
std::pair< std::vector< uint16_t >, std::vector< std::string > > _dividerFrequency
First: List of RateDividers, Second: List of Matching Frequencies.
Definition VectorNavSensor.hpp:132
NAV::VectorNavSensor::_lastSyncInCnt
int64_t _lastSyncInCnt
Last received syncInCnt.
Definition VectorNavSensor.hpp:192
NAV::VectorNavSensor::resetNode
bool resetNode() override
Resets the node. It is guaranteed that the node is initialized when this is called.
Definition VectorNavSensor.cpp:5793
NAV::VectorNavSensor::_binaryGroupGNSS
static const std::array< BinaryGroupData, 16 > _binaryGroupGNSS
Binary group 4 provides all outputs which are dependent upon the measurements collected from the prim...
Definition VectorNavSensor.hpp:927
NAV::VectorNavSensor::_velocityCompensationControlRegister
vn::sensors::VelocityCompensationControlRegister _velocityCompensationControlRegister
Velocity Compensation Control.
Definition VectorNavSensor.hpp:474
NAV::VectorNavSensor::mergeVectorNavBinaryObservations
static void mergeVectorNavBinaryObservations(const std::shared_ptr< VectorNavBinaryOutput > &target, const std::shared_ptr< VectorNavBinaryOutput > &source)
Merges the content of the two observations into one.
Definition VectorNavSensor.cpp:6501
NAV::InsTimeUtil::SECONDS_PER_DAY
constexpr int32_t SECONDS_PER_DAY
Seconds / Day.
Definition InsTime.hpp:60
NAV::InsTimeUtil::DAYS_PER_WEEK
constexpr int32_t DAYS_PER_WEEK
Days / Week.
Definition InsTime.hpp:52
NAV::flow::ApplyChanges
void ApplyChanges()
Signals that there have been changes to the flow.
Definition FlowManager.cpp:682
NAV::gui::widgets::HelpMarker
void HelpMarker(const char *desc, const char *symbol="(?)")
Text Help Marker, e.g. '(?)', with Tooltip.
Definition HelpMarker.cpp:14
NAV::trafo::b_Quat_p
Eigen::Quaternion< Scalar > b_Quat_p(Scalar mountingAngleX, Scalar mountingAngleY, Scalar mountingAngleZ)
Quaternion for rotations from platform to body frame.
Definition CoordinateFrames.hpp:381
NAV::trafo::quat2eulerZYX
Eigen::Vector3< typename Derived::Scalar > quat2eulerZYX(const Eigen::QuaternionBase< Derived > &q)
Converts the quaternion to Euler rotation angles with rotation sequence ZYX.
Definition CoordinateFrames.hpp:141
NAV::GPST
@ GPST
GPS Time.
Definition TimeSystem.hpp:30
NAV::deg2rad
constexpr auto deg2rad(const T &deg)
Convert Degree to Radians.
Definition Units.hpp:21
NAV::rad2deg
constexpr auto rad2deg(const T &rad)
Convert Radians to Degree.
Definition Units.hpp:39
vn::math::from_json
static void from_json(const json &j, vn::math::vec3f &vec)
Definition VectorNavSensor.cpp:40
vn::math::to_json
static void to_json(json &j, const vn::math::vec3f &vec)
Definition VectorNavSensor.cpp:36
vn::sensors::from_json
static void from_json(const json &j, SynchronizationControlRegister &synchronizationControlRegister)
Definition VectorNavSensor.cpp:80
vn::sensors::to_json
static void to_json(json &j, const SynchronizationControlRegister &synchronizationControlRegister)
Definition VectorNavSensor.cpp:68
NAV::InsTime_GPSweekTow
GPS week and time of week in GPS standard time [GPST].
Definition InsTime.hpp:369
NAV::Pin::Type::None
@ None
Not initialized.
Definition Pin.hpp:51
NAV::Pin::Type::Flow
@ Flow
NodeData Trigger.
Definition Pin.hpp:52
NAV::Pin::Type::Object
@ Object
Generic Object.
Definition Pin.hpp:57
NAV::VectorNavSensor::TimeSync
Information needed to sync Master/Slave sensors.
Definition VectorNavSensor.hpp:48
NAV::VectorNavSensor::TimeSync::ppsTime
InsTime ppsTime
Time of the last message with GNSS Time available (or empty otherwise)
Definition VectorNavSensor.hpp:49