1 | #include "protocol.h"
|
2 |
|
3 | protocol::protocol()
|
4 | {
|
5 | this->check_byte = 0x00;
|
6 | this->mode = 0x00;
|
7 | this->reset_data();
|
8 | }
|
9 |
|
10 | protocol::protocol(protocol *forward)
|
11 | {
|
12 | this->mode = 0xff;
|
13 | this->forward = forward;
|
14 | this->check_byte = 0x00;
|
15 | this->reset_data();
|
16 | }
|
17 |
|
18 | unsigned char protocol::decode(unsigned char inChar)
|
19 | {
|
20 | static unsigned char buffer[100];
|
21 | static unsigned char indexer;
|
22 |
|
23 | if(inChar==START_BYTE)
|
24 | {
|
25 | for(unsigned char i=0; i<100; i++)
|
26 | {
|
27 | buffer[i] = 0;
|
28 | }
|
29 | indexer = 0;
|
30 | }
|
31 |
|
32 | buffer[indexer] = inChar;
|
33 |
|
34 | if(buffer[indexer]==END_BYTE)
|
35 | {
|
36 | if(buffer[0]==START_BYTE)
|
37 | {
|
38 | unsigned char tmp_indexer = 2;
|
39 | unsigned char tmp_letter = buffer[1];
|
40 | while(tmp_indexer<indexer)
|
41 | {
|
42 | switch(tmp_letter)
|
43 | {
|
44 | case SERVO: //Servo
|
45 | this->data[SERVO] = buffer[tmp_indexer];
|
46 | if(this->mode==0xff)
|
47 | {
|
48 | if(this->data[AUTOPILOT]==0)
|
49 | {
|
50 | this->forward->data[SERVO] = this->data[SERVO];
|
51 | this->forward->check_byte |= (1<<SERVO);
|
52 | }
|
53 | }else{
|
54 | this->check_byte |= (1<<SERVO);
|
55 | }
|
56 | break;
|
57 | case MOTOR: //Motor
|
58 | this->data[MOTOR] = buffer[tmp_indexer];
|
59 | if(this->mode==0xff)
|
60 | {
|
61 | if(this->data[AUTOPILOT]==0)
|
62 | {
|
63 | this->forward->data[MOTOR] = this->data[MOTOR];
|
64 | this->forward->check_byte |= (1<<MOTOR);
|
65 | }
|
66 | }else{
|
67 | this->check_byte |= (1<<MOTOR);
|
68 | }
|
69 | break;
|
70 | case BOW:
|
71 | this->data[BOW] = buffer[tmp_indexer];
|
72 | if(this->mode==0xff)
|
73 | {
|
74 | if(this->data[AUTOPILOT]==0)
|
75 | {
|
76 | this->forward->data[BOW] = this->data[BOW];
|
77 | this->forward->check_byte |= (1<<BOW);
|
78 | }
|
79 | }else{
|
80 | this->check_byte |= (1<<BOW);
|
81 | }
|
82 | break;
|
83 | case STATUS_1:
|
84 | this->data[STATUS_1] = buffer[tmp_indexer];
|
85 | tmp_indexer++;
|
86 | this->data[STATUS_2] = buffer[tmp_indexer];
|
87 | this->check_byte |= (1<<STATUS_1);
|
88 | break;
|
89 | case KRAN:
|
90 | this->data[KRAN] = buffer[tmp_indexer];
|
91 | if(this->mode==0xff)
|
92 | {
|
93 | this->forward->data[KRAN] = this->data[KRAN];
|
94 | this->forward->check_byte |= (1<<KRAN);
|
95 | }else{
|
96 | this->check_byte |= (1<<KRAN);
|
97 | }
|
98 | break;
|
99 | case AUTOPILOT:
|
100 | this->data[AUTOPILOT] = buffer[tmp_indexer];
|
101 | this->check_byte |= (1<<AUTOPILOT);
|
102 | break;
|
103 | case NEW_WAYPOINT_ID:
|
104 | this->data[NEW_WAYPOINT_ID] = buffer[tmp_indexer];
|
105 | tmp_indexer++;
|
106 | this->data[NEW_WAYPOINT_LON1] = buffer[tmp_indexer];
|
107 | tmp_indexer++;
|
108 | this->data[NEW_WAYPOINT_LON2] = buffer[tmp_indexer];
|
109 | tmp_indexer++;
|
110 | this->data[NEW_WAYPOINT_LON3] = buffer[tmp_indexer];
|
111 | tmp_indexer++;
|
112 | this->data[NEW_WAYPOINT_LON4] = buffer[tmp_indexer];
|
113 | tmp_indexer++;
|
114 | this->data[NEW_WAYPOINT_LAT1] = buffer[tmp_indexer];
|
115 | tmp_indexer++;
|
116 | this->data[NEW_WAYPOINT_LAT2] = buffer[tmp_indexer];
|
117 | tmp_indexer++;
|
118 | this->data[NEW_WAYPOINT_LAT3] = buffer[tmp_indexer];
|
119 | tmp_indexer++;
|
120 | this->data[NEW_WAYPOINT_LAT4] = buffer[tmp_indexer];
|
121 | this->check_byte |= (1<<NEW_WAYPOINT_ID);
|
122 | break;
|
123 | case DIRECT:
|
124 | this->data[DIRECT] = buffer[tmp_indexer];
|
125 | this->check_byte |= (1<<DIRECT);
|
126 | break;
|
127 | case RGB_ID:
|
128 | if(mode==0xff)
|
129 | {
|
130 | this->forward->data[RGB_ID] = buffer[tmp_indexer];
|
131 | tmp_indexer++;
|
132 | this->forward->data[RGB_R] = buffer[tmp_indexer];
|
133 | tmp_indexer++;
|
134 | this->forward->data[RGB_G] = buffer[tmp_indexer];
|
135 | tmp_indexer++;
|
136 | this->forward->data[RGB_B] = buffer[tmp_indexer];
|
137 | this->forward->check_byte |= (1<<RGB_ID);
|
138 | }
|
139 | else
|
140 | {
|
141 | this->data[RGB_ID] = buffer[tmp_indexer];
|
142 | tmp_indexer++;
|
143 | this->data[RGB_R] = buffer[tmp_indexer];
|
144 | tmp_indexer++;
|
145 | this->data[RGB_G] = buffer[tmp_indexer];
|
146 | tmp_indexer++;
|
147 | this->data[RGB_B] = buffer[tmp_indexer];
|
148 | this->check_byte |= (1<<RGB_ID);
|
149 | }
|
150 | break;
|
151 | case LICHT_1:
|
152 | if(mode==0xff)
|
153 | {
|
154 | this->forward->data[LICHT_1] = buffer[tmp_indexer];
|
155 | tmp_indexer++;
|
156 | this->forward->data[LICHT_2] = buffer[tmp_indexer];
|
157 | tmp_indexer++;
|
158 | this->forward->data[LICHT_3] = buffer[tmp_indexer];
|
159 | tmp_indexer++;
|
160 | this->forward->data[LICHT_4] = buffer[tmp_indexer];
|
161 | this->forward->check_byte |= (1<<LICHT_1);
|
162 | break;
|
163 | }else{
|
164 | this->data[LICHT_1] = buffer[tmp_indexer];
|
165 | tmp_indexer++;
|
166 | this->data[LICHT_2] = buffer[tmp_indexer];
|
167 | tmp_indexer++;
|
168 | this->data[LICHT_3] = buffer[tmp_indexer];
|
169 | tmp_indexer++;
|
170 | this->data[LICHT_4] = buffer[tmp_indexer];
|
171 | this->check_byte |= (1<<LICHT_1);
|
172 | break;
|
173 | }
|
174 | }
|
175 | tmp_letter = buffer[tmp_indexer];
|
176 | tmp_indexer++;
|
177 | }
|
178 | return 0xff;
|
179 | }
|
180 | return 0x01;
|
181 | }
|
182 |
|
183 | indexer++;
|
184 | return 0x00;
|
185 | }
|
186 |
|
187 | #ifndef DEBUG_PC
|
188 | unsigned char protocol::encode(fifo_t *fifo)
|
189 | {
|
190 | if(this->check_byte!=0x00)
|
191 | {
|
192 | fifo_put(&fifo,STARTBYTE);
|
193 | if(this->check_byte&(1<<SERVO))
|
194 | {
|
195 | fifo_put(&fifo,SERVO);
|
196 | fifo_put(&fifo,data[SERVO]);
|
197 | }
|
198 |
|
199 | if(this->check_byte&(1<<MOTOR))
|
200 | {
|
201 | fifo_put(&fifo,MOTOR);
|
202 | fifo_put(&fifo,data[MOTOR]);
|
203 | }
|
204 |
|
205 | if(this->check_byte&(1<<BOW))
|
206 | {
|
207 | fifo_put(&fifo,BOW);
|
208 | fifo_put(&fifo,data[BOW]);
|
209 | }
|
210 |
|
211 | if(this->check_byte&(1<<STATUS_1))
|
212 | {
|
213 | fifo_put(&fifo,STATUS_1);
|
214 | fifo_put(&fifo,data[STATUS_1]);
|
215 | fifo_put(&fifo,data[STATUS_2]);
|
216 | }
|
217 |
|
218 | if(this->check_byte&(1<<KRAN))
|
219 | {
|
220 | fifo_put(&fifo,KRAN);
|
221 | fifo_put(&fifo,data[KRAN]);
|
222 | }
|
223 |
|
224 | if(this->check_byte&(1<<AUTOPILOT))
|
225 | {
|
226 | fifo_put(&fifo,AUTOPILOT);
|
227 | fifo_put(&fifo,data[AUTOPILOT]);
|
228 | }
|
229 |
|
230 | if(this->check_byte&(1<<NEW_WAYPOINT_ID))
|
231 | {
|
232 | fifo_put(&fifo,NEW_WAYPOINT_ID);
|
233 | fifo_put(&fifo,data[NEW_WAYPOINT_ID]);
|
234 | fifo_put(&fifo,data[NEW_WAYPOINT_LON1]);
|
235 | fifo_put(&fifo,data[NEW_WAYPOINT_LON2]);
|
236 | fifo_put(&fifo,data[NEW_WAYPOINT_LON3]);
|
237 | fifo_put(&fifo,data[NEW_WAYPOINT_LON4]);
|
238 | fifo_put(&fifo,data[NEW_WAYPOINT_LAT1]);
|
239 | fifo_put(&fifo,data[NEW_WAYPOINT_LAT2]);
|
240 | fifo_put(&fifo,data[NEW_WAYPOINT_LAT3]);
|
241 | fifo_put(&fifo,data[NEW_WAYPOINT_LAT4]);
|
242 | }
|
243 |
|
244 | if(this->check_byte&(1<<DIRECT))
|
245 | {
|
246 | fifo_put(&fifo,DIRECT);
|
247 | fifo_put(&fifo,data[DIRECT]);
|
248 | }
|
249 |
|
250 | if(this->check_byte&(1<<KURS_AKTUELL))
|
251 | {
|
252 | fifo_put(&fifo,KURS_AKTUELL);
|
253 | fifo_put(&fifo,data[KURS_AKTUELL]);
|
254 | }
|
255 |
|
256 | if(this->check_byte&(1<<KURS_SOLL))
|
257 | {
|
258 | fifo_put(&fifo,KURS_SOLL);
|
259 | fifo_put(&fifo,data[KURS_SOLL]);
|
260 | }
|
261 |
|
262 | if(this->check_byte&(1<<RGB_ID))
|
263 | {
|
264 | fifo_put(&fifo,RGB_ID;
|
265 | fifo_put(&fifo,data[RGB_ID]);
|
266 | fifo_put(&fifo,data[RGB_R]);
|
267 | fifo_put(&fifo,data[RGB_G]);
|
268 | fifo_put(&fifo,data[RGB_B]);
|
269 | }
|
270 |
|
271 | if(this->check_byte&(1<<LICHT_1))
|
272 | {
|
273 | fifo_put(&fifo,LICHT_1;
|
274 | fifo_put(&fifo,data[LICHT_1]);
|
275 | fifo_put(&fifo,data[LICHT_2]);
|
276 | fifo_put(&fifo,data[LICHT_3]);
|
277 | fifo_put(&fifo,data[LICHT_4]);
|
278 | fifo_put(&fifo,data[LICHT_5]);
|
279 | }
|
280 |
|
281 | fifo_put(&fifo,END_BYTE);
|
282 | return 0xff;
|
283 | }
|
284 | return 0;
|
285 | }
|
286 | #endif
|
287 |
|
288 | unsigned char protocol::encode(unsigned char* buffer)
|
289 | {
|
290 | if(this->check_byte!=0x00)
|
291 | {
|
292 | buffer[0] = START_BYTE;
|
293 | unsigned char indexer = 1;
|
294 | if(this->check_byte&(1<<SERVO))
|
295 | {
|
296 | buffer[indexer] = SERVO;
|
297 | buffer[indexer+1] = data[SERVO];
|
298 | indexer+=2;
|
299 | }
|
300 |
|
301 | if(this->check_byte&(1<<MOTOR))
|
302 | {
|
303 | buffer[indexer] = MOTOR;
|
304 | buffer[indexer+1] = data[MOTOR];
|
305 | indexer+=2;
|
306 | }
|
307 |
|
308 | if(this->check_byte&(1<<BOW))
|
309 | {
|
310 | buffer[indexer] = BOW;
|
311 | buffer[indexer+1] = data[BOW];
|
312 | indexer+=2;
|
313 | }
|
314 |
|
315 | if(this->check_byte&(1<<STATUS_1))
|
316 | {
|
317 | buffer[indexer] = STATUS_1;
|
318 | buffer[indexer+1] = data[STATUS_1];
|
319 | buffer[indexer+2] = data[STATUS_2];
|
320 | indexer+=3;
|
321 | }
|
322 |
|
323 | if(this->check_byte&(1<<KRAN))
|
324 | {
|
325 | buffer[indexer] = KRAN;
|
326 | buffer[indexer+1] = data[KRAN];
|
327 | indexer+=2;
|
328 | }
|
329 |
|
330 | if(this->check_byte&(1<<AUTOPILOT))
|
331 | {
|
332 | buffer[indexer] = AUTOPILOT;
|
333 | buffer[indexer+1] = data[AUTOPILOT];
|
334 | indexer+=2;
|
335 | }
|
336 |
|
337 | if(this->check_byte&(1<<NEW_WAYPOINT_ID))
|
338 | {
|
339 | buffer[indexer] = NEW_WAYPOINT_ID;
|
340 | buffer[indexer+1] = data[NEW_WAYPOINT_ID];
|
341 | buffer[indexer+2] = data[NEW_WAYPOINT_LON1];
|
342 | buffer[indexer+3] = data[NEW_WAYPOINT_LON2];
|
343 | buffer[indexer+4] = data[NEW_WAYPOINT_LON3];
|
344 | buffer[indexer+5] = data[NEW_WAYPOINT_LON4];
|
345 | indexer+=6;
|
346 | buffer[indexer] = data[NEW_WAYPOINT_LAT1];
|
347 | buffer[indexer+1] = data[NEW_WAYPOINT_LAT2];
|
348 | buffer[indexer+2] = data[NEW_WAYPOINT_LAT3];
|
349 | buffer[indexer+3] = data[NEW_WAYPOINT_LAT4];
|
350 | indexer+=4;
|
351 | }
|
352 |
|
353 | if(this->check_byte&(1<<DIRECT))
|
354 | {
|
355 | buffer[indexer] = DIRECT;
|
356 | buffer[indexer+1] = data[DIRECT];
|
357 | indexer+=2;
|
358 | }
|
359 |
|
360 | if(this->check_byte&(1<<KURS_AKTUELL))
|
361 | {
|
362 | buffer[indexer] = KURS_AKTUELL;
|
363 | buffer[indexer+1] = data[KURS_AKTUELL];
|
364 | indexer+=2;
|
365 | }
|
366 |
|
367 | if(this->check_byte&(1<<KURS_SOLL))
|
368 | {
|
369 | buffer[indexer] = KURS_SOLL;
|
370 | buffer[indexer+1] = data[KURS_SOLL];
|
371 | indexer+=2;
|
372 | }
|
373 |
|
374 | if(this->check_byte&(1<<RGB_ID))
|
375 | {
|
376 | buffer[indexer] = RGB_ID;
|
377 | buffer[indexer+1] = data[RGB_ID];
|
378 | buffer[indexer+2] = data[RGB_R];
|
379 | buffer[indexer+3] = data[RGB_G];
|
380 | buffer[indexer+4] = data[RGB_B];
|
381 | indexer+=5;
|
382 | }
|
383 |
|
384 | if(this->check_byte&(1<<LICHT_1))
|
385 | {
|
386 | buffer[indexer] = LICHT_1;
|
387 | buffer[indexer+1] = data[LICHT_1];
|
388 | buffer[indexer+2] = data[LICHT_2];
|
389 | buffer[indexer+3] = data[LICHT_3];
|
390 | buffer[indexer+4] = data[LICHT_4];
|
391 | buffer[indexer+5] = data[LICHT_5];
|
392 | indexer+=6;
|
393 | }
|
394 |
|
395 | buffer[indexer] = END_BYTE;
|
396 | return 0xff;
|
397 | }
|
398 | return 0x00;
|
399 | }
|
400 |
|
401 | unsigned char protocol::get_servo()
|
402 | {
|
403 | return this->data[SERVO];
|
404 | }
|
405 |
|
406 | unsigned char protocol::get_motor()
|
407 | {
|
408 | return this->data[MOTOR];
|
409 | }
|
410 |
|
411 | unsigned char protocol::get_bow()
|
412 | {
|
413 | return this->data[BOW];
|
414 | }
|
415 |
|
416 | void protocol::reset_data(void)
|
417 | {
|
418 | for(unsigned char i=0;i<30;i++)
|
419 | {
|
420 | this->data[i] = 0;
|
421 | }
|
422 | check_byte = 0x00;
|
423 | }
|