source: ntrip/trunk/BNC/src/GPSS/gpssDecoder.cpp@ 5738

Last change on this file since 5738 was 5070, checked in by mervart, 11 years ago
File size: 3.8 KB
Line 
1
2/* -------------------------------------------------------------------------
3 * BKG NTRIP Client
4 * -------------------------------------------------------------------------
5 *
6 * Class: gpssDecoder
7 *
8 * Purpose: Decode Data in GPSS Format
9 *
10 * Author: L. Mervart
11 *
12 * Created: 20-Dec-2008
13 *
14 * Changes:
15 *
16 * -----------------------------------------------------------------------*/
17
18#include "gpssDecoder.h"
19#include "bnccore.h"
20
21using namespace std;
22
23struct t_epochHeader {
24 double t_epoch;
25 int n_svs;
26};
27
28// Cyclic Redundancy Check
29////////////////////////////////////////////////////////////////////////////
30unsigned short cal_crc(unsigned char *buf, int num) {
31 unsigned short polynomial = 0x8408;
32 unsigned short crc = 0;
33 int i;
34 while ( num-- ) {
35 crc = ( crc & 0xFF00 ) | ( *buf++^( crc & 0x00FF ) );
36 for( i=0; i<8; i++ ){
37 if( crc & 0x0001 ){
38 crc >>= 1;
39 crc ^= polynomial;
40 }
41 else{
42 crc >>= 1;
43 }
44 }
45 }
46 return (crc);
47}
48
49// Constructor
50////////////////////////////////////////////////////////////////////////////
51gpssDecoder::gpssDecoder() : GPSDecoder() {
52 connect(this, SIGNAL(newGPSEph(gpsephemeris*)),
53 BNC_CORE, SLOT(slotNewGPSEph(gpsephemeris*)));
54}
55
56// Destructor
57////////////////////////////////////////////////////////////////////////////
58gpssDecoder::~gpssDecoder() {
59}
60
61//
62////////////////////////////////////////////////////////////////////////////
63t_irc gpssDecoder::Decode(char* data, int dataLen, vector<string>& errmsg) {
64
65 errmsg.clear();
66
67 _buffer += QByteArray(data, dataLen);
68
69 bool obsFound = false;
70 bool ephFound = false;
71 int iBeg;
72 while ( (iBeg = _buffer.indexOf(0x02)) != -1) {
73 _buffer = _buffer.mid(iBeg);
74
75 int recordSize;
76 int crc;
77
78 // Observations
79 // ------------
80 if (_buffer.length() > 0 && char(_buffer[1]) == 0x00) {
81
82 int reqLength = 2 + sizeof(recordSize) + sizeof(t_epochHeader);
83
84 if (_buffer.length() >= reqLength) {
85 t_epochHeader epochHdr;
86 memcpy(&epochHdr, _buffer.data() + 2 + sizeof(recordSize),
87 sizeof(epochHdr));
88
89 reqLength += epochHdr.n_svs * sizeof(t_obs) + sizeof(crc) + 1;
90
91 if (_buffer.length() >= reqLength) {
92
93 int checkLen = 2 + sizeof(recordSize) + sizeof(t_epochHeader) +
94 epochHdr.n_svs * sizeof(t_obs);
95 memcpy(&crc, _buffer.data() + checkLen, sizeof(crc));
96 int crcCal = cal_crc((unsigned char*) _buffer.data(), checkLen);
97
98 if (crc == crcCal) {
99 for (int is = 0; is < epochHdr.n_svs; is++) {
100 obsFound = true;
101 t_obs obs;
102 memcpy(&obs, _buffer.data() + 2 + sizeof(recordSize) +
103 sizeof(epochHdr) + is * sizeof(t_obs), sizeof(t_obs));
104 _obsList.push_back(obs);
105 }
106 }
107 }
108 }
109 _buffer = _buffer.mid(reqLength);
110 }
111
112 // Ephemeris
113 // ---------
114 else if (_buffer.length() > 0 && char(_buffer[1]) == 0x01) {
115 int reqLength = 2 + sizeof(recordSize) + sizeof(gpsephemeris) +
116 sizeof(crc) + 1;
117
118 if (_buffer.length() >= reqLength) {
119
120 int checkLen = 2 + sizeof(recordSize) + sizeof(gpsephemeris);
121 memcpy(&crc, _buffer.data() + checkLen, sizeof(crc));
122 int crcCal = cal_crc((unsigned char*) _buffer.data(), checkLen);
123
124 if (crc == crcCal) {
125 ephFound = true;
126 gpsephemeris* gpsEph = new gpsephemeris;
127 memcpy(gpsEph, _buffer.data() + 2 + sizeof(recordSize),
128 sizeof(gpsephemeris));
129 emit newGPSEph(gpsEph);
130 }
131 }
132 _buffer = _buffer.mid(reqLength);
133 }
134
135 else {
136 _buffer == _buffer.mid(1);
137 }
138 }
139
140 if (obsFound || ephFound) {
141 return success;
142 }
143 else {
144 return failure;
145 }
146}
Note: See TracBrowser for help on using the repository browser.