Simple Geolocalization and Course Transmission Protocol (SGCTP)
 All Classes Namespaces Files Functions Variables Enumerations Enumerator Friends Macros Pages
payload.cpp
Go to the documentation of this file.
1 // INDENTING (emacs/vi): -*- mode:c++; tab-width:2; c-basic-offset:2; intent-tabs-mode:nil; -*- ex: set tabstop=2 expandtab:
2 
3 /*
4  * Simple Geolocalization and Course Transmission Protocol (SGCTP)
5  * Copyright (C) 2014 Cedric Dufour <http://cedric.dufour.name>
6  *
7  * The Simple Geolocalization and Course Transmission Protocol (SGCTP) is
8  * free software:
9  * you can redistribute it and/or modify it under the terms of the GNU General
10  * Public License as published by the Free Software Foundation, Version 3.
11  *
12  * The Simple Geolocalization and Course Transmission Protocol (SGCTP) is
13  * distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
14  * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
15  * PARTICULAR PURPOSE.
16  *
17  * See the GNU General Public License for more details.
18  */
19 
20 // C
21 #include <errno.h>
22 #include <stdlib.h>
23 #include <string.h>
24 
25 // SGCTP
26 #include "sgctp/data.hpp"
27 #include "sgctp/payload.hpp"
28 using namespace SGCTP;
29 
30 
31 //----------------------------------------------------------------------
32 // CONSTANTS / STATIC
33 //----------------------------------------------------------------------
34 
35 const uint8_t CPayload::BITMASK[9] = { 0, 1, 3, 7, 15, 31, 63, 127, 255 };
36 
37 
38 //----------------------------------------------------------------------
39 // METHODS
40 //----------------------------------------------------------------------
41 
42 void CPayload::putBits( uint8_t _ui8tBitsCount,
43  uint8_t _ui8tBits )
44 {
45  // WARNING: code MUST be hardware-independent, so as to have most significant bits first!
46  uint32_t __ui32tBufferByteOffset = ( ui32tBufferBitOffset >> 3 );
47  uint16_t __ui16tData =
48  ( _ui8tBits & BITMASK[_ui8tBitsCount] )
49  << ( 16 - _ui8tBitsCount - ( ui32tBufferBitOffset & 0x7 ) );
50  *(unsigned char*)(pucBufferPut+__ui32tBufferByteOffset) |=
51  (unsigned char)( __ui16tData >> 8 );
52  *(unsigned char*)(pucBufferPut+__ui32tBufferByteOffset+1) |=
53  (unsigned char)( __ui16tData & 0xFF );
54  ui32tBufferBitOffset += _ui8tBitsCount;
55 }
56 
57 void CPayload::putBytes( uint16_t _ui16tBytesCount,
58  const unsigned char *_pucBytes )
59 {
60  if( uint8_t __ui8tBufferBitAlign = ( ui32tBufferBitOffset & 0x7 ) )
61  putBits( 8 - __ui8tBufferBitAlign, 0 );
62  if( _ui16tBytesCount == 0 )
63  return;
64  uint32_t __ui32tBufferByteOffset = ( ui32tBufferBitOffset >> 3 );
65  memcpy( pucBufferPut+__ui32tBufferByteOffset, _pucBytes, _ui16tBytesCount );
66  ui32tBufferBitOffset += _ui16tBytesCount * 8;
67 }
68 
69 uint8_t CPayload::getBits( uint8_t _ui8tBitsCount )
70 {
71  // WARNING: code MUST be hardware-independent, so as to have most significant bits first!
72  uint32_t __ui32tBufferByteOffset = ( ui32tBufferBitOffset >> 3 );
73  uint16_t __ui16tBits =
74  (uint16_t)( *(unsigned char*)(pucBufferGet+__ui32tBufferByteOffset) ) << 8
75  | *(unsigned char*)(pucBufferGet+__ui32tBufferByteOffset+1);
76  __ui16tBits >>= ( 16 - _ui8tBitsCount - ( ui32tBufferBitOffset & 0x7 ) );
77  ui32tBufferBitOffset += _ui8tBitsCount;
78  return __ui16tBits & BITMASK[_ui8tBitsCount];
79 }
80 
81 void CPayload::getBytes( uint16_t _ui16tBytesCount,
82  unsigned char *_pucBytes )
83 {
84  if( uint8_t __ui8tBufferBitAlign = ( ui32tBufferBitOffset & 0x7 ) )
85  getBits( 8 - __ui8tBufferBitAlign );
86  if( _ui16tBytesCount == 0 )
87  return;
88  uint32_t __ui32tBufferByteOffset = ( ui32tBufferBitOffset >> 3 );
89  memcpy( _pucBytes, pucBufferGet+__ui32tBufferByteOffset, _ui16tBytesCount );
90  ui32tBufferBitOffset += _ui16tBytesCount * 8;
91 }
92 
93 int CPayload::serialize( unsigned char *_pucBuffer,
94  const CData &_roData )
95 {
96  // Check buffer
97  if( !_pucBuffer )
98  return -EINVAL;
99  pucBufferPut = _pucBuffer;
100 
101  // Zero buffer and reset bit counter
102  zeroBuffer( pucBufferPut );
103  ui32tBufferBitOffset = 0;
104 
105  // Payload parameters
106  uint8_t __ui8tIDLength = strlen( _roData.pcID );
107  bool __bTime =
108  !( _roData.ui32tTime & CData::UNDEFINED_UINT32 );
109  bool __b2DPosition =
110  !( _roData.ui32tLatitude & CData::UNDEFINED_UINT32
111  || _roData.ui32tLongitude & CData::UNDEFINED_UINT32 );
112  bool __b3DPosition =
113  __b2DPosition
114  && !( _roData.ui32tElevation & CData::UNDEFINED_UINT32 );
115  bool __b2DGndCourse =
116  !( _roData.ui32tBearing & CData::UNDEFINED_UINT32
117  || _roData.ui32tGndSpeed & CData::UNDEFINED_UINT32 );
118  bool __b3DGndCourse =
119  __b2DGndCourse
120  && !( _roData.ui32tVrtSpeed & CData::UNDEFINED_UINT32 );
121  bool __b2DGndCourseDt =
122  !( _roData.ui32tBearingDt & CData::UNDEFINED_UINT32
123  || _roData.ui32tGndSpeedDt & CData::UNDEFINED_UINT32 );
124  bool __b3DGndCourseDt =
125  __b2DGndCourseDt
126  && !( _roData.ui32tVrtSpeedDt & CData::UNDEFINED_UINT32 );
127  bool __b2DAppCourse =
128  !( _roData.ui32tHeading & CData::UNDEFINED_UINT32
129  || _roData.ui32tAppSpeed & CData::UNDEFINED_UINT32 );
130  // ... extended content
131  bool __bExtendedContent = false;
132  bool __b2SourceType =
133  !( _roData.ui32tSourceType & CData::UNDEFINED_UINT32 );
134  __bExtendedContent |= __b2SourceType;
135  bool __b2DPositionError =
136  !( _roData.ui32tLatitudeError & CData::UNDEFINED_UINT32
137  || _roData.ui32tLongitudeError & CData::UNDEFINED_UINT32 );
138  __bExtendedContent |= __b2DPositionError;
139  bool __b3DPositionError =
140  __b2DPositionError
141  && !( _roData.ui32tElevationError & CData::UNDEFINED_UINT32 );
142  __bExtendedContent |= __b3DPositionError;
143  bool __b2DGndCourseError =
144  !( _roData.ui32tBearingError & CData::UNDEFINED_UINT32
145  || _roData.ui32tGndSpeedError & CData::UNDEFINED_UINT32 );
146  __bExtendedContent |= __b2DGndCourseError;
147  bool __b3DGndCourseError =
148  __b2DGndCourseError
149  && !( _roData.ui32tVrtSpeedError & CData::UNDEFINED_UINT32 );
150  __bExtendedContent |= __b3DGndCourseError;
151  bool __b2DGndCourseDtError =
152  !( _roData.ui32tBearingDtError & CData::UNDEFINED_UINT32
153  || _roData.ui32tGndSpeedDtError & CData::UNDEFINED_UINT32 );
154  __bExtendedContent |= __b2DGndCourseDtError;
155  bool __b3DGndCourseDtError =
156  __b2DGndCourseDtError
157  && !( _roData.ui32tVrtSpeedDtError & CData::UNDEFINED_UINT32 );
158  __bExtendedContent |= __b3DGndCourseDtError;
159  bool __b2DAppCourseError =
160  !( _roData.ui32tHeadingError & CData::UNDEFINED_UINT32
161  || _roData.ui32tAppSpeedError & CData::UNDEFINED_UINT32 );
162  __bExtendedContent |= __b2DAppCourseError;
163 
164  // Content
165  putBits( 1, __b2DPosition );
166  putBits( 1, __b3DPosition );
167  putBits( 1, __b2DGndCourse );
168  putBits( 1, __b3DGndCourse );
169  putBits( 1, __b2DGndCourseDt );
170  putBits( 1, __b3DGndCourseDt );
171  putBits( 1, __b2DAppCourse );
172  putBits( 1, __bExtendedContent );
173 
174  // ID' + ID
175  putBits( 7, __ui8tIDLength );
176  putBits( 1, _roData.ui16tDataSize > 0 );
177  if( __ui8tIDLength > 0 )
178  putBytes( __ui8tIDLength, (unsigned char*)_roData.pcID );
179 
180  // Data' + Data" + Data
181  if( _roData.ui16tDataSize > 0 )
182  {
183  if( _roData.ui16tDataSize > 127 )
184  {
185  putBits( 7, (uint8_t)( _roData.ui16tDataSize ) );
186  putBits( 1, 1 );
187  putBits( 8, (uint8_t)( _roData.ui16tDataSize >> 7 ) );
188  }
189  else
190  {
191  putBits( 7, (uint8_t)( _roData.ui16tDataSize ) );
192  putBits( 1, 0 );
193  }
194  }
195  if( _roData.ui16tDataSize > 0 )
196  putBytes( _roData.ui16tDataSize, _roData.pucData );
197 
198  // Content
199 
200  // ... Time
201  if( __bTime )
202  {
203  putBits( 7, (uint8_t)( _roData.ui32tTime >> 16 ) );
204  putBits( 8, (uint8_t)( _roData.ui32tTime >> 8 ) );
205  putBits( 8, (uint8_t)( _roData.ui32tTime ) );
206  }
207  else
208  {
209  putBits( 7, (uint8_t)( CData::OVERFLOW_UINT32 >> 16 ) );
210  putBits( 8, (uint8_t)( CData::OVERFLOW_UINT32 >> 8 ) );
211  putBits( 8, (uint8_t)( CData::OVERFLOW_UINT32 ) );
212  }
213 
214  // ... Position
215  if( __b2DPosition )
216  {
217  // ... Latitude
218  putBits( 6, (uint8_t)( _roData.ui32tLatitude >> 24 ) );
219  putBits( 8, (uint8_t)( _roData.ui32tLatitude >> 16 ) );
220  putBits( 8, (uint8_t)( _roData.ui32tLatitude >> 8 ) );
221  putBits( 8, (uint8_t)( _roData.ui32tLatitude ) );
222  // ... Longitude
223  putBits( 7, (uint8_t)( _roData.ui32tLongitude >> 24 ) );
224  putBits( 8, (uint8_t)( _roData.ui32tLongitude >> 16 ) );
225  putBits( 8, (uint8_t)( _roData.ui32tLongitude >> 8 ) );
226  putBits( 8, (uint8_t)( _roData.ui32tLongitude ) );
227  }
228  if( __b3DPosition )
229  {
230  // ... Elevation
231  putBits( 3, (uint8_t)( _roData.ui32tElevation >> 16 ) );
232  putBits( 8, (uint8_t)( _roData.ui32tElevation >> 8 ) );
233  putBits( 8, (uint8_t)( _roData.ui32tElevation ) );
234  }
235 
236  // ... Ground course
237  if( __b2DGndCourse )
238  {
239  // ... Bearing
240  putBits( 4, (uint8_t)( _roData.ui32tBearing >> 8) );
241  putBits( 8, (uint8_t)( _roData.ui32tBearing ) );
242  // ... Ground speed
243  putBits( 8, (uint8_t)( _roData.ui32tGndSpeed >> 8 ) );
244  putBits( 8, (uint8_t)( _roData.ui32tGndSpeed ) );
245  }
246  if( __b3DGndCourse )
247  {
248  // ... Vertical speed
249  putBits( 5, (uint8_t)( _roData.ui32tVrtSpeed >> 8) );
250  putBits( 8, (uint8_t)( _roData.ui32tVrtSpeed ) );
251  }
252 
253  // ... Ground course variation over time
254  if( __b2DGndCourseDt )
255  {
256  // ... Bearing
257  putBits( 2, (uint8_t)( _roData.ui32tBearingDt >> 8) );
258  putBits( 8, (uint8_t)( _roData.ui32tBearingDt ) );
259  // ... Ground speed
260  putBits( 4, (uint8_t)( _roData.ui32tGndSpeedDt >> 8 ) );
261  putBits( 8, (uint8_t)( _roData.ui32tGndSpeedDt ) );
262  }
263  if( __b3DGndCourseDt )
264  {
265  // ... Vertical speed
266  putBits( 4, (uint8_t)( _roData.ui32tVrtSpeedDt >> 8) );
267  putBits( 8, (uint8_t)( _roData.ui32tVrtSpeedDt ) );
268  }
269 
270  // ... Apparent course
271  if( __b2DAppCourse )
272  {
273  // ... Heading
274  putBits( 4, (uint8_t)( _roData.ui32tHeading >> 8) );
275  putBits( 8, (uint8_t)( _roData.ui32tHeading ) );
276  // ... Apparent speed
277  putBits( 8, (uint8_t)( _roData.ui32tAppSpeed >> 8 ) );
278  putBits( 8, (uint8_t)( _roData.ui32tAppSpeed ) );
279  }
280 
281  // Extended content
282  if( __bExtendedContent )
283  {
284  putBytes( 0, NULL ); // re-align
285  putBits( 1, __b2DPositionError );
286  putBits( 1, __b3DPositionError );
287  putBits( 1, __b2DGndCourseError );
288  putBits( 1, __b3DGndCourseError );
289  putBits( 1, __b2DGndCourseDtError );
290  putBits( 1, __b3DGndCourseDtError );
291  putBits( 1, __b2DAppCourseError );
292  putBits( 1, 0 );
293  }
294  // ... Source type
295  putBits( 8, (uint8_t)( _roData.ui32tSourceType ) );
296 
297  // ... Position error
298  if( __b2DPositionError )
299  {
300  // ... Latitude
301  putBits( 4, (uint8_t)( _roData.ui32tLatitudeError >> 8 ) );
302  putBits( 8, (uint8_t)( _roData.ui32tLatitudeError ) );
303  // ... Longitude
304  putBits( 4, (uint8_t)( _roData.ui32tLongitudeError >> 8 ) );
305  putBits( 8, (uint8_t)( _roData.ui32tLongitudeError ) );
306  }
307  if( __b3DPositionError )
308  {
309  // ... Elevation
310  putBits( 4, (uint8_t)( _roData.ui32tElevationError >> 8 ) );
311  putBits( 8, (uint8_t)( _roData.ui32tElevationError ) );
312  }
313 
314  // ... Ground course error
315  if( __b2DGndCourseError )
316  {
317  // ... Bearing
318  putBits( 8, (uint8_t)( _roData.ui32tBearingError ) );
319  // ... Ground speed
320  putBits( 8, (uint8_t)( _roData.ui32tGndSpeedError ) );
321  }
322  if( __b3DGndCourseError )
323  {
324  // ... Vertical speed
325  putBits( 8, (uint8_t)( _roData.ui32tVrtSpeedError ) );
326  }
327 
328  // ... Ground course variation over time error
329  if( __b2DGndCourseDtError )
330  {
331  // ... Bearing
332  putBits( 8, (uint8_t)( _roData.ui32tBearingDtError ) );
333  // ... Ground speed
334  putBits( 8, (uint8_t)( _roData.ui32tGndSpeedDtError ) );
335  }
336  if( __b3DGndCourseDtError )
337  {
338  // ... Vertical speed
339  putBits( 8, (uint8_t)( _roData.ui32tVrtSpeedDtError ) );
340  }
341 
342  // ... Apparent course error
343  if( __b2DAppCourseError )
344  {
345  // ... Heading
346  putBits( 8, (uint8_t)( _roData.ui32tHeadingError ) );
347  // ... Apparent speed
348  putBits( 8, (uint8_t)( _roData.ui32tAppSpeedError ) );
349  }
350 
351  // Done
352  return ( ui32tBufferBitOffset + 7 ) >> 3;
353 }
354 
355 int CPayload::unserialize( CData *_poData,
356  const unsigned char *_pucBuffer,
357  uint16_t _ui16tPayloadSize )
358 {
359  // Check buffer
360  if( !_pucBuffer )
361  return -EINVAL;
362  pucBufferGet = _pucBuffer;
363 
364  // Reset bit counter
365  ui32tBufferBitOffset = 0;
366 
367  // Zero data container
368  _poData->reset( false );
369 
370  // Content
371  // ... Geolocalization data
372  bool __b2DPosition = (bool)getBits( 1 );
373  bool __b3DPosition = (bool)getBits( 1 );
374  bool __b2DGndCourse = (bool)getBits( 1 );
375  bool __b3DGndCourse = (bool)getBits( 1 );
376  bool __b2DGndCourseDt = (bool)getBits( 1 );
377  bool __b3DGndCourseDt = (bool)getBits( 1 );
378  bool __b2DAppCourse = (bool)getBits( 1 );
379  bool __bExtendedContent = (bool)getBits( 1 );
380 
381  // ID' + ID
382  uint8_t __ui8tIDLength = getBits( 7 );
383  bool __bData = (bool)getBits( 1 );
384  if( __ui8tIDLength > 0 )
385  getBytes( __ui8tIDLength, (unsigned char*)_poData->pcID );
386  _poData->pcID[__ui8tIDLength] = '\0';
387 
388  // Data' + Data" + Data
389  if( __bData )
390  {
391  uint16_t __ui16tDataSize = getBits( 7 );
392  if( (bool)getBits( 1 ) )
393  __ui16tDataSize |= getBits( 8 ) << 7;
394  _poData->allocData( __ui16tDataSize );
395  }
396  else
397  _poData->freeData();
398  if( _poData->ui16tDataSize > 0 )
399  getBytes( _poData->ui16tDataSize, _poData->pucData );
400 
401  // Content
402  uint32_t __ui32tData;
403 
404  // ... Time
405  __ui32tData = getBits( 7 );
406  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
407  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
408  _poData->ui32tTime =
409  ( __ui32tData == ( 0xFFFFFFFF >> 9 ) )
410  ? CData::UNDEFINED_UINT32
411  : __ui32tData;
412 
413  // ... Position
414  if( __b2DPosition )
415  {
416  // ... Latitude
417  __ui32tData = getBits( 6 );
418  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
419  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
420  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
421  _poData->ui32tLatitude =
422  ( __ui32tData == ( 0xFFFFFFFF >> 2 ) )
423  ? CData::OVERFLOW_UINT32
424  : __ui32tData;
425  // ... Longitude
426  __ui32tData = getBits( 7 );
427  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
428  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
429  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
430  _poData->ui32tLongitude =
431  ( __ui32tData == ( 0xFFFFFFFF >> 1 ) )
432  ? CData::OVERFLOW_UINT32
433  : __ui32tData;
434  }
435  if( __b3DPosition )
436  {
437  // ... Elevation
438  __ui32tData = getBits( 3 );
439  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
440  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
441  _poData->ui32tElevation =
442  ( __ui32tData == ( 0xFFFFFFFF >> 13 ) )
443  ? CData::OVERFLOW_UINT32
444  : __ui32tData;
445  }
446 
447  // ... Ground course
448  if( __b2DGndCourse )
449  {
450  // ... Bearing
451  __ui32tData = getBits( 4 );
452  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
453  _poData->ui32tBearing =
454  ( __ui32tData == ( 0xFFFFFFFF >> 20 ) )
455  ? CData::OVERFLOW_UINT32
456  : __ui32tData;
457  // ... Ground speed
458  __ui32tData = getBits( 8 );
459  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
460  _poData->ui32tGndSpeed =
461  ( __ui32tData == ( 0xFFFFFFFF >> 16 ) )
462  ? CData::OVERFLOW_UINT32
463  : __ui32tData;
464  }
465  if( __b3DGndCourse )
466  {
467  // ... Vertical speed
468  __ui32tData = getBits( 5 );
469  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
470  _poData->ui32tVrtSpeed =
471  ( __ui32tData == ( 0xFFFFFFFF >> 19 ) )
472  ? CData::OVERFLOW_UINT32
473  : __ui32tData;
474  }
475 
476  // ... Ground course variation over time
477  if( __b2DGndCourseDt )
478  {
479  // ... Bearing
480  __ui32tData = getBits( 2 );
481  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
482  _poData->ui32tBearingDt =
483  ( __ui32tData == ( 0xFFFFFFFF >> 22 ) )
484  ? CData::OVERFLOW_UINT32
485  : __ui32tData;
486  // ... Ground speed
487  __ui32tData = getBits( 4 );
488  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
489  _poData->ui32tGndSpeedDt =
490  ( __ui32tData == ( 0xFFFFFFFF >> 20 ) )
491  ? CData::OVERFLOW_UINT32
492  : __ui32tData;
493  }
494  if( __b3DGndCourseDt )
495  {
496  // ... Vertical speed
497  __ui32tData = getBits( 4 );
498  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
499  _poData->ui32tVrtSpeedDt =
500  ( __ui32tData == ( 0xFFFFFFFF >> 20 ) )
501  ? CData::OVERFLOW_UINT32
502  : __ui32tData;
503  }
504 
505  // ... Apparent course
506  if( __b2DAppCourse )
507  {
508  // ... Heading
509  __ui32tData = getBits( 4 );
510  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
511  _poData->ui32tHeading =
512  ( __ui32tData == ( 0xFFFFFFFF >> 20 ) )
513  ? CData::OVERFLOW_UINT32
514  : __ui32tData;
515  // ... Apparent speed
516  __ui32tData = getBits( 8 );
517  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
518  _poData->ui32tAppSpeed =
519  ( __ui32tData == ( 0xFFFFFFFF >> 16 ) )
520  ? CData::OVERFLOW_UINT32
521  : __ui32tData;
522  }
523 
524 
525  // Extended content
526  bool __b2DPositionError = false;
527  bool __b3DPositionError = false;
528  bool __b2DGndCourseError = false;
529  bool __b3DGndCourseError = false;
530  bool __b2DGndCourseDtError = false;
531  bool __b3DGndCourseDtError = false;
532  bool __b2DAppCourseError = false;
533  if( __bExtendedContent )
534  {
535  getBytes( 0, NULL ); // re-align
536  __b2DPositionError = (bool)getBits( 1 );
537  __b3DPositionError = (bool)getBits( 1 );
538  __b2DGndCourseError = (bool)getBits( 1 );
539  __b3DGndCourseError = (bool)getBits( 1 );
540  __b2DGndCourseDtError = (bool)getBits( 1 );
541  __b3DGndCourseDtError = (bool)getBits( 1 );
542  __b2DAppCourseError = (bool)getBits( 1 );
543  getBits( 1 );
544  }
545  // ... Source type
546  __ui32tData = getBits( 8 );
547  _poData->ui32tSourceType =
548  ( __ui32tData != CData::SOURCE_UNDEFINED )
549  ? __ui32tData
550  : CData::UNDEFINED_UINT32;
551 
552  // ... Position error
553  if( __b2DPositionError )
554  {
555  // ... Latitude
556  __ui32tData = getBits( 4 );
557  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
558  _poData->ui32tLatitudeError =
559  ( __ui32tData == ( 0xFFFFFFFF >> 20 ) )
560  ? CData::OVERFLOW_UINT32
561  : __ui32tData;
562  // ... Longitude
563  __ui32tData = getBits( 4 );
564  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
565  _poData->ui32tLongitudeError =
566  ( __ui32tData == ( 0xFFFFFFFF >> 20 ) )
567  ? CData::OVERFLOW_UINT32
568  : __ui32tData;
569  }
570  if( __b3DPositionError )
571  {
572  // ... Elevation
573  __ui32tData = getBits( 4 );
574  __ui32tData <<= 8; __ui32tData |= getBits( 8 );
575  _poData->ui32tElevationError =
576  ( __ui32tData == ( 0xFFFFFFFF >> 20 ) )
577  ? CData::OVERFLOW_UINT32
578  : __ui32tData;
579  }
580 
581  // ... Ground course error
582  if( __b2DGndCourseError )
583  {
584  // ... Bearing
585  __ui32tData = getBits( 8 );
586  _poData->ui32tBearingError =
587  ( __ui32tData == ( 0xFFFFFFFF >> 24 ) )
588  ? CData::OVERFLOW_UINT32
589  : __ui32tData;
590  // ... Ground speed
591  __ui32tData = getBits( 8 );
592  _poData->ui32tGndSpeedError =
593  ( __ui32tData == ( 0xFFFFFFFF >> 24 ) )
594  ? CData::OVERFLOW_UINT32
595  : __ui32tData;
596  }
597  if( __b3DGndCourseError )
598  {
599  // ... Vertical speed
600  __ui32tData = getBits( 8 );
601  _poData->ui32tVrtSpeedError =
602  ( __ui32tData == ( 0xFFFFFFFF >> 24 ) )
603  ? CData::OVERFLOW_UINT32
604  : __ui32tData;
605  }
606 
607  // ... Ground course variation over time error
608  if( __b2DGndCourseDtError )
609  {
610  // ... Bearing
611  __ui32tData = getBits( 8 );
612  _poData->ui32tBearingDtError =
613  ( __ui32tData == ( 0xFFFFFFFF >> 24 ) )
614  ? CData::OVERFLOW_UINT32
615  : __ui32tData;
616  // ... Ground speed
617  __ui32tData = getBits( 8 );
618  _poData->ui32tGndSpeedDtError =
619  ( __ui32tData == ( 0xFFFFFFFF >> 24 ) )
620  ? CData::OVERFLOW_UINT32
621  : __ui32tData;
622  }
623  if( __b3DGndCourseDtError )
624  {
625  // ... Vertical speed
626  __ui32tData = getBits( 8 );
627  _poData->ui32tVrtSpeedDtError =
628  ( __ui32tData == ( 0xFFFFFFFF >> 24 ) )
629  ? CData::OVERFLOW_UINT32
630  : __ui32tData;
631  }
632 
633  // ... Apparent course error
634  if( __b2DAppCourseError )
635  {
636  // ... Heading
637  __ui32tData = getBits( 8 );
638  _poData->ui32tHeadingError =
639  ( __ui32tData == ( 0xFFFFFFFF >> 24 ) )
640  ? CData::OVERFLOW_UINT32
641  : __ui32tData;
642  // ... Apparent speed
643  __ui32tData = getBits( 8 );
644  _poData->ui32tAppSpeedError =
645  ( __ui32tData == ( 0xFFFFFFFF >> 24 ) )
646  ? CData::OVERFLOW_UINT32
647  : __ui32tData;
648  }
649 
650  // Check size
651  if( ( ui32tBufferBitOffset + 7 ) >> 3 != _ui16tPayloadSize )
652  return -EBADMSG;
653 
654  // Done
655  return ( ui32tBufferBitOffset + 7 ) >> 3;
656 }
SGCTP::CPayload::unserialize
virtual int unserialize(CData *_poData, const unsigned char *_pucBuffer, uint16_t _ui16tBufferSize)
Unserialize the SGCTP data from the given payload buffer.
Definition: payload.cpp:355
SGCTP::CData::ui32tHeadingError
uint32_t ui32tHeadingError
Heading error.
Definition: data.hpp:175
SGCTP::CData::ui32tElevationError
uint32_t ui32tElevationError
Elevation error.
Definition: data.hpp:161
SGCTP::CData::ui32tSourceType
uint32_t ui32tSourceType
Source type.
Definition: data.hpp:155
SGCTP::CData::pcID
char pcID[MAX_ID_SIZE]
ID string (max. 127 characters)
Definition: data.hpp:125
SGCTP::CData::ui32tBearingError
uint32_t ui32tBearingError
Bearing error.
Definition: data.hpp:163
SGCTP::CData::ui32tVrtSpeed
uint32_t ui32tVrtSpeed
Vertical speed.
Definition: data.hpp:143
SGCTP::CData::ui32tGndSpeed
uint32_t ui32tGndSpeed
Ground speed.
Definition: data.hpp:141
SGCTP::CPayload::getBits
uint8_t getBits(uint8_t _ui8tBitsSize)
Retrieve the given bits from the payload.
Definition: payload.cpp:69
SGCTP::CData::ui16tDataSize
uint16_t ui16tDataSize
Data size.
Definition: data.hpp:129
SGCTP::CData::reset
void reset(bool _bDataFree=true)
Reset (undefine) all data.
Definition: data.cpp:175
SGCTP::CPayload::ui32tBufferBitOffset
uint32_t ui32tBufferBitOffset
SGCTP payload bit position.
Definition: payload.hpp:94
SGCTP::CData::ui32tElevation
uint32_t ui32tElevation
Elevation.
Definition: data.hpp:137
SGCTP::CData::ui32tLatitudeError
uint32_t ui32tLatitudeError
Latitude error.
Definition: data.hpp:157
SGCTP::CData
SGCTP data container.
Definition: data.hpp:44
SGCTP::CData::OVERFLOW_UINT32
static const uint32_t OVERFLOW_UINT32
Internal (integer) positive overflow value.
Definition: data.hpp:66
SGCTP::CPayload::serialize
virtual int serialize(unsigned char *_pucBuffer, const CData &_roData)
Serialize the given SGCTP data into the given payload buffer.
Definition: payload.cpp:93
SGCTP::CPayload::putBytes
void putBytes(uint16_t _ui16tBytesSize, const unsigned char *_pucBytes)
Add the given bytes to the payload.
Definition: payload.cpp:57
SGCTP::CPayload::pucBufferPut
unsigned char * pucBufferPut
SGCTP payload export buffer pointer.
Definition: payload.hpp:82
SGCTP::CData::ui32tGndSpeedError
uint32_t ui32tGndSpeedError
Ground speed error.
Definition: data.hpp:165
SGCTP::CData::pucData
unsigned char * pucData
Data (max. 32767 symbols)
Definition: data.hpp:127
SGCTP::CData::ui32tBearingDtError
uint32_t ui32tBearingDtError
Bearing variation over time (rate of turn) error.
Definition: data.hpp:169
SGCTP::CData::ui32tLatitude
uint32_t ui32tLatitude
Latitude.
Definition: data.hpp:133
SGCTP::CData::ui32tHeading
uint32_t ui32tHeading
Heading.
Definition: data.hpp:151
SGCTP::CData::ui32tLongitudeError
uint32_t ui32tLongitudeError
Longitude error.
Definition: data.hpp:159
SGCTP::CPayload::putBits
void putBits(uint8_t _ui8tBitsSize, uint8_t _ui8tBits)
Add the given bits to the payload.
Definition: payload.cpp:42
SGCTP::CData::ui32tVrtSpeedDt
uint32_t ui32tVrtSpeedDt
Vertical speed variation over time (acceleration)
Definition: data.hpp:149
SGCTP::CData::UNDEFINED_UINT32
static const uint32_t UNDEFINED_UINT32
Internal (integer) undefined value.
Definition: data.hpp:64
SGCTP::CData::ui32tAppSpeedError
uint32_t ui32tAppSpeedError
Apparent speed error.
Definition: data.hpp:177
SGCTP::CData::ui32tTime
uint32_t ui32tTime
Time.
Definition: data.hpp:131
SGCTP::CData::freeData
void freeData()
Clear (free) the data container.
Definition: data.cpp:706
SGCTP::CPayload::zeroBuffer
static void zeroBuffer(unsigned char *_pucBuffer)
Zero-out a buffer required for payload (un-)serialization.
Definition: payload.hpp:70
SGCTP::CPayload::pucBufferGet
const unsigned char * pucBufferGet
SGCTP payload import buffer pointer.
Definition: payload.hpp:92
SGCTP::CData::allocData
uint16_t allocData(uint16_t _ui16tDataSize)
Allocate the memory for the data container (max. 32767 symbols)
Definition: data.cpp:687
SGCTP::CData::ui32tLongitude
uint32_t ui32tLongitude
Longitude.
Definition: data.hpp:135
SGCTP::CPayload::BITMASK
static const uint8_t BITMASK[9]
Bit masks used to (un-)serialize data bit-per-bit.
Definition: payload.hpp:54
SGCTP::CData::ui32tBearingDt
uint32_t ui32tBearingDt
Bearing variation over time (rate of turn)
Definition: data.hpp:145
SGCTP::CData::ui32tBearing
uint32_t ui32tBearing
Bearing.
Definition: data.hpp:139
SGCTP::CData::ui32tGndSpeedDt
uint32_t ui32tGndSpeedDt
Ground speed variation over time (acceleration)
Definition: data.hpp:147
SGCTP::CData::ui32tVrtSpeedError
uint32_t ui32tVrtSpeedError
Vertical speed error.
Definition: data.hpp:167
SGCTP::CData::ui32tGndSpeedDtError
uint32_t ui32tGndSpeedDtError
Ground speed variation over time (acceleration) error.
Definition: data.hpp:171
SGCTP::CPayload::getBytes
void getBytes(uint16_t _ui16tBytesSize, unsigned char *_pucBytes)
Retrieve the given bytes from the payload.
Definition: payload.cpp:81
SGCTP::CData::ui32tVrtSpeedDtError
uint32_t ui32tVrtSpeedDtError
Vertical speed variation over time (acceleration) error.
Definition: data.hpp:173
SGCTP::CData::ui32tAppSpeed
uint32_t ui32tAppSpeed
Apparent speed.
Definition: data.hpp:153