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