INSTINCT Code Coverage Report

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 PosVelAttInitializer.hpp
10			/// @brief Position, Velocity, Attitude Initializer from GPS and IMU data
11			/// @author T. Topp (topp@ins.uni-stuttgart.de)
12			/// @date 2021-02-03
13
14			#pragma once
15
16			#include "internal/Node/Node.hpp"
17
18			#include "Navigation/Time/InsTime.hpp"
19
20			#include "NodeData/IMU/ImuObs.hpp"
21			#include "NodeData/GNSS/UbloxObs.hpp"
22			#include "NodeData/GNSS/RtklibPosObs.hpp"
23			#include "NodeData/State/PosVelAtt.hpp"
24
25			#include "internal/gui/widgets/TimeEdit.hpp"
26			#include "internal/gui/widgets/PositionInput.hpp"
27
28			#include <limits>
29
30			namespace NAV
31			{
32			/// Position, Velocity, Attitude Initializer from GPS and IMU data
33			class PosVelAttInitializer : public Node
34			{
35			public:
36			/// @brief Default constructor
37			PosVelAttInitializer();
38			/// @brief Destructor
39			~PosVelAttInitializer() override;
40			/// @brief Copy constructor
41			PosVelAttInitializer(const PosVelAttInitializer&) = delete;
42			/// @brief Move constructor
43			PosVelAttInitializer(PosVelAttInitializer&&) = delete;
44			/// @brief Copy assignment operator
45			PosVelAttInitializer& operator=(const PosVelAttInitializer&) = delete;
46			/// @brief Move assignment operator
47			PosVelAttInitializer& operator=(PosVelAttInitializer&&) = delete;
48
49			/// @brief String representation of the Class Type
50			[[nodiscard]] static std::string typeStatic();
51
52			/// @brief String representation of the Class Type
53			[[nodiscard]] std::string type() const override;
54
55			/// @brief String representation of the Class Category
56			[[nodiscard]] static std::string category();
57
58			/// @brief ImGui config window which is shown on double click
59			/// @attention Don't forget to set _hasConfig to true in the constructor of the node
60			void guiConfig() override;
61
62			/// @brief Saves the node into a json object
63			[[nodiscard]] json save() const override;
64
65			/// @brief Restores the node from a json object
66			/// @param[in] j Json object with the node state
67			void restore(const json& j) override;
68
69			private:
70			constexpr static size_t OUTPUT_PORT_INDEX_POS_VEL_ATT = 0; ///< @brief Flow (PosVelAtt)
71
72			/// Index of the input pin for IMU observations
73			int _inputPinIdxIMU = -1;
74			/// Index of the input pin for GNSS observations
75			int _inputPinIdxGNSS = -1;
76
77			/// @brief Initialize the node
78			bool initialize() override;
79
80			/// @brief Deinitialize the node
81			void deinitialize() override;
82
83			/// Add or removes input pins depending on the settings and modifies the output pin
84			void updatePins();
85
86			/// Checks whether all Flags are set and writes logs messages
87			void finalizeInit();
88
89			/// @brief Receive Imu Observations
90			/// @param[in] queue Queue with all the received data messages
91			/// @param[in] pinIdx Index of the pin the data is received on
92			void receiveImuObs(InputPin::NodeDataQueue& queue, size_t pinIdx);
93
94			/// @brief Receive Gnss Observations
95			/// @param[in] queue Queue with all the received data messages
96			/// @param[in] pinIdx Index of the pin the data is received on
97			void receiveGnssObs(InputPin::NodeDataQueue& queue, size_t pinIdx);
98
99			/// @brief Receive Ublox Observations
100			/// @param[in] obs Ublox Data
101			void receiveUbloxObs(const std::shared_ptr<const UbloxObs>& obs);
102
103			/// @brief Receive Pos Observations
104			/// @param[in] obs Pos Data
105			void receivePosObs(const std::shared_ptr<const Pos>& obs);
106
107			/// @brief Receive PosVel Observations
108			/// @param[in] obs PosVel Data
109			void receivePosVelObs(const std::shared_ptr<const PosVel>& obs);
110
111			/// @brief Receive PosVelAtt Observations
112			/// @param[in] obs PosVelAtt Data
113			void receivePosVelAttObs(const std::shared_ptr<const PosVelAtt>& obs);
114
115			/// @brief Polls the PVA solution if all is set in the GUI
116			/// @return The PVA solution
117			[[nodiscard]] std::shared_ptr<const NodeData> pollPVASolution();
118
119			/// Time in [s] to initialize the state
120			double _initDuration = 5.0;
121
122			/// Start time of the averageing process
123			uint64_t _startTime = 0;
124
125			/// Initialization source for attitude
126			enum class AttitudeMode : uint8_t
127			{
128			BOTH, ///< Use IMU and GNSS Observations for attitude initialization
129			IMU, ///< Use IMU Observations for attitude initialization
130			GNSS, ///< Use GNSS Observations for attitude initialization
131			COUNT, ///< Amount of items in the enum
132			};
133
134			/// @brief Converts the enum to a string
135			/// @param[in] attitudeMode Enum value to convert into text
136			/// @return String representation of the enum
137			friend constexpr const char* to_string(AttitudeMode attitudeMode);
138
139			/// GUI option to pecify the initialization source for attitude
140			AttitudeMode _attitudeMode = AttitudeMode::BOTH;
141
142			/// Whether the GNSS values should be used or we want to override the values manually
143			bool _overridePosition = false;
144			/// Values to override the Position in ECEF coordinates in [m]
145			gui::widgets::PositionWithFrame _overridePositionValue;
146
147			/// Position Accuracy to achieve in [cm]
148			double _positionAccuracyThreshold = 10;
149			/// Last position accuracy in [cm] for XYZ or NED
150			std::array<double, 3> _lastPositionAccuracy = { std::numeric_limits<double>::infinity(),
151			std::numeric_limits<double>::infinity(),
152			std::numeric_limits<double>::infinity() };
153
154			/// Override options for Position
155			enum class VelocityOverride : uint8_t
156			{
157			OFF, ///< Do not override the values
158			ECEF, ///< Override with ECEF values
159			NED, ///< Override with NED values
160			COUNT, ///< Amount of items in the enum
161			};
162
163			/// @brief Converts the enum to a string
164			/// @param[in] velOverride Enum value to convert into text
165			/// @return String representation of the enum
166			friend constexpr const char* to_string(VelocityOverride velOverride);
167
168			/// Whether the GNSS values should be used or we want to override the values manually
169			VelocityOverride _overrideVelocity = VelocityOverride::OFF;
170			/// Values to override the Velocity in [m/s]
171			Eigen::Vector3d _overrideVelocityValues = Eigen::Vector3d::Zero();
172			/// Velocity Accuracy to achieve in [cm/s]
173			double _velocityAccuracyThreshold = 10;
174			/// Last velocity accuracy in [cm/s] for XYZ or NED
175			std::array<double, 3> _lastVelocityAccuracy = { std::numeric_limits<double>::infinity(),
176			std::numeric_limits<double>::infinity(),
177			std::numeric_limits<double>::infinity() };
178
179			/// Count of received attitude measurements
180			double _countAveragedAttitude = 0.0;
181			/// Averaged Attitude (roll, pitch, yaw) in [rad]
182			std::array<double, 3> _averagedAttitude = { 0.0, 0.0, 0.0 };
183			/// Whether the IMU values should be used or we want to override the values manually
184			std::array<bool, 3> _overrideRollPitchYaw = { false, false, false };
185			/// Values to override Roll, Pitch and Yaw with in [deg]
186			std::array<double, 3> _overrideRollPitchYawValues = {};
187
188			/// Whether the states are initialized (pos, vel, att, messages send)
189			std::array<bool, 4> _posVelAttInitialized = { false, false, false, false };
190
191			/// Time Format to input the init time with
192			gui::widgets::TimeEditFormat _initTimeEditFormat;
193
194			/// Initialization time
195			InsTime _initTime = InsTime(InsTime_GPSweekTow(0, 0, 0));
196			/// Initialized Quaternion body to navigation frame (roll, pitch, yaw)
197			Eigen::Quaterniond _n_Quat_b_init;
198			/// Position in ECEF coordinates
199			Eigen::Vector3d _e_initPosition;
200			/// Velocity in navigation coordinates
201			Eigen::Vector3d _n_initVelocity;
202			};
203
204			/// @brief Converts the enum to a string
205			/// @param[in] attitudeMode Enum value to convert into text
206			/// @return String representation of the enum
207		✗	constexpr const char* to_string(PosVelAttInitializer::AttitudeMode attitudeMode)
208			{
209		✗	switch (attitudeMode)
210			{
211		✗	case NAV::PosVelAttInitializer::AttitudeMode::BOTH:
212		✗	return "Both";
213		✗	case NAV::PosVelAttInitializer::AttitudeMode::IMU:
214		✗	return "IMU";
215		✗	case NAV::PosVelAttInitializer::AttitudeMode::GNSS:
216		✗	return "GNSS";
217		✗	case NAV::PosVelAttInitializer::AttitudeMode::COUNT:
218		✗	return "";
219			}
220		✗	return "";
221			}
222
223			/// @brief Converts the enum to a string
224			/// @param[in] velOverride Enum value to convert into text
225			/// @return String representation of the enum
226		✗	constexpr const char* to_string(PosVelAttInitializer::VelocityOverride velOverride)
227			{
228		✗	switch (velOverride)
229			{
230		✗	case NAV::PosVelAttInitializer::VelocityOverride::OFF:
231		✗	return "OFF";
232		✗	case NAV::PosVelAttInitializer::VelocityOverride::ECEF:
233		✗	return "ECEF";
234		✗	case NAV::PosVelAttInitializer::VelocityOverride::NED:
235		✗	return "NED";
236		✗	case NAV::PosVelAttInitializer::VelocityOverride::COUNT:
237		✗	return "";
238			}
239		✗	return "";
240			}
241
242			} // namespace NAV
243