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;
221 ligi 30
    public MKWatchDog watchdog;
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;
164 ligi 49
 
50
 
51
    /******************  Section: public Methods ************************************************/
52
    public MKCommunicator(String url)     // Constructor with URL string e.g. "btspp://XXXXXXXXXXXX:1" - the X-Part is the MAC-Adress of the Bluetooth-Device connected to the Fligth-Control
53
    {
210 ligi 54
        version=new MKVersion();
181 ligi 55
        debug_data=new MKDebugData();
164 ligi 56
        mk_url=url; // remember URL for connecting / reconnecting later
181 ligi 57
 
164 ligi 58
        new Thread( this ).start(); // fire up main Thread 
59
    }
60
 
61
 
62
    /******************  Section: private Methods ************************************************/
63
    private void connect()
64
    {
65
        try{
66
                connection = (StreamConnection) Connector.open(mk_url, Connector.READ_WRITE);
67
                reader=connection.openInputStream();
68
                writer=connection.openOutputStream();
181 ligi 69
 
70
                connection_start_time=System.currentTimeMillis();
164 ligi 71
                connected=true; // if we get here everything seems to be OK
181 ligi 72
                get_version();
221 ligi 73
                lcd_data_count=0;
74
                debug_data_count=0;
75
                version_data_count=0;
76
 
208 ligi 77
                LCD= new MKLCD(this);
221 ligi 78
                watchdog=new MKWatchDog(this);
164 ligi 79
           }
80
        catch (Exception ex)
81
            {
82
                // TODO difference fatal errors from those which will lead to reconnection
83
                msg="Problem connecting" + "\n" + ex;
84
            }  
181 ligi 85
 
86
 
87
 
164 ligi 88
    }
89
 
90
 
91
 
92
    public int[] Decode64(int[] in_arr, int offset,int len)
93
    {
94
        int ptrIn=offset;      
95
        int a,b,c,d,x,y,z;
96
        int ptr=0;
97
 
98
        int[] out_arr=new int[len];
99
 
100
        while(len!=0)
101
            {
102
                a = in_arr[ptrIn++] - '=';
103
                b = in_arr[ptrIn++] - '=';
104
                c = in_arr[ptrIn++] - '=';
105
                d = in_arr[ptrIn++] - '=';
106
                //if(ptrIn > max - 2) break;     // nicht mehr Daten verarbeiten, als empfangen wurden
107
 
108
                x = (a << 2) | (b >> 4);
109
                y = ((b & 0x0f) << 4) | (c >> 2);
110
                z = ((c & 0x03) << 6) | d;
111
 
112
                if((len--)!=0) out_arr[ptr++] = x; else break;
113
                if((len--)!=0) out_arr[ptr++] = y; else break;
114
                if((len--)!=0) out_arr[ptr++] = z; else break;
115
            }
116
 
117
        return out_arr;
118
 
119
    }
120
 
121
 
122
    // send a version Request to the FC - the reply to this request will be processed in process_data when it arrives
123
    public void get_version()
124
    {
125
        send_command(0,'v',new int[0]);
126
    }
127
 
208 ligi 128
 
231 ligi 129
    public void motor_test(int[] params)
130
    {
131
        send_command(0,'t',params);
132
    }
133
 
134
 
164 ligi 135
    // send command to FC ( add crc and pack into pseudo Base64
136
    public void send_command(int modul,char cmd,int[] params)
