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 |