Subversion Repositories Projects

Rev

Rev 222 | Rev 224 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
206 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
 *
7
 * see README for further Infos
8
 *
9
 *
10
 *******************************************************************************************************************************/
11
 
12
 
13
package org.ligi.ufo;
14
 
15
 
16
//#ifdef j2me
17
//# import javax.microedition.io.*;
18
//#endif
19
 
20
//#ifdef android
21
import android.util.Log;
22
//#endif
23
 
24
 
25
 
26
import java.io.*;
27
 
28
 
29
public class MKCommunicator
219 ligi 30
    implements Runnable,DUBwiseDefinitions
206 ligi 31
{
222 ligi 32
 
33
    public int angle_nick=-4242;
34
    public int angle_roll=-4242;
219 ligi 35
    public byte bl_retrys=0;
36
    public boolean init_bootloader=false;
213 ligi 37
 
38
    public byte lib_version_major=0;
223 ligi 39
    public byte lib_version_minor=4;
213 ligi 40
 
41
    public String lib_version_str()
42
    {
43
        return "V"+lib_version_major+"."+lib_version_minor;
44
    }
45
 
46
 
206 ligi 47
    /***************** Section: public Attributes **********************************************/
48
    public boolean connected=false; // flag for the connection state
49
 
50
    public String mk_url=""; // buffer the url which is given in the constuctor for reconnectin purposes
51
 
52
    public final static int DATA_BUFF_LEN = 20; // in lines
53
 
54
    public String[] data_buff;
55
 
56
    //    boolean do_log=false;
57
    boolean do_log=true;
58
 
59
    int data_buff_pos=0;
60
 
61
    public byte user_intent=0;
219 ligi 62
    public final static int[] crc16_table = {
63
        0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
64
        0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
65
        0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
66
        0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
67
        0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
68
        0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
69
        0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
70
        0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
71
        0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
72
        0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
73
        0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
74
        0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
75
        0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
76
        0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
77
        0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
78
        0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
79
        0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
80
        0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
81
        0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
82
        0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
83
        0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
84
        0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
85
        0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
86
        0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
87
        0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
88
        0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
89
        0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
90
        0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
91
        0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
92
        0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
93
        0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
94
        0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
95
    };
206 ligi 96
 
97
    public void log(String str)
98
    {
99
//#ifdef android
219 ligi 100
        if (do_log)     Log.d("MK-Comm",str);
206 ligi 101
//#endif
102
    }
103
 
104
    public int CRC16(int ch, int crc)
219 ligi 105
    {
106
        return crc16_table[((crc >> 8) ^ (ch)) & 0xFF] ^ (crc << 8);
206 ligi 107
    }
108
 
109
 
110
    public int conn_time_in_s()
111
    {
112
        if (connected)
113
            return (int)((System.currentTimeMillis()-connection_start_time)/1000);
114
        else
115
            return 0;
116
    }
219 ligi 117
    public final static byte BOOTLOADER_STAGE_NONE=0;
118
    public final static byte BOOTLOADER_STAGE_GOT_MKBL=1;
206 ligi 119
 
219 ligi 120
    byte bootloader_stage= BOOTLOADER_STAGE_NONE;
206 ligi 121
 
122
    public MKLCD LCD;
123
    public MKVersion version;
124
    public MKDebugData debug_data;
125
 
219 ligi 126
    public int[] extern_control;
127
 
206 ligi 128
    public MKGPSPosition gps_position;
129
 
130
    public MKStickData stick_data;
131
    public MKParamsParser params;
132
    public MKWatchDog watchdog;
133
    public MKProxy proxy=null;
134
    public MKStatistics stats ;
135
    //    public DUBwiseDebug debug;
136
 
137
    public UFOProber    ufo_prober;
138
    public long connection_start_time=-1;
139
 
140
 
141
    public String error_str = null;
142
 
143
 
144
    public final static int FC_SLAVE_ADDR              = 'a'+1;
145
    public final static int NAVI_SLAVE_ADDR            = 'a'+2;
146
    public final static int MK3MAG_SLAVE_ADDR          = 'a'+3;
147
 
148
 
149
 
150
 
151
    /****************** Section: private Attributes **********************************************/
152
//#ifdef j2me
153
//#    private javax.microedition.io.StreamConnection connection;
154
//#endif
155
 
156
//#ifdef android
157
//    java.net.Socket connection;
158
    java.net.Socket connection;
159
//#endif
160
 
161
 
162
    private java.io.InputStream reader;    
163
    private java.io.OutputStream writer;    
164
 
165
 
166
 
167
    public String name;
168
    //    DUBwise root;
169
 
170
 
171
    private boolean sending=false;
172
    private boolean recieving=false;
173
 
174
 
175
 
176
    /******************  Section: public Methods ************************************************/
177
    public MKCommunicator()  
178
    {
179
 
180
        data_buff=new String[DATA_BUFF_LEN];
181
        for (int i=0;i<DATA_BUFF_LEN;i++)
182
            data_buff[i]="";
183
        //      debug=debug_;
184
        //      root=root_;
185
        version=new MKVersion();
186
        debug_data=new MKDebugData();
187
        stick_data=new MKStickData();
188
        params=new MKParamsParser();
219 ligi 189
        extern_control=new int[EXTERN_CONTROL_LENGTH];
190
        extern_control[EXTERN_CONTROL_CONFIG]=1;
191
        extern_control[EXTERN_CONTROL_FRAME]=1;
192
 
206 ligi 193
        LCD= new MKLCD(this);
194
        watchdog=new MKWatchDog(this);
195
        gps_position=new MKGPSPosition();
196
        stats = new MKStatistics();
197
        proxy =new MKProxy(this);
198
        ufo_prober=new UFOProber();
199
        new Thread( this ).start(); // fire up main Thread 
200
    }
201
 
202
 
203
 
204
    public void write_raw(byte[] _data)
205
    {
206
        wait4send();
207
        sending=true;
208
        try {
209
        writer.write(_data,0,_data.length);
210
        writer.flush();
211
 
212
        stats.bytes_out+=_data.length;
213
        }
214
        catch ( Exception e){}
215
        sending=false;
216
    }
217
 
218
    public void do_proxy(String proxy_url)
219
    {
220
        proxy.connect(proxy_url);
221
    }
222
 
223
    //    int port;
224
 
225
    //  URL string: "btspp://XXXXXXXXXXXX:1" - the X-Part is the MAC-Adress of the Bluetooth-Device connected to the Fligth-Control
226
    public void connect_to(String _url,String _name)
227
    {
228
        //      port=_port;
229
        mk_url=_url; // remember URL for connecting / reconnecting later
230
        name=_name;
231
        force_disconnect=false;
232
        connected=false;
233
    }
234
 
235
    public boolean ready()
236
    {
237
        return (connected&&(version.major!=-1));
238
    }
239
 
240
 
241
    public String get_buff(int age)
242
    {
243
 
244
        age%=DATA_BUFF_LEN;
245
 
219 ligi 246
        if (age<=data_buff_pos)
206 ligi 247
            return ""+data_buff[data_buff_pos-age];
248
        else
249
            return ""+data_buff[DATA_BUFF_LEN+data_buff_pos-age];
250
 
251
 
252
    }
253
    /******************  Section: private Methods ************************************************/
254
    private void connect()
255
    {
256
        log("trying to connect to" + mk_url);
257
        try{
258
 
259
            // old call
260
            // connection = (StreamConnection) Connector.open(mk_url, Connector.READ_WRITE);
261
 
262
//#ifdef android
263
            connection = (new java.net.Socket(mk_url.split(":")[0],Integer.parseInt(mk_url.split(":")[1])));
264
            //.Socket 
265
 
266
            reader=connection.getInputStream();
267
            writer=connection.getOutputStream();
268
 
269
            String magic="conn:foo bar\r\n";
270
            writer.write(magic.getBytes());
271
            writer.flush();
272
 
273
//#else
274
 
275
//#         connection = (StreamConnection) Connector.open(mk_url);
276
 
277
//#         reader=connection.openInputStream();
278
//#         writer=connection.openOutputStream();
279
 
280
//#endif
281
            connection_start_time=System.currentTimeMillis();
282
            connected=true; // if we get here everything seems to be OK
283
 
284
            stats.reset();
285
 
286
            log("connecting OK");
287
        }
288
        catch (Exception ex)
289
            {
290
                // TODO difference fatal errors from those which will lead to reconnection
291
                log("Problem connecting" + "\n" + ex);
292
            }  
293
    }
294
 
295
    public int[] Decode64(byte[] in_arr, int offset,int len)
296
    {
297
        int ptrIn=offset;      
298
        int a,b,c,d,x,y,z;
299
        int ptr=0;
300
 
301
        int[] out_arr=new int[len];
302
 
303
        while(len!=0)
304
            {
305
                a=0;
306
                b=0;
307
                c=0;
308
                d=0;
309
                try {
310
                a = in_arr[ptrIn++] - '=';
311
                b = in_arr[ptrIn++] - '=';
312
                c = in_arr[ptrIn++] - '=';
313
                d = in_arr[ptrIn++] - '=';
314
                }
315
                catch (Exception e) {}
316
                //if(ptrIn > max - 2) break;     // nicht mehr Daten verarbeiten, als empfangen wurden
317
 
318
                x = (a << 2) | (b >> 4);
319
                y = ((b & 0x0f) << 4) | (c >> 2);
320
                z = ((c & 0x03) << 6) | d;
321
 
322
                if((len--)!=0) out_arr[ptr++] = x; else break;
323
                if((len--)!=0) out_arr[ptr++] = y; else break;
324
                if((len--)!=0) out_arr[ptr++] = z; else break;
325
            }
326
 
327
        return out_arr;
328
 
329
    }
330
 
331
    public void wait4send()
332
    {
333
        while(sending) //||recieving)
334
            sleep(50);
335
    }
336
 
337
 
338
    public void sleep(int time)
339
    {
340
        try { Thread.sleep(time); }
341
        catch (Exception e)  {   }
342
    }
343
 
344
    // FC - Function Mappers
345
 
346
    // send a version Request to the FC - the reply to this request will be processed in process_data when it arrives
347
    public void get_version()
348
    {
349
        stats.version_data_request_count++;
350
        send_command(0,'v');
351
    }
352
 
353
    public void set_gps_target(int longitude,int latitude)
354
    {
355
        int[] target=new int[8];
356
        target[0]= (0xFF)&(longitude<<24);
357
        target[1]= (0xFF)&(longitude<<16);
358
        target[2]= (0xFF)&(longitude<<8);
359
        target[3]= (0xFF)&(longitude);
360
        //      send_command(0,'s',target);
361
    }
362
 
363
    // send a MotorTest request - params are the speed for each Motor
364
    public void motor_test(int[] params)
365
    {
366
        stats.motortest_request_count++;
367
        send_command(FC_SLAVE_ADDR,'t',params);
368
    }
369
 
219 ligi 370
 
371
    public void send_extern_control()
206 ligi 372
    {
219 ligi 373
 
374
        stats.external_control_request_count++;
375
        send_command(FC_SLAVE_ADDR,'b',extern_control);
206 ligi 376
    }
377
 
219 ligi 378
    /*    public void send_keys(int[] params)
379
          {
380
          send_command(FC_SLAVE_ADDR,'k',params);
381
          }*/
382
 
206 ligi 383
    // get params
384
    public void get_params(int id)
385
    {
386
        wait4send();
387
        send_command(FC_SLAVE_ADDR,'q',id+1);
388
        stats.params_data_request_count++;
389
    }
390
 
391
   public void get_debug_name(int id)
392
    {
393
 
394
        wait4send();
395
        send_command(0,'a',id);
396
    }
397
 
398
 
399
    public void trigger_LCD_by_page(int page)
400
    {
401
        wait4send();
402
        send_command(0,'l',page);
403
        stats.lcd_data_request_count++;
404
    }
405
 
406
    public void trigger_debug()
407
    {
408
        if (sending||recieving) return; // its not that important - can be dropped
409
        send_command(0,'c');
410
    }
411
 
412
 
216 ligi 413
    public void switch_todo()
414
    {
415
        sleep(50);
416
        version=new MKVersion();
417
        LCD= new MKLCD(this);
418
        debug_data=new MKDebugData();
206 ligi 419
 
216 ligi 420
    }
421
 
206 ligi 422
    public void switch_to_fc()
423
    {
424
        wait4send();
425
        send_command(NAVI_SLAVE_ADDR,'u',0);
216 ligi 426
        switch_todo();
427
 
206 ligi 428
    }
429
 
430
 
431
    public void switch_to_mk3mag()
432
    {
433
        wait4send();
434
        send_command(NAVI_SLAVE_ADDR   ,'u',1);
216 ligi 435
        switch_todo();
206 ligi 436
    }
437
 
215 ligi 438
    public final static byte[] navi_switch_magic={27,27,0x55,(byte)0xAA,0,(byte)'\r'};
206 ligi 439
    public void switch_to_navi()
440
    {
441
        wait4send();
442
        sending=true;
443
        try
444
            {
215 ligi 445
                writer.write(navi_switch_magic);
206 ligi 446
                stats.bytes_out+=6;
447
                writer.flush();
448
            }
449
        catch (Exception e)  {   }
450
        sending=false;
451
 
216 ligi 452
        switch_todo();
453
 
206 ligi 454
    }
455
 
456
    public String[] flash_msgs;
457
        int msg_pos=0;
458
 
459
 
460
    public boolean bootloader_intension_flash=false;
461
    public void jump_bootloader()
462
    {
463
 
464
        msg_pos=0;
465
        bootloader_stage= BOOTLOADER_STAGE_NONE;
466
        flash_msgs=new String[100];
467
        flash_msgs[msg_pos++]="Initiializing Bootloader";
468
        wait4send();
469
        sending=true;
470
 
471
        try
472
            {
473
                int attempt=0;
474
 
475
                while(bootloader_stage!= BOOTLOADER_STAGE_GOT_MKBL)
476
                    {
477
                        flash_msgs[msg_pos]="attempt "+attempt;
478
                        attempt++;
479
                        send_command_nocheck((byte)FC_SLAVE_ADDR,'R',new int[0]);
219 ligi 480
 
481
                        try{
206 ligi 482
                        writer.write( 27);
483
                        writer.flush();
484
 
485
                        sleep(20);
486
 
487
                        writer.write( 0xAA);
488
                        writer.flush();
219 ligi 489
                        }
490
                        catch (Exception e)  { }
206 ligi 491
                        sleep((attempt%2==0)?80:800); //800
492
                    }
493
                msg_pos++;
494
            }
495
 
496
        catch (Exception e)  {  
497
                flash_msgs[msg_pos++]="Exception:" +e.getMessage() ;
498
                flash_msgs[msg_pos++]=e.toString() ;
499
        }
500
 
501
        new Thread( this ).start(); // fire up main Thread 
502
    }
503
 
504
 
505
    public void get_error_str()
506
    {
507
        send_command(NAVI_SLAVE_ADDR,'e');
508
    }
509
 
510
    public void trigger_rcdata()
511
    {
512
        send_command(FC_SLAVE_ADDR,'p');
513
    }
514
 
515
 
223 ligi 516
    public void write_params(int to)
206 ligi 517
    {
223 ligi 518
        params.update_backup(to);
206 ligi 519
        wait4send();
223 ligi 520
        send_command(FC_SLAVE_ADDR,'s',params.field_bak[to]);
206 ligi 521
    }
522
 
523
    public void send_command(int modul,char cmd)
524
    {
525
        send_command(modul,cmd,new int[0]);
526
    }
527
 
528
    public void send_command(int modul,char cmd,int param)
529
    {
530
        int[] params=new int[1];
531
        params[0]=param;
532
        send_command(modul,cmd,params);
533
    }
534
 
535
    public void send_command_nocheck(byte modul,char cmd,int[] params)
536
    {
537
//      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
538
 
539
        byte[] send_buff=new byte[3 + (params.length/3 + (params.length%3==0?0:1) )*4]; // 5=1*start_char+1*addr+1*cmd+2*crc
540
        send_buff[0]='#';
541
        send_buff[1]=modul;
542
        send_buff[2]=(byte)cmd;
543
 
544
        for(int param_pos=0;param_pos<(params.length/3 + (params.length%3==0?0:1)) ;param_pos++)
545
            {
546
                int a = (param_pos*3<params.length)?params[param_pos*3]:0;
547
                int b = ((param_pos*3+1)<params.length)?params[param_pos*3+1]:0;
548
                int c = ((param_pos*3+2)<params.length)?params[param_pos*3+2]:0;
549
 
550
                send_buff[3+param_pos*4] =  (byte)((a >> 2)+'=' );
551
                send_buff[3+param_pos*4+1] = (byte)('=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)));
552
                send_buff[3+param_pos*4+2] = (byte)('=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)));
553
                send_buff[3+param_pos*4+3] = (byte)('=' + ( c & 0x3f));
554
 
555
                //send_buff[3+foo]='=';
556
            }
557
 
558
        /*      for(int foo=0;foo<(params.length/3 + (params.length%3==0?0:1) )*4;foo++)
559
                {
560
                int a = (foo<params.length) params[foo];
561
                int a = params[foo];
562
 
563
                //send_buff[3+foo]='=';
564
                }
565
        */
566
        try
567
            {
568
                int tmp_crc=0;
569
                for ( int tmp_i=0; tmp_i<send_buff.length;tmp_i++)
570
                    tmp_crc+=(int)send_buff[tmp_i];
571
 
572
                writer.write(send_buff,0,send_buff.length);
573
                tmp_crc%=4096;
574
 
575
                writer.write( (char)(tmp_crc/64 + '='));
576
                writer.write( (char)(tmp_crc%64 + '='));
577
                writer.write('\r');
578
                stats.bytes_out+=send_buff.length+3;
579
                writer.flush();
580
            }
581
        catch (Exception e)
582
            { // problem sending data to FC
583
            }
584
 
585
    }
586
    // send command to FC ( add crc and pack into pseudo Base64
587
    public void send_command(int modul,char cmd,int[] params)
588
    {
589
        //      if (modul==0) return;
590
        sending=true;
591
        send_command_nocheck((byte)modul,cmd,params);
592
        sending=false;
593
    }
594
 
595
 
596
    public int slave_addr=-1;
597
 
598
 
599
    public int UBatt()
600
    {
601
        if (ufo_prober.is_mk())
602
            return debug_data.UBatt();
603
        else  if (ufo_prober.is_navi())
604
            return gps_position.UBatt;
605
 
606
        return -1;
607
 
608
    }
609
 
610
 
611
 
612
    public int SenderOkay()
613
    {
614
        if (ufo_prober.is_mk())
615
            return debug_data.SenderOkay();
616
        else  if (ufo_prober.is_navi())
617
            return gps_position.SenderOkay;
618
 
619
        return -1;
620
 
621
    }
622
 
623
 
624
    public void process_data(byte[] data,int len)
625
    {
626
 
627
 
628
        log("command " +(char)data[2] );               
629
        switch((char)data[2])
630
            {
631
 
632
            case 'A': // debug Data Names
633
                stats.debug_names_count++;
634
                debug_data.set_names_by_mk_data(Decode64(data,3,len-3));
635
                break;
636
 
219 ligi 637
            case 'B': // external_control confirm frames
638
                stats.external_control_confirm_frame_count++;
639
                break;
640
 
206 ligi 641
            case 'L': // LCD Data
642
                stats.lcd_data_count++;
643
                LCD.handle_lcd_data(Decode64(data,3,len-3));
644
 
645
                break;
646
 
647
            case 'D': // debug Data
648
                log("got debug data");
649
                stats.debug_data_count++;
650
                debug_data.set_by_mk_data(Decode64(data,3,len-3),version);
651
                log("processed debug data");
652
                break;
653
 
654
            case 'V': // Version Info
655
                stats.version_data_count++;
656
                slave_addr=data[1];
657
 
658
                switch(slave_addr)
659
                    {
660
                    case FC_SLAVE_ADDR:
661
                        ufo_prober.set_to_mk();
662
                        break;
663
 
664
                    case NAVI_SLAVE_ADDR:
665
                        ufo_prober.set_to_navi();
666
                        break;
667
 
668
                    case MK3MAG_SLAVE_ADDR:
669
                        //                      ufo_prober.set_to_mk();
670
                        ufo_prober.set_to_mk3mag();
671
                        break;
672
 
673
                    default:
674
                        ufo_prober.set_to_incompatible();
675
                        break;
676
                    }
677
 
678
 
679
                version.set_by_mk_data(Decode64(data,3,len-3));
680
                break;
681
 
682
            case 'w':
222 ligi 683
                int[] dec=Decode64(data,3,len-3);
223 ligi 684
                angle_nick=MKHelper.parse_signed_int_2(dec[0],dec[1]);
685
                angle_roll=MKHelper.parse_signed_int_2(dec[2],dec[3]);
206 ligi 686
                stats.angle_data_count++;
687
 
688
                break;
689
 
690
 
691
            case 'Q':
692
                if (ufo_prober.is_mk())
693
                    {
694
                        stats.params_data_count++;
695
                        params.set_by_mk_data(Decode64(data,3,len-3));
696
                    }
697
                break;
698
 
699
            case 'P':
700
                stats.stick_data_count++;
701
                stick_data.set_by_mk_data(Decode64(data,3,20));
702
                break;
703
 
704
 
705
            case 'E':
706
                int[] dec_data=Decode64(data,3,len-3);
707
                error_str="";
708
                for(int foo=0;foo<20;foo++)
709
                    if (dec_data[foo]!=0)
710
                        error_str+=(char)dec_data[foo];
711
                break;
712
 
713
 
714
            case 'O':
715
                stats.navi_data_count++;
716
                log("got navi data(" + len +"):");
717
 
718
                gps_position.set_by_mk_data(Decode64(data,3,len-3),version);
719
 
720
                log("long:" + gps_position.Longitude);
721
                log("lat:" + gps_position.Latitude);
722
 
723
                break;
724
 
725
 
726
                // Error from Navi
727
 
728
 
729
            default:
730
                stats.other_data_count++;
731
                break;
732
 
733
            }
734
 
735
    }
736
 
737
    public boolean force_disconnect=true;
738
 
739
    public void close_connections(boolean force)
740
    {
741
        //      if ((!force)&&root.canvas.do_vibra) root.vibrate(500);
742
        force_disconnect=force;
743
        try{ reader.close(); }
744
        catch (Exception inner_ex) { }
745
 
746
        try{ writer.close(); }
747
        catch (Exception inner_ex) { }
748
 
749
//#ifdef j2me
750
//#     try{ connection.close(); }
751
//#     catch (Exception inner_ex) { }
752
//#endif
753
        ufo_prober.set_to_none();
219 ligi 754
        stats.reset();
206 ligi 755
        connected=false;
756
        version=new MKVersion();
757
    }
758
 
759
    // Thread to recieve data from Connection
760
    public void run()
761
    {
219 ligi 762
        boolean sigfail=false;
206 ligi 763
        if (bootloader_stage==BOOTLOADER_STAGE_GOT_MKBL)
764
            {
765
            try {
766
                        flash_msgs[msg_pos++]="reading avr_sig";
767
 
768
                        writer.write( 't');
769
                        writer.flush();
770
 
771
                        int avr_sig=reader.read();
219 ligi 772
 
773
                        while (avr_sig==63)
774
                            avr_sig=reader.read();
775
 
206 ligi 776
                        flash_msgs[msg_pos++]="got avr sig " + avr_sig;
777
 
778
 
779
                        if (reader.read()!=0)
780
                            throw new Exception("val after avrsig isnt 0");
781
 
782
                        if ((avr_sig!=0x74)&&(avr_sig!=224)&&(avr_sig!=120))
219 ligi 783
                            {
784
                                sigfail=true;
785
                                throw new Exception("avr sig" + avr_sig + " unknown");
786
                            }
206 ligi 787
 
788
                        writer.write('T');
789
                        //              writer.flush();
790
                        writer.write(avr_sig);   // set devicetyp = 0x74 oder 0x76  
791
                        writer.flush();
792
 
793
                        if (reader.read()!=0x0d)
794
                            throw new Exception("cant get buffer size");
795
 
796
                        writer.write('V');
797
                        writer.flush();
798
 
799
                        int bl_version_major=reader.read();
800
                        int bl_version_minor=reader.read();
801
 
802
                        flash_msgs[msg_pos++]="BL Version " + bl_version_major+"."+bl_version_minor;
803
 
804
 
805
                        writer.write('b');
806
                        writer.flush();
807
 
808
                        if (reader.read()!='Y')
809
                            throw new Exception("cant get buffer size");
810
 
811
                        int send_buff_size1=reader.read();
812
                        int send_buff_size2=reader.read();
813
                        int send_buff_size=send_buff_size1*0x100+send_buff_size2;
814
 
815
                        flash_msgs[msg_pos++]="BUFF Size:" + send_buff_size;
816
                        //                      if (send_buff_size>128)
817
                        //    send_buff_size=128;
219 ligi 818
                        if (bootloader_intension_flash)
819
                            {          
206 ligi 820
 
219 ligi 821
                                byte[] flash_buff =new byte[send_buff_size]; ///!!
822
 
823
                                String firmware_filename=(avr_sig==224)?"/navi.bin":((avr_sig==120)?"/mk3.bin":"/fc.bin");
824
                                flash_msgs[msg_pos++]="Opening firmware " + firmware_filename + "..";
825
 
826
 
827
                                InputStream in;
828
                                try {
829
                                    in=this.getClass().getResourceAsStream(firmware_filename);     
830
                                }
831
 
832
                                catch (Exception e) {               throw new Exception(" .. cant open firmware");                      }
833
                                int firmware_size=-1;
834
                                try {
835
 
836
                                firmware_size= ((int)in.read()<<24) |((int)in.read()<<16) | ((int)in.read()<<8) | ((int)in.read()&0xff) ;
837
                                }
838
                                catch (Exception e) {               throw new Exception(" .. cant read size");                  }
839
 
840
 
841
 
842
                                int blocks2write=((firmware_size/send_buff_size));
843
                                flash_msgs[msg_pos++]=".. open("+blocks2write+" blocks," + firmware_size + "bytes)";
844
 
845
 
846
 
847
                                //                      if (true) throw new Exception("before erasing");
848
 
849
                                //      if (true) throw new Exception("before erasing" );               
850
 
851
                                flash_msgs[msg_pos++]="Erasing Flash ..";
852
                                writer.write('e');
206 ligi 853
                                writer.flush();
854
 
219 ligi 855
                                if (reader.read()!=0x0d)
856
                                    throw new Exception("cant erase flash");
857
 
858
                                flash_msgs[msg_pos]+="OK";
859
 
860
 
861
                                writer.write('A');
862
                                writer.write(0);
863
                                writer.write(0);
864
                                writer.flush();
865
 
866
                                if (reader.read()!=0x0d)
867
                                    throw new Exception("cant set addr");
868
 
869
                                flash_msgs[msg_pos++]="addr set";
870
 
871
 
872
                                //                      int blocks2write=((firmware_size/send_buff_size));
873
                                if ((firmware_size%send_buff_size)>0)
874
                                    blocks2write++;
875
 
876
                                for ( int block=0; block<blocks2write; block ++)
206 ligi 877
                                    {
219 ligi 878
                                        int hex_bytes_read=in.read(flash_buff,0,send_buff_size);
879
 
880
                                        flash_msgs[msg_pos]="bl:" + block + "/" + blocks2write + " si:"+hex_bytes_read ;
881
 
882
 
883
                                        writer.write('B');
884
                                        writer.write((hex_bytes_read>>8)& 0xFF);
885
                                        writer.write((hex_bytes_read)& 0xFF);
886
                                        writer.write('F');
887
                                        writer.flush();
888
 
889
 
890
                                        writer.write(flash_buff,0,hex_bytes_read);
891
                                        writer.flush();                                
892
 
893
 
894
                                        if (avr_sig==224)
895
                                            {
896
                                                int crc=0xFFFF;
897
                                                for (int crc_pos=0;crc_pos<hex_bytes_read;crc_pos++)
898
                                                    crc=CRC16(flash_buff[crc_pos],crc);
899
                                                writer.write(crc>>8);
900
                                                writer.write(crc&0xff);
901
                                                writer.flush();
902
                                            }
206 ligi 903
                                        //  flash_msgs[msg_pos]+="ok";
904
                                        //                              writer.flush();
905
 
906
 
907
 
219 ligi 908
                                        if (reader.read()!=0x0d)
909
                                            throw new Exception("abort write at block"+block);
910
 
911
 
912
 
913
                                        //                             sleep(1000);
914
                                    }
915
                                //              flash_msgs[msg_pos]="bl:" + block + "/" + blocks2write + " si:"+hex_bytes_read ;
916
                                /*
917
 
206 ligi 918
                        int inp=0;
919
                        int block=0;
920
                        while (inp!=-1)
921
                            {
922
                                int flash_buff_pos=0;
923
                                int crc=0xFFFF;
924
 
925
                                while ((flash_buff_pos<send_buff_size)&&(inp!=-1))
926
                                    {
927
                                        inp=in.read();
928
                                        if (inp!=-1)
929
                                            {
930
                                                crc=CRC16(inp,crc);
931
                                                flash_buff[flash_buff_pos++]=(byte)inp;
932
                                            }
933
                                    }
934
                                //                              flash_msgs[msg_pos]="block" + block + "size:"+flash_buff_pos;
935
 
936
                                block++;        
937
 
938
                                boolean block_fin=false;
939
 
940
 
941
                                while(!block_fin)
942
                                    {
943
 
944
                                        writer.write('B');
945
                                        writer.write((flash_buff_pos>>8)& 0xFF);
946
                                        writer.write((flash_buff_pos)& 0xFF);
947
                                        writer.write('F');
948
                                        writer.flush();
949
 
950
                                        //                                      int ret_v=-1;
951
 
952
                                        writer.write(flash_buff,0,flash_buff_pos);
953
                                        flash_msgs[msg_pos]="bl:" + block + "si:"+flash_buff_pos ;
954
 
955
                                        writer.flush();                                
956
                                        //                                  flash_msgs[msg_pos]+="wtc";
957
 
958
 
959
                                        // append crc if navi
960
                                        if (avr_sig==224)
961
                                            {
962
                                                writer.write(crc>>8);
963
                                                writer.write(crc&0xff);
964
                                                writer.flush();
965
                                            }
966
                                        //  flash_msgs[msg_pos]+="ok";
967
                                        //                              writer.flush();
968
                                        //                      if (reader.read()!=0x0d)
969
                                        //                                  throw new Exception("abort write at block"+block);
970
 
971
 
972
                                        //ret_v=reader.read();
973
                                        //                                  flash_msgs[msg_pos]="ret"+ret_v + "crc"+crc;
974
 
975
                                        if (reader.read()==0x0d)
976
                                            block_fin=true;
977
 
978
                                    }
979
 
980
                            }
981
                        */
982
                        flash_msgs[++msg_pos]="written last block ";
983
                        msg_pos++;
984
                        flash_buff=null;
985
 
986
                        ufo_prober.set_to_none();
987
                        stats.reset();
988
                        version=new MKVersion();
989
                        System.gc();
990
                    }
991
                else // bootloader intension clear settings
992
                    {
993
 
994
                        flash_msgs[msg_pos]="reset params ..";
995
                        writer.write('B');
996
                        writer.write(0);
997
                        writer.write(4);
998
                        writer.write('E');
999
                        writer.flush();
1000
 
1001
                        writer.write(0xFF);
1002
                        writer.write(0xFF);
1003
                        writer.write(0xFF);
1004
                        writer.write(0xFF);
1005
                        writer.flush();
1006
                        flash_msgs[msg_pos++]+=" done";
1007
                    }
1008
 
1009
            flash_msgs[msg_pos++]="Exiting Bootloader" ;
1010
            params=new MKParamsParser();
1011
            try{
1012
                writer.write('E');
1013
                writer.flush();
1014
            }
1015
            catch (Exception e)  {  
1016
                flash_msgs[msg_pos++]="cant exit bootloader" ;
1017
            }
1018
            flash_msgs[msg_pos++]="Exit BL done" ;         
1019
 
1020
 
1021
            }
1022
 
1023
            catch (Exception e)  {  
1024
                flash_msgs[msg_pos++]="Fail:" +e.getMessage() ;
1025
 
1026
 
1027
            flash_msgs[msg_pos++]="Exiting Bootloader" ;
1028
            params=new MKParamsParser();
1029
            try{
1030
                writer.write('E');
1031
                writer.flush();
1032
            }
1033
            catch (Exception e2)  {  
1034
                flash_msgs[msg_pos++]="cant exit bootloader" ;
1035
            }
1036
            flash_msgs[msg_pos++]="Exit BL done" ;
219 ligi 1037
            if (sigfail&&(bl_retrys<3))
1038
                {
1039
                    bl_retrys++;
1040
                    init_bootloader=true;
1041
                }
1042
            close_connections(false);
206 ligi 1043
            }
1044
 
1045
 
1046
            sending=false;
1047
            }
1048
 
1049
 
1050
        byte[] data_set=new byte[1024];
1051
        int data_set_pos=0;
1052
        byte[] data_in_buff=new byte[2048];
1053
 
1054
        int input;
1055
        int pos=0;
1056
 
1057
 
1058
 
1059
 
1060
 
1061
        log("Thread started");
1062
        while(true)
1063
            {
1064
 
1065
                if (!connected)
1066
                    {
1067
                        if (!force_disconnect) connect();
1068
                        sleep(100);
1069
                    }
1070
                else
1071
                    try{
1072
 
1073
                        /*             
1074
                                while(sending)
1075
                                {try { Thread.sleep(50); }
1076
                                catch (Exception e)  {   }
1077
                                }
1078
                        */
1079
 
1080
                        recieving=true;
1081
                        int read_count =reader.read(data_in_buff,0,reader.available());
1082
                        log("Connected - reading data " + read_count);         
1083
                        //      pos=0;
1084
                        input=0;
1085
                        //data_buff[data_buff_pos]="";
1086
                        // recieve data-set
1087
                        if (read_count==0) sleep(50);
1088
 
1089
                        //                      int read_count =reader.read(data_in_buff,0,reader.available());
1090
                        stats.bytes_in+=read_count;
1091
                        if (read_count>0)
1092
                            {
1093
                                log("read" + read_count + " ds_pos" + data_set_pos);           
1094
 
1095
                                for ( pos=0;pos<read_count;pos++)
1096
                                    {
1097
                                        if (data_in_buff[pos]==13)
1098
                                            {
1099
                                                data_buff[data_buff_pos]=""+data_buff_pos+ "--"+new String(data_set, 0, data_set_pos);
1100
                                                data_buff_pos++;
1101
                                                data_buff_pos%=DATA_BUFF_LEN;
1102
 
1103
 
1104
                                                try{process_data(data_set,data_set_pos); }
1105
                                                catch (Exception e)
1106
                                                    {                  
1107
                                                        log(".. problem processing");
1108
                                                        log(e.toString());
1109
                                                        }
1110
 
1111
 
1112
 
1113
 
1114
                                                proxy.write(data_set,0,data_set_pos);
1115
                                                //                                                      proxy.writer.write('\r');
1116
                                                //proxy.writer.write('\n');
1117
                                                //proxy.writer.flush();
1118
                                                /*
1119
                                                if (proxy!=null)
1120
                                                    {
1121
 
1122
 
1123
 
1124
                                                    }
1125
                                                */
1126
                                                data_set_pos=0;
1127
 
1128
                                            }
1129
                                        else
1130
                                            {
1131
                                                data_set[data_set_pos++]=data_in_buff[pos];
1132
 
1133
 
1134
 
1135
                                                if ( (data_set_pos>4) && (data_set[data_set_pos-4]==(byte)'M') && (data_set[data_set_pos-3]==(byte)'K')  && (data_set[data_set_pos-2]==(byte)'B') && (data_set[data_set_pos-1]==(byte)'L'))
1136
                                                    {
1137
                                                        bootloader_stage= BOOTLOADER_STAGE_GOT_MKBL;
1138
                                                        return;
1139
                                                    }
1140
 
1141
                                            }
1142
 
1143
                                    }
1144
 
1145
 
1146
                            }
1147
                        else
1148
                            sleep(50);
1149
                        /*
1150
                        while ((input != 13)) //&&(input!=-1))
1151
                            {
1152
                                {
1153
                                    //log("pre read");         
1154
                                    log(""+reader.available());
1155
                                    input = reader.read() ;
1156
                                    log("Byte rcv" + input +"pos"+ pos);               
1157
 
1158
                                    proxy.write(input);
1159
 
1160
                                    data_buff[data_buff_pos]+=(char)input;
1161
 
1162
                                    if ((data_buff[data_buff_pos].length()>3)&&(data_buff[data_buff_pos].substring(data_buff[data_buff_pos].length()-4,data_buff[data_buff_pos].length()).equals("MKBL")))
1163
                                        {
1164
                                            bootloader_stage= BOOTLOADER_STAGE_GOT_MKBL;
1165
                                            return;
1166
                                        }
1167
                                    if (input==-1) throw new Exception("disconnect");
1168
                                    else
1169
                                        {
1170
                                            stats.bytes_in++;
1171
                                            data_set[pos]=input;
1172
                                            pos++;
1173
                                        }
1174
                                }
1175
 
1176
                            }
1177
 
1178
 
1179
 
1180
                        data_buff_pos++;
1181
                        data_buff_pos%=DATA_BUFF_LEN;
1182
                        recieving=false;
1183
                        log("Data recieved (" + pos + "Bytes)");               
1184
                        log("processing ..");          
1185
                        */
1186
 
1187
                        /*
1188
                          if (proxy!=null)
1189
                          {
1190
                          proxy.writer.write('\r');
1191
                          proxy.writer.write('\n');
1192
                          proxy.writer.flush();
1193
                          }
1194
                        */
1195
                        /*if (pos>5)
1196
                            {
1197
                                try{process_data(data_set,pos); }
1198
                                catch (Exception e)
1199
                                    {                  
1200
                                        log(".. problem processing");
1201
                                        log(e.toString());
1202
                                    }
1203
 
1204
                                log(".. processing done");             
1205
                            }
1206
                        */
1207
                    }
1208
                    catch (Exception ex)
1209
                        {
1210
                            log("Problem reading from MK -> closing conn");
1211
                            log(ex.toString());
1212
                            // close the connection 
1213
                            close_connections(false);
1214
                        }      
1215
 
1216
                // sleep a bit to  get someting more done
1217
                //              sleep(5); //50
1218
 
1219
            } // while
1220
        //      log("Leaving Communicator thread");
1221
    } // run()
1222
}