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