source: pacpussensors/trunk/Gps/ui/polarxmainwindow_utils.cpp

Last change on this file was 59, checked in by DHERBOMEZ Gérald, 10 years ago

Integration of new modules:

  • GPS NMEA0183 decoder
  • Span CPT Decoder

Update of:

File size: 5.9 KB
Line 
1#include "polarxmainwindow.hpp"
2
3using namespace std ;
4
5const long PolarxMainWindow::bufferToLong(int startByte, vector<unsigned char> *pbuffer){
6
7 unsigned char tab4[4];
8 long *plong;
9
10 for (int i=0;i<4;i++)
11 tab4[i]=pbuffer->at(startByte+i);
12
13 plong = reinterpret_cast<long *>(tab4);
14 return *plong;
15}
16
17const unsigned long PolarxMainWindow::bufferToULong(int startByte, vector<unsigned char> *pbuffer){
18
19 unsigned char tab4[4];
20 unsigned long *pULong;
21
22 for (int i=0;i<4;i++)
23 tab4[i]=pbuffer->at(startByte+i);
24
25 pULong = reinterpret_cast<unsigned long *>(tab4);
26 return *pULong;
27}
28
29
30const float PolarxMainWindow::bufferToFloat(int startByte, vector<unsigned char> *pbuffer){
31
32 unsigned char tab4[4];
33 float *pfloat;
34
35 for (int i=0;i<4;i++)
36 tab4[i]=pbuffer->at(startByte+i);
37
38 pfloat = reinterpret_cast<float *>(tab4);
39 return *pfloat;
40}
41
42
43const double PolarxMainWindow::bufferToDouble(int startByte, vector<unsigned char> *pbuffer){
44
45 unsigned char tab8[8];
46 double *pdouble;
47
48 for (int i=0;i<8;i++)
49 tab8[i]=pbuffer->at(startByte+i);
50
51 pdouble = reinterpret_cast<double *>(tab8);
52 return *pdouble;
53}
54
55
56
57const unsigned short PolarxMainWindow::bufferToUShort(int startByte, vector<unsigned char> *pbuffer){
58
59 unsigned char tab2[2];
60 unsigned short *pushort;
61
62 for (int i=0;i<2;i++)
63 tab2[i]=pbuffer->at(startByte+i);
64
65 pushort = reinterpret_cast<unsigned short *>(tab2);
66 return *pushort;
67}
68
69
70const short PolarxMainWindow::bufferToShort(int startByte, vector<unsigned char> *pbuffer){
71
72 unsigned char tab2[2];
73 short *pshort;
74
75 for (int i=0;i<2;i++)
76 tab2[i]=pbuffer->at(startByte+i);
77
78 pshort = reinterpret_cast<short *>(tab2);
79 return *pshort;
80}
81
82char PolarxMainWindow::extractChar(vector<unsigned char> *pbuffer, unsigned short startBit, unsigned short length)
83{
84 char output;
85 unsigned short indexDB, firstOffsetDB, lastOffsetDB;
86 char charMask2[9] = {0, 1, 3, 7, 15, 31, 63, 127, 255 };
87
88 if (length>8)
89 length = 8;
90
91 //ATTENTION index start at 0
92 indexDB = (startBit-1)/8;
93 firstOffsetDB = (startBit-1)%8;
94 lastOffsetDB = (startBit+length-2)%8;
95
96 if (bytesSpread( startBit, length)<=1)
97 {
98 output = pbuffer->at(indexDB);
99 output = ((output>>(7-lastOffsetDB)) & charMask2[length]);
100 }
101 else
102 {
103 char c1, c2;
104 c1 = pbuffer->at(indexDB+1)>>(7-lastOffsetDB);
105 c2 = (pbuffer->at(indexDB) & charMask2[8-firstOffsetDB]);
106 output = c1 + (c2<<(lastOffsetDB+1));
107 }
108
109 return output;
110}
111
112short PolarxMainWindow::extractShort(vector<unsigned char> *pbuffer, unsigned short startBit, unsigned short length)
113{
114 short Unsgnd;
115 short comp = (1<<(length-1));
116
117 Unsgnd = extractUShort(pbuffer, startBit, length);
118
119 if (Unsgnd>comp)
120 return (2*comp - Unsgnd)*-1;
121 else
122 return Unsgnd;
123}
124
125unsigned short PolarxMainWindow::extractUShort(vector<unsigned char> *pbuffer, unsigned short startBit, unsigned short length)
126{
127 unsigned short output;
128 unsigned short indexDB, firstOffsetDB, lastOffsetDB;
129 char charMask2[9] = {0, 1, 3, 7, 15, 31, 63, 127, 255 };
130
131
132 if (length>16)
133 length = 16;
134
135 if (length<=8)
136 return (short)extractChar(pbuffer, startBit, length);
137
138 indexDB = (startBit-1)/8;
139 firstOffsetDB = (startBit-1)%8;
140 lastOffsetDB = (startBit+length-2)%8;
141
142 if (bytesSpread( startBit, length)<=2)
143 {
144 unsigned short s1, s2;
145 s1 = pbuffer->at(indexDB+1)>>(7-lastOffsetDB);
146 s2 = (pbuffer->at(indexDB) & charMask2[8-firstOffsetDB]);
147 output = s1 + (s2<<(lastOffsetDB+1));
148 }
149 else
150 {
151 unsigned short s1, s2, s3;
152 s1 = pbuffer->at(indexDB+2)>>(7-lastOffsetDB);
153 s2 = pbuffer->at(indexDB+1);
154 s3 = (pbuffer->at(indexDB) & charMask2[8-firstOffsetDB]);
155 output = s1 + (s2<<(lastOffsetDB+1)) + (s3<<(lastOffsetDB+9));
156 }
157
158 return output;
159}
160
161
162long PolarxMainWindow::extractLong(vector<unsigned char> *pbuffer, unsigned short startBit, unsigned short length)
163{
164 long Unsgnd;
165 long comp = (1<<(length-1));
166
167 Unsgnd = extractULong(pbuffer, startBit, length);
168
169 if (Unsgnd>comp)
170 return (2*comp - Unsgnd)*-1;
171 else
172 return Unsgnd;
173}
174
175
176unsigned long PolarxMainWindow::extractULong(vector<unsigned char> *pbuffer, unsigned short startBit, unsigned short length)
177{
178 unsigned long output;
179 unsigned short indexDB, firstOffsetDB, lastOffsetDB;
180 char charMask2[9] = {0, 1, 3, 7, 15, 31, 63, 127, 255 };
181
182
183 if (length>32)
184 length = 32;
185
186 if (length<=16)
187 return (long)extractShort(pbuffer, startBit, length);
188
189 indexDB = (startBit-1)/8;
190 firstOffsetDB = (startBit-1)%8;
191 lastOffsetDB = (startBit+length-2)%8;
192
193 unsigned long l1, l2, l3, l4, l5;
194 l1 = 0; l2 = 0; l3 = 0; l4 = 0; l5 = 0;
195
196 switch(bytesSpread( startBit, length))
197 {
198 case 3:
199 {
200 l1 = pbuffer->at(indexDB+2)>>(7-lastOffsetDB);
201 l2 = pbuffer->at(indexDB+1);
202 l3 = (pbuffer->at(indexDB) & charMask2[8-firstOffsetDB]);
203 output = l1 + (l2<<(lastOffsetDB+1)) + (l3<<(lastOffsetDB+9));
204 break;
205 }
206 case 4:
207 {
208 l1 = pbuffer->at(indexDB+3)>>(7-lastOffsetDB);
209 l2 = pbuffer->at(indexDB+2);
210 l3 = pbuffer->at(indexDB+1);
211 l4 = (pbuffer->at(indexDB) & charMask2[8-firstOffsetDB]);
212 output = l1 + (l2<<(lastOffsetDB+1)) + (l3<<(lastOffsetDB+9)) + (l3<<(lastOffsetDB+17));
213 break;
214 }
215 case 5:
216 {
217 l1 = pbuffer->at(indexDB+4)>>(7-lastOffsetDB);
218 l2 = pbuffer->at(indexDB+3);
219 l3 = pbuffer->at(indexDB+2);
220 l3 = pbuffer->at(indexDB+1);
221 l4 = (pbuffer->at(indexDB) & charMask2[8-firstOffsetDB]);
222 output = l1 + (l2<<(lastOffsetDB+1)) + (l3<<(lastOffsetDB+9)) + (l3<<(lastOffsetDB+17)) + (l4<<(lastOffsetDB+25));
223 break;
224 }
225 }
226 return output;
227}
228
229unsigned short PolarxMainWindow::bytesSpread(unsigned short startBit, unsigned short length)
230{
231 unsigned short nbBytes;
232 nbBytes = ((startBit+length-2)/8 - (startBit-1)/8 +1);
233 return nbBytes;
234}
Note: See TracBrowser for help on using the repository browser.