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