Subversion Repositories FlightCtrl

Rev

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