Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
231 | 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 | * Project-Start: 9/2007 |
||
7 | * Version: 0.07 |
||
8 | * Mailto: ligi@smart4mobile.de |
||
9 | * Licence: Creative Commons / Non Commercial |
||
10 | * Big Up: Holger&Ingo |
||
11 | * |
||
12 | ************************************************************************************************/ |
||
164 | ligi | 13 | |
14 | import javax.microedition.io.*; |
||
15 | import java.io.*; |
||
16 | |||
17 | public class MKCommunicator |
||
18 | implements Runnable |
||
19 | { |
||
20 | /***************** Section: public Attributes **********************************************/ |
||
21 | public boolean connected=false; // flag for the connection state |
||
22 | 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. |
||
23 | |||
24 | |||
25 | public String mk_url=""; // buffer the url which is given in the constuctor for reconnectin purposes |
||
26 | |||
208 | ligi | 27 | public MKLCD LCD; |
210 | ligi | 28 | public MKVersion version; |
29 | public MKDebugData debug_data; |
||
303 | ligi | 30 | public MKParamsParser params; |
221 | ligi | 31 | public MKWatchDog watchdog; |
208 | ligi | 32 | |
181 | ligi | 33 | public long connection_start_time=-1; |
164 | ligi | 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 | |||
208 | ligi | 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; |
||
303 | ligi | 50 | public int params_data_count=0; |
164 | ligi | 51 | |
52 | |||
303 | ligi | 53 | String name; |
54 | |||
164 | ligi | 55 | /****************** Section: public Methods ************************************************/ |
303 | ligi | 56 | public MKCommunicator() |
164 | ligi | 57 | { |
210 | ligi | 58 | version=new MKVersion(); |
181 | ligi | 59 | debug_data=new MKDebugData(); |
303 | ligi | 60 | params=new MKParamsParser(); |
61 | // mk_url=url; // remember URL for connecting / reconnecting later |
||
181 | ligi | 62 | |
303 | ligi | 63 | LCD= new MKLCD(this); |
64 | watchdog=new MKWatchDog(this); |
||
164 | ligi | 65 | new Thread( this ).start(); // fire up main Thread |
66 | } |
||
67 | |||
68 | |||
303 | ligi | 69 | |
70 | // URL string: "btspp://XXXXXXXXXXXX:1" - the X-Part is the MAC-Adress of the Bluetooth-Device connected to the Fligth-Control |
||
71 | public void connect_to(String _url,String _name) |
||
72 | { |
||
73 | mk_url=_url; |
||
74 | name=_name; |
||
75 | force_disconnect=false; |
||
76 | connected=false; |
||
77 | } |
||
78 | |||
164 | ligi | 79 | /****************** Section: private Methods ************************************************/ |
80 | private void connect() |
||
81 | { |
||
303 | ligi | 82 | |
164 | ligi | 83 | try{ |
84 | connection = (StreamConnection) Connector.open(mk_url, Connector.READ_WRITE); |
||
85 | reader=connection.openInputStream(); |
||
86 | writer=connection.openOutputStream(); |
||
181 | ligi | 87 | |
88 | connection_start_time=System.currentTimeMillis(); |
||
164 | ligi | 89 | connected=true; // if we get here everything seems to be OK |
181 | ligi | 90 | get_version(); |
221 | ligi | 91 | lcd_data_count=0; |
92 | debug_data_count=0; |
||
93 | version_data_count=0; |
||
94 | |||
303 | ligi | 95 | |
164 | ligi | 96 | } |
97 | catch (Exception ex) |
||
98 | { |
||
99 | // TODO difference fatal errors from those which will lead to reconnection |
||
100 | msg="Problem connecting" + "\n" + ex; |
||
101 | } |
||
181 | ligi | 102 | |
103 | |||
104 | |||
164 | ligi | 105 | } |
106 | |||
107 | |||
108 | |||
109 | public int[] Decode64(int[] in_arr, int offset,int len) |
||
110 | { |
||
111 | int ptrIn=offset; |
||
112 | int a,b,c,d,x,y,z; |
||
113 | int ptr=0; |
||
114 | |||
115 | int[] out_arr=new int[len]; |
||
116 | |||
117 | while(len!=0) |
||
118 | { |
||
119 | a = in_arr[ptrIn++] - '='; |
||
120 | b = in_arr[ptrIn++] - '='; |
||
121 | c = in_arr[ptrIn++] - '='; |
||
122 | d = in_arr[ptrIn++] - '='; |
||
123 | //if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden |
||
124 | |||
125 | x = (a << 2) | (b >> 4); |
||
126 | y = ((b & 0x0f) << 4) | (c >> 2); |
||
127 | z = ((c & 0x03) << 6) | d; |
||
128 | |||
129 | if((len--)!=0) out_arr[ptr++] = x; else break; |
||
130 | if((len--)!=0) out_arr[ptr++] = y; else break; |
||
131 | if((len--)!=0) out_arr[ptr++] = z; else break; |
||
132 | } |
||
133 | |||
134 | return out_arr; |
||
135 | |||
136 | } |
||
137 | |||
138 | |||
139 | // send a version Request to the FC - the reply to this request will be processed in process_data when it arrives |
||
140 | public void get_version() |
||
141 | { |
||
142 | send_command(0,'v',new int[0]); |
||
143 | } |
||
144 | |||
303 | ligi | 145 | // send a MotorTest request - params are the speed for each Motor |
231 | ligi | 146 | public void motor_test(int[] params) |
147 | { |
||
148 | send_command(0,'t',params); |
||
149 | } |
||
150 | |||
303 | ligi | 151 | // get params |
152 | public void get_params(int id) |
||
153 | { |
||
154 | int[] params=new int[1]; |
||
155 | params[0]=id; |
||
231 | ligi | 156 | |
303 | ligi | 157 | send_command(0,'q',params); |
158 | } |
||
159 | |||
160 | |||
161 | |||
162 | |||
164 | ligi | 163 | // send command to FC ( add crc and pack into pseudo Base64 |
164 | public void send_command(int modul,char cmd,int[] params) |
||
165 | { |
||
166 | 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 |
||
167 | send_buff[0]='#'; |
||
168 | send_buff[1]=(char)modul; |
||
169 | send_buff[2]=cmd; |
||
170 | |||
208 | ligi | 171 | for(int param_pos=0;param_pos<(params.length/3 + (params.length%3==0?0:1)) ;param_pos++) |
172 | { |
||
231 | ligi | 173 | int a = (param_pos*3<params.length)?params[param_pos*3]:0; |
174 | int b = ((param_pos*3+1)<params.length)?params[param_pos*3+1]:0; |
||
175 | int c = ((param_pos*3+2)<params.length)?params[param_pos*3+2]:0; |
||
208 | ligi | 176 | |
177 | send_buff[3+param_pos*4] = (char)((a >> 2)+'=' ); |
||
178 | send_buff[3+param_pos*4+1] = (char)('=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4))); |
||
179 | send_buff[3+param_pos*4+2] = (char)('=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6))); |
||
180 | send_buff[3+param_pos*4+3] = (char)('=' + ( c & 0x3f)); |
||
181 | |||
182 | //send_buff[3+foo]='='; |
||
183 | } |
||
184 | |||
185 | /* for(int foo=0;foo<(params.length/3 + (params.length%3==0?0:1) )*4;foo++) |
||
186 | { |
||
187 | int a = (foo<params.length) params[foo]; |
||
188 | int a = params[foo]; |
||
189 | |||
190 | //send_buff[3+foo]='='; |
||
191 | } |
||
192 | */ |
||
164 | ligi | 193 | try |
194 | { |
||
195 | int tmp_crc=0; |
||
196 | for ( int tmp_i=0; tmp_i<send_buff.length;tmp_i++) |
||
197 | { |
||
198 | tmp_crc+=(int)send_buff[tmp_i]; |
||
199 | writer.write(send_buff[tmp_i]); |
||
200 | } |
||
201 | tmp_crc%=4096; |
||
202 | writer.write( (char)(tmp_crc/64 + '=')); |
||
203 | writer.write( (char)(tmp_crc%64 + '=')); |
||
204 | writer.write('\r'); |
||
205 | writer.flush(); |
||
206 | } |
||
207 | catch (Exception e) |
||
208 | { // problem sending data to FC |
||
209 | } |
||
210 | |||
211 | } |
||
212 | |||
210 | ligi | 213 | |
164 | ligi | 214 | public void process_data(int[] data,int len) |
215 | { |
||
216 | int[] decoded_data; |
||
217 | |||
181 | ligi | 218 | |
164 | ligi | 219 | switch((char)data[2]) |
220 | { |
||
221 | |||
222 | case 'D': // debug Data |
||
208 | ligi | 223 | debug_data_count++; |
210 | ligi | 224 | debug_data.set_by_mk_data(Decode64(data,3,50),version); |
164 | ligi | 225 | break; |
226 | |||
227 | case 'V': // Version Info |
||
208 | ligi | 228 | version_data_count++; |
210 | ligi | 229 | version.set_by_mk_data(Decode64(data,3,6)); |
208 | ligi | 230 | break; |
231 | |||
232 | case '0': |
||
233 | case '1': |
||
234 | case '2': |
||
235 | case '3': |
||
236 | LCD.handle_lcd_data(Decode64(data,3,20),data[2]-(int)'0'); |
||
237 | lcd_data_count++; |
||
164 | ligi | 238 | break; |
208 | ligi | 239 | |
303 | ligi | 240 | case 'L': |
241 | case 'M': |
||
242 | case 'N': |
||
243 | case 'O': |
||
244 | case 'P': |
||
245 | params.set_by_mk_data(Decode64(data,3,58),version); |
||
246 | params_data_count++; |
||
247 | break; |
||
248 | |||
164 | ligi | 249 | default: |
208 | ligi | 250 | other_data_count++; |
164 | ligi | 251 | break; |
252 | |||
253 | } |
||
254 | |||
255 | |||
208 | ligi | 256 | |
164 | ligi | 257 | |
258 | } |
||
259 | |||
208 | ligi | 260 | String o_msg=""; |
181 | ligi | 261 | |
303 | ligi | 262 | public boolean force_disconnect=true; |
181 | ligi | 263 | |
221 | ligi | 264 | public void close_connections(boolean force) |
208 | ligi | 265 | { |
221 | ligi | 266 | force_disconnect=force; |
208 | ligi | 267 | try{ reader.close(); } |
268 | catch (Exception inner_ex) { } |
||
181 | ligi | 269 | |
208 | ligi | 270 | try{ writer.close(); } |
271 | catch (Exception inner_ex) { } |
||
272 | |||
273 | try{ connection.close(); } |
||
274 | catch (Exception inner_ex) { } |
||
275 | |||
276 | connected=false; |
||
277 | } |
||
278 | |||
164 | ligi | 279 | // Thread to recieve data from Connection |
280 | public void run() |
||
281 | { |
||
282 | int[] data_set=new int[150]; |
||
283 | int input; |
||
284 | int pos=0; |
||
285 | msg+="!!run started!!"; |
||
286 | while(true) |
||
287 | { |
||
288 | if (!connected) |
||
181 | ligi | 289 | { |
208 | ligi | 290 | if (!force_disconnect) connect(); |
181 | ligi | 291 | } |
164 | ligi | 292 | else |
293 | try{ |
||
181 | ligi | 294 | |
164 | ligi | 295 | pos=0; |
296 | input=0; |
||
297 | // recieve data-set |
||
298 | while ((input != 13)) // &&(input!=-1)) |
||
299 | { |
||
300 | input = reader.read() ; |
||
301 | if (input==-1) throw new Exception("test"); |
||
302 | data_set[pos]=input; |
||
303 | pos++; |
||
304 | |||
305 | } |
||
306 | process_data(data_set,pos); |
||
307 | |||
308 | } |
||
309 | catch (Exception ex) |
||
310 | { |
||
311 | msg="Problem reading from MK"; |
||
312 | // close the connection |
||
221 | ligi | 313 | close_connections(false); |
208 | ligi | 314 | |
315 | |||
164 | ligi | 316 | } |
317 | |||
318 | // sleep a bit to get someting more done |
||
319 | try { Thread.sleep(50); } |
||
320 | catch (Exception e) { } |
||
321 | |||
322 | } // while |
||
323 | |||
324 | |||
325 | } // run() |
||
326 | |||
327 | |||
328 | } |