0.2.0
Loading...
Searching...
No Matches
VectorNavSensor.hpp
Go to the documentation of this file.
1// This file is part of INSTINCT, the INS Toolkit for Integrated
2// Navigation Concepts and Training by the Institute of Navigation of
3// the University of Stuttgart, Germany.
4//
5// This Source Code Form is subject to the terms of the Mozilla Public
6// License, v. 2.0. If a copy of the MPL was not distributed with this
7// file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
13
14#pragma once
15
16#include "Nodes/DataProvider/IMU/Imu.hpp"
17#include "Nodes/DataProvider/Protocol/UartSensor.hpp"
18#include "vn/sensors.h"
19
20#include "NodeData/IMU/VectorNavBinaryOutput.hpp"
21
22#include "Navigation/Time/InsTime.hpp"
23#include "util/Container/ScrollingBuffer.hpp"
24
25#include <vector>
26#include <array>
27#include <cstdint>
28
29namespace NAV
30{
31class VectorNavFile;
32
34class VectorNavSensor : public Imu, public UartSensor
35{
36 public:
38 struct TimeSync
39 {
40 InsTime ppsTime{};
41 uint32_t syncOutCnt{};
42 };
43
45 VectorNavSensor();
47 ~VectorNavSensor() override;
49 VectorNavSensor(const VectorNavSensor&) = delete;
51 VectorNavSensor(VectorNavSensor&&) = delete;
53 VectorNavSensor& operator=(const VectorNavSensor&) = delete;
55 VectorNavSensor& operator=(VectorNavSensor&&) = delete;
56
58 [[nodiscard]] static std::string typeStatic();
59
61 [[nodiscard]] std::string type() const override;
62
64 [[nodiscard]] static std::string category();
65
68 void guiConfig() override;
69
71 [[nodiscard]] json save() const override;
72
75 void restore(const json& j) override;
76
78 bool resetNode() override;
79
80 private:
81 constexpr static size_t OUTPUT_PORT_INDEX_ASCII_OUTPUT = 0;
82
84 bool initialize() override;
85
87 void deinitialize() override;
88
92 static void mergeVectorNavBinaryObservations(const std::shared_ptr<VectorNavBinaryOutput>& target, const std::shared_ptr<VectorNavBinaryOutput>& source);
93
98 static void asciiOrBinaryAsyncMessageReceived(void* userData, vn::protocol::uart::Packet& p, size_t index);
99
101 enum class VectorNavModel : int
102 {
105 VN100_VN110,
107 VN310,
108 };
109
111 VectorNavModel _sensorModel = VectorNavModel::VN100_VN110;
112
114 vn::sensors::VnSensor _vs;
115
117 std::string _connectedSensorPort;
118
120 static constexpr double IMU_DEFAULT_FREQUENCY = 800;
121
123 std::pair<std::vector<uint16_t>, std::vector<std::string>> _dividerFrequency;
124
126 std::array<InsTime, 3> _lastMessageTime{};
127
129 std::array<uint64_t, 3> _lastMessageTimeSinceStartup{};
130
132 struct
133 {
134 InsTime lastGnssTime{};
135 uint64_t timeSinceStartup{};
136 } _gnssTimeCounter;
137
138 // ###########################################################################################################
139 // SYSTEM MODULE
140 // ###########################################################################################################
141
144 vn::protocol::uart::AsciiAsync _asyncDataOutputType = vn::protocol::uart::AsciiAsync::VNOFF;
145
148 static constexpr std::array _possibleAsyncDataOutputFrequency = { 1, 2, 4, 5, 10, 20, 25, 40, 50, 100, 200 };
149
152 uint32_t _asyncDataOutputFrequency = 40;
154 int _asyncDataOutputFrequencySelected = 7;
155
157 int _asciiOutputBufferSize = 10;
158
160 ScrollingBuffer<std::string> _asciiOutputBuffer{ static_cast<size_t>(_asciiOutputBufferSize) };
161
166 vn::sensors::SynchronizationControlRegister _synchronizationControlRegister{
167 vn::protocol::uart::SyncInMode::SYNCINMODE_COUNT, // SyncInMode
168 vn::protocol::uart::SyncInEdge::SYNCINEDGE_RISING, // SyncInEdge
169 0, // SyncInSkipFactor
170 vn::protocol::uart::SyncOutMode::SYNCOUTMODE_NONE, // SyncOutMode
171 vn::protocol::uart::SyncOutPolarity::SYNCOUTPOLARITY_POSITIVE, // SyncOutPolarity
172 0, // SyncOutSkipFactor
173 100000000 // SyncOutPulseWidth
174 };
175
177 TimeSync _timeSyncOut;
178
180 bool _syncInPin = false;
181
183 bool _coupleImuRateOutput = true;
184
189 static void coupleImuFilterRates(NAV::VectorNavSensor* sensor, vn::sensors::BinaryOutputRegister& bor, uint32_t& binaryField);
190
195 vn::sensors::CommunicationProtocolControlRegister _communicationProtocolControlRegister{
196 vn::protocol::uart::CountMode::COUNTMODE_NONE, // SerialCount
197 vn::protocol::uart::StatusMode::STATUSMODE_OFF, // SerialStatus
198 vn::protocol::uart::CountMode::COUNTMODE_NONE, // SPICount
199 vn::protocol::uart::StatusMode::STATUSMODE_OFF, // SPIStatus
200 vn::protocol::uart::ChecksumMode::CHECKSUMMODE_CHECKSUM, // SerialChecksum
201 vn::protocol::uart::ChecksumMode::CHECKSUMMODE_OFF, // SPIChecksum
202 vn::protocol::uart::ErrorMode::ERRORMODE_SEND // ErrorMode
203 };
204
206 enum class BinaryRegisterMerge
207 {
208 None,
209 Output1_Output2,
210 Output1_Output3,
211 Output2_Output3,
212 };
213
215 BinaryRegisterMerge _binaryOutputRegisterMerge = BinaryRegisterMerge::None;
216
218 std::shared_ptr<VectorNavBinaryOutput> _binaryOutputRegisterMergeObservation = nullptr;
220 size_t _binaryOutputRegisterMergeIndex{};
221
227 std::array<vn::sensors::BinaryOutputRegister, 3> _binaryOutputRegister = { vn::sensors::BinaryOutputRegister{
228 vn::protocol::uart::AsyncMode::ASYNCMODE_NONE, // AsyncMode
229 800, // RateDivisor
230 vn::protocol::uart::CommonGroup::COMMONGROUP_NONE, // CommonGroup
231 vn::protocol::uart::TimeGroup::TIMEGROUP_NONE, // TimeGroup
232 vn::protocol::uart::ImuGroup::IMUGROUP_NONE, // IMUGroup
233 vn::protocol::uart::GpsGroup::GPSGROUP_NONE, // GNSS1Group
234 vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_NONE, // AttitudeGroup
235 vn::protocol::uart::InsGroup::INSGROUP_NONE, // INSGroup
236 vn::protocol::uart::GpsGroup::GPSGROUP_NONE // GNSS2Group
237 },
238 vn::sensors::BinaryOutputRegister{
239 vn::protocol::uart::AsyncMode::ASYNCMODE_NONE, // AsyncMode
240 800, // RateDivisor
241 vn::protocol::uart::CommonGroup::COMMONGROUP_NONE, // CommonGroup
242 vn::protocol::uart::TimeGroup::TIMEGROUP_NONE, // TimeGroup
243 vn::protocol::uart::ImuGroup::IMUGROUP_NONE, // IMUGroup
244 vn::protocol::uart::GpsGroup::GPSGROUP_NONE, // GNSS1Group
245 vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_NONE, // AttitudeGroup
246 vn::protocol::uart::InsGroup::INSGROUP_NONE, // INSGroup
247 vn::protocol::uart::GpsGroup::GPSGROUP_NONE // GNSS2Group
248 },
249 vn::sensors::BinaryOutputRegister{
250 vn::protocol::uart::AsyncMode::ASYNCMODE_NONE, // AsyncMode
251 800, // RateDivisor
252 vn::protocol::uart::CommonGroup::COMMONGROUP_NONE, // CommonGroup
253 vn::protocol::uart::TimeGroup::TIMEGROUP_NONE, // TimeGroup
254 vn::protocol::uart::ImuGroup::IMUGROUP_NONE, // IMUGroup
255 vn::protocol::uart::GpsGroup::GPSGROUP_NONE, // GNSS1Group
256 vn::protocol::uart::AttitudeGroup::ATTITUDEGROUP_NONE, // AttitudeGroup
257 vn::protocol::uart::InsGroup::INSGROUP_NONE, // INSGroup
258 vn::protocol::uart::GpsGroup::GPSGROUP_NONE // GNSS2Group
259 } };
261 std::array<size_t, 3> _binaryOutputSelectedFrequency{};
262
263 // ###########################################################################################################
264 // IMU SUBSYSTEM
265 // ###########################################################################################################
266
271 vn::math::mat3f _referenceFrameRotationMatrix{ { 1, 0, 0 },
272 { 0, 1, 0 },
273 { 0, 0, 1 } };
274
279 vn::sensors::ImuFilteringConfigurationRegister _imuFilteringConfigurationRegister{
280 4, // MagWindowSize
281 4, // AccelWindowSize
282 4, // GyroWindowSize
283 4, // TempWindowSize
284 4, // PresWindowSize
285 vn::protocol::uart::FilterMode::FILTERMODE_NOFILTERING, // MagFilterMode
286 vn::protocol::uart::FilterMode::FILTERMODE_BOTH, // AccelFilterMode
287 vn::protocol::uart::FilterMode::FILTERMODE_BOTH, // GyroFilterMode
288 vn::protocol::uart::FilterMode::FILTERMODE_BOTH, // TempFilterMode
289 vn::protocol::uart::FilterMode::FILTERMODE_NOFILTERING // PresFilterMode
290 };
291
296 vn::sensors::DeltaThetaAndDeltaVelocityConfigurationRegister _deltaThetaAndDeltaVelocityConfigurationRegister{
297 vn::protocol::uart::IntegrationFrame::INTEGRATIONFRAME_BODY, // IntegrationFrame
298 vn::protocol::uart::CompensationMode::COMPENSATIONMODE_NONE, // GyroCompensation
299 vn::protocol::uart::AccCompensationMode::ACCCOMPENSATIONMODE_NONE, // AccelCompensation
300 vn::protocol::uart::EarthRateCorrection::EARTHRATECORR_NONE // EarthRateCorrection
301 };
302
303 // ###########################################################################################################
304 // GNSS SUBSYSTEM
305 // ###########################################################################################################
306
309 vn::sensors::GpsConfigurationRegister _gpsConfigurationRegister{
310 vn::protocol::uart::GpsMode::GPSMODE_ONBOARDGPS, // Mode
311 vn::protocol::uart::PpsSource::PPSSOURCE_GPSPPSRISING, // PpsSource
312 vn::protocol::uart::GpsRate::GPSRATE_5HZ, // Rate
313 vn::protocol::uart::AntPower::ANTPOWER_INTERNAL // AntPower
314 };
315
320 vn::math::vec3f _gpsAntennaOffset{
321 0, 0, 0 // [m]
322 };
323
329 vn::sensors::GpsCompassBaselineRegister _gpsCompassBaselineRegister{
330 vn::math::vec3f{ 1.0F, 0.0F, 0.0F }, // Position [m]
331 vn::math::vec3f{ 0.254F, 0.254F, 0.254F } // Uncertainty [m]
332 };
333
334 // ###########################################################################################################
335 // ATTITUDE SUBSYSTEM
336 // ###########################################################################################################
337
342 vn::sensors::VpeBasicControlRegister _vpeBasicControlRegister{
343 vn::protocol::uart::VpeEnable::VPEENABLE_ENABLE, // Enable
344 vn::protocol::uart::HeadingMode::HEADINGMODE_RELATIVE, // HeadingMode
345 vn::protocol::uart::VpeMode::VPEMODE_MODE1, // FilteringMode
346 vn::protocol::uart::VpeMode::VPEMODE_MODE1 // TuningMode
347 };
348
353 vn::sensors::VpeMagnetometerBasicTuningRegister _vpeMagnetometerBasicTuningRegister{
354 vn::math::vec3f{ 4.0F, 4.0F, 4.0F }, // BaseTuning [0 - 10]
355 vn::math::vec3f{ 5.0F, 5.0F, 5.0F }, // AdaptiveTuning [0 - 10]
356 vn::math::vec3f{ 5.5F, 5.5F, 5.5F } // AdaptiveFiltering [0 - 10]
357 };
358
363 vn::sensors::VpeAccelerometerBasicTuningRegister _vpeAccelerometerBasicTuningRegister{
364 vn::math::vec3f{ 6.0F, 6.0F, 6.0F }, // BaseTuning [0 - 10]
365 vn::math::vec3f{ 3.0F, 3.0F, 3.0F }, // AdaptiveTuning [0 - 10]
366 vn::math::vec3f{ 5.0F, 5.0F, 5.0F } // AdaptiveFiltering [0 - 10]
367 };
368
373 vn::sensors::VpeGyroBasicTuningRegister _vpeGyroBasicTuningRegister{
374 vn::math::vec3f{ 8.0F, 8.0F, 8.0F }, // VarianceAngularWalk [0 - 10]
375 vn::math::vec3f{ 4.0F, 4.0F, 4.0F }, // BaseTuning [0 - 10]
376 vn::math::vec3f{ 0.0F, 0.0F, 0.0F } // AdaptiveTuning [0 - 10]
377 };
378
383 vn::math::vec3f _filterStartupGyroBias{
384 0, 0, 0 // [rad/s]
385 };
386
387 // ###########################################################################################################
388 // INS SUBSYSTEM
389 // ###########################################################################################################
390
393 vn::sensors::InsBasicConfigurationRegisterVn300 _insBasicConfigurationRegisterVn300{
394 vn::protocol::uart::Scenario::SCENARIO_GPSMOVINGBASELINEDYNAMIC, // Scenario
395 true, // AhrsAiding
396 true // EstBaseline
397 };
398
403 vn::sensors::StartupFilterBiasEstimateRegister _startupFilterBiasEstimateRegister{
404 vn::math::vec3f{ 0, 0, 0 }, // GyroBias [rad/s]
405 vn::math::vec3f{ 0, 0, 0 }, // AccelBias [m/s^2]
406 0.0F // PressureBiasIn [m]
407 };
408
409 // ###########################################################################################################
410 // HARD/SOFT IRON ESTIMATOR SUBSYSTEM
411 // ###########################################################################################################
412
417 vn::sensors::MagnetometerCalibrationControlRegister _magnetometerCalibrationControlRegister{
418 vn::protocol::uart::HsiMode::HSIMODE_RUN, // HSIMode
419 vn::protocol::uart::HsiOutput::HSIOUTPUT_USEONBOARD, // HSIOutput
420 5 // ConvergeRate
421 };
422
423 // ###########################################################################################################
424 // WORLD MAGNETIC & GRAVITY MODULE
425 // ###########################################################################################################
426
431 vn::sensors::MagneticAndGravityReferenceVectorsRegister _magneticAndGravityReferenceVectorsRegister{
432 vn::math::vec3f{ 1.0F, 0.0F, 1.8F }, // MagRef [Gauss]
433 vn::math::vec3f{ 0.0F, 0.0F, -9.793746F } // AccRef [m/s^2]
434 };
435
440 vn::sensors::ReferenceVectorConfigurationRegister _referenceVectorConfigurationRegister{
441 true, // UseMagModel
442 true, // UseGravityModel
443 1000, // RecalcThreshold [m]
444 0.0F, // Year [years]
445 vn::math::vec3d{ 0, 0, 0 } // Position (Lat Lon Alt [deg deg m])
446 };
447
448 // ###########################################################################################################
449 // VELOCITY AIDING
450 // ###########################################################################################################
451
456 vn::sensors::VelocityCompensationControlRegister _velocityCompensationControlRegister{
457 vn::protocol::uart::VelocityCompensationMode::VELOCITYCOMPENSATIONMODE_BODYMEASUREMENT, // Mode
458 0.1F, // VelocityTuning
459 0.01F // RateTuning
460 };
461
462 // ###########################################################################################################
463 // Binary Group GUI Definitions
464 // ###########################################################################################################
465
467 struct BinaryGroupData
468 {
470 const char* name = nullptr;
472 int flagsValue = 0;
474 void (*tooltip)() = nullptr;
476 bool (*isEnabled)(VectorNavModel sensorModel, const vn::sensors::BinaryOutputRegister& bor, uint32_t binaryField) =
477 [](VectorNavModel /* sensorModel */, const vn::sensors::BinaryOutputRegister& /* bor */, uint32_t /* binaryField */) { return true; };
479 void (*toggleFields)(VectorNavSensor* sensor, vn::sensors::BinaryOutputRegister& bor, uint32_t& /* binaryField */) = nullptr;
480 };
481
490 // static const std::array<BinaryGroupData, 15> _binaryGroupCommon;
491
496 static const std::array<BinaryGroupData, 10> _binaryGroupTime;
497
500 static const std::array<BinaryGroupData, 11> _binaryGroupIMU;
501
510 static const std::array<BinaryGroupData, 16> _binaryGroupGNSS;
511
516 static const std::array<BinaryGroupData, 9> _binaryGroupAttitude;
517
521 static const std::array<BinaryGroupData, 11> _binaryGroupINS;
522
523 friend class NAV::VectorNavFile;
524};
525
526} // namespace NAV
json
nlohmann::json json
json namespace
Definition FlowManager.hpp:21
None
@ None
None.
Definition GlobalActions.hpp:19
Imu.hpp
Abstract IMU Class.
InsTime.hpp
The class is responsible for all time-related tasks.
ScrollingBuffer.hpp
A buffer which is overwriting itself from the start when full.
UartSensor.hpp
Abstract Uart Sensor Class.
VectorNavBinaryOutput.hpp
Binary Outputs from VectorNav Sensors.
NAV::Imu
Abstract IMU Class.
Definition Imu.hpp:24
NAV::InsTime
The class is responsible for all time-related tasks.
Definition InsTime.hpp:667
NAV::UartSensor
Abstract Uart Sensor Class.
Definition UartSensor.hpp:26
NAV::VectorNavFile
File Reader for Vector Nav log files.
Definition VectorNavFile.hpp:25
NAV::VectorNavSensor
Vector Nav Sensor Class.
Definition VectorNavSensor.hpp:35
NAV::VectorNavSensor::VectorNavSensor
VectorNavSensor(VectorNavSensor &&)=delete
Move constructor.
NAV::VectorNavSensor::save
json save() const override
Saves the node into a json object.
NAV::VectorNavSensor::type
std::string type() const override
String representation of the Class Type.
NAV::VectorNavSensor::operator=
VectorNavSensor & operator=(VectorNavSensor &&)=delete
Move assignment operator.
NAV::VectorNavSensor::typeStatic
static std::string typeStatic()
String representation of the Class Type.
NAV::VectorNavSensor::guiConfig
void guiConfig() override
ImGui config window which is shown on double click.
NAV::VectorNavSensor::~VectorNavSensor
~VectorNavSensor() override
Destructor.
NAV::VectorNavSensor::timeSinceStartup
uint64_t timeSinceStartup
Time since startup when the GNSS time was received.
Definition VectorNavSensor.hpp:135
NAV::VectorNavSensor::VectorNavSensor
VectorNavSensor()
Default constructor.
NAV::VectorNavSensor::restore
void restore(const json &j) override
Restores the node from a json object.
NAV::VectorNavSensor::lastGnssTime
InsTime lastGnssTime
Last GNSS time received.
Definition VectorNavSensor.hpp:134
NAV::VectorNavSensor::VectorNavSensor
VectorNavSensor(const VectorNavSensor &)=delete
Copy constructor.
NAV::VectorNavSensor::category
static std::string category()
String representation of the Class Category.
NAV::VectorNavSensor::operator=
VectorNavSensor & operator=(const VectorNavSensor &)=delete
Copy assignment operator.
NAV::VectorNavSensor::resetNode
bool resetNode() override
Resets the node. It is guaranteed that the node is initialized when this is called.
NAV::VectorNavSensor::TimeSync
Information needed to sync Master/Slave sensors.
Definition VectorNavSensor.hpp:39
NAV::VectorNavSensor::TimeSync::ppsTime
InsTime ppsTime
Time of the last message with GNSS Time available (or empty otherwise)
Definition VectorNavSensor.hpp:40
NAV::VectorNavSensor::TimeSync::syncOutCnt
uint32_t syncOutCnt
The number of SyncOut trigger events that have occurred.
Definition VectorNavSensor.hpp:41