INSTINCT Code Coverage Report


Directory: src/
File: util/Vendor/VectorNav/VectorNavTypes.hpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 41 143 28.7%
Functions: 21 37 56.8%
Branches: 25 56 44.6%

Line Branch Exec Source
1 // This file is part of INSTINCT, the INS Toolkit for Integrated
2 // Navigation Concepts and Training by the Institute of Navigation of
3 // the University of Stuttgart, Germany.
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9 /// @file VectorNavTypes.hpp
10 /// @brief Type Definitions for VectorNav messages
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2021-07-01
13
14 #pragma once
15
16 // VectorNav library includes <winsock2.h>, but <boost/asio.hpp> needs to be included before (even though not used in this file)
17 // https://stackoverflow.com/questions/9750344/boostasio-winsock-and-winsock-2-compatibility-issue
18 #ifdef _WIN32
19 // Set the proper SDK version before including boost/Asio
20 #include <SDKDDKVer.h>
21 // Note boost/ASIO includes Windows.h.
22 #include <boost/asio.hpp>
23 #endif //_WIN32
24
25 #include <cstdint>
26 #include <vector>
27 #include <iostream>
28 #include <fmt/format.h>
29 #include <fmt/ostream.h>
30 #include <vn/vector.h>
31 #include <vn/matrix.h>
32 #include <vn/types.h>
33
34 namespace NAV::vendor::vectornav
35 {
36 /// @brief The VPE status bitfield
37 ///
38 /// Bit | Name | Description
39 /// 0 | timeOk | GpsTow is valid.
40 /// 1 | dateOk | TimeGps and GpsWeek are valid.
41 /// 2 | utcTimeValid | UTC time is valid.
42 class TimeStatus
43 {
44 public:
45 /// Constructor
46 /// @param[in] status Status to set
47 explicit TimeStatus(uint8_t status) : _status(status) {}
48
49 /// @brief Assignment operator
50 /// @param[in] status Status to set
51 57173 TimeStatus& operator=(const uint8_t& status)
52 {
53 57173 _status = status;
54 57173 return *this;
55 }
56
57 /// @brief Default constructor
58 TimeStatus() = default;
59
60 /// @brief Returns a reference to the status
61 426 [[nodiscard]] constexpr uint8_t& status()
62 {
63 426 return _status;
64 }
65
66 /// GpsTow is valid
67 29074 [[nodiscard]] constexpr uint8_t timeOk() const
68 {
69 29074 return ((_status & (1U << 0U)) >> 0U);
70 }
71 /// TimeGps and GpsWeek are valid.
72 29074 [[nodiscard]] constexpr uint8_t dateOk() const
73 {
74 29074 return ((_status & (1U << 1U)) >> 1U); // NOLINT
75 }
76 /// UTC time is valid.
77 690 [[nodiscard]] constexpr uint8_t utcTimeValid() const
78 {
79 690 return ((_status & (1U << 2U)) >> 2U); // NOLINT
80 }
81
82 private:
83 /// The storage field
84 uint8_t _status;
85 };
86
87 /// @brief Storage class for UTC Time
88 struct UTC
89 {
90 int8_t year{}; ///< The year is given as a signed byte year offset from the year 2000. For example the year 2013 would be given as year 13.
91 uint8_t month{}; ///< Months
92 uint8_t day{}; ///< Days
93 uint8_t hour{}; ///< Hours
94 uint8_t min{}; ///< Minutes
95 uint8_t sec{}; ///< Seconds
96 uint16_t ms{}; ///< Milliseconds
97 };
98
99 /// @brief GNSS fix.
100 enum GnssFix : uint8_t
101 {
102 GnssFix_NoFix, ///< No fix
103 GnssFix_TimeOnly, ///< Time only
104 GnssFix_2D, ///< 2D
105 GnssFix_3D, ///< 3D
106 GnssFix_SBAS, ///< SBAS
107 GnssFix_RTK_Float, ///< RTK Float (only GNSS1)
108 GnssFix_RTK_Fixed ///< RTK Fixed (only GNSS1)
109 };
110
111 /// @brief Flags for valid GPS TOW, week number and UTC and current leap seconds.
112 struct TimeInfo
113 {
114 /// Fields: timeOk | dateOk | utcTimeValid | resv | resv | resv | resv | resv
115 /// Bit Offset: 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7
116 ///
117 /// Name | Description
118 /// ------------ | ----------------------------------
119 /// timeOk | 1 – GpsTow is valid.
120 /// dateOk | 1 – TimeGps and GpsWeek are valid.
121 /// utcTimeValid | 1 – UTC time is valid.
122 /// resv | Reserved for future use.
123 TimeStatus status{};
124 /// @brief Amount of leap seconds
125 int8_t leapSeconds{};
126 };
127
128 /// @brief Dilution of precision
129 struct DOP
130 {
131 float gDop{}; ///< Geometric DOP
132 float pDop{}; ///< Positional DOP (Overall 3D position precision)
133 float tDop{}; ///< Time DOP (time precision)
134 float vDop{}; ///< Vertical DOP (vertical position precision)
135 float hDop{}; ///< Horizontal DOP (2D position precision)
136 float nDop{}; ///< North DOP
137 float eDop{}; ///< East DOP
138 };
139
140 /// @brief Satellite Constellation
141 enum class SatSys : uint8_t
142 {
143 GPS = 0, ///< GPS
144 SBAS = 1, ///< SBAS
145 Galileo = 2, ///< Galileo
146 BeiDou = 3, ///< BeiDou
147 IMES = 4, ///< IMES
148 QZSS = 5, ///< QZSS
149 GLONASS = 6, ///< GLONASS
150 };
151
152 /// @brief Stream insertion operator overload
153 /// @param[in, out] os Output stream where data gets printed to
154 /// @param[in] satSys Satellite Constellation
155 /// @return Output stream object
156 std::ostream& operator<<(std::ostream& os, const SatSys& satSys);
157
158 /// @brief Information and measurements pertaining to each GNSS satellite in view.
159 ///
160 /// The size of this packet will vary depending upon the number of satellites in view. To parse this packet you
161 /// will first need to read the number of satellites (numSats) in the beginning of the packet to determine the
162 /// packets overall length. The total length of the packet payload will be 2 + N*8 bytes where N is the number of
163 /// satellites (numSats).
164 struct SatInfo
165 {
166 /// @brief Information for a certain satellite
167 struct SatInfoElement
168 {
169 /// @brief Tracking info flags
170 enum class Flags : uint8_t
171 {
172 None = 0, ///< No flag set
173 Healthy = 1 << 0, ///< Healthy
174 Almanac = 1 << 1, ///< Almanac
175 Ephemeris = 1 << 2, ///< Ephemeris
176 DifferentialCorrection = 1 << 3, ///< Differential Correction
177 UsedForNavigation = 1 << 4, ///< Used for Navigation
178 AzimuthElevationValid = 1 << 5, ///< Azimuth / Elevation Valid
179 UsedForRTK = 1 << 6, ///< Used for RTK
180 };
181
182 /// @brief Binary or-operator
183 /// @param[in] lhs Left-hand side
184 /// @param[in] rhs Right-hand side
185 /// @return Binary or-ed result
186 2090130 friend Flags operator|(Flags lhs, Flags rhs)
187 {
188 2090130 return Flags(int(lhs) | int(rhs));
189 }
190
191 /// @brief Quality Indicator
192 enum class QualityIndicator : uint8_t
193 {
194 NoSignal = 0, ///< No signal
195 SearchingSignal = 1, ///< Searching signal
196 SignalAcquired = 2, ///< Signal acquired
197 SignalDetectedButUnstable = 3, ///< Signal detected but unstable
198 CodeLockedAndTimeSynchronized = 4, ///< Code locked and time synchronized
199 CodeAndCarrierLockedAndTimeSynchronized1 = 5, ///< Code and carrier locked and time synchronized
200 CodeAndCarrierLockedAndTimeSynchronized2 = 6, ///< Code and carrier locked and time synchronized
201 CodeAndCarrierLockedAndTimeSynchronized3 = 7, ///< Code and carrier locked and time synchronized
202 };
203
204 /// @brief Default Constructor
205 2267 SatInfoElement() = default;
206
207 /// @brief Constructor
208 /// @param[in] sys GNSS constellation indicator
209 /// @param[in] svId Space vehicle Id
210 /// @param[in] flags Tracking info flags
211 /// @param[in] cno Carrier-to-noise density ratio (signal strength) [dB-Hz]
212 /// @param[in] qi Quality Indicator
213 /// @param[in] el Elevation in degrees
214 /// @param[in] az Azimuth angle in degrees
215 69531 SatInfoElement(uint8_t sys, uint8_t svId, uint8_t flags, uint8_t cno, uint8_t qi, int8_t el, int16_t az)
216 69531 : sys(static_cast<SatSys>(sys)), svId(svId), flags(static_cast<Flags>(flags)), cno(cno), qi(static_cast<QualityIndicator>(qi)), el(el), az(az) {}
217
218 /// @brief Constructor
219 /// @param[in] sys GNSS constellation indicator
220 /// @param[in] svId Space vehicle Id
221 /// @param[in] healthy Healthy
222 /// @param[in] almanac Almanac
223 /// @param[in] ephemeris Ephemeris
224 /// @param[in] differentialCorrection Differential Correction
225 /// @param[in] usedForNavigation Used for Navigation
226 /// @param[in] azimuthElevationValid Azimuth / Elevation Valid
227 /// @param[in] usedForRTK Used for RTK
228 /// @param[in] cno Carrier-to-noise density ratio (signal strength) [dB-Hz]
229 /// @param[in] qi Quality Indicator
230 /// @param[in] el Elevation in degrees
231 /// @param[in] az Azimuth angle in degrees
232 348355 SatInfoElement(uint8_t sys, uint8_t svId,
233 uint8_t healthy, uint8_t almanac, uint8_t ephemeris, uint8_t differentialCorrection, uint8_t usedForNavigation, uint8_t azimuthElevationValid, uint8_t usedForRTK,
234 uint8_t cno, uint8_t qi, int8_t el, int16_t az)
235
14/14
✓ Branch 0 taken 60525 times.
✓ Branch 1 taken 287830 times.
✓ Branch 2 taken 269269 times.
✓ Branch 3 taken 79086 times.
✓ Branch 4 taken 193680 times.
✓ Branch 5 taken 154675 times.
✓ Branch 6 taken 7263 times.
✓ Branch 7 taken 341092 times.
✓ Branch 8 taken 217352 times.
✓ Branch 9 taken 131003 times.
✓ Branch 10 taken 2690 times.
✓ Branch 11 taken 345665 times.
✓ Branch 12 taken 206054 times.
✓ Branch 13 taken 142301 times.
348355 : sys(static_cast<SatSys>(sys)), svId(svId), flags((healthy ? Flags::Healthy : Flags::None) | (almanac ? Flags::Almanac : Flags::None) | (ephemeris ? Flags::Ephemeris : Flags::None) | (differentialCorrection ? Flags::DifferentialCorrection : Flags::None) | (usedForNavigation ? Flags::UsedForNavigation : Flags::None) | (azimuthElevationValid ? Flags::AzimuthElevationValid : Flags::None) | (usedForRTK ? Flags::UsedForRTK : Flags::None)), cno(cno), qi(static_cast<QualityIndicator>(qi)), el(el), az(az) {}
236
237 SatSys sys{}; ///< GNSS constellation indicator
238 uint8_t svId{}; ///< Space vehicle Id
239 Flags flags{}; ///< Tracking info flags
240 uint8_t cno{}; ///< Carrier-to-noise density ratio (signal strength) [dB-Hz]
241 QualityIndicator qi{}; ///< Quality Indicator
242 int8_t el{}; ///< Elevation in degrees
243 int16_t az{}; ///< Azimuth angle in degrees
244 };
245
246 /// @brief Number of measurements to follow.
247 uint8_t numSats{};
248 /// @brief SatInfo container
249 std::vector<SatInfoElement> satellites;
250 };
251
252 /// @brief Allows combining flags of the SatInfo::SatInfoElement::Flags enum.
253 ///
254 /// @param[in] lhs Left-hand side enum value.
255 /// @param[in] rhs Right-hand side enum value.
256 /// @return The binary ANDed value.
257 constexpr SatInfo::SatInfoElement::Flags operator&(SatInfo::SatInfoElement::Flags lhs, SatInfo::SatInfoElement::Flags rhs)
258 {
259 return SatInfo::SatInfoElement::Flags(int(lhs) & int(rhs));
260 }
261
262 /// @brief Raw measurements pertaining to each GNSS satellite in view.
263 struct RawMeas
264 {
265 /// @brief Raw measurements for a certain satellite
266 struct SatRawElement
267 {
268 /// @brief Tracking info flags
269 enum class Flags : uint16_t
270 {
271 None = 0, ///< No flag set
272 Searching = 1 << 0, ///< Searching
273 Tracking = 1 << 1, ///< Tracking
274 TimeValid = 1 << 2, ///< Time Valid
275 CodeLock = 1 << 3, ///< Code Lock
276 PhaseLock = 1 << 4, ///< Phase Lock
277 PhaseHalfAmbiguity = 1 << 5, ///< Phase Half Ambiguity
278 PhaseHalfSub = 1 << 6, ///< Phase Half Sub
279 PhaseSlip = 1 << 7, ///< Phase Slip
280 PseudorangeSmoothed = 1 << 8, ///< Pseudorange Smoothed
281 };
282
283 /// @brief Binary or-operator
284 /// @param[in] lhs Left-hand side
285 /// @param[in] rhs Right-hand side
286 /// @return Binary or-ed result
287 2578096 friend Flags operator|(Flags lhs, Flags rhs)
288 {
289 2578096 return Flags(int(lhs) | int(rhs));
290 }
291
292 /// @brief Channel Indicator
293 enum class Chan : uint8_t
294 {
295 P_Code = 0, ///< P-code (GPS,GLO)
296 CA_Code = 1, ///< C/A-code (GPS,GLO,SBAS,QZSS), C chan (GAL)
297 SemiCodeless = 2, ///< semi-codeless (GPS)
298 Y_Code = 3, ///< Y-code (GPS)
299 M_Code = 4, ///< M-code (GPS)
300 Codeless = 5, ///< codeless (GPS)
301 A_Chan = 6, ///< A chan (GAL)
302 B_Chan = 7, ///< B chan (GAL)
303 I_Chan = 8, ///< I chan (GPS,GAL,QZSS,BDS)
304 Q_Chan = 9, ///< Q chan (GPS,GAL,QZSS,BDS)
305 M_Chan = 10, ///< M chan (L2CGPS, L2CQZSS), D chan (GPS,QZSS)
306 L_Chan = 11, ///< L chan (L2CGPS, L2CQZSS), P chan (GPS,QZSS)
307 BC_Chan = 12, ///< B+C chan (GAL), I+Q chan (GPS,GAL,QZSS,BDS), M+L chan (GPS,QZSS), D+P chan (GPS,QZSS)
308 Z_Tracking = 13, ///< based on Z-tracking (GPS)
309 ABC = 14, ///< A+B+C (GAL)
310 };
311
312 /// @brief Stream insertion operator overload
313 /// @param[in, out] os Output stream where data gets printed to
314 /// @param[in] chan Channel indicator
315 /// @return Output stream object
316 friend std::ostream& operator<<(std::ostream& os, const Chan& chan)
317 {
318 switch (chan)
319 {
320 case Chan::P_Code:
321 os << "P-Code";
322 break;
323 case Chan::CA_Code:
324 os << "C/A-Code or C-Chan";
325 break;
326 case Chan::SemiCodeless:
327 os << "Semi-codeless";
328 break;
329 case Chan::Y_Code:
330 os << "Y-Code";
331 break;
332 case Chan::M_Code:
333 os << "M-Code";
334 break;
335 case Chan::Codeless:
336 os << "Codeless";
337 break;
338 case Chan::A_Chan:
339 os << "A Chan";
340 break;
341 case Chan::B_Chan:
342 os << "B Chan";
343 break;
344 case Chan::I_Chan:
345 os << "I Chan";
346 break;
347 case Chan::Q_Chan:
348 os << "Q Chan";
349 break;
350 case Chan::M_Chan:
351 os << "M Chan or D Chan";
352 break;
353 case Chan::L_Chan:
354 os << "L Chan or P Chan";
355 break;
356 case Chan::BC_Chan:
357 os << "B+C Chan, I+Q Chan, M+L Chan or D+P Chan";
358 break;
359 case Chan::Z_Tracking:
360 os << "based on Z-tracking";
361 break;
362 case Chan::ABC:
363 os << "A+B+C";
364 break;
365 }
366 return os;
367 }
368
369 /// @brief Frequency indicator
370 enum class Freq : uint8_t
371 {
372 RxChannel = 0, ///< Rx Channel
373 L1 = 1, ///< L1(GPS,QZSS,SBAS), G1(GLO), E2-L1-E1(GAL), B1(BDS)
374 L2 = 2, ///< L2(GPS,QZSS), G2(GLO)
375 L5 = 3, ///< L5(GPS,QZSS,SBAS), E5a(GAL)
376 E6 = 4, ///< E6(GAL), LEX(QZSS), B3(BDS)
377 E5b = 5, ///< E5b(GAL), B2(BDS)
378 E5a = 6, ///< E5a+b(GAL)
379 };
380
381 /// @brief Stream insertion operator overload
382 /// @param[in, out] os Output stream where data gets printed to
383 /// @param[in] freq Frequency indicator
384 /// @return Output stream object
385 friend std::ostream& operator<<(std::ostream& os, const Freq& freq)
386 {
387 switch (freq)
388 {
389 case Freq::RxChannel:
390 os << "Rx Channel";
391 break;
392 case Freq::L1:
393 os << "L1, G1, E2-L1-E1 or B1";
394 break;
395 case Freq::L2:
396 os << "L2 or G2";
397 break;
398 case Freq::L5:
399 os << "L5 or E5a";
400 break;
401 case Freq::E6:
402 os << "E6, LEX or B3";
403 break;
404 case Freq::E5b:
405 os << "E5b or B2";
406 break;
407 case Freq::E5a:
408 os << "E5a+b";
409 break;
410 }
411 return os;
412 }
413
414 /// @brief Default Constructor
415 1198 SatRawElement() = default;
416
417 /// @brief Constructor
418 /// @param[in] sys GNSS constellation indicator
419 /// @param[in] svId Space vehicle Id
420 /// @param[in] freq Frequency indicator
421 /// @param[in] chan Channel Indicator
422 /// @param[in] slot Slot Id
423 /// @param[in] cno Carrier-to-noise density ratio (signal strength) [dB-Hz]
424 /// @param[in] flags Tracking info flags
425 /// @param[in] pr Pseudorange measurement in meters
426 /// @param[in] cp Carrier phase measurement in cycles
427 /// @param[in] dp Doppler measurement in Hz. Positive sign for approaching satellites
428 110718 SatRawElement(uint8_t sys, uint8_t svId, uint8_t freq, uint8_t chan, int8_t slot, uint8_t cno, uint16_t flags, double pr, double cp, float dp)
429 110718 : sys(static_cast<SatSys>(sys)), svId(svId), freq(static_cast<Freq>(freq)), chan(static_cast<Chan>(chan)), slot(slot), cno(cno), flags(static_cast<Flags>(flags)), pr(pr), cp(cp), dp(dp) {}
430
431 /// @brief Constructor
432 /// @param[in] sys GNSS constellation indicator
433 /// @param[in] svId Space vehicle Id
434 /// @param[in] freq Frequency indicator
435 /// @param[in] chan Channel Indicator
436 /// @param[in] slot Slot Id
437 /// @param[in] cno Carrier-to-noise density ratio (signal strength) [dB-Hz]
438 /// @param[in] searching Searching
439 /// @param[in] tracking Tracking
440 /// @param[in] timeValid Time Valid
441 /// @param[in] codeLock Code Lock
442 /// @param[in] phaseLock Phase Lock
443 /// @param[in] phaseHalfAmbiguity Phase Half Ambiguity
444 /// @param[in] phaseHalfSub Phase Half Sub
445 /// @param[in] phaseSlip Phase Slip
446 /// @param[in] pseudorangeSmoothed Pseudorange Smoothed
447 /// @param[in] pr Pseudorange measurement in meters
448 /// @param[in] cp Carrier phase measurement in cycles
449 /// @param[in] dp Doppler measurement in Hz. Positive sign for approaching satellites
450 322262 SatRawElement(uint8_t sys, uint8_t svId, uint8_t freq, uint8_t chan, int8_t slot, uint8_t cno,
451 uint8_t searching, uint8_t tracking, uint8_t timeValid, uint8_t codeLock, uint8_t phaseLock, uint8_t phaseHalfAmbiguity, uint8_t phaseHalfSub, uint8_t phaseSlip, uint8_t pseudorangeSmoothed,
452 double pr, double cp, double dp)
453
11/18
✗ Branch 0 not taken.
✓ Branch 1 taken 322262 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 322262 times.
✓ Branch 4 taken 81776 times.
✓ Branch 5 taken 240486 times.
✓ Branch 6 taken 4842 times.
✓ Branch 7 taken 317420 times.
✓ Branch 8 taken 322262 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 322262 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 322262 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 322262 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 322262 times.
322262 : sys(static_cast<SatSys>(sys)), svId(svId), freq(static_cast<Freq>(freq)), chan(static_cast<Chan>(chan)), slot(slot), cno(cno), flags((searching ? Flags::Searching : Flags::None) | (tracking ? Flags::Tracking : Flags::None) | (timeValid ? Flags::TimeValid : Flags::None) | (codeLock ? Flags::CodeLock : Flags::None) | (phaseLock ? Flags::PhaseLock : Flags::None) | (phaseHalfAmbiguity ? Flags::PhaseHalfAmbiguity : Flags::None) | (phaseHalfSub ? Flags::PhaseHalfSub : Flags::None) | (phaseSlip ? Flags::PhaseSlip : Flags::None) | (pseudorangeSmoothed ? Flags::PseudorangeSmoothed : Flags::None)), pr(pr), cp(cp), dp(static_cast<float>(dp)) {}
454
455 SatSys sys{}; ///< GNSS constellation indicator
456 uint8_t svId{}; ///< Space vehicle Id
457 Freq freq{}; ///< Frequency indicator
458 Chan chan{}; ///< Channel Indicator
459 int8_t slot{}; ///< Slot Id
460 uint8_t cno{}; ///< Carrier-to-noise density ratio (signal strength) [dB-Hz]
461 Flags flags{}; ///< Tracking info flags
462 double pr{}; ///< Pseudorange measurement in meters
463 double cp{}; ///< Carrier phase measurement in cycles
464 float dp{}; ///< Doppler measurement in Hz. Positive sign for approaching satellites
465 };
466
467 /// @brief Time of week in seconds
468 double tow{};
469 /// @brief GPS week number
470 uint16_t week{};
471 /// @brief Number of measurements to follow
472 uint8_t numSats{};
473 /// @brief SatRaw container
474 std::vector<SatRawElement> satellites;
475 };
476
477 /// @brief Allows combining flags of the RawMeas::SatRawElement::Flags enum.
478 ///
479 /// @param[in] lhs Left-hand side enum value.
480 /// @param[in] rhs Right-hand side enum value.
481 /// @return The binary ANDed value.
482 constexpr RawMeas::SatRawElement::Flags operator&(RawMeas::SatRawElement::Flags lhs, RawMeas::SatRawElement::Flags rhs)
483 {
484 return RawMeas::SatRawElement::Flags(int(lhs) & int(rhs));
485 }
486
487 /// @brief The VPE status bitfield
488 ///
489 /// Bit | Name | Description
490 /// 0+1 | AttitudeQuality | Provides an indication of the quality of the attitude solution. 0 - Excellent, 1 - Good, 2 - Bad, 3 - Not tracking
491 /// 2 | GyroSaturation | At least one gyro axis is currently saturated.
492 /// 3 | GyroSaturationRecovery | Filter is in the process of recovering from a gyro saturation event.
493 /// 4+5 | MagDisturbance | A magnetic DC disturbance has been detected. 0 – No magnetic disturbance. 1 to 3 – Magnetic disturbance is present.
494 /// 6 | MagSaturation | At least one magnetometer axis is currently saturated.
495 /// 7+8 | AccDisturbance | A strong acceleration disturbance has been detected. 0 – No acceleration disturbance. 1 to 3 – Acceleration disturbance has been detected.
496 /// 9 | AccSaturation | At least one accelerometer axis is currently saturated.
497 /// 11 | KnownMagDisturbance | A known magnetic disturbance has been reported by the user and the magnetometer is currently tuned out.
498 /// 12 | KnownAccelDisturbance | A known acceleration disturbance has been reported by the user and the accelerometer is currently tuned out.
499 class VpeStatus
500 {
501 public:
502 /// Constructor
503 /// @param[in] status Status to set
504 explicit VpeStatus(uint16_t status) : _status(status) {}
505
506 /// @brief Assignment operator
507 /// @param[in] status Status to set
508 VpeStatus& operator=(const uint16_t& status)
509 {
510 _status = status;
511 return *this;
512 }
513
514 /// @brief Default constructor
515 VpeStatus() = default;
516
517 /// @brief Returns a reference to the status
518 [[nodiscard]] constexpr uint16_t& status()
519 {
520 return _status;
521 }
522
523 /// Extract the attitude quality from the vpe status
524 [[nodiscard]] constexpr uint8_t attitudeQuality() const
525 {
526 return ((_status & (1U << 0U | 1U << 1U)) >> 0U);
527 }
528 /// Extract the gyro saturation from the vpe status
529 [[nodiscard]] constexpr uint8_t gyroSaturation() const
530 {
531 return ((_status & (1U << 2U)) >> 2U); // NOLINT
532 }
533 /// Extract the gyro saturation recovery from the vpe status
534 [[nodiscard]] constexpr uint8_t gyroSaturationRecovery() const
535 {
536 return ((_status & (1U << 3U)) >> 3U); // NOLINT
537 }
538 /// Extract the magnetic disturbance from the vpe status
539 [[nodiscard]] constexpr uint8_t magDisturbance() const
540 {
541 return ((_status & (1U << 4U | 1U << 5U)) >> 4U); // NOLINT
542 }
543 /// Extract the magnetic saturation from the vpe status
544 [[nodiscard]] constexpr uint8_t magSaturation() const
545 {
546 return ((_status & (1U << 6U)) >> 6U); // NOLINT
547 }
548 /// Extract the acceleration disturbance from the vpe status
549 [[nodiscard]] constexpr uint8_t accDisturbance() const
550 {
551 return ((_status & (1U << 7U | 1U << 8U)) >> 7U); // NOLINT
552 }
553 /// Extract the acceleration saturation from the vpe status
554 [[nodiscard]] constexpr uint8_t accSaturation() const
555 {
556 return ((_status & (1U << 9U)) >> 9U); // NOLINT
557 }
558 /// Extract the known magnetic disturbance from the vpe status
559 [[nodiscard]] constexpr uint8_t knownMagDisturbance() const
560 {
561 return ((_status & (1U << 11U)) >> 11U); // NOLINT
562 }
563 /// Extract the known acceleration disturbance from the vpe status
564 [[nodiscard]] constexpr uint8_t knownAccelDisturbance() const
565 {
566 return ((_status & (1U << 12U)) >> 12U); // NOLINT
567 }
568
569 private:
570 /// The storage field
571 uint16_t _status;
572 };
573
574 /// @brief The INS status bitfield
575 ///
576 /// Bit | Name | Description
577 /// 0+1 | Mode | Indicates the current mode of the INS filter.
578 /// | 0 = Not tracking. GNSS Compass is initializing. Output heading is based on magnetometer measurements.
579 /// | 1 = Aligning.
580 /// | INS Filter is dynamically aligning.
581 /// | For a stationary startup: GNSS Compass has initialized and INS Filter is
582 /// | aligning from the magnetic heading to the GNSS Compass heading.
583 /// | For a dynamic startup: INS Filter has initialized and is dynamically aligning to
584 /// | True North heading.
585 /// | In operation, if the INS Filter drops from INS Mode 2 back down to 1, the
586 /// | attitude uncertainty has increased above 2 degrees.
587 /// | 2 = Tracking. The INS Filter is tracking and operating within specification.
588 /// | 3 = Loss of GNSS. A GNSS outage has lasted more than 45 seconds. The INS Filter will
589 /// | no longer update the position and velocity outputs, but the attitude remains valid.
590 /// 2 | GpsFix | Indicates whether the GNSS has a proper fix.
591 /// 4 | IMU Error | High if IMU communication error is detected.
592 /// 5 | Mag/Pres Error | High if Magnetometer or Pressure sensor error is detected.
593 /// 6 | GNSS Error | High if GNSS communication error is detected,
594 /// 8 | GpsHeadingIns | In stationary operation, if set the INS Filter has fully aligned to the GNSS Compass solution.
595 /// | In dynamic operation, the GNSS Compass solution is currently aiding the INS Filter heading solution.
596 /// 9 | GpsCompass | Indicates if the GNSS compass is operational and reporting a heading solution.
597 class InsStatus
598 {
599 public:
600 /// @brief Indicates the current mode of the INS filter.
601 enum class Mode : uint8_t
602 {
603 /// @brief Not tracking.
604 ///
605 /// GNSS Compass is initializing. Output heading is based on magnetometer measurements.
606 NotTracking = 0,
607 /// @brief INS Filter is dynamically aligning.
608 ///
609 /// For a stationary startup: GNSS Compass has initialized and INS Filter is
610 /// aligning from the magnetic heading to the GNSS Compass heading.
611 /// For a dynamic startup: INS Filter has initialized and is dynamically aligning to
612 /// True North heading.
613 /// In operation, if the INS Filter drops from INS Mode 2 back down to 1, the
614 /// attitude uncertainty has increased above 2 degrees.
615 Aligning = 1,
616 /// @brief Tracking.
617 ///
618 /// The INS Filter is tracking and operating within specification.
619 Tracking = 2,
620 /// @brief Loss of GNSS.
621 ///
622 /// A GNSS outage has lasted more than 45 seconds. The INS Filter will
623 /// no longer update the position and velocity outputs, but the attitude remains valid.
624 LossOfGNSS = 3,
625 };
626
627 /// Constructor
628 /// @param[in] status Status to set
629 explicit InsStatus(uint16_t status) : _status(status) {}
630
631 /// @brief Assignment operator
632 /// @param[in] status Status to set
633 InsStatus& operator=(const uint16_t& status)
634 {
635 _status = status;
636 return *this;
637 }
638
639 /// @brief Default constructor
640 InsStatus() = default;
641
642 /// @brief Returns a reference to the status
643 2803 [[nodiscard]] constexpr uint16_t& status()
644 {
645 2803 return _status;
646 }
647
648 /// Extract the current mode of the INS filter from the ins status
649 5362 [[nodiscard]] constexpr Mode mode() const
650 {
651 5362 return static_cast<Mode>((_status & (1U << 0U | 1U << 1U)) >> 0U);
652 }
653 /// Extract the GPS Fix from the ins status
654 146 [[nodiscard]] constexpr bool gpsFix() const
655 {
656 146 return ((_status & (1U << 2U)) >> 2U); // NOLINT
657 }
658 /// Extract the IMU Error from the ins status
659 146 [[nodiscard]] constexpr bool errorIMU() const
660 {
661 146 return ((_status & (1U << 4U)) >> 4U); // NOLINT
662 }
663 /// Extract the Mag/Pres Error from the ins status
664 146 [[nodiscard]] constexpr bool errorMagPres() const
665 {
666 146 return ((_status & (1U << 5U)) >> 5U); // NOLINT
667 }
668 /// Extract the GNSS Error from the ins status
669 146 [[nodiscard]] constexpr bool errorGnss() const
670 {
671 146 return ((_status & (1U << 6U)) >> 6U); // NOLINT
672 }
673 /// Extract the GPS Heading INS from the ins status
674 146 [[nodiscard]] constexpr bool gpsHeadingIns() const
675 {
676 146 return ((_status & (1U << 8U)) >> 8U); // NOLINT
677 }
678 /// Extract the GPS Compass from the ins status
679 146 [[nodiscard]] constexpr bool gpsCompass() const
680 {
681 146 return ((_status & (1U << 9U)) >> 9U); // NOLINT
682 }
683
684 private:
685 /// The storage field
686 uint16_t _status;
687 };
688
689 } // namespace NAV::vendor::vectornav
690
691 #ifndef DOXYGEN_IGNORE
692
693 template<>
694 struct fmt::formatter<vn::protocol::uart::ErrorDetectionMode> : ostream_formatter
695 {};
696 template<>
697 struct fmt::formatter<vn::protocol::uart::AsciiAsync> : ostream_formatter
698 {};
699 template<>
700 struct fmt::formatter<vn::protocol::uart::AsyncMode> : ostream_formatter
701 {};
702 template<>
703 struct fmt::formatter<vn::protocol::uart::BinaryGroup> : ostream_formatter
704 {};
705 template<>
706 struct fmt::formatter<vn::protocol::uart::CommonGroup> : ostream_formatter
707 {};
708 template<>
709 struct fmt::formatter<vn::protocol::uart::TimeGroup> : ostream_formatter
710 {};
711 template<>
712 struct fmt::formatter<vn::protocol::uart::ImuGroup> : ostream_formatter
713 {};
714 template<>
715 struct fmt::formatter<vn::protocol::uart::GpsGroup> : ostream_formatter
716 {};
717 template<>
718 struct fmt::formatter<vn::protocol::uart::AttitudeGroup> : ostream_formatter
719 {};
720 template<>
721 struct fmt::formatter<vn::protocol::uart::InsGroup> : ostream_formatter
722 {};
723 template<>
724 struct fmt::formatter<vn::protocol::uart::SensorError> : ostream_formatter
725 {};
726 template<>
727 struct fmt::formatter<vn::protocol::uart::BootloaderError> : ostream_formatter
728 {};
729 template<>
730 struct fmt::formatter<vn::protocol::uart::SyncInMode> : ostream_formatter
731 {};
732 template<>
733 struct fmt::formatter<vn::protocol::uart::SyncInEdge> : ostream_formatter
734 {};
735 template<>
736 struct fmt::formatter<vn::protocol::uart::SyncOutMode> : ostream_formatter
737 {};
738 template<>
739 struct fmt::formatter<vn::protocol::uart::SyncOutPolarity> : ostream_formatter
740 {};
741 template<>
742 struct fmt::formatter<vn::protocol::uart::CountMode> : ostream_formatter
743 {};
744 template<>
745 struct fmt::formatter<vn::protocol::uart::StatusMode> : ostream_formatter
746 {};
747 template<>
748 struct fmt::formatter<vn::protocol::uart::ChecksumMode> : ostream_formatter
749 {};
750 template<>
751 struct fmt::formatter<vn::protocol::uart::ErrorMode> : ostream_formatter
752 {};
753 template<>
754 struct fmt::formatter<vn::protocol::uart::FilterMode> : ostream_formatter
755 {};
756 template<>
757 struct fmt::formatter<vn::protocol::uart::IntegrationFrame> : ostream_formatter
758 {};
759 template<>
760 struct fmt::formatter<vn::protocol::uart::CompensationMode> : ostream_formatter
761 {};
762 template<>
763 struct fmt::formatter<vn::protocol::uart::AccCompensationMode> : ostream_formatter
764 {};
765 template<>
766 struct fmt::formatter<vn::protocol::uart::EarthRateCorrection> : ostream_formatter
767 {};
768 template<>
769 struct fmt::formatter<vn::protocol::uart::GpsFix> : ostream_formatter
770 {};
771 template<>
772 struct fmt::formatter<vn::protocol::uart::GpsMode> : ostream_formatter
773 {};
774 template<>
775 struct fmt::formatter<vn::protocol::uart::PpsSource> : ostream_formatter
776 {};
777 template<>
778 struct fmt::formatter<vn::protocol::uart::GpsRate> : ostream_formatter
779 {};
780 template<>
781 struct fmt::formatter<vn::protocol::uart::AntPower> : ostream_formatter
782 {};
783 template<>
784 struct fmt::formatter<vn::protocol::uart::VpeEnable> : ostream_formatter
785 {};
786 template<>
787 struct fmt::formatter<vn::protocol::uart::HeadingMode> : ostream_formatter
788 {};
789 template<>
790 struct fmt::formatter<vn::protocol::uart::VpeMode> : ostream_formatter
791 {};
792 template<>
793 struct fmt::formatter<vn::protocol::uart::Scenario> : ostream_formatter
794 {};
795 template<>
796 struct fmt::formatter<vn::protocol::uart::HsiMode> : ostream_formatter
797 {};
798 template<>
799 struct fmt::formatter<vn::protocol::uart::HsiOutput> : ostream_formatter
800 {};
801 template<>
802 struct fmt::formatter<vn::protocol::uart::VelocityCompensationMode> : ostream_formatter
803 {};
804 template<>
805 struct fmt::formatter<vn::protocol::uart::MagneticMode> : ostream_formatter
806 {};
807 template<>
808 struct fmt::formatter<vn::protocol::uart::ExternalSensorMode> : ostream_formatter
809 {};
810 template<>
811 struct fmt::formatter<vn::protocol::uart::FoamInit> : ostream_formatter
812 {};
813 template<>
814 struct fmt::formatter<vn::protocol::uart::SensSat> : ostream_formatter
815 {};
816 template<>
817 struct fmt::formatter<vn::protocol::uart::InsStatus> : ostream_formatter
818 {};
819
820 template<>
821 struct fmt::formatter<NAV::vendor::vectornav::SatSys> : ostream_formatter
822 {};
823 template<>
824 struct fmt::formatter<NAV::vendor::vectornav::RawMeas::SatRawElement::Chan> : ostream_formatter
825 {};
826 template<>
827 struct fmt::formatter<NAV::vendor::vectornav::RawMeas::SatRawElement::Freq> : ostream_formatter
828 {};
829
830 template<size_t tdim, typename T>
831 struct fmt::formatter<vn::math::vec<tdim, T>> : ostream_formatter
832 {};
833
834 template<size_t m, size_t n, typename T>
835 struct fmt::formatter<vn::math::mat<m, n, T>> : ostream_formatter
836 {};
837
838 #endif
839