INSTINCT Code Coverage Report

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