stdbin_decoder.test.cc
Go to the documentation of this file.
1 #include <gtest/gtest.h>
3 
8 
9 TEST(StdBinDecoder, WeCannotParseAFrameWithSomeMissingFields)
10 {
11  // Given a frame with only attitude :
12  // clang-format off
13  const std::vector<uint8_t> memory{
14  'I', 'X', /*IX blue header */
15  0x02, /*Protocol Version */
16  0x00, 0x00, 0x00, 0x01, /* navigation bitmask (0x00000001 means only AttitudeAndHeading) */
17  0x00, 0x00, 0x00, 0x00, /* external data bitmask */
18  0x00, 0x21, /* Telegram size */
19  0x00, 0x00, 0x00, 0x05, /* navigation validity time (500 us) */
20  0x00, 0x00, 0x01, 0x23, /* counter (0x123) */
21  0x00, 0x00, 0xa0, 0x3f, /* Heading : 1.25f */
22  0x00, 0x00, 0xc0, 0xbf, /* roll : -1.5f */
23  0xcd, 0xcc, 0x48, 0x41, /* Pitch : 12.55f */
24  /* Checksum missing */
25  };
26  // clang-format on
27 
29  parser.addNewData(memory);
30  EXPECT_THROW(parser.parseNextFrame(), std::runtime_error);
31 }
32 
33 TEST(StdBinDecoder, WeCanParseAMinimalV2Frame)
34 {
35  // Given a frame with only attitude :
36  // clang-format off
37  const std::vector<uint8_t> memory{
38  'I', 'X', /*IX blue header */
39  0x02, /*Protocol Version */
40  0x00, 0x00, 0x00, 0x01, /* navigation bitmask (0x00000001 means only AttitudeAndHeading) */
41  0x00, 0x00, 0x00, 0x00, /* external data bitmask */
42  0x00, 0x25, /* Telegram size */
43  0x00, 0x00, 0x00, 0x05, /* navigation validity time (500 us) */
44  0x00, 0x00, 0x01, 0x23, /* counter (0x123) */
45  0x00, 0x00, 0xa0, 0x3f, /* Heading : 1.25f */
46  0x00, 0x00, 0xc0, 0xbf, /* roll : -1.5f */
47  0xcd, 0xcc, 0x48, 0x41, /* Pitch : 12.55f */
48  0x00, 0x00, 0x5, 0x72 /* Checksum */
49  };
50  // clang-format on
51 
53  parser.addNewData(memory);
54  ASSERT_TRUE(parser.parseNextFrame());
55  auto result = parser.getLastNavData();
56 
57  ASSERT_FALSE(result.attitudeHeadingDeviation.is_initialized());
58  ASSERT_TRUE(result.attitudeHeading.is_initialized());
59  // We don't test attitude heading is parsed with good values, we already have a test
60  // for this.
61 }
62 
63 TEST(StdBinDecoder, WeCanParseV2Protocol)
64 {
66 
67  parser.addNewData(log_STDBIN_V2);
68  ASSERT_TRUE(parser.parseNextFrame());
69  auto result = parser.getLastNavData();
70 
71  EXPECT_TRUE(result.attitudeHeading.is_initialized());
72  EXPECT_FLOAT_EQ(result.attitudeHeading.get().heading_deg, 209.982);
73  EXPECT_FLOAT_EQ(result.attitudeHeading.get().roll_deg, 0.016);
74  EXPECT_FLOAT_EQ(result.attitudeHeading.get().pitch_deg, 0.206);
75 
76  EXPECT_TRUE(result.attitudeHeadingDeviation.is_initialized());
77  EXPECT_FLOAT_EQ(result.attitudeHeadingDeviation.get().heading_stdDev_deg, 2.4517534);
78  EXPECT_FLOAT_EQ(result.attitudeHeadingDeviation.get().roll_stdDev_deg, 0.004243818);
79  EXPECT_FLOAT_EQ(result.attitudeHeadingDeviation.get().pitch_stdDev_deg, 0.004769328);
80 
81  EXPECT_TRUE(result.rtHeaveSurgeSway.is_initialized());
82  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_heave_withoutBdL, 0.0);
83  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_heave_atBdL, 0.0);
84  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_surge_atBdL, 0.0);
85  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_sway_atBdL, 0.0);
86 
87  EXPECT_TRUE(result.smartHeave.is_initialized());
88  EXPECT_EQ(result.smartHeave.get().validityTime_100us, 9215311);
89  EXPECT_FLOAT_EQ(result.smartHeave.get().smartHeave_m, 0.0);
90 
91  EXPECT_TRUE(result.headingRollPitchRate.is_initialized());
92  EXPECT_FLOAT_EQ(result.headingRollPitchRate.get().heading_rate, 0.0);
93  EXPECT_FLOAT_EQ(result.headingRollPitchRate.get().roll_rate, 0.0);
94  EXPECT_FLOAT_EQ(result.headingRollPitchRate.get().pitch_rate, -0.002);
95 
96  EXPECT_TRUE(result.rotationRateVesselFrame.is_initialized());
97  EXPECT_FLOAT_EQ(result.rotationRateVesselFrame.get().xv1_degsec, 0.001287996);
98  EXPECT_FLOAT_EQ(result.rotationRateVesselFrame.get().xv2_degsec, 0.0010229985);
99  EXPECT_FLOAT_EQ(result.rotationRateVesselFrame.get().xv3_degsec, 0.0026694948);
100 
101  EXPECT_TRUE(result.accelerationVesselFrame.is_initialized());
102  EXPECT_FLOAT_EQ(result.accelerationVesselFrame.get().xv1_msec2, -0.00034000012);
103  EXPECT_FLOAT_EQ(result.accelerationVesselFrame.get().xv2_msec2, 0.000035000077);
104  EXPECT_FLOAT_EQ(result.accelerationVesselFrame.get().xv3_msec2, 0.000055000033);
105 
106  EXPECT_TRUE(result.position.is_initialized());
107  EXPECT_DOUBLE_EQ(result.position.get().latitude_deg, 2.1332412116407853);
108  EXPECT_DOUBLE_EQ(result.position.get().longitude_deg, 48.000037178805215);
109  EXPECT_EQ(result.position.get().altitude_ref, 0);
110  EXPECT_FLOAT_EQ(result.position.get().altitude_m, 1.0547082);
111 
112  EXPECT_TRUE(result.positionDeviation.is_initialized());
113  EXPECT_FLOAT_EQ(result.positionDeviation.get().north_stddev_m, 14.192304);
114  EXPECT_FLOAT_EQ(result.positionDeviation.get().east_stddev_m, 14.168153);
115  EXPECT_FLOAT_EQ(result.positionDeviation.get().north_east_corr, 0.58203787);
116  EXPECT_FLOAT_EQ(result.positionDeviation.get().altitude_stddev_m, 7.6183596);
117 
118  EXPECT_TRUE(result.speedGeographicFrame.is_initialized());
119  EXPECT_FLOAT_EQ(result.speedGeographicFrame.get().north_msec, -0.0015761498);
120  EXPECT_FLOAT_EQ(result.speedGeographicFrame.get().east_msec, -0.027757198);
121  EXPECT_FLOAT_EQ(result.speedGeographicFrame.get().up_msec, 0.045121633);
122 
123  EXPECT_TRUE(result.speedGeographicFrameDeviation.is_initialized());
124  EXPECT_FLOAT_EQ(result.speedGeographicFrameDeviation.get().north_stddev_msec,
125  0.0114673115);
126  EXPECT_FLOAT_EQ(result.speedGeographicFrameDeviation.get().east_stddev_msec,
127  0.009463781);
128  EXPECT_FLOAT_EQ(result.speedGeographicFrameDeviation.get().up_stddev_msec,
129  0.029867083);
130 
131  EXPECT_TRUE(result.currentGeographicFrame.is_initialized());
132  EXPECT_FLOAT_EQ(result.currentGeographicFrame.get().north_msec, 0.0);
133  EXPECT_FLOAT_EQ(result.currentGeographicFrame.get().east_msec, -0.0);
134 
135  EXPECT_TRUE(result.currentGeographicFrameDeviation.is_initialized());
136  EXPECT_FLOAT_EQ(result.currentGeographicFrameDeviation.get().north_stddev_msec, 0.0);
137  EXPECT_FLOAT_EQ(result.currentGeographicFrameDeviation.get().east_stddev_msec, 0.0);
138 
139  EXPECT_TRUE(result.systemDate.is_initialized());
140  EXPECT_EQ(result.systemDate.get().day, 1);
141  EXPECT_EQ(result.systemDate.get().month, 1);
142  EXPECT_EQ(result.systemDate.get().year, 2006);
143 
144  EXPECT_TRUE(result.sensorStatus.is_initialized());
145  EXPECT_EQ(result.sensorStatus.get().status1, 0);
146  EXPECT_EQ(result.sensorStatus.get().status2, 0);
147 
148  EXPECT_TRUE(result.insAlgorithmStatus.is_initialized());
149  EXPECT_EQ(result.insAlgorithmStatus.get().status1, 0x12);
150  EXPECT_EQ(result.insAlgorithmStatus.get().status2, 0x30000);
151  EXPECT_EQ(result.insAlgorithmStatus.get().status3, 0x4000000);
152  EXPECT_EQ(result.insAlgorithmStatus.get().status4, 0x200);
153 
154  EXPECT_TRUE(result.insSystemStatus.is_initialized());
155  EXPECT_EQ(result.insSystemStatus.get().status1, 0x200);
156  EXPECT_EQ(result.insSystemStatus.get().status2, 0x1000);
157  EXPECT_EQ(result.insSystemStatus.get().status3, 0x0);
158 
159  EXPECT_TRUE(result.insUserStatus.is_initialized());
160  EXPECT_EQ(result.insUserStatus.get().status, 0x4c000000);
161 
162  EXPECT_FALSE(result.ahrsAlgorithmStatus.is_initialized());
163 
164  EXPECT_FALSE(result.ahrsSystemStatus.is_initialized());
165 
166  EXPECT_FALSE(result.ahrsUserStatus.is_initialized());
167 
168  EXPECT_TRUE(result.heaveSurgeSwaySpeed.is_initialized());
169  EXPECT_FLOAT_EQ(result.heaveSurgeSwaySpeed.get().realtime_heave_speed, 0.0);
170  EXPECT_FLOAT_EQ(result.heaveSurgeSwaySpeed.get().surge_speed, 0.0);
171  EXPECT_FLOAT_EQ(result.heaveSurgeSwaySpeed.get().sway_speed, 0.0);
172 
173  EXPECT_TRUE(result.speedVesselFrame.is_initialized());
174  EXPECT_FLOAT_EQ(result.speedVesselFrame.get().xv1_msec, 0.014972644);
175  EXPECT_FLOAT_EQ(result.speedVesselFrame.get().xv2_msec, -0.023280365);
176  EXPECT_FLOAT_EQ(result.speedVesselFrame.get().xv3_msec, 0.0449414);
177 
178  EXPECT_TRUE(result.accelerationGeographicFrame.is_initialized());
179  EXPECT_FLOAT_EQ(result.accelerationGeographicFrame.get().north_msec2, 0.0);
180  EXPECT_FLOAT_EQ(result.accelerationGeographicFrame.get().east_msec2, 0.0);
181  EXPECT_FLOAT_EQ(result.accelerationGeographicFrame.get().up_msec2, 9.809999);
182 
183  EXPECT_TRUE(result.courseSpeedoverGround.is_initialized());
184  EXPECT_FLOAT_EQ(result.courseSpeedoverGround.get().course_over_ground, 266.96848);
185  EXPECT_FLOAT_EQ(result.courseSpeedoverGround.get().speed_over_ground, 0.027778123);
186 
187  EXPECT_TRUE(result.temperatures.is_initialized());
188  EXPECT_FLOAT_EQ(result.temperatures.get().mean_temp_fog, 26.158884);
189  EXPECT_FLOAT_EQ(result.temperatures.get().mean_temp_acc, 26.158884);
190  EXPECT_FLOAT_EQ(result.temperatures.get().board_temperature, 158.65643);
191 
192  EXPECT_FALSE(result.attitudeQuaternion.is_initialized()); /* From V3 */
193 
194  EXPECT_FALSE(result.attitudeQuaternionDeviation.is_initialized()); /* From V3 */
195 
196  EXPECT_FALSE(result.rawAccelerationVesselFrame.is_initialized()); /* From V3 */
197 
198  EXPECT_FALSE(result.accelerationVesselFrameDeviation.is_initialized()); /* From V3 */
199 
200  EXPECT_FALSE(result.rotationRateVesselFrameDeviation.is_initialized()); /* From V3 */
201 }
202 
203 TEST(StdBinDecoder, WeCanParseV2ProtocolReceivedInTwoPartsWhereverCutpointIs)
204 {
205  // By instanciating parser out of the loop, we also test that we can parse
206  // multiple messages.
208 
209  for(size_t i = 1; i < log_STDBIN_V2.size(); ++i)
210  {
211  std::vector<uint8_t> part1, part2;
212  auto cutPoint = log_STDBIN_V2.begin();
213  // we cut the packet everywhere:
214  std::advance(cutPoint, i);
215  std::copy(std::begin(log_STDBIN_V2), cutPoint, std::back_inserter(part1));
216  std::copy(cutPoint, std::end(log_STDBIN_V2), std::back_inserter(part2));
217 
218  parser.addNewData(part1);
219  EXPECT_FALSE(parser.parseNextFrame());
220  parser.addNewData(part2);
221  ASSERT_TRUE(parser.parseNextFrame());
222  auto result = parser.getLastNavData();
223 
224  EXPECT_TRUE(result.attitudeHeading.is_initialized());
225  EXPECT_FLOAT_EQ(result.attitudeHeading.get().heading_deg, 209.982);
226  EXPECT_FLOAT_EQ(result.attitudeHeading.get().roll_deg, 0.016);
227  EXPECT_FLOAT_EQ(result.attitudeHeading.get().pitch_deg, 0.206);
228  }
229 }
230 
231 TEST(StdBinDecoder, WeCanParseV2ProtocolReceivedWithGarbageAtFront)
232 {
234 
235  std::vector<uint8_t> frame;
236  // First add half-frame (=garbage)
237  std::copy(log_STDBIN_V2.begin() + log_STDBIN_V2.size() / 2, log_STDBIN_V2.end(),
238  std::back_inserter(frame));
239  // Then complete frame
240  std::copy(log_STDBIN_V2.begin(), log_STDBIN_V2.end(), std::back_inserter(frame));
241 
242  parser.addNewData(frame);
243  ASSERT_TRUE(parser.parseNextFrame());
244  auto result = parser.getLastNavData();
245 
246  EXPECT_TRUE(result.attitudeHeading.is_initialized());
247  EXPECT_FLOAT_EQ(result.attitudeHeading.get().heading_deg, 209.982);
248  EXPECT_FLOAT_EQ(result.attitudeHeading.get().roll_deg, 0.016);
249  EXPECT_FLOAT_EQ(result.attitudeHeading.get().pitch_deg, 0.206);
250 }
251 
252 TEST(StdBinDecoder, WeCanParse2FramesInTheSameBufferWithGarbageOnFront)
253 {
255 
256  std::vector<uint8_t> frame;
257  // First add half-frame (=garbage)
258  std::copy(log_STDBIN_V2.begin() + log_STDBIN_V2.size() / 2, log_STDBIN_V2.end(),
259  std::back_inserter(frame));
260  // Then a V2 complete frame
261  std::copy(log_STDBIN_V2.begin(), log_STDBIN_V2.end(), std::back_inserter(frame));
262  // And a V4 complete frame
263  std::copy(log_STDBIN_V4.begin(), log_STDBIN_V4.end(), std::back_inserter(frame));
264 
265  parser.addNewData(frame);
266 
267  ASSERT_TRUE(parser.parseNextFrame());
268  const auto v2Header = parser.getLastHeaderData();
269  EXPECT_EQ(v2Header.protocolVersion, 2);
270 
271  ASSERT_TRUE(parser.parseNextFrame());
272  const auto v4Header = parser.getLastHeaderData();
273  EXPECT_EQ(v4Header.protocolVersion, 4);
274 
275  // Internal buffer is now empty, cannot parse a frame anymore
276  ASSERT_FALSE(parser.parseNextFrame());
277 }
278 
279 TEST(StdBinDecoder, WeCanParseV3Protocol)
280 {
282 
283  parser.addNewData(log_STDBIN_V3);
284  ASSERT_TRUE(parser.parseNextFrame());
285  auto result = parser.getLastNavData();
286 
287  // --- Navigation data block
288  EXPECT_TRUE(result.attitudeHeading.is_initialized());
289  EXPECT_FLOAT_EQ(result.attitudeHeading.get().heading_deg, 0.0);
290  EXPECT_FLOAT_EQ(result.attitudeHeading.get().roll_deg, 0.809);
291  EXPECT_FLOAT_EQ(result.attitudeHeading.get().pitch_deg, -0.521);
292 
293  EXPECT_TRUE(result.attitudeHeadingDeviation.is_initialized());
294  EXPECT_FLOAT_EQ(result.attitudeHeadingDeviation.get().heading_stdDev_deg, 60.0);
295  EXPECT_FLOAT_EQ(result.attitudeHeadingDeviation.get().roll_stdDev_deg, 4.9997573);
296  EXPECT_FLOAT_EQ(result.attitudeHeadingDeviation.get().pitch_stdDev_deg, 4.999801);
297 
298  EXPECT_TRUE(result.rtHeaveSurgeSway.is_initialized());
299  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_heave_withoutBdL, 0.0);
300  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_heave_atBdL, 0.0);
301  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_surge_atBdL, 0.0);
302  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_sway_atBdL, 0.0);
303 
304  EXPECT_TRUE(result.smartHeave.is_initialized());
305  EXPECT_EQ(result.smartHeave.get().validityTime_100us, 565983535); // Sec
306  EXPECT_FLOAT_EQ(result.smartHeave.get().smartHeave_m, 0.0);
307 
308  EXPECT_TRUE(result.headingRollPitchRate.is_initialized());
309  EXPECT_FLOAT_EQ(result.headingRollPitchRate.get().heading_rate, 0.0);
310  EXPECT_FLOAT_EQ(result.headingRollPitchRate.get().roll_rate, 0.0);
311  EXPECT_FLOAT_EQ(result.headingRollPitchRate.get().pitch_rate, -0.0029999998);
312 
313  EXPECT_TRUE(result.rotationRateVesselFrame.is_initialized());
314  EXPECT_FLOAT_EQ(result.rotationRateVesselFrame.get().xv1_degsec, -0.0013599998);
315  EXPECT_FLOAT_EQ(result.rotationRateVesselFrame.get().xv2_degsec, -0.0021450005);
316  EXPECT_FLOAT_EQ(result.rotationRateVesselFrame.get().xv3_degsec, -0.000024999996);
317 
318  EXPECT_TRUE(result.accelerationVesselFrame.is_initialized());
319  EXPECT_FLOAT_EQ(result.accelerationVesselFrame.get().xv1_msec2, -0.0004);
320  EXPECT_FLOAT_EQ(result.accelerationVesselFrame.get().xv2_msec2, -0.00059999997);
321  EXPECT_FLOAT_EQ(result.accelerationVesselFrame.get().xv3_msec2, -0.07685006);
322 
323  EXPECT_TRUE(result.position.is_initialized());
324  EXPECT_DOUBLE_EQ(result.position.get().latitude_deg, 48.899099791755994);
325  EXPECT_DOUBLE_EQ(result.position.get().longitude_deg, 2.061999802664471);
326  EXPECT_EQ(result.position.get().altitude_ref, 0);
327  EXPECT_FLOAT_EQ(result.position.get().altitude_m, 3004.5398);
328 
329  EXPECT_TRUE(result.positionDeviation.is_initialized());
330  EXPECT_FLOAT_EQ(result.positionDeviation.get().north_stddev_m, 50.0);
331  EXPECT_FLOAT_EQ(result.positionDeviation.get().east_stddev_m, 50.0);
332  EXPECT_FLOAT_EQ(result.positionDeviation.get().north_east_corr, -0.00005929864);
333  EXPECT_FLOAT_EQ(result.positionDeviation.get().altitude_stddev_m, 1.1056086);
334 
335  EXPECT_TRUE(result.speedGeographicFrame.is_initialized());
336  EXPECT_FLOAT_EQ(result.speedGeographicFrame.get().north_msec, -0.018215187);
337  EXPECT_FLOAT_EQ(result.speedGeographicFrame.get().east_msec, -0.012175002);
338  EXPECT_FLOAT_EQ(result.speedGeographicFrame.get().up_msec, 0.10336427);
339 
340  EXPECT_TRUE(result.speedGeographicFrameDeviation.is_initialized());
341  EXPECT_FLOAT_EQ(result.speedGeographicFrameDeviation.get().north_stddev_msec,
342  12.129728);
343  EXPECT_FLOAT_EQ(result.speedGeographicFrameDeviation.get().east_stddev_msec,
344  12.129744);
345  EXPECT_FLOAT_EQ(result.speedGeographicFrameDeviation.get().up_stddev_msec,
346  0.78773665);
347 
348  EXPECT_TRUE(result.currentGeographicFrame.is_initialized());
349  EXPECT_FLOAT_EQ(result.currentGeographicFrame.get().north_msec, 1.919778 * 1e-14);
350  EXPECT_FLOAT_EQ(result.currentGeographicFrame.get().east_msec, 4.5171327 * 1e-15);
351 
352  EXPECT_TRUE(result.currentGeographicFrameDeviation.is_initialized());
353  EXPECT_FLOAT_EQ(result.currentGeographicFrameDeviation.get().north_stddev_msec,
354  0.50110954);
355  EXPECT_FLOAT_EQ(result.currentGeographicFrameDeviation.get().east_stddev_msec,
356  0.5011094);
357 
358  EXPECT_TRUE(result.systemDate.is_initialized());
359  EXPECT_EQ(result.systemDate.get().day, 14);
360  EXPECT_EQ(result.systemDate.get().month, 3);
361  EXPECT_EQ(result.systemDate.get().year, 2019);
362 
363  EXPECT_TRUE(result.sensorStatus.is_initialized());
364  EXPECT_EQ(result.sensorStatus.get().status1, 0x0);
365  EXPECT_EQ(result.sensorStatus.get().status2, 0x100);
366 
367  EXPECT_TRUE(result.insAlgorithmStatus.is_initialized());
368  EXPECT_EQ(result.insAlgorithmStatus.get().status1, 0x81013112);
369  EXPECT_EQ(result.insAlgorithmStatus.get().status2, 0x1003011);
370  EXPECT_EQ(result.insAlgorithmStatus.get().status3, 0x4000100);
371  EXPECT_EQ(result.insAlgorithmStatus.get().status4, 0x200);
372 
373  EXPECT_TRUE(result.insSystemStatus.is_initialized());
374  EXPECT_EQ(result.insSystemStatus.get().status1, 0x08011e00);
375  EXPECT_EQ(result.insSystemStatus.get().status2, 0x8eff);
376  EXPECT_EQ(result.insSystemStatus.get().status3, 0x0);
377 
378  EXPECT_TRUE(result.insUserStatus.is_initialized());
379  EXPECT_EQ(result.insUserStatus.get().status, 0x4c001102);
380 
381  EXPECT_FALSE(result.ahrsAlgorithmStatus.is_initialized());
382 
383  EXPECT_FALSE(result.ahrsSystemStatus.is_initialized());
384 
385  EXPECT_FALSE(result.ahrsUserStatus.is_initialized());
386 
387  EXPECT_TRUE(result.heaveSurgeSwaySpeed.is_initialized());
388  EXPECT_FLOAT_EQ(result.heaveSurgeSwaySpeed.get().realtime_heave_speed, 0.0);
389  EXPECT_FLOAT_EQ(result.heaveSurgeSwaySpeed.get().surge_speed, 0.0);
390  EXPECT_FLOAT_EQ(result.heaveSurgeSwaySpeed.get().sway_speed, 0.0);
391 
392  EXPECT_TRUE(result.speedVesselFrame.is_initialized());
393  EXPECT_FLOAT_EQ(result.speedVesselFrame.get().xv1_msec, -0.017242743);
394  EXPECT_FLOAT_EQ(result.speedVesselFrame.get().xv2_msec, 0.013600622);
395  EXPECT_FLOAT_EQ(result.speedVesselFrame.get().xv3_msec, 0.10299126);
396 
397  EXPECT_TRUE(result.accelerationGeographicFrame.is_initialized());
398  EXPECT_FLOAT_EQ(result.accelerationGeographicFrame.get().north_msec2, 0.0);
399  EXPECT_FLOAT_EQ(result.accelerationGeographicFrame.get().east_msec2, 0.0);
400  EXPECT_FLOAT_EQ(result.accelerationGeographicFrame.get().up_msec2, 9.83);
401 
402  EXPECT_TRUE(result.courseSpeedoverGround.is_initialized());
403  EXPECT_FLOAT_EQ(result.courseSpeedoverGround.get().course_over_ground, 213.74023);
404  EXPECT_FLOAT_EQ(result.courseSpeedoverGround.get().speed_over_ground, 0.021863433);
405 
406  EXPECT_TRUE(result.temperatures.is_initialized());
407  EXPECT_FLOAT_EQ(result.temperatures.get().mean_temp_fog, 28.700487);
408  EXPECT_FLOAT_EQ(result.temperatures.get().mean_temp_acc, 37.533264);
409  EXPECT_FLOAT_EQ(result.temperatures.get().board_temperature, 47.899994);
410 
411  EXPECT_TRUE(result.attitudeQuaternion.is_initialized());
412  EXPECT_FLOAT_EQ(result.attitudeQuaternion.get().q0, 0.9999646544456482);
413  EXPECT_FLOAT_EQ(result.attitudeQuaternion.get().q1, -0.00706720445305109);
414  EXPECT_FLOAT_EQ(result.attitudeQuaternion.get().q2, 0.00454969797283411);
415  EXPECT_FLOAT_EQ(result.attitudeQuaternion.get().q3, -0.00003185356399626471);
416 
417  EXPECT_TRUE(result.attitudeQuaternionDeviation.is_initialized());
418  EXPECT_FLOAT_EQ(result.attitudeQuaternionDeviation.get().quat_stddev_xi1,
419  0.08726222813129425);
420  EXPECT_FLOAT_EQ(result.attitudeQuaternionDeviation.get().quat_stddev_xi2,
421  0.08726298809051514);
422  EXPECT_FLOAT_EQ(result.attitudeQuaternionDeviation.get().quat_stddev_xi3,
423  1.0471975803375244);
424 
425  EXPECT_TRUE(result.rawAccelerationVesselFrame.is_initialized());
426  EXPECT_FLOAT_EQ(result.rawAccelerationVesselFrame.get().xv1_msec2, 0.0);
427  EXPECT_FLOAT_EQ(result.rawAccelerationVesselFrame.get().xv2_msec2, 0.0);
428  EXPECT_FLOAT_EQ(result.rawAccelerationVesselFrame.get().xv3_msec2, 7.999994);
429 
430  EXPECT_TRUE(result.accelerationVesselFrameDeviation.is_initialized());
431  EXPECT_FLOAT_EQ(result.accelerationVesselFrameDeviation.get().xv1_stddev_msec2,
432  16.82827);
433  EXPECT_FLOAT_EQ(result.accelerationVesselFrameDeviation.get().xv2_stddev_msec2,
434  16.827307);
435  EXPECT_FLOAT_EQ(result.accelerationVesselFrameDeviation.get().xv3_stddev_msec2,
436  1.1194783);
437 
438  EXPECT_TRUE(result.rotationRateVesselFrameDeviation.is_initialized());
439  EXPECT_FLOAT_EQ(result.rotationRateVesselFrameDeviation.get().xv1_stddev_degsec,
440  0.00008998095);
441  EXPECT_FLOAT_EQ(result.rotationRateVesselFrameDeviation.get().xv2_stddev_degsec,
442  0.0029352242);
443  EXPECT_FLOAT_EQ(result.rotationRateVesselFrameDeviation.get().xv3_stddev_degsec,
444  0.0032638267);
445 
446  // --- Extended navigation data block
447  EXPECT_TRUE(result.rotationAccelerationVesselFrame.is_initialized());
448  EXPECT_FLOAT_EQ(result.rotationAccelerationVesselFrame.get().xv1_degsec2,
449  0.0003888396);
450  EXPECT_FLOAT_EQ(result.rotationAccelerationVesselFrame.get().xv2_degsec2,
451  0.0017579537);
452  EXPECT_FLOAT_EQ(result.rotationAccelerationVesselFrame.get().xv3_degsec2,
453  0.0013762031);
454 
455  EXPECT_TRUE(result.rotationAccelerationVesselFrameDeviation.is_initialized());
456  EXPECT_FLOAT_EQ(
457  result.rotationAccelerationVesselFrameDeviation.get().xv1_stddev_degsec2,
458  0.0000141353985);
459  EXPECT_FLOAT_EQ(
460  result.rotationAccelerationVesselFrameDeviation.get().xv2_stddev_degsec2,
461  0.000014134977);
462  EXPECT_FLOAT_EQ(
463  result.rotationAccelerationVesselFrameDeviation.get().xv3_stddev_degsec2,
464  0.000014134976);
465 
466  EXPECT_TRUE(result.rawRotationRateVesselFrame.is_initialized());
467  EXPECT_FLOAT_EQ(result.rawRotationRateVesselFrame.get().xv1_degsec, 0.0);
468  EXPECT_FLOAT_EQ(result.rawRotationRateVesselFrame.get().xv2_degsec, 0.0);
469  EXPECT_FLOAT_EQ(result.rawRotationRateVesselFrame.get().xv3_degsec, 0.0);
470 
471  EXPECT_FALSE(result.vehicleAttitudeHeading.is_initialized());
472 
473  EXPECT_FALSE(result.vehicleAttitudeHeadingDeviation.is_initialized());
474 
475  EXPECT_FALSE(result.vehiclePosition.is_initialized());
476 
477  EXPECT_FALSE(result.vehiclePositionDeviation.is_initialized());
478 
479  // --- External data block
480  EXPECT_TRUE(result.utc.is_initialized());
481  EXPECT_EQ(result.utc.get().validityTime_100us, 566980000); // sec
482  EXPECT_EQ(result.utc.get().source, 0);
483 
484  EXPECT_TRUE(result.gnss1.is_initialized());
485  EXPECT_EQ(result.gnss1.get().validityTime_100us, 566981130); // sec
486  EXPECT_EQ(result.gnss1.get().gnss_id, 0);
487  EXPECT_EQ(result.gnss1.get().gnss_quality, 4);
488  EXPECT_DOUBLE_EQ(result.gnss1.get().latitude_deg, 48.89909998575846);
489  EXPECT_DOUBLE_EQ(result.gnss1.get().longitude_deg, 2.062000000476837);
490  EXPECT_FLOAT_EQ(result.gnss1.get().altitude_m, 3004.5);
491  EXPECT_FLOAT_EQ(result.gnss1.get().latitude_stddev_m, 0.1);
492  EXPECT_FLOAT_EQ(result.gnss1.get().longitude_stddev_m, 0.1);
493  EXPECT_FLOAT_EQ(result.gnss1.get().altitude_stddev_m, 0.1);
494  EXPECT_FLOAT_EQ(result.gnss1.get().lat_lon_stddev_m2, 0.0);
495  EXPECT_FLOAT_EQ(result.gnss1.get().geoidal_separation_m, 0.0);
496 
497  EXPECT_TRUE(result.gnss2.is_initialized());
498  EXPECT_EQ(result.gnss2.get().validityTime_100us, 566981130); // Sec
499  EXPECT_EQ(result.gnss2.get().gnss_id, 1);
500  EXPECT_EQ(result.gnss2.get().gnss_quality, 4);
501  EXPECT_DOUBLE_EQ(result.gnss2.get().latitude_deg, 48.89909998575846);
502  EXPECT_DOUBLE_EQ(result.gnss2.get().longitude_deg, 2.062000000476837);
503  EXPECT_FLOAT_EQ(result.gnss2.get().altitude_m, 3004.5);
504  EXPECT_FLOAT_EQ(result.gnss2.get().latitude_stddev_m, 0.1);
505  EXPECT_FLOAT_EQ(result.gnss2.get().longitude_stddev_m, 0.2);
506  EXPECT_FLOAT_EQ(result.gnss2.get().altitude_stddev_m, 0.3);
507  EXPECT_FLOAT_EQ(result.gnss2.get().lat_lon_stddev_m2, -0.0);
508  EXPECT_FLOAT_EQ(result.gnss2.get().geoidal_separation_m, 0.0);
509 
510  EXPECT_FALSE(result.gnssManual.is_initialized());
511 
512  EXPECT_TRUE(result.emlog1.is_initialized());
513  EXPECT_EQ(result.emlog1.get().validityTime_100us, 566980110);
514  EXPECT_EQ(result.emlog1.get().emlog_id, 0);
515  EXPECT_FLOAT_EQ(result.emlog1.get().xv1_waterSpeed_ms, 60.0);
516  EXPECT_FLOAT_EQ(result.emlog1.get().xv1_speed_stddev_ms, 0.01);
517 
518  EXPECT_FALSE(result.emlog2.is_initialized());
519 
520  EXPECT_TRUE(result.usbl1.is_initialized());
521  EXPECT_EQ(result.usbl1.get().validityTime_100us, 566980558);
522  EXPECT_EQ(result.usbl1.get().usbl_id, 0);
523  std::array<uint8_t, 8> beacon_usbl1 = {
524  {'A', 'B', 'C', '\0', '\0', '\0', '\0', '\0'}}; // TODO : VERIF
525  EXPECT_EQ(result.usbl1.get().beacon_id, beacon_usbl1);
526  EXPECT_EQ(result.usbl1.get().latitude_deg, 48.9);
527  EXPECT_DOUBLE_EQ(result.usbl1.get().longitude_deg, 2.0639999985694883);
528  EXPECT_DOUBLE_EQ(result.usbl1.get().altitude_m, 0.0);
529  EXPECT_FLOAT_EQ(result.usbl1.get().north_stddev_m, 2.0);
530  EXPECT_FLOAT_EQ(result.usbl1.get().east_stddev_m, 2.0);
531  EXPECT_FLOAT_EQ(result.usbl1.get().lat_lon_cov_m2, 0.0);
532  EXPECT_FLOAT_EQ(result.usbl1.get().altitude_stddev_m, 2.0);
533 
534  EXPECT_TRUE(result.usbl2.is_initialized());
535  EXPECT_EQ(result.usbl2.get().validityTime_100us, 566980110);
536  EXPECT_EQ(result.usbl2.get().usbl_id, 1);
537  std::array<uint8_t, 8> beacon_usbl2 = {
538  {'D', 'E', 'F', '\0', '\0', '\0', '\0', '\0'}}; // TODO : VERIF
539  EXPECT_EQ(result.usbl2.get().beacon_id, beacon_usbl2);
540  EXPECT_EQ(result.usbl2.get().latitude_deg, -48.898);
541  EXPECT_DOUBLE_EQ(result.usbl2.get().longitude_deg, 289.5933);
542  EXPECT_DOUBLE_EQ(result.usbl2.get().altitude_m, 0.0);
543  EXPECT_FLOAT_EQ(result.usbl2.get().north_stddev_m, 10.0);
544  EXPECT_FLOAT_EQ(result.usbl2.get().east_stddev_m, 10.0);
545  EXPECT_FLOAT_EQ(result.usbl2.get().lat_lon_cov_m2, 0.0);
546  EXPECT_FLOAT_EQ(result.usbl2.get().altitude_stddev_m, 10.0);
547 
548  EXPECT_FALSE(result.usbl3.is_initialized());
549 
550  EXPECT_TRUE(result.depth.is_initialized());
551  EXPECT_EQ(result.depth.get().validityTime_100us, 566980110); // Sec
552  EXPECT_EQ(result.depth.get().depth_m, -102.0);
553  EXPECT_EQ(result.depth.get().depth_stddev_m, 100.0);
554 
555  EXPECT_TRUE(result.dvlGroundSpeed1.is_initialized());
556  EXPECT_EQ(result.dvlGroundSpeed1.get().validityTime_100us, 566980110);
557  EXPECT_EQ(result.dvlGroundSpeed1.get().dvl_id, 0);
558  EXPECT_FLOAT_EQ(result.dvlGroundSpeed1.get().xv1_groundspeed_ms, 39.0);
559  EXPECT_FLOAT_EQ(result.dvlGroundSpeed1.get().xv2_groundspeed_ms, 39.0);
560  EXPECT_FLOAT_EQ(result.dvlGroundSpeed1.get().xv3_groundspeed_ms, 39.0);
561  EXPECT_FLOAT_EQ(result.dvlGroundSpeed1.get().dvl_speedofsound_ms, 1400.0);
562  EXPECT_FLOAT_EQ(result.dvlGroundSpeed1.get().dvl_altitude_m, 20.0);
563  EXPECT_FLOAT_EQ(result.dvlGroundSpeed1.get().xv1_stddev_ms, 3.0);
564  EXPECT_FLOAT_EQ(result.dvlGroundSpeed1.get().xv2_stddev_ms, 3.0);
565  EXPECT_FLOAT_EQ(result.dvlGroundSpeed1.get().xv3_stddev_ms, 3.0);
566 
567  EXPECT_TRUE(result.dvlWaterSpeed1.is_initialized());
568  EXPECT_EQ(result.dvlWaterSpeed1.get().validityTime_100us, 566980110);
569  EXPECT_EQ(result.dvlWaterSpeed1.get().dvl_id, 0);
570  EXPECT_FLOAT_EQ(result.dvlWaterSpeed1.get().xv1_waterspeed_ms, 18.571428);
571  EXPECT_FLOAT_EQ(result.dvlWaterSpeed1.get().xv2_waterspeed_ms, 18.571428);
572  EXPECT_FLOAT_EQ(result.dvlWaterSpeed1.get().xv3_waterspeed_ms, 18.571428);
573  EXPECT_FLOAT_EQ(result.dvlWaterSpeed1.get().dvl_speedofsound_ms, 1400.0);
574  EXPECT_FLOAT_EQ(result.dvlWaterSpeed1.get().xv1_stddev_ms, 3.0);
575  EXPECT_FLOAT_EQ(result.dvlWaterSpeed1.get().xv2_stddev_ms, 3.0);
576  EXPECT_FLOAT_EQ(result.dvlWaterSpeed1.get().xv3_stddev_ms, 3.0);
577 
578  EXPECT_TRUE(result.soundVelocity.is_initialized());
579  // EXPECT_EQ(result.soundVelocity.get().validityTime_100us, 0);
580  EXPECT_EQ(result.soundVelocity.get().ext_speedofsound_ms, 1300.0);
581 
582  EXPECT_FALSE(result.dmi.is_initialized());
583 
584  EXPECT_TRUE(result.lbl1.is_initialized());
585  EXPECT_EQ(result.lbl1.get().validityTime_100us, 566980558);
586  EXPECT_EQ(result.lbl1.get().rfu, 0);
587  std::array<uint8_t, 8> beacon_lbl1 = {
588  {'0', '\0', '\0', '\0', '\0', '\0', '\0', '\x1'}}; // TODO : verif
589  EXPECT_EQ(result.lbl1.get().beacon_id, beacon_lbl1);
590  EXPECT_DOUBLE_EQ(result.lbl1.get().beacon_latitude_deg, -48.89800001780192);
591  EXPECT_DOUBLE_EQ(result.lbl1.get().beacon_longitude_deg, 289.5960000038147);
592  EXPECT_FLOAT_EQ(result.lbl1.get().beacon_altitude_m, 0.0);
593  EXPECT_FLOAT_EQ(result.lbl1.get().range_m, 238.0);
594  EXPECT_FLOAT_EQ(result.lbl1.get().range_stddev_m, 10.0);
595 
596  EXPECT_FALSE(result.lbl2.is_initialized());
597 
598  EXPECT_FALSE(result.lbl3.is_initialized());
599 
600  EXPECT_FALSE(result.lbl4.is_initialized());
601 
602  EXPECT_FALSE(result.eventMarkerA.is_initialized());
603 
604  EXPECT_FALSE(result.eventMarkerB.is_initialized());
605 
606  EXPECT_FALSE(result.eventMarkerC.is_initialized());
607 
608  EXPECT_FALSE(result.dvlGroundSpeed2.is_initialized());
609 
610  EXPECT_FALSE(result.dvlWaterSpeed2.is_initialized());
611 
612  EXPECT_FALSE(result.turretAngles.is_initialized());
613 
614  EXPECT_FALSE(result.vtg1.is_initialized());
615 
616  EXPECT_FALSE(result.vtg2.is_initialized());
617 
618  EXPECT_FALSE(result.logBook.is_initialized());
619 }
620 
621 TEST(StdBinDecoder, WeCanParseV4Protocol)
622 {
624 
625  parser.addNewData(log_STDBIN_V4);
626  ASSERT_TRUE(parser.parseNextFrame());
627  auto result = parser.getLastNavData();
628 
629  // --- Navigation data block
630  EXPECT_TRUE(result.attitudeHeading.is_initialized());
631  EXPECT_FLOAT_EQ(result.attitudeHeading.get().heading_deg, 357.428);
632  EXPECT_FLOAT_EQ(result.attitudeHeading.get().roll_deg, -0.04);
633  EXPECT_FLOAT_EQ(result.attitudeHeading.get().pitch_deg, 0.178);
634 
635  EXPECT_TRUE(result.attitudeHeadingDeviation.is_initialized());
636  EXPECT_FLOAT_EQ(result.attitudeHeadingDeviation.get().heading_stdDev_deg, 54.47794);
637  EXPECT_FLOAT_EQ(result.attitudeHeadingDeviation.get().roll_stdDev_deg, 0.010677356);
638  EXPECT_FLOAT_EQ(result.attitudeHeadingDeviation.get().pitch_stdDev_deg, 0.016798064);
639 
640  EXPECT_TRUE(result.rtHeaveSurgeSway.is_initialized());
641  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_heave_withoutBdL, 0.0);
642  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_heave_atBdL, 0.0);
643  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_surge_atBdL, 0.0);
644  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_sway_atBdL, 0.0);
645 
646  EXPECT_TRUE(result.smartHeave.is_initialized());
647  EXPECT_EQ(result.smartHeave.get().validityTime_100us, 8715306); // Sec
648  EXPECT_FLOAT_EQ(result.smartHeave.get().smartHeave_m, 0.0);
649 
650  EXPECT_TRUE(result.headingRollPitchRate.is_initialized());
651  EXPECT_FLOAT_EQ(result.headingRollPitchRate.get().heading_rate, -0.002);
652  EXPECT_FLOAT_EQ(result.headingRollPitchRate.get().roll_rate, -0.005);
653  EXPECT_FLOAT_EQ(result.headingRollPitchRate.get().pitch_rate, 0.0);
654 
655  EXPECT_TRUE(result.rotationRateVesselFrame.is_initialized());
656  EXPECT_FLOAT_EQ(result.rotationRateVesselFrame.get().xv1_degsec, -0.0061404924);
657  EXPECT_FLOAT_EQ(result.rotationRateVesselFrame.get().xv2_degsec, -0.00091849366);
658  EXPECT_FLOAT_EQ(result.rotationRateVesselFrame.get().xv3_degsec, 0.0024884976);
659 
660  EXPECT_TRUE(result.accelerationVesselFrame.is_initialized());
661  EXPECT_FLOAT_EQ(result.accelerationVesselFrame.get().xv1_msec2, 0.00004);
662  EXPECT_FLOAT_EQ(result.accelerationVesselFrame.get().xv2_msec2, 0.00040999975);
663  EXPECT_FLOAT_EQ(result.accelerationVesselFrame.get().xv3_msec2, 0.0007599999);
664 
665  EXPECT_TRUE(result.position.is_initialized());
666  EXPECT_DOUBLE_EQ(result.position.get().latitude_deg, 2.133332706885363);
667  EXPECT_DOUBLE_EQ(result.position.get().longitude_deg, 47.999999832595385);
668  EXPECT_EQ(result.position.get().altitude_ref, 0);
669  EXPECT_FLOAT_EQ(result.position.get().altitude_m, 0.15949036);
670 
671  EXPECT_TRUE(result.positionDeviation.is_initialized());
672  EXPECT_FLOAT_EQ(result.positionDeviation.get().north_stddev_m, 14.204068);
673  EXPECT_FLOAT_EQ(result.positionDeviation.get().east_stddev_m, 14.156303);
674  EXPECT_FLOAT_EQ(result.positionDeviation.get().north_east_corr, 0.03173876);
675  EXPECT_FLOAT_EQ(result.positionDeviation.get().altitude_stddev_m, 2.828566);
676 
677  EXPECT_TRUE(result.speedGeographicFrame.is_initialized());
678  EXPECT_FLOAT_EQ(result.speedGeographicFrame.get().north_msec, -0.0003105167);
679  EXPECT_FLOAT_EQ(result.speedGeographicFrame.get().east_msec, -0.011570915);
680  EXPECT_FLOAT_EQ(result.speedGeographicFrame.get().up_msec, 0.04521207);
681 
682  EXPECT_TRUE(result.speedGeographicFrameDeviation.is_initialized());
683  EXPECT_FLOAT_EQ(result.speedGeographicFrameDeviation.get().north_stddev_msec,
684  0.006963054);
685  EXPECT_FLOAT_EQ(result.speedGeographicFrameDeviation.get().east_stddev_msec,
686  0.0066441046);
687  EXPECT_FLOAT_EQ(result.speedGeographicFrameDeviation.get().up_stddev_msec,
688  0.009995009);
689 
690  EXPECT_TRUE(result.currentGeographicFrame.is_initialized());
691  EXPECT_FLOAT_EQ(result.currentGeographicFrame.get().north_msec, 1.919778 * 0.0);
692  EXPECT_FLOAT_EQ(result.currentGeographicFrame.get().east_msec, 4.5171327 * 0.0);
693 
694  EXPECT_TRUE(result.currentGeographicFrameDeviation.is_initialized());
695  EXPECT_FLOAT_EQ(result.currentGeographicFrameDeviation.get().north_stddev_msec, 0.0);
696  EXPECT_FLOAT_EQ(result.currentGeographicFrameDeviation.get().east_stddev_msec, 0.0);
697 
698  EXPECT_TRUE(result.systemDate.is_initialized());
699  EXPECT_EQ(result.systemDate.get().day, 1);
700  EXPECT_EQ(result.systemDate.get().month, 1);
701  EXPECT_EQ(result.systemDate.get().year, 2006);
702 
703  EXPECT_TRUE(result.sensorStatus.is_initialized());
704  EXPECT_EQ(result.sensorStatus.get().status1, 0x0);
705  EXPECT_EQ(result.sensorStatus.get().status2, 0x0);
706 
707  EXPECT_TRUE(result.insAlgorithmStatus.is_initialized());
708  EXPECT_EQ(result.insAlgorithmStatus.get().status1, 0x00000012);
709  EXPECT_EQ(result.insAlgorithmStatus.get().status2, 0x00030000);
710  EXPECT_EQ(result.insAlgorithmStatus.get().status3, 0x04000000);
711  EXPECT_EQ(result.insAlgorithmStatus.get().status4, 0x00000200);
712 
713  EXPECT_TRUE(result.insSystemStatus.is_initialized());
714  EXPECT_EQ(result.insSystemStatus.get().status1, 0x00000200);
715  EXPECT_EQ(result.insSystemStatus.get().status2, 0x00001000);
716  EXPECT_EQ(result.insSystemStatus.get().status3, 0x0);
717 
718  EXPECT_TRUE(result.insUserStatus.is_initialized());
719  EXPECT_EQ(result.insUserStatus.get().status, 0x4c000000);
720 
721  EXPECT_FALSE(result.ahrsAlgorithmStatus.is_initialized());
722 
723  EXPECT_FALSE(result.ahrsSystemStatus.is_initialized());
724 
725  EXPECT_FALSE(result.ahrsUserStatus.is_initialized());
726 
727  EXPECT_TRUE(result.heaveSurgeSwaySpeed.is_initialized());
728  EXPECT_FLOAT_EQ(result.heaveSurgeSwaySpeed.get().realtime_heave_speed, 0.0);
729  EXPECT_FLOAT_EQ(result.heaveSurgeSwaySpeed.get().surge_speed, 0.0);
730  EXPECT_FLOAT_EQ(result.heaveSurgeSwaySpeed.get().sway_speed, 0.0);
731 
732  EXPECT_TRUE(result.speedVesselFrame.is_initialized());
733  EXPECT_FLOAT_EQ(result.speedVesselFrame.get().xv1_msec, 0.000061522915);
734  EXPECT_FLOAT_EQ(result.speedVesselFrame.get().xv2_msec, 0.011496336);
735  EXPECT_FLOAT_EQ(result.speedVesselFrame.get().xv3_msec, 0.04489224);
736 
737  EXPECT_TRUE(result.accelerationGeographicFrame.is_initialized());
738  EXPECT_FLOAT_EQ(result.accelerationGeographicFrame.get().north_msec2, 0.0);
739  EXPECT_FLOAT_EQ(result.accelerationGeographicFrame.get().east_msec2, 0.0);
740  EXPECT_FLOAT_EQ(result.accelerationGeographicFrame.get().up_msec2, 9.809999);
741 
742  EXPECT_TRUE(result.courseSpeedoverGround.is_initialized());
743  EXPECT_FLOAT_EQ(result.courseSpeedoverGround.get().course_over_ground, 268.42966);
744  EXPECT_FLOAT_EQ(result.courseSpeedoverGround.get().speed_over_ground, 0.011529781);
745 
746  EXPECT_TRUE(result.temperatures.is_initialized());
747  EXPECT_FLOAT_EQ(result.temperatures.get().mean_temp_fog, 25.993078);
748  EXPECT_FLOAT_EQ(result.temperatures.get().mean_temp_acc, 25.993078);
749  EXPECT_FLOAT_EQ(result.temperatures.get().board_temperature, 158.65643);
750 
751  EXPECT_TRUE(result.attitudeQuaternion.is_initialized());
752  EXPECT_FLOAT_EQ(result.attitudeQuaternion.get().q0, 0.9997469186782837);
753  EXPECT_FLOAT_EQ(result.attitudeQuaternion.get().q1, 0.00038782256888225675);
754  EXPECT_FLOAT_EQ(result.attitudeQuaternion.get().q2, -0.0015508179785683751);
755  EXPECT_FLOAT_EQ(result.attitudeQuaternion.get().q3, -0.02244032360613346);
756 
757  EXPECT_TRUE(result.attitudeQuaternionDeviation.is_initialized());
758  EXPECT_FLOAT_EQ(result.attitudeQuaternionDeviation.get().quat_stddev_xi1,
759  0.00018635205924510956);
760  EXPECT_FLOAT_EQ(result.attitudeQuaternionDeviation.get().quat_stddev_xi2,
761  0.00029318343149498105);
762  EXPECT_FLOAT_EQ(result.attitudeQuaternionDeviation.get().quat_stddev_xi3,
763  0.9508194327354431);
764 
765  EXPECT_TRUE(result.rawAccelerationVesselFrame.is_initialized());
766  EXPECT_FLOAT_EQ(result.rawAccelerationVesselFrame.get().xv1_msec2, 0.0);
767  EXPECT_FLOAT_EQ(result.rawAccelerationVesselFrame.get().xv2_msec2, 0.0);
768  EXPECT_FLOAT_EQ(result.rawAccelerationVesselFrame.get().xv3_msec2, 8.000145);
769 
770  EXPECT_TRUE(result.accelerationVesselFrameDeviation.is_initialized());
771  EXPECT_FLOAT_EQ(result.accelerationVesselFrameDeviation.get().xv1_stddev_msec2,
772  0.009960833);
773  EXPECT_FLOAT_EQ(result.accelerationVesselFrameDeviation.get().xv2_stddev_msec2,
774  0.009610326);
775  EXPECT_FLOAT_EQ(result.accelerationVesselFrameDeviation.get().xv3_stddev_msec2,
776  0.01413504);
777 
778  EXPECT_TRUE(result.rotationRateVesselFrameDeviation.is_initialized());
779  EXPECT_FLOAT_EQ(result.rotationRateVesselFrameDeviation.get().xv1_stddev_degsec,
780  0.0005122008);
781  EXPECT_FLOAT_EQ(result.rotationRateVesselFrameDeviation.get().xv2_stddev_degsec,
782  0.004000983);
783  EXPECT_FLOAT_EQ(result.rotationRateVesselFrameDeviation.get().xv3_stddev_degsec,
784  0.00051041954);
785 
786  // --- Extended navigation data block
787  EXPECT_TRUE(result.rotationAccelerationVesselFrame.is_initialized());
788  EXPECT_FLOAT_EQ(result.rotationAccelerationVesselFrame.get().xv1_degsec2,
789  0.000536854);
790  EXPECT_FLOAT_EQ(result.rotationAccelerationVesselFrame.get().xv2_degsec2,
791  0.002185823);
792  EXPECT_FLOAT_EQ(result.rotationAccelerationVesselFrame.get().xv3_degsec2,
793  -0.00046422562);
794 
795  EXPECT_TRUE(result.rotationAccelerationVesselFrameDeviation.is_initialized());
796  EXPECT_FLOAT_EQ(
797  result.rotationAccelerationVesselFrameDeviation.get().xv1_stddev_degsec2,
798  0.000706749);
799  EXPECT_FLOAT_EQ(
800  result.rotationAccelerationVesselFrameDeviation.get().xv2_stddev_degsec2,
801  0.0007067489);
802  EXPECT_FLOAT_EQ(
803  result.rotationAccelerationVesselFrameDeviation.get().xv3_stddev_degsec2,
804  0.000706749);
805 
806  EXPECT_TRUE(result.rawRotationRateVesselFrame.is_initialized());
807  EXPECT_FLOAT_EQ(result.rawRotationRateVesselFrame.get().xv1_degsec, 0.0);
808  EXPECT_FLOAT_EQ(result.rawRotationRateVesselFrame.get().xv2_degsec, 0.0);
809  EXPECT_FLOAT_EQ(result.rawRotationRateVesselFrame.get().xv3_degsec, 0.0);
810 
811  EXPECT_FALSE(result.vehicleAttitudeHeading.is_initialized());
812 
813  EXPECT_FALSE(result.vehicleAttitudeHeadingDeviation.is_initialized());
814 
815  EXPECT_FALSE(result.vehiclePosition.is_initialized());
816 
817  EXPECT_FALSE(result.vehiclePositionDeviation.is_initialized());
818 
819  // --- External data block ==> No external data in the log
820  EXPECT_FALSE(result.utc.is_initialized());
821 
822  EXPECT_FALSE(result.gnss1.is_initialized());
823 
824  EXPECT_FALSE(result.gnss2.is_initialized());
825 
826  EXPECT_FALSE(result.emlog1.is_initialized());
827 
828  EXPECT_FALSE(result.emlog2.is_initialized());
829 
830  EXPECT_FALSE(result.usbl1.is_initialized());
831 
832  EXPECT_FALSE(result.usbl2.is_initialized());
833 
834  EXPECT_FALSE(result.usbl3.is_initialized());
835 
836  EXPECT_FALSE(result.depth.is_initialized());
837 
838  EXPECT_FALSE(result.dvlGroundSpeed1.is_initialized());
839 
840  EXPECT_FALSE(result.dvlWaterSpeed1.is_initialized());
841 
842  EXPECT_FALSE(result.soundVelocity.is_initialized());
843 
844  EXPECT_FALSE(result.dmi.is_initialized());
845 
846  EXPECT_FALSE(result.lbl1.is_initialized());
847 
848  EXPECT_FALSE(result.lbl2.is_initialized());
849 
850  EXPECT_FALSE(result.lbl3.is_initialized());
851 
852  EXPECT_FALSE(result.lbl4.is_initialized());
853 
854  EXPECT_FALSE(result.eventMarkerA.is_initialized());
855 
856  EXPECT_FALSE(result.eventMarkerB.is_initialized());
857 
858  EXPECT_FALSE(result.eventMarkerC.is_initialized());
859 
860  EXPECT_FALSE(result.dvlGroundSpeed2.is_initialized());
861 
862  EXPECT_FALSE(result.dvlWaterSpeed2.is_initialized());
863 
864  EXPECT_FALSE(result.turretAngles.is_initialized());
865 
866  EXPECT_FALSE(result.vtg1.is_initialized());
867 
868  EXPECT_FALSE(result.vtg2.is_initialized());
869 
870  EXPECT_FALSE(result.logBook.is_initialized());
871 }
872 
873 TEST(StdBinDecoder, WeCanParseV5Protocol)
874 {
876 
877  parser.addNewData(log_STDBIN_V5);
878  ASSERT_TRUE(parser.parseNextFrame());
879  auto result = parser.getLastNavData();
880 
881  // --- Navigation data block
882  EXPECT_TRUE(result.attitudeHeading.is_initialized());
883  EXPECT_FLOAT_EQ(result.attitudeHeading.get().heading_deg, 357.404);
884  EXPECT_FLOAT_EQ(result.attitudeHeading.get().roll_deg, -0.04);
885  EXPECT_FLOAT_EQ(result.attitudeHeading.get().pitch_deg, 0.178);
886 
887  EXPECT_TRUE(result.attitudeHeadingDeviation.is_initialized());
888  EXPECT_FLOAT_EQ(result.attitudeHeadingDeviation.get().heading_stdDev_deg, 54.47794);
889  EXPECT_FLOAT_EQ(result.attitudeHeadingDeviation.get().roll_stdDev_deg, 0.010677356);
890  EXPECT_FLOAT_EQ(result.attitudeHeadingDeviation.get().pitch_stdDev_deg, 0.016798064);
891 
892  EXPECT_TRUE(result.rtHeaveSurgeSway.is_initialized());
893  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_heave_withoutBdL, 0.0);
894  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_heave_atBdL, 0.0);
895  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_surge_atBdL, 0.0);
896  EXPECT_FLOAT_EQ(result.rtHeaveSurgeSway.get().rt_sway_atBdL, 0.0);
897 
898  EXPECT_TRUE(result.smartHeave.is_initialized());
899  EXPECT_EQ(result.smartHeave.get().validityTime_100us, 8715456); // Sec
900  EXPECT_FLOAT_EQ(result.smartHeave.get().smartHeave_m, 0.0);
901 
902  EXPECT_TRUE(result.headingRollPitchRate.is_initialized());
903  EXPECT_FLOAT_EQ(result.headingRollPitchRate.get().heading_rate, -0.002);
904  EXPECT_FLOAT_EQ(result.headingRollPitchRate.get().roll_rate, -0.005);
905  EXPECT_FLOAT_EQ(result.headingRollPitchRate.get().pitch_rate, -0.001);
906 
907  EXPECT_TRUE(result.rotationRateVesselFrame.is_initialized());
908  EXPECT_FLOAT_EQ(result.rotationRateVesselFrame.get().xv1_degsec, -0.006139992);
909  EXPECT_FLOAT_EQ(result.rotationRateVesselFrame.get().xv2_degsec, -0.0009184938);
910  EXPECT_FLOAT_EQ(result.rotationRateVesselFrame.get().xv3_degsec, 0.0024874974);
911 
912  EXPECT_TRUE(result.accelerationVesselFrame.is_initialized());
913  EXPECT_FLOAT_EQ(result.accelerationVesselFrame.get().xv1_msec2, 0.000034999997);
914  EXPECT_FLOAT_EQ(result.accelerationVesselFrame.get().xv2_msec2, 0.00041499975);
915  EXPECT_FLOAT_EQ(result.accelerationVesselFrame.get().xv3_msec2, 0.000715);
916 
917  EXPECT_TRUE(result.position.is_initialized());
918  EXPECT_DOUBLE_EQ(result.position.get().latitude_deg, 2.133332701513265);
919  EXPECT_DOUBLE_EQ(result.position.get().longitude_deg, 47.99999983221664);
920  EXPECT_EQ(result.position.get().altitude_ref, 0);
921  EXPECT_FLOAT_EQ(result.position.get().altitude_m, 0.16032125);
922 
923  EXPECT_TRUE(result.positionDeviation.is_initialized());
924  EXPECT_FLOAT_EQ(result.positionDeviation.get().north_stddev_m, 14.204068);
925  EXPECT_FLOAT_EQ(result.positionDeviation.get().east_stddev_m, 14.156303);
926  EXPECT_FLOAT_EQ(result.positionDeviation.get().north_east_corr, 0.03173876);
927  EXPECT_FLOAT_EQ(result.positionDeviation.get().altitude_stddev_m, 2.828566);
928 
929  EXPECT_TRUE(result.speedGeographicFrame.is_initialized());
930  EXPECT_FLOAT_EQ(result.speedGeographicFrame.get().north_msec, -0.00031041596);
931  EXPECT_FLOAT_EQ(result.speedGeographicFrame.get().east_msec, -0.011805205);
932  EXPECT_FLOAT_EQ(result.speedGeographicFrame.get().up_msec, 0.045028113);
933 
934  EXPECT_TRUE(result.speedGeographicFrameDeviation.is_initialized());
935  EXPECT_FLOAT_EQ(result.speedGeographicFrameDeviation.get().north_stddev_msec,
936  0.0069630453);
937  EXPECT_FLOAT_EQ(result.speedGeographicFrameDeviation.get().east_stddev_msec,
938  0.006644114);
939  EXPECT_FLOAT_EQ(result.speedGeographicFrameDeviation.get().up_stddev_msec,
940  0.009995009);
941 
942  EXPECT_TRUE(result.currentGeographicFrame.is_initialized());
943  EXPECT_FLOAT_EQ(result.currentGeographicFrame.get().north_msec, 1.919778 * 0.0);
944  EXPECT_FLOAT_EQ(result.currentGeographicFrame.get().east_msec, 4.5171327 * 0.0);
945 
946  EXPECT_TRUE(result.currentGeographicFrameDeviation.is_initialized());
947  EXPECT_FLOAT_EQ(result.currentGeographicFrameDeviation.get().north_stddev_msec, 0.0);
948  EXPECT_FLOAT_EQ(result.currentGeographicFrameDeviation.get().east_stddev_msec, 0.0);
949 
950  EXPECT_TRUE(result.systemDate.is_initialized());
951  EXPECT_EQ(result.systemDate.get().day, 1);
952  EXPECT_EQ(result.systemDate.get().month, 1);
953  EXPECT_EQ(result.systemDate.get().year, 2006);
954 
955  EXPECT_TRUE(result.sensorStatus.is_initialized());
956  EXPECT_EQ(result.sensorStatus.get().status1, 0x0);
957  EXPECT_EQ(result.sensorStatus.get().status2, 0x0);
958 
959  EXPECT_TRUE(result.insAlgorithmStatus.is_initialized());
960  EXPECT_EQ(result.insAlgorithmStatus.get().status1, 0x00000012);
961  EXPECT_EQ(result.insAlgorithmStatus.get().status2, 0x00030000);
962  EXPECT_EQ(result.insAlgorithmStatus.get().status3, 0x04000000);
963  EXPECT_EQ(result.insAlgorithmStatus.get().status4, 0x00000200);
964 
965  EXPECT_TRUE(result.insSystemStatus.is_initialized());
966  EXPECT_EQ(result.insSystemStatus.get().status1, 0x00000200);
967  EXPECT_EQ(result.insSystemStatus.get().status2, 0x00001000);
968  EXPECT_EQ(result.insSystemStatus.get().status3, 0x0);
969 
970  EXPECT_TRUE(result.insUserStatus.is_initialized());
971  EXPECT_EQ(result.insUserStatus.get().status, 0x4c000000);
972 
973  EXPECT_FALSE(result.ahrsAlgorithmStatus.is_initialized());
974 
975  EXPECT_FALSE(result.ahrsSystemStatus.is_initialized());
976 
977  EXPECT_FALSE(result.ahrsUserStatus.is_initialized());
978 
979  EXPECT_TRUE(result.heaveSurgeSwaySpeed.is_initialized());
980  EXPECT_FLOAT_EQ(result.heaveSurgeSwaySpeed.get().realtime_heave_speed, 0.0);
981  EXPECT_FLOAT_EQ(result.heaveSurgeSwaySpeed.get().surge_speed, 0.0);
982  EXPECT_FLOAT_EQ(result.heaveSurgeSwaySpeed.get().sway_speed, 0.0);
983 
984  EXPECT_TRUE(result.speedVesselFrame.is_initialized());
985  EXPECT_FLOAT_EQ(result.speedVesselFrame.get().xv1_msec, 0.00010496828);
986  EXPECT_FLOAT_EQ(result.speedVesselFrame.get().xv2_msec, 0.011636287);
987  EXPECT_FLOAT_EQ(result.speedVesselFrame.get().xv3_msec, 0.04487192);
988 
989  EXPECT_TRUE(result.accelerationGeographicFrame.is_initialized());
990  EXPECT_FLOAT_EQ(result.accelerationGeographicFrame.get().north_msec2, 0.0);
991  EXPECT_FLOAT_EQ(result.accelerationGeographicFrame.get().east_msec2, -0.01);
992  EXPECT_FLOAT_EQ(result.accelerationGeographicFrame.get().up_msec2, 9.809999);
993 
994  EXPECT_TRUE(result.courseSpeedoverGround.is_initialized());
995  EXPECT_FLOAT_EQ(result.courseSpeedoverGround.get().course_over_ground, 268.6071);
996  EXPECT_FLOAT_EQ(result.courseSpeedoverGround.get().speed_over_ground, 0.011670535);
997 
998  EXPECT_TRUE(result.temperatures.is_initialized());
999  EXPECT_FLOAT_EQ(result.temperatures.get().mean_temp_fog, 25.993126);
1000  EXPECT_FLOAT_EQ(result.temperatures.get().mean_temp_acc, 25.993126);
1001  EXPECT_FLOAT_EQ(result.temperatures.get().board_temperature, 158.65643);
1002 
1003  EXPECT_TRUE(result.attitudeQuaternion.is_initialized());
1004  EXPECT_FLOAT_EQ(result.attitudeQuaternion.get().q0, 0.999742329120636);
1005  EXPECT_FLOAT_EQ(result.attitudeQuaternion.get().q1, 0.00038825065712444484);
1006  EXPECT_FLOAT_EQ(result.attitudeQuaternion.get().q2, -0.0015505076153203845);
1007  EXPECT_FLOAT_EQ(result.attitudeQuaternion.get().q3, -0.022644637152552605);
1008 
1009  EXPECT_TRUE(result.attitudeQuaternionDeviation.is_initialized());
1010  EXPECT_FLOAT_EQ(result.attitudeQuaternionDeviation.get().quat_stddev_xi1,
1011  0.00018635205924510956);
1012  EXPECT_FLOAT_EQ(result.attitudeQuaternionDeviation.get().quat_stddev_xi2,
1013  0.00029318343149498105);
1014  EXPECT_FLOAT_EQ(result.attitudeQuaternionDeviation.get().quat_stddev_xi3,
1015  0.9508194327354431);
1016 
1017  EXPECT_TRUE(result.rawAccelerationVesselFrame.is_initialized());
1018  EXPECT_FLOAT_EQ(result.rawAccelerationVesselFrame.get().xv1_msec2, 0.0);
1019  EXPECT_FLOAT_EQ(result.rawAccelerationVesselFrame.get().xv2_msec2, 0.0);
1020  EXPECT_FLOAT_EQ(result.rawAccelerationVesselFrame.get().xv3_msec2, 8.000145);
1021 
1022  EXPECT_TRUE(result.accelerationVesselFrameDeviation.is_initialized());
1023  EXPECT_FLOAT_EQ(result.accelerationVesselFrameDeviation.get().xv1_stddev_msec2,
1024  0.009960833);
1025  EXPECT_FLOAT_EQ(result.accelerationVesselFrameDeviation.get().xv2_stddev_msec2,
1026  0.009610326);
1027  EXPECT_FLOAT_EQ(result.accelerationVesselFrameDeviation.get().xv3_stddev_msec2,
1028  0.01413504);
1029 
1030  EXPECT_TRUE(result.rotationRateVesselFrameDeviation.is_initialized());
1031  EXPECT_FLOAT_EQ(result.rotationRateVesselFrameDeviation.get().xv1_stddev_degsec,
1032  0.0005122008);
1033  EXPECT_FLOAT_EQ(result.rotationRateVesselFrameDeviation.get().xv2_stddev_degsec,
1034  0.004000983);
1035  EXPECT_FLOAT_EQ(result.rotationRateVesselFrameDeviation.get().xv3_stddev_degsec,
1036  0.00051041954);
1037 
1038  // --- Extended navigation data block
1039  EXPECT_TRUE(result.rotationAccelerationVesselFrame.is_initialized());
1040  EXPECT_FLOAT_EQ(result.rotationAccelerationVesselFrame.get().xv1_degsec2,
1041  0.00032014688);
1042  EXPECT_FLOAT_EQ(result.rotationAccelerationVesselFrame.get().xv2_degsec2,
1043  0.001597333);
1044  EXPECT_FLOAT_EQ(result.rotationAccelerationVesselFrame.get().xv3_degsec2,
1045  -0.00042017436);
1046 
1047  EXPECT_TRUE(result.rotationAccelerationVesselFrameDeviation.is_initialized());
1048  EXPECT_FLOAT_EQ(
1049  result.rotationAccelerationVesselFrameDeviation.get().xv1_stddev_degsec2,
1050  0.000706749);
1051  EXPECT_FLOAT_EQ(
1052  result.rotationAccelerationVesselFrameDeviation.get().xv2_stddev_degsec2,
1053  0.0007067489);
1054  EXPECT_FLOAT_EQ(
1055  result.rotationAccelerationVesselFrameDeviation.get().xv3_stddev_degsec2,
1056  0.000706749);
1057 
1058  EXPECT_TRUE(result.rawRotationRateVesselFrame.is_initialized());
1059  EXPECT_FLOAT_EQ(result.rawRotationRateVesselFrame.get().xv1_degsec, 0.0);
1060  EXPECT_FLOAT_EQ(result.rawRotationRateVesselFrame.get().xv2_degsec, 0.0);
1061  EXPECT_FLOAT_EQ(result.rawRotationRateVesselFrame.get().xv3_degsec, 0.0);
1062 
1063  EXPECT_FALSE(result.vehicleAttitudeHeading.is_initialized());
1064 
1065  EXPECT_FALSE(result.vehicleAttitudeHeadingDeviation.is_initialized());
1066 
1067  EXPECT_FALSE(result.vehiclePosition.is_initialized());
1068 
1069  EXPECT_FALSE(result.vehiclePositionDeviation.is_initialized());
1070 
1071  // --- External data block ==> No external data in the log
1072  EXPECT_FALSE(result.utc.is_initialized());
1073 
1074  EXPECT_FALSE(result.gnss1.is_initialized());
1075 
1076  EXPECT_FALSE(result.gnss2.is_initialized());
1077 
1078  EXPECT_FALSE(result.emlog1.is_initialized());
1079 
1080  EXPECT_FALSE(result.emlog2.is_initialized());
1081 
1082  EXPECT_FALSE(result.usbl1.is_initialized());
1083 
1084  EXPECT_FALSE(result.usbl2.is_initialized());
1085 
1086  EXPECT_FALSE(result.usbl3.is_initialized());
1087 
1088  EXPECT_FALSE(result.depth.is_initialized());
1089 
1090  EXPECT_FALSE(result.dvlGroundSpeed1.is_initialized());
1091 
1092  EXPECT_FALSE(result.dvlWaterSpeed1.is_initialized());
1093 
1094  EXPECT_FALSE(result.soundVelocity.is_initialized());
1095 
1096  EXPECT_FALSE(result.dmi.is_initialized());
1097 
1098  EXPECT_FALSE(result.lbl1.is_initialized());
1099 
1100  EXPECT_FALSE(result.lbl2.is_initialized());
1101 
1102  EXPECT_FALSE(result.lbl3.is_initialized());
1103 
1104  EXPECT_FALSE(result.lbl4.is_initialized());
1105 
1106  EXPECT_FALSE(result.eventMarkerA.is_initialized());
1107 
1108  EXPECT_FALSE(result.eventMarkerB.is_initialized());
1109 
1110  EXPECT_FALSE(result.eventMarkerC.is_initialized());
1111 
1112  EXPECT_FALSE(result.dvlGroundSpeed2.is_initialized());
1113 
1114  EXPECT_FALSE(result.dvlWaterSpeed2.is_initialized());
1115 
1116  EXPECT_FALSE(result.turretAngles.is_initialized());
1117 
1118  EXPECT_FALSE(result.vtg1.is_initialized());
1119 
1120  EXPECT_FALSE(result.vtg2.is_initialized());
1121 
1122  EXPECT_FALSE(result.logBook.is_initialized());
1123 }
1124 
1125 TEST(StdBinDecoder, WeCanParseAnAnswerFrame)
1126 {
1127  // clang-format off
1128  const std::vector<uint8_t> memory{
1129  'A', 'N', /* IX blue header */
1130  0x03, /* Protocol Version */
1131  0x00, 0x0d, /* Telegram size */
1132  0xde, 0xad, 0xbe, 0xef, /* payload */
1133  0x00, 0x00, 0x03, 0xd7, /* checksum */
1134  };
1135  // clang-format on
1136 
1138  parser.addNewData(memory);
1139  ASSERT_TRUE(parser.parseNextFrame());
1140  const auto header = parser.getLastHeaderData();
1141  const auto nav = parser.getLastNavData();
1142  const auto answer = parser.getLastAnswerData();
1143  ASSERT_EQ(header.messageType,
1145  EXPECT_FALSE(nav.attitudeHeading.is_initialized());
1146  const std::vector<uint8_t> expectedAnswer{0xde, 0xad, 0xbe, 0xef};
1147  EXPECT_EQ(answer, expectedAnswer);
1148 }
1149 
1150 int main(int argc, char** argv)
1151 {
1152  ::testing::InitGoogleTest(&argc, argv);
1153  return RUN_ALL_TESTS();
1154 }
void addNewData(const uint8_t *data, std::size_t length)
Add new binary data to the parser internal buffer The new data can only be a part of a frame...
Data::BinaryNav getLastNavData(void) const
const std::vector< uint8_t > log_STDBIN_V5
Definition: log_STDBIN_V5.h:5
bool parseNextFrame()
Try to parse a frame from the parser internal buffer. Some binary data must have been added with the ...
const std::vector< uint8_t > & getLastAnswerData(void) const
Data::NavHeader getLastHeaderData(void) const
const std::vector< uint8_t > log_STDBIN_V4
Definition: log_STDBIN_V4.h:5
TEST(StdBinDecoder, WeCannotParseAFrameWithSomeMissingFields)
const std::vector< uint8_t > log_STDBIN_V2
Definition: log_STDBIN_V2.h:5
int main(int argc, char **argv)
const std::vector< uint8_t > log_STDBIN_V3
Definition: log_STDBIN_V3.h:5
Parser of a STDBIN IXblue message. This is the entry point of the library. Usage of this class is as ...


ixblue_stdbin_decoder
Author(s): Adrien BARRAL , Laure LEBROTON
autogenerated on Sat Jan 9 2021 03:13:21