Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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