Rev 82 | 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 | |
49 | |||
50 | // for statistics |
||
4 | ligi | 51 | public int debug_data_count=0; |
135 | ligi | 52 | public int debug_names_count=0; |
53 | public int angle_data_count=0; |
||
4 | ligi | 54 | public int version_data_count=0; |
55 | public int other_data_count=0; |
||
56 | public int lcd_data_count=0; |
||
57 | public int params_data_count=0; |
||
135 | ligi | 58 | public int navi_data_count=0; |
4 | ligi | 59 | |
60 | |||
61 | String name; |
||
62 | DUBwise root; |
||
135 | ligi | 63 | |
64 | |||
81 | ligi | 65 | DUBwiseDebug debug; |
4 | ligi | 66 | /****************** Section: public Methods ************************************************/ |
81 | ligi | 67 | public MKCommunicator(DUBwise root_,DUBwiseDebug debug_) |
4 | ligi | 68 | { |
81 | ligi | 69 | debug=debug_; |
4 | ligi | 70 | root=root_; |
71 | version=new MKVersion(); |
||
72 | debug_data=new MKDebugData(); |
||
81 | ligi | 73 | stick_data=new MKStickData(); |
4 | ligi | 74 | params=new MKParamsParser(); |
75 | LCD= new MKLCD(this); |
||
76 | watchdog=new MKWatchDog(this); |
||
135 | ligi | 77 | gps_position=new MKGPSPosition(); |
4 | ligi | 78 | new Thread( this ).start(); // fire up main Thread |
79 | } |
||
80 | |||
81 | |||
135 | ligi | 82 | public void do_proxy(String proxy_url) |
4 | ligi | 83 | { |
84 | proxy=new MKProxy(proxy_url); |
||
85 | } |
||
135 | ligi | 86 | |
4 | ligi | 87 | |
88 | // URL string: "btspp://XXXXXXXXXXXX:1" - the X-Part is the MAC-Adress of the Bluetooth-Device connected to the Fligth-Control |
||
89 | public void connect_to(String _url,String _name) |
||
90 | { |
||
91 | mk_url=_url; // remember URL for connecting / reconnecting later |
||
92 | name=_name; |
||
93 | force_disconnect=false; |
||
94 | connected=false; |
||
95 | } |
||
96 | |||
135 | ligi | 97 | public boolean ready() |
98 | { |
||
99 | return (connected&&(version.major!=-1)); |
||
100 | } |
||
4 | ligi | 101 | /****************** Section: private Methods ************************************************/ |
102 | private void connect() |
||
103 | { |
||
59 | ligi | 104 | System.out.println("trying to connect to" + mk_url); |
4 | ligi | 105 | try{ |
81 | ligi | 106 | connection = (StreamConnection) Connector.open(mk_url); |
107 | // old call |
||
108 | // connection = (StreamConnection) Connector.open(mk_url, Connector.READ_WRITE); |
||
109 | reader=connection.openInputStream(); |
||
110 | writer=connection.openOutputStream(); |
||
4 | ligi | 111 | |
81 | ligi | 112 | // |
113 | String magic="\rmk-mode\r"; |
||
114 | writer.write(magic.getBytes()); |
||
115 | writer.flush(); |
||
116 | // |
||
117 | |||
118 | |||
119 | connection_start_time=System.currentTimeMillis(); |
||
120 | connected=true; // if we get here everything seems to be OK |
||
121 | get_version(); |
||
122 | lcd_data_count=0; |
||
123 | debug_data_count=0; |
||
124 | version_data_count=0; |
||
125 | |||
126 | |||
127 | } |
||
4 | ligi | 128 | catch (Exception ex) |
129 | { |
||
130 | // TODO difference fatal errors from those which will lead to reconnection |
||
131 | msg="Problem connecting" + "\n" + ex; |
||
59 | ligi | 132 | System.out.println("problem connecting " + ex); |
4 | ligi | 133 | } |
134 | |||
135 | |||
136 | |||
137 | } |
||
138 | |||
139 | public int[] Decode64(int[] in_arr, int offset,int len) |
||
140 | { |
||
141 | int ptrIn=offset; |
||
142 | int a,b,c,d,x,y,z; |
||
143 | int ptr=0; |
||
144 | |||
145 | int[] out_arr=new int[len]; |
||
146 | |||
147 | while(len!=0) |
||
148 | { |
||
135 | ligi | 149 | a=0; |
150 | b=0; |
||
151 | c=0; |
||
152 | d=0; |
||
153 | try { |
||
4 | ligi | 154 | a = in_arr[ptrIn++] - '='; |
155 | b = in_arr[ptrIn++] - '='; |
||
156 | c = in_arr[ptrIn++] - '='; |
||
157 | d = in_arr[ptrIn++] - '='; |
||
135 | ligi | 158 | } |
159 | catch (Exception e) {} |
||
4 | ligi | 160 | //if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden |
161 | |||
162 | x = (a << 2) | (b >> 4); |
||
163 | y = ((b & 0x0f) << 4) | (c >> 2); |
||
164 | z = ((c & 0x03) << 6) | d; |
||
165 | |||
166 | if((len--)!=0) out_arr[ptr++] = x; else break; |
||
167 | if((len--)!=0) out_arr[ptr++] = y; else break; |
||
168 | if((len--)!=0) out_arr[ptr++] = z; else break; |
||
169 | } |
||
170 | |||
171 | return out_arr; |
||
172 | |||
173 | } |
||
174 | |||
175 | // FC - Function Mappers |
||
176 | |||
177 | // send a version Request to the FC - the reply to this request will be processed in process_data when it arrives |
||
135 | ligi | 178 | public void set_gps_target(int longitude,int latitude) |
179 | { |
||
180 | int[] target=new int[8]; |
||
181 | target[0]= (0xFF)&(longitude<<24); |
||
182 | target[1]= (0xFF)&(longitude<<16); |
||
183 | target[2]= (0xFF)&(longitude<<8); |
||
184 | target[3]= (0xFF)&(longitude); |
||
185 | |||
186 | send_command(0,'s',target); |
||
187 | } |
||
188 | |||
189 | |||
4 | ligi | 190 | public void get_version() |
191 | { |
||
192 | send_command(0,'v',new int[0]); |
||
193 | } |
||
194 | |||
195 | // send a MotorTest request - params are the speed for each Motor |
||
196 | public void motor_test(int[] params) |
||
197 | { |
||
198 | send_command(0,'t',params); |
||
199 | } |
||
200 | |||
201 | public void send_keys(int[] params) |
||
202 | { |
||
203 | send_command(0,'k',params); |
||
204 | } |
||
205 | |||
206 | // get params |
||
207 | public void get_params(int id) |
||
208 | { |
||
209 | int[] params=new int[1]; |
||
210 | params[0]=id; |
||
211 | |||
135 | ligi | 212 | while(sending||recieving) |
4 | ligi | 213 | {try { Thread.sleep(50); } |
214 | catch (Exception e) { } |
||
215 | } |
||
216 | |||
217 | send_command(0,'q',params); |
||
218 | } |
||
219 | |||
220 | |||
221 | public void get_debug_name(int id) |
||
222 | { |
||
223 | int[] params=new int[1]; |
||
224 | params[0]=id; |
||
225 | |||
135 | ligi | 226 | while(sending||recieving) |
4 | ligi | 227 | {try { Thread.sleep(50); } |
228 | catch (Exception e) { } |
||
229 | } |
||
230 | |||
231 | send_command(0,'a',params); |
||
232 | } |
||
233 | |||
234 | |||
235 | |||
236 | |||
81 | ligi | 237 | public void trigger_LCD(int key) |
4 | ligi | 238 | { |
135 | ligi | 239 | while(sending||recieving) |
240 | {try { Thread.sleep(50); } |
||
241 | catch (Exception e) { } |
||
242 | } |
||
4 | ligi | 243 | |
135 | ligi | 244 | //if (sending||recieving) return; |
4 | ligi | 245 | |
246 | int[] params=new int[3]; |
||
247 | params[0]=key; |
||
248 | params[1]=0; |
||
249 | params[2]=0; |
||
135 | ligi | 250 | |
4 | ligi | 251 | send_command(0,'h',params); |
252 | } |
||
253 | |||
254 | |||
255 | public void write_params() |
||
256 | { |
||
135 | ligi | 257 | while(sending||recieving) |
4 | ligi | 258 | {try { Thread.sleep(50); } |
259 | catch (Exception e) { } |
||
260 | } |
||
261 | |||
262 | send_command(0,(char)('l'+params.act_paramset),params.field[params.act_paramset]); |
||
263 | } |
||
264 | |||
265 | |||
266 | boolean sending=false; |
||
135 | ligi | 267 | boolean recieving=false; |
4 | ligi | 268 | |
269 | // send command to FC ( add crc and pack into pseudo Base64 |
||
270 | public void send_command(int modul,char cmd,int[] params) |
||
271 | { |
||
135 | ligi | 272 | // if (modul==0) return; |
4 | ligi | 273 | sending=true; |
274 | 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 |
||
275 | send_buff[0]='#'; |
||
276 | send_buff[1]=(char)modul; |
||
277 | send_buff[2]=cmd; |
||
278 | |||
279 | for(int param_pos=0;param_pos<(params.length/3 + (params.length%3==0?0:1)) ;param_pos++) |
||
280 | { |
||
281 | int a = (param_pos*3<params.length)?params[param_pos*3]:0; |
||
282 | int b = ((param_pos*3+1)<params.length)?params[param_pos*3+1]:0; |
||
283 | int c = ((param_pos*3+2)<params.length)?params[param_pos*3+2]:0; |
||
284 | |||
285 | send_buff[3+param_pos*4] = (char)((a >> 2)+'=' ); |
||
286 | send_buff[3+param_pos*4+1] = (char)('=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4))); |
||
287 | send_buff[3+param_pos*4+2] = (char)('=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6))); |
||
288 | send_buff[3+param_pos*4+3] = (char)('=' + ( c & 0x3f)); |
||
289 | |||
290 | //send_buff[3+foo]='='; |
||
291 | } |
||
292 | |||
81 | ligi | 293 | /* for(int foo=0;foo<(params.length/3 + (params.length%3==0?0:1) )*4;foo++) |
294 | { |
||
4 | ligi | 295 | int a = (foo<params.length) params[foo]; |
296 | int a = params[foo]; |
||
297 | |||
298 | //send_buff[3+foo]='='; |
||
81 | ligi | 299 | } |
300 | */ |
||
4 | ligi | 301 | try |
302 | { |
||
303 | int tmp_crc=0; |
||
304 | for ( int tmp_i=0; tmp_i<send_buff.length;tmp_i++) |
||
305 | { |
||
306 | tmp_crc+=(int)send_buff[tmp_i]; |
||
307 | writer.write(send_buff[tmp_i]); |
||
308 | } |
||
309 | tmp_crc%=4096; |
||
310 | writer.write( (char)(tmp_crc/64 + '=')); |
||
311 | writer.write( (char)(tmp_crc%64 + '=')); |
||
312 | writer.write('\r'); |
||
313 | writer.flush(); |
||
314 | } |
||
315 | catch (Exception e) |
||
316 | { // problem sending data to FC |
||
317 | } |
||
318 | |||
319 | sending=false; |
||
320 | } |
||
321 | |||
322 | |||
135 | ligi | 323 | public int slave_addr=-1; |
324 | |||
4 | ligi | 325 | public void process_data(int[] data,int len) |
326 | { |
||
327 | |||
135 | ligi | 328 | slave_addr=data[1]; |
329 | |||
4 | ligi | 330 | switch((char)data[2]) |
331 | { |
||
332 | |||
333 | case 'D': // debug Data |
||
334 | debug_data_count++; |
||
335 | debug_data.set_by_mk_data(Decode64(data,3,len-3),version); |
||
336 | break; |
||
337 | |||
338 | case 'A': // debug Data Names |
||
135 | ligi | 339 | debug_names_count++; |
4 | ligi | 340 | debug_data.set_names_by_mk_data(data[1]-'0',Decode64(data,3,len-3)); |
341 | break; |
||
342 | |||
343 | case 'V': // Version Info |
||
344 | version_data_count++; |
||
345 | version.set_by_mk_data(Decode64(data,3,6)); |
||
346 | break; |
||
347 | |||
348 | case '0': |
||
349 | case '1': |
||
350 | case '2': |
||
351 | case '3': |
||
81 | ligi | 352 | lcd_data_count++; |
4 | ligi | 353 | LCD.handle_lcd_data(Decode64(data,3,20),data[2]-(int)'0'); |
81 | ligi | 354 | |
4 | ligi | 355 | break; |
81 | ligi | 356 | case '4': |
357 | stick_data.set_by_mk_data(Decode64(data,3,20)); |
||
358 | String tmp_s=""; |
||
359 | for (int tmp_c=0;tmp_c<10;tmp_c++) |
||
360 | tmp_s+="s"+tmp_c+"v"+stick_data.stick[tmp_c]+" "; |
||
361 | debug.log(tmp_s); |
||
362 | break; |
||
135 | ligi | 363 | |
4 | ligi | 364 | case 'L': |
365 | case 'M': |
||
366 | case 'N': |
||
367 | case 'O': |
||
368 | case 'P': |
||
135 | ligi | 369 | //int[] foo=Decode64(data,3,len-3); |
4 | ligi | 370 | params.set_by_mk_data((int)(data[2]-'L'),Decode64(data,3,len-3),version); |
371 | params_data_count++; |
||
372 | break; |
||
373 | |||
135 | ligi | 374 | |
375 | |||
376 | // data from navi |
||
377 | case 'Q': |
||
378 | navi_data_count++; |
||
379 | debug.log("got navi data(" + len +"):"); |
||
380 | |||
381 | |||
382 | gps_position.set_by_mk_data(Decode64(data,3,len-3),version); |
||
383 | |||
384 | debug.log("long:" + gps_position.Longitude); |
||
385 | debug.log("lat:" + gps_position.Latitude); |
||
386 | |||
387 | root.canvas.ufo_prober.set_to_navi(); |
||
388 | break; |
||
389 | |||
390 | |||
391 | case 'w': |
||
392 | angle_data_count++; |
||
393 | root.canvas.ufo_prober.set_to_mk(); |
||
394 | debug.log("got angle data:"); |
||
395 | |||
396 | |||
397 | break; |
||
398 | |||
4 | ligi | 399 | default: |
400 | other_data_count++; |
||
135 | ligi | 401 | debug.log("got other data:"+ (char)data[2] + "=>" + (byte)data[2]); |
81 | ligi | 402 | |
135 | ligi | 403 | /* |
81 | ligi | 404 | String tmp_str=""; |
405 | for (int tmp_i=0;tmp_i<len;tmp_i++) |
||
406 | tmp_str+=(char)data[tmp_i]; |
||
135 | ligi | 407 | debug.log(tmp_str);*/ |
4 | ligi | 408 | break; |
409 | |||
410 | } |
||
411 | |||
412 | |||
413 | |||
414 | |||
415 | } |
||
416 | |||
417 | String o_msg=""; |
||
418 | |||
419 | public boolean force_disconnect=true; |
||
420 | |||
421 | public void close_connections(boolean force) |
||
422 | { |
||
135 | ligi | 423 | // if ((!force)&&root.canvas.do_vibra) root.vibrate(500); |
4 | ligi | 424 | force_disconnect=force; |
425 | try{ reader.close(); } |
||
426 | catch (Exception inner_ex) { } |
||
427 | |||
428 | try{ writer.close(); } |
||
429 | catch (Exception inner_ex) { } |
||
430 | |||
431 | try{ connection.close(); } |
||
432 | catch (Exception inner_ex) { } |
||
433 | |||
434 | connected=false; |
||
435 | } |
||
436 | |||
437 | // Thread to recieve data from Connection |
||
438 | public void run() |
||
439 | { |
||
135 | ligi | 440 | int[] data_set=new int[300]; |
4 | ligi | 441 | int input; |
442 | int pos=0; |
||
135 | ligi | 443 | |
444 | debug.log("Thread started"); |
||
4 | ligi | 445 | while(true) |
446 | { |
||
135 | ligi | 447 | debug.log("Connection Thread run"); |
81 | ligi | 448 | if (!connected) |
449 | { |
||
450 | if (!force_disconnect) connect(); |
||
451 | } |
||
452 | else |
||
453 | try{ |
||
135 | ligi | 454 | |
455 | |||
456 | while(sending) |
||
457 | {try { Thread.sleep(50); } |
||
458 | catch (Exception e) { } |
||
459 | } |
||
460 | |||
461 | |||
462 | |||
463 | recieving=true; |
||
464 | debug.log("Connected - reading data"); |
||
81 | ligi | 465 | pos=0; |
466 | input=0; |
||
467 | // recieve data-set |
||
135 | ligi | 468 | while ((input != 13)) //&&(input!=-1)) |
81 | ligi | 469 | { |
4 | ligi | 470 | |
81 | ligi | 471 | input = reader.read() ; |
472 | if (proxy!=null) |
||
473 | proxy.writer.write(input); |
||
135 | ligi | 474 | if (input==-1) throw new Exception("test"); |
81 | ligi | 475 | if (input!=-1) |
476 | { |
||
477 | data_set[pos]=input; |
||
478 | pos++; |
||
479 | } |
||
135 | ligi | 480 | |
4 | ligi | 481 | |
81 | ligi | 482 | } |
135 | ligi | 483 | recieving=false; |
484 | debug.log("Data recieved (" + pos + "Bytes) - processing .."); |
||
485 | /* |
||
81 | ligi | 486 | if (proxy!=null) |
487 | { |
||
488 | proxy.writer.write('\r'); |
||
489 | proxy.writer.write('\n'); |
||
490 | proxy.writer.flush(); |
||
491 | } |
||
135 | ligi | 492 | */ |
81 | ligi | 493 | if (pos>5) |
494 | process_data(data_set,pos); |
||
135 | ligi | 495 | |
496 | debug.log("Processing done"); |
||
497 | |||
81 | ligi | 498 | } |
499 | catch (Exception ex) |
||
500 | { |
||
501 | debug.log("Problem reading from MK -> closing conn"); |
||
502 | debug.log(ex.toString()); |
||
503 | // close the connection |
||
504 | close_connections(false); |
||
4 | ligi | 505 | |
506 | |||
81 | ligi | 507 | } |
4 | ligi | 508 | |
135 | ligi | 509 | // sleep a bit to get someting more done |
81 | ligi | 510 | try { Thread.sleep(50); } |
511 | catch (Exception e) { } |
||
4 | ligi | 512 | |
513 | } // while |
||
514 | |||
135 | ligi | 515 | |
516 | // debug.log("Leaving Communicator thread"); |
||
4 | ligi | 517 | |
518 | } // run() |
||
519 | |||
520 | |||
521 | } |