Rev 135 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
4 | ligi | 1 | /******************************************************************************************************************************** |
2 | * |
||
3 | * Abstaction Layer to Communicate via J2ME and Bluetooth with the FlightCtrl of the MikroKopter Project (www.mikrokopter.de ) |
||
4 | * |
||
5 | * Author: Marcus -LiGi- Bueschleb |
||
6 | * |
||
7 | * see README for further Infos |
||
8 | * |
||
9 | * |
||
10 | *******************************************************************************************************************************/ |
||
11 | |||
12 | import javax.microedition.io.*; |
||
13 | import java.io.*; |
||
14 | |||
135 | ligi | 15 | |
4 | ligi | 16 | public class MKCommunicator |
81 | ligi | 17 | implements Runnable |
4 | ligi | 18 | { |
19 | /***************** Section: public Attributes **********************************************/ |
||
20 | public boolean connected=false; // flag for the connection state |
||
21 | public boolean fatal=false; // flag which is set when an error is so fatal that reconnecting won't be tried - e.g. unknown version number. |
||
22 | public String mk_url=""; // buffer the url which is given in the constuctor for reconnectin purposes |
||
23 | |||
24 | public MKLCD LCD; |
||
25 | public MKVersion version; |
||
26 | public MKDebugData debug_data; |
||
135 | ligi | 27 | |
28 | public MKGPSPosition gps_position; |
||
29 | |||
81 | ligi | 30 | public MKStickData stick_data; |
4 | ligi | 31 | public MKParamsParser params; |
32 | public MKWatchDog watchdog; |
||
33 | public MKProxy proxy=null; |
||
135 | ligi | 34 | |
4 | ligi | 35 | public long connection_start_time=-1; |
36 | |||
37 | |||
38 | /****************** Section: private Attributes **********************************************/ |
||
39 | private javax.microedition.io.StreamConnection connection; |
||
40 | private java.io.InputStream reader; |
||
41 | private java.io.OutputStream writer; |
||
42 | |||
43 | |||
44 | // temp - to be removed |
||
45 | String p_msg="--"; |
||
46 | public String msg="BT_INIT"; |
||
47 | |||
135 | ligi | 48 | // for statistics |
4 | ligi | 49 | public int debug_data_count=0; |
135 | ligi | 50 | public int debug_names_count=0; |
51 | public int angle_data_count=0; |
||
4 | ligi | 52 | public int version_data_count=0; |
53 | public int other_data_count=0; |
||
54 | public int lcd_data_count=0; |
||
55 | public int params_data_count=0; |
||
135 | ligi | 56 | public int navi_data_count=0; |
4 | ligi | 57 | |
58 | |||
149 | ligi | 59 | public int bytes_in_count=0; |
60 | public int bytes_out_count=0; |
||
61 | |||
62 | |||
4 | ligi | 63 | String name; |
64 | DUBwise root; |
||
135 | ligi | 65 | |
66 | |||
81 | ligi | 67 | DUBwiseDebug debug; |
4 | ligi | 68 | /****************** Section: public Methods ************************************************/ |
81 | ligi | 69 | public MKCommunicator(DUBwise root_,DUBwiseDebug debug_) |
4 | ligi | 70 | { |
81 | ligi | 71 | debug=debug_; |
4 | ligi | 72 | root=root_; |
73 | version=new MKVersion(); |
||
74 | debug_data=new MKDebugData(); |
||
81 | ligi | 75 | stick_data=new MKStickData(); |
4 | ligi | 76 | params=new MKParamsParser(); |
77 | LCD= new MKLCD(this); |
||
78 | watchdog=new MKWatchDog(this); |
||
135 | ligi | 79 | gps_position=new MKGPSPosition(); |
149 | ligi | 80 | proxy =new MKProxy(); |
4 | ligi | 81 | new Thread( this ).start(); // fire up main Thread |
82 | } |
||
83 | |||
84 | |||
149 | ligi | 85 | |
86 | public void do_proxy(String proxy_url) |
||
4 | ligi | 87 | { |
149 | ligi | 88 | proxy.connect(proxy_url); |
4 | ligi | 89 | } |
135 | ligi | 90 | |
4 | ligi | 91 | |
92 | // URL string: "btspp://XXXXXXXXXXXX:1" - the X-Part is the MAC-Adress of the Bluetooth-Device connected to the Fligth-Control |
||
93 | public void connect_to(String _url,String _name) |
||
94 | { |
||
95 | mk_url=_url; // remember URL for connecting / reconnecting later |
||
96 | name=_name; |
||
97 | force_disconnect=false; |
||
98 | connected=false; |
||
99 | } |
||
100 | |||
135 | ligi | 101 | public boolean ready() |
102 | { |
||
103 | return (connected&&(version.major!=-1)); |
||
104 | } |
||
4 | ligi | 105 | /****************** Section: private Methods ************************************************/ |
106 | private void connect() |
||
107 | { |
||
59 | ligi | 108 | System.out.println("trying to connect to" + mk_url); |
4 | ligi | 109 | try{ |
81 | ligi | 110 | connection = (StreamConnection) Connector.open(mk_url); |
111 | // old call |
||
112 | // connection = (StreamConnection) Connector.open(mk_url, Connector.READ_WRITE); |
||
113 | reader=connection.openInputStream(); |
||
114 | writer=connection.openOutputStream(); |
||
4 | ligi | 115 | |
81 | ligi | 116 | // |
149 | ligi | 117 | //String magic="\rmk-mode\r"; |
118 | //writer.write(magic.getBytes()); |
||
119 | //writer.flush(); |
||
81 | ligi | 120 | // |
121 | |||
122 | |||
123 | connection_start_time=System.currentTimeMillis(); |
||
124 | connected=true; // if we get here everything seems to be OK |
||
125 | get_version(); |
||
126 | lcd_data_count=0; |
||
127 | debug_data_count=0; |
||
128 | version_data_count=0; |
||
129 | |||
130 | |||
131 | } |
||
4 | ligi | 132 | catch (Exception ex) |
133 | { |
||
134 | // TODO difference fatal errors from those which will lead to reconnection |
||
135 | msg="Problem connecting" + "\n" + ex; |
||
59 | ligi | 136 | System.out.println("problem connecting " + ex); |
4 | ligi | 137 | } |
138 | |||
139 | |||
140 | |||
141 | } |
||
142 | |||
143 | public int[] Decode64(int[] in_arr, int offset,int len) |
||
144 | { |
||
145 | int ptrIn=offset; |
||
146 | int a,b,c,d,x,y,z; |
||
147 | int ptr=0; |
||
148 | |||
149 | int[] out_arr=new int[len]; |
||
150 | |||
151 | while(len!=0) |
||
152 | { |
||
135 | ligi | 153 | a=0; |
154 | b=0; |
||
155 | c=0; |
||
156 | d=0; |
||
157 | try { |
||
4 | ligi | 158 | a = in_arr[ptrIn++] - '='; |
159 | b = in_arr[ptrIn++] - '='; |
||
160 | c = in_arr[ptrIn++] - '='; |
||
161 | d = in_arr[ptrIn++] - '='; |
||
135 | ligi | 162 | } |
163 | catch (Exception e) {} |
||
4 | ligi | 164 | //if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden |
165 | |||
166 | x = (a << 2) | (b >> 4); |
||
167 | y = ((b & 0x0f) << 4) | (c >> 2); |
||
168 | z = ((c & 0x03) << 6) | d; |
||
169 | |||
170 | if((len--)!=0) out_arr[ptr++] = x; else break; |
||
171 | if((len--)!=0) out_arr[ptr++] = y; else break; |
||
172 | if((len--)!=0) out_arr[ptr++] = z; else break; |
||
173 | } |
||
174 | |||
175 | return out_arr; |
||
176 | |||
177 | } |
||
178 | |||
179 | // FC - Function Mappers |
||
180 | |||
181 | // send a version Request to the FC - the reply to this request will be processed in process_data when it arrives |
||
135 | ligi | 182 | public void set_gps_target(int longitude,int latitude) |
183 | { |
||
184 | int[] target=new int[8]; |
||
185 | target[0]= (0xFF)&(longitude<<24); |
||
186 | target[1]= (0xFF)&(longitude<<16); |
||
187 | target[2]= (0xFF)&(longitude<<8); |
||
188 | target[3]= (0xFF)&(longitude); |
||
149 | ligi | 189 | // send_command(0,'s',target); |
135 | ligi | 190 | } |
191 | |||
192 | |||
4 | ligi | 193 | public void get_version() |
194 | { |
||
195 | send_command(0,'v',new int[0]); |
||
196 | } |
||
197 | |||
198 | // send a MotorTest request - params are the speed for each Motor |
||
199 | public void motor_test(int[] params) |
||
200 | { |
||
201 | send_command(0,'t',params); |
||
202 | } |
||
203 | |||
204 | public void send_keys(int[] params) |
||
205 | { |
||
206 | send_command(0,'k',params); |
||
207 | } |
||
208 | |||
209 | // get params |
||
210 | public void get_params(int id) |
||
211 | { |
||
212 | int[] params=new int[1]; |
||
213 | params[0]=id; |
||
214 | |||
135 | ligi | 215 | while(sending||recieving) |
4 | ligi | 216 | {try { Thread.sleep(50); } |
217 | catch (Exception e) { } |
||
218 | } |
||
219 | |||
220 | send_command(0,'q',params); |
||
221 | } |
||
222 | |||
223 | |||
224 | public void get_debug_name(int id) |
||
225 | { |
||
226 | int[] params=new int[1]; |
||
227 | params[0]=id; |
||
228 | |||
135 | ligi | 229 | while(sending||recieving) |
4 | ligi | 230 | {try { Thread.sleep(50); } |
231 | catch (Exception e) { } |
||
232 | } |
||
233 | |||
234 | send_command(0,'a',params); |
||
235 | } |
||
236 | |||
237 | |||
149 | ligi | 238 | |
239 | |||
240 | public void trigger_debug() |
||
241 | { |
||
242 | if (sending||recieving) return; // its not that important - can be dropped |
||
243 | |||
244 | int[] params=new int[0]; |
||
245 | send_command(0,'c',params); |
||
246 | } |
||
247 | |||
248 | |||
249 | |||
250 | public void switch_to_fc() |
||
251 | { |
||
252 | |||
253 | |||
254 | while(sending||recieving) |
||
255 | { |
||
256 | try { Thread.sleep(50); } |
||
257 | catch (Exception e) { } |
||
258 | } |
||
259 | |||
260 | |||
261 | |||
262 | int[] params=new int[1]; |
||
263 | params[0]=0; |
||
264 | send_command(0,'u',params); |
||
265 | |||
266 | try { Thread.sleep(50); } |
||
267 | catch (Exception e) { } |
||
268 | |||
269 | version=new MKVersion(); |
||
270 | LCD= new MKLCD(this); |
||
271 | |||
272 | } |
||
273 | |||
274 | |||
275 | public void switch_to_navi() |
||
276 | |||
277 | { |
||
278 | |||
279 | while(sending||recieving) |
||
280 | {try { Thread.sleep(50); } |
||
281 | catch (Exception e) { } |
||
282 | } |
||
283 | |||
284 | |||
285 | sending=true; |
||
286 | try |
||
287 | { |
||
288 | writer.write( 27); |
||
289 | writer.write( 27); |
||
290 | writer.write( 0x55); |
||
291 | writer.write( 0xaa); |
||
292 | writer.write( 0); |
||
293 | writer.write('\r'); |
||
294 | bytes_out_count+=6; |
||
295 | writer.flush(); |
||
296 | } |
||
297 | catch (Exception e) { } |
||
298 | sending=false; |
||
299 | |||
300 | try { Thread.sleep(50); } |
||
301 | catch (Exception e) { } |
||
302 | version=new MKVersion(); |
||
303 | LCD= new MKLCD(this); |
||
304 | } |
||
305 | |||
4 | ligi | 306 | |
307 | |||
81 | ligi | 308 | public void trigger_LCD(int key) |
4 | ligi | 309 | { |
149 | ligi | 310 | while(sending||recieving) // check if not only sending matters |
135 | ligi | 311 | {try { Thread.sleep(50); } |
312 | catch (Exception e) { } |
||
313 | } |
||
4 | ligi | 314 | |
135 | ligi | 315 | //if (sending||recieving) return; |
4 | ligi | 316 | |
317 | int[] params=new int[3]; |
||
318 | params[0]=key; |
||
319 | params[1]=0; |
||
320 | params[2]=0; |
||
135 | ligi | 321 | |
4 | ligi | 322 | send_command(0,'h',params); |
323 | } |
||
324 | |||
325 | |||
326 | public void write_params() |
||
327 | { |
||
149 | ligi | 328 | params.update_backup(); |
135 | ligi | 329 | while(sending||recieving) |
4 | ligi | 330 | {try { Thread.sleep(50); } |
331 | catch (Exception e) { } |
||
332 | } |
||
333 | |||
334 | send_command(0,(char)('l'+params.act_paramset),params.field[params.act_paramset]); |
||
335 | } |
||
336 | |||
337 | |||
338 | boolean sending=false; |
||
135 | ligi | 339 | boolean recieving=false; |
4 | ligi | 340 | |
341 | // send command to FC ( add crc and pack into pseudo Base64 |
||
342 | public void send_command(int modul,char cmd,int[] params) |
||
343 | { |
||
149 | ligi | 344 | |
135 | ligi | 345 | // if (modul==0) return; |
4 | ligi | 346 | sending=true; |
149 | ligi | 347 | // char[] send_buff=new char[5 + (params.length/3 + (params.length%3==0?0:1) )*4]; // 5=1*start_char+1*addr+1*cmd+2*crc |
348 | |||
349 | char[] send_buff=new char[3 + (params.length/3 + (params.length%3==0?0:1) )*4]; // 5=1*start_char+1*addr+1*cmd+2*crc |
||
4 | ligi | 350 | send_buff[0]='#'; |
351 | send_buff[1]=(char)modul; |
||
352 | send_buff[2]=cmd; |
||
353 | |||
354 | for(int param_pos=0;param_pos<(params.length/3 + (params.length%3==0?0:1)) ;param_pos++) |
||
355 | { |
||
356 | int a = (param_pos*3<params.length)?params[param_pos*3]:0; |
||
357 | int b = ((param_pos*3+1)<params.length)?params[param_pos*3+1]:0; |
||
358 | int c = ((param_pos*3+2)<params.length)?params[param_pos*3+2]:0; |
||
359 | |||
360 | send_buff[3+param_pos*4] = (char)((a >> 2)+'=' ); |
||
361 | send_buff[3+param_pos*4+1] = (char)('=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4))); |
||
362 | send_buff[3+param_pos*4+2] = (char)('=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6))); |
||
363 | send_buff[3+param_pos*4+3] = (char)('=' + ( c & 0x3f)); |
||
364 | |||
365 | //send_buff[3+foo]='='; |
||
366 | } |
||
367 | |||
81 | ligi | 368 | /* for(int foo=0;foo<(params.length/3 + (params.length%3==0?0:1) )*4;foo++) |
369 | { |
||
4 | ligi | 370 | int a = (foo<params.length) params[foo]; |
371 | int a = params[foo]; |
||
372 | |||
373 | //send_buff[3+foo]='='; |
||
81 | ligi | 374 | } |
375 | */ |
||
4 | ligi | 376 | try |
377 | { |
||
378 | int tmp_crc=0; |
||
379 | for ( int tmp_i=0; tmp_i<send_buff.length;tmp_i++) |
||
380 | { |
||
381 | tmp_crc+=(int)send_buff[tmp_i]; |
||
382 | writer.write(send_buff[tmp_i]); |
||
149 | ligi | 383 | bytes_out_count++; |
4 | ligi | 384 | } |
385 | tmp_crc%=4096; |
||
386 | writer.write( (char)(tmp_crc/64 + '=')); |
||
387 | writer.write( (char)(tmp_crc%64 + '=')); |
||
388 | writer.write('\r'); |
||
149 | ligi | 389 | bytes_out_count+=3; |
4 | ligi | 390 | writer.flush(); |
391 | } |
||
392 | catch (Exception e) |
||
393 | { // problem sending data to FC |
||
394 | } |
||
395 | |||
396 | sending=false; |
||
397 | } |
||
398 | |||
399 | |||
135 | ligi | 400 | public int slave_addr=-1; |
401 | |||
4 | ligi | 402 | public void process_data(int[] data,int len) |
403 | { |
||
404 | |||
135 | ligi | 405 | slave_addr=data[1]; |
406 | |||
4 | ligi | 407 | switch((char)data[2]) |
408 | { |
||
409 | |||
410 | case 'D': // debug Data |
||
411 | debug_data_count++; |
||
412 | debug_data.set_by_mk_data(Decode64(data,3,len-3),version); |
||
413 | break; |
||
414 | |||
415 | case 'A': // debug Data Names |
||
135 | ligi | 416 | debug_names_count++; |
4 | ligi | 417 | debug_data.set_names_by_mk_data(data[1]-'0',Decode64(data,3,len-3)); |
418 | break; |
||
419 | |||
420 | case 'V': // Version Info |
||
421 | version_data_count++; |
||
422 | version.set_by_mk_data(Decode64(data,3,6)); |
||
149 | ligi | 423 | if (version.minor>60) // FIXME - NO good detection |
424 | root.canvas.ufo_prober.set_to_mk(); |
||
4 | ligi | 425 | break; |
426 | |||
427 | case '0': |
||
428 | case '1': |
||
429 | case '2': |
||
430 | case '3': |
||
81 | ligi | 431 | lcd_data_count++; |
4 | ligi | 432 | LCD.handle_lcd_data(Decode64(data,3,20),data[2]-(int)'0'); |
81 | ligi | 433 | |
4 | ligi | 434 | break; |
81 | ligi | 435 | case '4': |
436 | stick_data.set_by_mk_data(Decode64(data,3,20)); |
||
437 | String tmp_s=""; |
||
438 | for (int tmp_c=0;tmp_c<10;tmp_c++) |
||
439 | tmp_s+="s"+tmp_c+"v"+stick_data.stick[tmp_c]+" "; |
||
440 | debug.log(tmp_s); |
||
441 | break; |
||
135 | ligi | 442 | |
4 | ligi | 443 | case 'L': |
444 | case 'M': |
||
445 | case 'N': |
||
446 | case 'O': |
||
447 | case 'P': |
||
135 | ligi | 448 | //int[] foo=Decode64(data,3,len-3); |
4 | ligi | 449 | params.set_by_mk_data((int)(data[2]-'L'),Decode64(data,3,len-3),version); |
450 | params_data_count++; |
||
451 | break; |
||
452 | |||
135 | ligi | 453 | |
454 | |||
455 | // data from navi |
||
456 | case 'Q': |
||
457 | navi_data_count++; |
||
458 | debug.log("got navi data(" + len +"):"); |
||
459 | |||
460 | |||
461 | gps_position.set_by_mk_data(Decode64(data,3,len-3),version); |
||
462 | |||
463 | debug.log("long:" + gps_position.Longitude); |
||
464 | debug.log("lat:" + gps_position.Latitude); |
||
465 | |||
466 | root.canvas.ufo_prober.set_to_navi(); |
||
467 | break; |
||
468 | |||
469 | |||
470 | case 'w': |
||
471 | angle_data_count++; |
||
149 | ligi | 472 | // root.canvas.ufo_prober.set_to_mk(); |
473 | debug.log("got angle data"); |
||
135 | ligi | 474 | |
475 | |||
476 | break; |
||
477 | |||
4 | ligi | 478 | default: |
479 | other_data_count++; |
||
135 | ligi | 480 | debug.log("got other data:"+ (char)data[2] + "=>" + (byte)data[2]); |
81 | ligi | 481 | |
135 | ligi | 482 | /* |
81 | ligi | 483 | String tmp_str=""; |
484 | for (int tmp_i=0;tmp_i<len;tmp_i++) |
||
485 | tmp_str+=(char)data[tmp_i]; |
||
135 | ligi | 486 | debug.log(tmp_str);*/ |
4 | ligi | 487 | break; |
488 | |||
489 | } |
||
490 | |||
491 | |||
492 | |||
493 | |||
494 | } |
||
495 | |||
496 | String o_msg=""; |
||
497 | |||
498 | public boolean force_disconnect=true; |
||
499 | |||
500 | public void close_connections(boolean force) |
||
501 | { |
||
135 | ligi | 502 | // if ((!force)&&root.canvas.do_vibra) root.vibrate(500); |
4 | ligi | 503 | force_disconnect=force; |
504 | try{ reader.close(); } |
||
505 | catch (Exception inner_ex) { } |
||
506 | |||
507 | try{ writer.close(); } |
||
508 | catch (Exception inner_ex) { } |
||
509 | |||
510 | try{ connection.close(); } |
||
511 | catch (Exception inner_ex) { } |
||
512 | |||
513 | connected=false; |
||
514 | } |
||
515 | |||
516 | // Thread to recieve data from Connection |
||
517 | public void run() |
||
518 | { |
||
135 | ligi | 519 | int[] data_set=new int[300]; |
4 | ligi | 520 | int input; |
521 | int pos=0; |
||
135 | ligi | 522 | |
523 | debug.log("Thread started"); |
||
4 | ligi | 524 | while(true) |
525 | { |
||
135 | ligi | 526 | debug.log("Connection Thread run"); |
81 | ligi | 527 | if (!connected) |
528 | { |
||
529 | if (!force_disconnect) connect(); |
||
530 | } |
||
531 | else |
||
532 | try{ |
||
135 | ligi | 533 | |
149 | ligi | 534 | /* |
135 | ligi | 535 | while(sending) |
536 | {try { Thread.sleep(50); } |
||
537 | catch (Exception e) { } |
||
538 | } |
||
149 | ligi | 539 | */ |
135 | ligi | 540 | |
541 | |||
542 | recieving=true; |
||
543 | debug.log("Connected - reading data"); |
||
81 | ligi | 544 | pos=0; |
545 | input=0; |
||
546 | // recieve data-set |
||
135 | ligi | 547 | while ((input != 13)) //&&(input!=-1)) |
81 | ligi | 548 | { |
4 | ligi | 549 | |
149 | ligi | 550 | debug.log("pre read"); |
81 | ligi | 551 | input = reader.read() ; |
149 | ligi | 552 | debug.log("Byte rcv" + input); |
553 | |||
554 | proxy.write(input); |
||
555 | |||
556 | if (input==-1) throw new Exception("disconnect"); |
||
81 | ligi | 557 | if (input!=-1) |
558 | { |
||
149 | ligi | 559 | bytes_in_count++; |
81 | ligi | 560 | data_set[pos]=input; |
561 | pos++; |
||
562 | } |
||
149 | ligi | 563 | |
564 | |||
135 | ligi | 565 | |
4 | ligi | 566 | |
81 | ligi | 567 | } |
135 | ligi | 568 | recieving=false; |
569 | debug.log("Data recieved (" + pos + "Bytes) - processing .."); |
||
570 | /* |
||
81 | ligi | 571 | if (proxy!=null) |
572 | { |
||
573 | proxy.writer.write('\r'); |
||
574 | proxy.writer.write('\n'); |
||
575 | proxy.writer.flush(); |
||
576 | } |
||
135 | ligi | 577 | */ |
81 | ligi | 578 | if (pos>5) |
579 | process_data(data_set,pos); |
||
135 | ligi | 580 | |
581 | debug.log("Processing done"); |
||
582 | |||
81 | ligi | 583 | } |
584 | catch (Exception ex) |
||
585 | { |
||
586 | debug.log("Problem reading from MK -> closing conn"); |
||
587 | debug.log(ex.toString()); |
||
588 | // close the connection |
||
589 | close_connections(false); |
||
4 | ligi | 590 | |
591 | |||
81 | ligi | 592 | } |
4 | ligi | 593 | |
135 | ligi | 594 | // sleep a bit to get someting more done |
81 | ligi | 595 | try { Thread.sleep(50); } |
596 | catch (Exception e) { } |
||
4 | ligi | 597 | |
598 | } // while |
||
599 | |||
135 | ligi | 600 | |
601 | // debug.log("Leaving Communicator thread"); |
||
4 | ligi | 602 | |
603 | } // run() |
||
604 | |||
605 | |||
606 | } |