Subversion Repositories Projects

Rev

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