137
    {
138
        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
139
        send_buff[0]='#';
140
        send_buff[1]=(char)modul;
141
        send_buff[2]=cmd;
142
 
208 ligi 143
        for(int param_pos=0;param_pos<(params.length/3 + (params.length%3==0?0:1)) ;param_pos++)
144
            {
231 ligi 145
                int a = (param_pos*3<params.length)?params[param_pos*3]:0;
146
                int b = ((param_pos*3+1)<params.length)?params[param_pos*3+1]:0;
147
                int c = ((param_pos*3+2)<params.length)?params[param_pos*3+2]:0;
208 ligi 148
 
149
                send_buff[3+param_pos*4] =  (char)((a >> 2)+'=' );
150
                send_buff[3+param_pos*4+1] = (char)('=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)));
151
                send_buff[3+param_pos*4+2] = (char)('=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)));
152
                send_buff[3+param_pos*4+3] = (char)('=' + ( c & 0x3f));
153
 
154
                //send_buff[3+foo]='=';
155
            }
156
 
157
/*      for(int foo=0;foo<(params.length/3 + (params.length%3==0?0:1) )*4;foo++)
158
            {
159
                int a = (foo<params.length) params[foo];
160
                int a = params[foo];
161
 
162
                //send_buff[3+foo]='=';
163
            }
164
*/
164 ligi 165
        try
166
            {
167
                int tmp_crc=0;
168
                for ( int tmp_i=0; tmp_i<send_buff.length;tmp_i++)
169
                    {
170
                        tmp_crc+=(int)send_buff[tmp_i];
171
                        writer.write(send_buff[tmp_i]);
172
                    }
173
                tmp_crc%=4096;
174
                writer.write( (char)(tmp_crc/64 + '='));
175
                writer.write( (char)(tmp_crc%64 + '='));
176
                writer.write('\r');
177
                writer.flush();
178
            }
179
        catch (Exception e)
180
            { // problem sending data to FC
181
            }
182
 
183
    }
184
 
210 ligi 185
 
164 ligi 186
    public void process_data(int[] data,int len)
187
    {
188
        int[] decoded_data;
189
 
181 ligi 190
 
164 ligi 191
        switch((char)data[2])
192
            {
193
 
194
            case 'D': // debug Data
208 ligi 195
                debug_data_count++;
210 ligi 196
                debug_data.set_by_mk_data(Decode64(data,3,50),version);
164 ligi 197
                break;
198
 
199
            case 'V': // Version Info
208 ligi 200
                version_data_count++;
210 ligi 201
                version.set_by_mk_data(Decode64(data,3,6));
208 ligi 202
                break;
203
 
204
            case '0':
205
            case '1':
206
            case '2':
207
            case '3':
208
                LCD.handle_lcd_data(Decode64(data,3,20),data[2]-(int)'0');
209
                lcd_data_count++;
164 ligi 210
                break;
208 ligi 211
 
164 ligi 212
            default:
208 ligi 213
                other_data_count++;
164 ligi 214
                break;
215
 
216
            }
217
 
218
 
208 ligi 219
 
164 ligi 220
 
221
    }
222
 
208 ligi 223
    String o_msg="";
181 ligi 224
 
208 ligi 225
    public boolean force_disconnect=false;
181 ligi 226
 
221 ligi 227
    public void close_connections(boolean force)
208 ligi 228
    {
221 ligi 229
        force_disconnect=force;
208 ligi 230
        try{ reader.close(); }
231
        catch (Exception inner_ex) { }
181 ligi 232
 
208 ligi 233
        try{ writer.close(); }
234
        catch (Exception inner_ex) { }
235
 
236
        try{ connection.close(); }
237
        catch (Exception inner_ex) { }
238
 
239
        connected=false;
240
    }
241
 
164 ligi 242
    // Thread to recieve data from Connection
243
    public void run()
244
    {
245
        int[] data_set=new int[150];
246
        int input;
247
        int pos=0;
248
        msg+="!!run started!!";
249
        while(true)
250
            {
251
            if (!connected)
181 ligi 252
                {
208 ligi 253
                   if (!force_disconnect) connect();
181 ligi 254
                }
164 ligi 255
            else
256
                try{
181 ligi 257
 
164 ligi 258
                    pos=0;
259
                    input=0;
260
                    // recieve data-set
261
                    while ((input != 13)) // &&(input!=-1))
262
                       {
263
                           input = reader.read() ;
264
                           if (input==-1) throw new Exception("test");
265
                           data_set[pos]=input;
266
                           pos++;
267
 
268
                       }
269
                    process_data(data_set,pos);
270
 
271
                   }
272
        catch (Exception ex)
273
            {
274
                msg="Problem reading from MK";
275
                // close the connection 
221 ligi 276
                close_connections(false);
208 ligi 277
 
278
 
164 ligi 279
            }  
280
 
281
            // sleep a bit to get someting more done
282
            try { Thread.sleep(50); }
283
            catch (Exception e)  {   }
284
 
285
            } // while
286
 
287
 
288
    } // run()
289
 
290
 
291
}