Subversion Repositories Projects

Rev

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