INSTINCT Code Coverage Report

Directory: src/
File: util/Vendor/Emlid/EmlidUtilities.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 0 110 0.0%
Functions: 0 2 0.0%
Branches: 0 236 0.0%
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 #include "EmlidUtilities.hpp"
10 #include "EmlidTypes.hpp"
11
12 #include "util/Logger.hpp"
13 #include "util/Time/TimeBase.hpp"
14
15 ✗ void NAV::vendor::emlid::decryptEmlidObs(const std::shared_ptr<NAV::EmlidObs>& obs, uart::protocol::Packet& packet)
16 {
17 ✗ if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY)
18 {
19 ✗ obs->msgId = packet.extractUint8();
20 ✗ obs->payloadLength = packet.extractUint16();
21
22 // Navigation Results Messages: Position
23 ✗ auto msgId = static_cast<ErbMessageID>(obs->msgId);
24 ✗ if (msgId == ErbMessageID::ERB_MessageId_VER)
25 {
26 ✗ obs->data = ErbVer();
27
28 ✗ std::get<ErbVer>(obs->data).iTOW = packet.extractUint32();
29 ✗ std::get<ErbVer>(obs->data).verH = packet.extractUint8();
30 ✗ std::get<ErbVer>(obs->data).verM = packet.extractUint8();
31 ✗ std::get<ErbVer>(obs->data).verL = packet.extractUint8();
32
33 // Calculate the insTime with the iTOW
34 ✗ auto currentTime = util::time::GetCurrentInsTime();
35 ✗ if (!currentTime.empty())
36 {
37 ✗ auto gpst = currentTime.toGPSweekTow();
38 ✗ currentTime = InsTime(gpst.gpsCycle,
39 gpst.gpsWeek,
40 ✗ static_cast<long double>(std::get<ErbVer>(obs->data).iTOW) / 1000.0L);
41 ✗ obs->insTime = currentTime;
42 }
43
44 LOG_DATA("Erb: VER, iTOW {}, verH {}, verH {}, verH {}",
45 std::get<ErbVer>(obs->data).iTOW,
46 std::get<ErbVer>(obs->data).verH,
47 std::get<ErbVer>(obs->data).verM,
48 std::get<ErbVer>(obs->data).verL);
49 }
50 ✗ else if (msgId == ErbMessageID::ERB_MessageId_POS)
51 {
52 ✗ obs->data = ErbPos();
53
54 ✗ std::get<ErbPos>(obs->data).iTOW = packet.extractUint32();
55 ✗ std::get<ErbPos>(obs->data).lon = packet.extractDouble();
56 ✗ std::get<ErbPos>(obs->data).lat = packet.extractDouble();
57 ✗ std::get<ErbPos>(obs->data).height = packet.extractDouble();
58 ✗ std::get<ErbPos>(obs->data).hMSL = packet.extractDouble();
59 ✗ std::get<ErbPos>(obs->data).hAcc = packet.extractUint32();
60 ✗ std::get<ErbPos>(obs->data).vAcc = packet.extractUint32();
61
62 // Calculate the insTime with the iTOW
63 ✗ auto currentTime = util::time::GetCurrentInsTime();
64 ✗ if (!currentTime.empty())
65 {
66 ✗ auto gpst = currentTime.toGPSweekTow();
67 ✗ currentTime = InsTime(gpst.gpsCycle,
68 gpst.gpsWeek,
69 ✗ static_cast<long double>(std::get<ErbPos>(obs->data).iTOW) / 1000.0L);
70 ✗ obs->insTime = currentTime;
71 }
72
73 LOG_DATA("Erb: POS, iTOW {}, lon {}, lat {}, height {}",
74 std::get<ErbPos>(obs->data).iTOW, std::get<ErbPos>(obs->data).lon,
75 std::get<ErbPos>(obs->data).lat, std::get<ErbPos>(obs->data).height);
76 }
77 ✗ else if (msgId == ErbMessageID::ERB_MessageId_STAT)
78 {
79 ✗ obs->data = ErbStat();
80
81 ✗ std::get<ErbStat>(obs->data).iTOW = packet.extractUint32();
82 ✗ std::get<ErbStat>(obs->data).weekGPS = packet.extractUint16();
83 ✗ std::get<ErbStat>(obs->data).fixType = packet.extractUint8();
84 ✗ std::get<ErbStat>(obs->data).fixStatus = packet.extractUint8();
85 ✗ std::get<ErbStat>(obs->data).numSV = packet.extractUint8();
86
87 // Calculate the insTime with the iTOW
88 ✗ auto currentTime = util::time::GetCurrentInsTime();
89 ✗ if (!currentTime.empty())
90 {
91 ✗ auto gpst = currentTime.toGPSweekTow();
92 ✗ currentTime = InsTime(gpst.gpsCycle,
93 gpst.gpsWeek,
94 ✗ static_cast<long double>(std::get<ErbStat>(obs->data).iTOW) / 1000.0L);
95 ✗ obs->insTime = currentTime;
96 }
97
98 LOG_DATA("Erb: STAT, iTOW {}, weekGPS {}, fixType {}, fixStatus {}, numSV {}",
99 std::get<ErbStat>(obs->data).iTOW,
100 std::get<ErbStat>(obs->data).weekGPS, std::get<ErbStat>(obs->data).fixType,
101 std::get<ErbStat>(obs->data).fixStatus, std::get<ErbStat>(obs->data).numSV);
102 }
103 ✗ else if (msgId == ErbMessageID::ERB_MessageId_DPOS)
104 {
105 ✗ obs->data = ErbDops();
106
107 ✗ std::get<ErbDops>(obs->data).iTOW = packet.extractUint32();
108 ✗ std::get<ErbDops>(obs->data).dopGeo = packet.extractUint16();
109 ✗ std::get<ErbDops>(obs->data).dopPos = packet.extractUint16();
110 ✗ std::get<ErbDops>(obs->data).dopVer = packet.extractUint16();
111 ✗ std::get<ErbDops>(obs->data).dopHor = packet.extractUint16();
112
113 // Calculate the insTime with the iTOW
114 ✗ auto currentTime = util::time::GetCurrentInsTime();
115 ✗ if (!currentTime.empty())
116 {
117 ✗ auto gpst = currentTime.toGPSweekTow();
118 ✗ currentTime = InsTime(gpst.gpsCycle,
119 gpst.gpsWeek,
120 ✗ static_cast<long double>(std::get<ErbDops>(obs->data).iTOW) / 1000.0L);
121 ✗ obs->insTime = currentTime;
122 }
123
124 LOG_DATA("Erb: DOPS, iTOW {}, dopGeo {}, dopPos {}, dopVer {}, dopHor {}",
125 std::get<ErbDops>(obs->data).iTOW,
126 std::get<ErbDops>(obs->data).dopGeo, std::get<ErbDops>(obs->data).dopPos,
127 std::get<ErbDops>(obs->data).dopVer, std::get<ErbDops>(obs->data).dopHor);
128 }
129 ✗ else if (msgId == ErbMessageID::ERB_MessageId_VEL)
130 {
131 ✗ obs->data = ErbVel();
132
133 ✗ std::get<ErbVel>(obs->data).iTOW = packet.extractUint32();
134 ✗ std::get<ErbVel>(obs->data).velN = packet.extractInt32();
135 ✗ std::get<ErbVel>(obs->data).velE = packet.extractInt32();
136 ✗ std::get<ErbVel>(obs->data).velD = packet.extractInt32();
137 ✗ std::get<ErbVel>(obs->data).speed = packet.extractUint32();
138 ✗ std::get<ErbVel>(obs->data).gSpeed = packet.extractUint32();
139 ✗ std::get<ErbVel>(obs->data).heading = packet.extractInt32();
140 ✗ std::get<ErbVel>(obs->data).sAcc = packet.extractUint32();
141
142 // Calculate the insTime with the iTOW
143 ✗ auto currentTime = util::time::GetCurrentInsTime();
144 ✗ if (!currentTime.empty())
145 {
146 ✗ auto gpst = currentTime.toGPSweekTow();
147 ✗ currentTime = InsTime(gpst.gpsCycle,
148 gpst.gpsWeek,
149 ✗ static_cast<long double>(std::get<ErbVel>(obs->data).iTOW) / 1000.0L);
150 ✗ obs->insTime = currentTime;
151 }
152
153 LOG_DATA("Erb: VEL, iTOW {}, gSpeed {} [cm/s], heading {} [deg*1e-5], velD {} [cm/s]",
154 std::get<ErbVel>(obs->data).iTOW, std::get<ErbVel>(obs->data).gSpeed,
155 std::get<ErbVel>(obs->data).heading, std::get<ErbVel>(obs->data).velD);
156 }
157 ✗ else if (msgId == ErbMessageID::ERB_MessageId_SVI)
158 {
159 ✗ obs->data = ErbSvi();
160
161 ✗ std::get<ErbSvi>(obs->data).iTOW = packet.extractUint32();
162 ✗ std::get<ErbSvi>(obs->data).nSV = packet.extractUint8();
163 ✗ std::get<ErbSvi>(obs->data).idSV = packet.extractUint8();
164 ✗ std::get<ErbSvi>(obs->data).typeSV = packet.extractUint8();
165 ✗ std::get<ErbSvi>(obs->data).carPh = packet.extractInt32();
166 ✗ std::get<ErbSvi>(obs->data).psRan = packet.extractInt32();
167 ✗ std::get<ErbSvi>(obs->data).freqD = packet.extractInt32();
168 ✗ std::get<ErbSvi>(obs->data).snr = packet.extractUint16();
169 ✗ std::get<ErbSvi>(obs->data).azim = packet.extractUint16();
170 ✗ std::get<ErbSvi>(obs->data).elev = packet.extractUint16();
171
172 // Calculate the insTime with the iTOW
173 ✗ auto currentTime = util::time::GetCurrentInsTime();
174 ✗ if (!currentTime.empty())
175 {
176 ✗ auto gpst = currentTime.toGPSweekTow();
177 ✗ currentTime = InsTime(gpst.gpsCycle,
178 gpst.gpsWeek,
179 ✗ static_cast<long double>(std::get<ErbSvi>(obs->data).iTOW) / 1000.0L);
180 ✗ obs->insTime = currentTime;
181 }
182
183 LOG_DATA("Erb: SVI, iTOW {}, nSV {} , idSV {} , typeSV {} , carPh {}, psRan {} , freqD {} , snr {}, , azim {} , elev {}",
184 std::get<ErbSvi>(obs->data).iTOW, std::get<ErbSvi>(obs->data).nSV, std::get<ErbSvi>(obs->data).idSV,
185 std::get<ErbSvi>(obs->data).typeSV, std::get<ErbSvi>(obs->data).carPh,
186 std::get<ErbSvi>(obs->data).psRan, std::get<ErbSvi>(obs->data).freqD,
187 std::get<ErbSvi>(obs->data).snr, std::get<ErbSvi>(obs->data).azim,
188 std::get<ErbSvi>(obs->data).elev);
189 }
190 ✗ else if (msgId == ErbMessageID::ERB_MessageId_RTK)
191 {
192 ✗ obs->data = ErbRtk();
193
194 ✗ std::get<ErbRtk>(obs->data).numSV = packet.extractUint8();
195 ✗ std::get<ErbRtk>(obs->data).age = packet.extractUint16();
196 ✗ std::get<ErbRtk>(obs->data).baselineN = packet.extractInt32();
197 ✗ std::get<ErbRtk>(obs->data).baselineE = packet.extractInt32();
198 ✗ std::get<ErbRtk>(obs->data).baselineD = packet.extractInt32();
199 ✗ std::get<ErbRtk>(obs->data).arRatio = packet.extractUint16();
200 ✗ std::get<ErbRtk>(obs->data).weekGPS = packet.extractUint16();
201 ✗ std::get<ErbRtk>(obs->data).timeGPS = packet.extractUint32();
202
203 LOG_DATA("Erb: RTK, numSV {}, age {} [s * 1e-2] , baselineN {} , baselineE {} , baselineD {}, arRatio {} [*1e-1] , weekGPS {} , timeGPS {}",
204 std::get<ErbRtk>(obs->data).numSV, std::get<ErbRtk>(obs->data).age, std::get<ErbRtk>(obs->data).baselineN,
205 std::get<ErbRtk>(obs->data).baselineE, std::get<ErbRtk>(obs->data).baselineD,
206 std::get<ErbRtk>(obs->data).arRatio, std::get<ErbRtk>(obs->data).weekGPS,
207 std::get<ErbRtk>(obs->data).timeGPS);
208 }
209 else
210 {
211 LOG_DATA("Erb: {:x}, Size {}, not implemented yet!", obs->msgId, packet.getRawDataLength());
212 }
213 }
214 ✗ }
215
216 ✗ std::pair<uint8_t, uint8_t> NAV::vendor::emlid::checksumUBX(const std::vector<uint8_t>& data)
217 {
218 ✗ uint8_t cka = 0;
219 ✗ uint8_t ckb = 0;
220
221 ✗ for (size_t i = 2; i < data.size() - 2; i++)
222 {
223 ✗ cka += data.at(i);
224 ✗ ckb += cka;
225 }
226 ✗ return std::make_pair(cka, ckb);
227 }
228