Subversion Repositories Projects

Rev

Rev 213 | Rev 216 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed

/********************************************************************************************************************************
 *                                                                    
 * Abstaction Layer to Communicate via J2ME and Bluetooth with the FlightCtrl of the MikroKopter Project (www.mikrokopter.de )  
 *                                                
 * Author:        Marcus -LiGi- Bueschleb          
 *
 * see README for further Infos
 *
 *
 *******************************************************************************************************************************/

 

package org.ligi.ufo;


//#ifdef j2me
//# import javax.microedition.io.*;
//#endif

//#ifdef android
import android.util.Log;
//#endif



import java.io.*;


public class MKCommunicator
    implements Runnable
{

    public byte lib_version_major=0;
    public byte lib_version_minor=1;

    public String lib_version_str()
    {
        return "V"+lib_version_major+"."+lib_version_minor;
    }
   

    /***************** Section: public Attributes **********************************************/
    public boolean connected=false; // flag for the connection state

    public String mk_url=""; // buffer the url which is given in the constuctor for reconnectin purposes

    public final static int DATA_BUFF_LEN = 20; // in lines

    public String[] data_buff;

    //    boolean do_log=false;
    boolean do_log=true;

    int data_buff_pos=0;

    public byte user_intent=0;
public final static int[] crc16_table = {
 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
 };

    public void log(String str)
    {
//#ifdef android
if (do_log)     Log.d("MK-Comm",str);
//#endif
    }

    public int CRC16(int ch, int crc)
    { return crc16_table[((crc >> 8) ^ (ch)) & 0xFF] ^ (crc << 8);
    }


    public int conn_time_in_s()
    {
        if (connected)
            return (int)((System.currentTimeMillis()-connection_start_time)/1000);
        else
            return 0;
    }
    public final static int BOOTLOADER_STAGE_NONE=0;
    public final static int BOOTLOADER_STAGE_GOT_MKBL=1;

    int bootloader_stage= BOOTLOADER_STAGE_NONE;

    public MKLCD LCD;
    public MKVersion version;
    public MKDebugData debug_data;

    public MKGPSPosition gps_position;

    public MKStickData stick_data;
    public MKParamsParser params;
    public MKWatchDog watchdog;
    public MKProxy proxy=null;
    public MKStatistics stats ;
    //    public DUBwiseDebug debug;

    public UFOProber    ufo_prober;
    public long connection_start_time=-1;


    public String error_str = null;
   

    public final static int FC_SLAVE_ADDR              = 'a'+1;
    public final static int NAVI_SLAVE_ADDR            = 'a'+2;
    public final static int MK3MAG_SLAVE_ADDR          = 'a'+3;




    /****************** Section: private Attributes **********************************************/
//#ifdef j2me
//#    private javax.microedition.io.StreamConnection connection;
//#endif

//#ifdef android
//    java.net.Socket connection;
    java.net.Socket connection;
//#endif


    private java.io.InputStream reader;    
    private java.io.OutputStream writer;    



    public String name;
    //    DUBwise root;


    private boolean sending=false;
    private boolean recieving=false;



    /******************  Section: public Methods ************************************************/
    public MKCommunicator()  
    {

        data_buff=new String[DATA_BUFF_LEN];
        for (int i=0;i<DATA_BUFF_LEN;i++)
            data_buff[i]="";
        //      debug=debug_;
        //      root=root_;
        version=new MKVersion();
        debug_data=new MKDebugData();
        stick_data=new MKStickData();
        params=new MKParamsParser();
        LCD= new MKLCD(this);
        watchdog=new MKWatchDog(this);
        gps_position=new MKGPSPosition();
        stats = new MKStatistics();
        proxy =new MKProxy(this);
        ufo_prober=new UFOProber();
        new Thread( this ).start(); // fire up main Thread
    }



    public void write_raw(byte[] _data)
    {
        wait4send();
        sending=true;
        try {
        writer.write(_data,0,_data.length);
        writer.flush();

        stats.bytes_out+=_data.length;
        }
        catch ( Exception e){}
        sending=false;
    }

    public void do_proxy(String proxy_url)
    {
        proxy.connect(proxy_url);
    }
   
    //    int port;

    //  URL string: "btspp://XXXXXXXXXXXX:1" - the X-Part is the MAC-Adress of the Bluetooth-Device connected to the Fligth-Control
    public void connect_to(String _url,String _name)
    {
        //      port=_port;
        mk_url=_url; // remember URL for connecting / reconnecting later
        name=_name;
        force_disconnect=false;
        connected=false;
    }

    public boolean ready()
    {
        return (connected&&(version.major!=-1));
    }


    public String get_buff(int age)
    {

        age%=DATA_BUFF_LEN;
       
        if (age<=data_buff_pos)
            return ""+data_buff[data_buff_pos-age];
        else
            return ""+data_buff[DATA_BUFF_LEN+data_buff_pos-age];
       

    }
    /******************  Section: private Methods ************************************************/
    private void connect()
    {
        log("trying to connect to" + mk_url);
        try{
           
            // old call
            // connection = (StreamConnection) Connector.open(mk_url, Connector.READ_WRITE);
         
//#ifdef android
            connection = (new java.net.Socket(mk_url.split(":")[0],Integer.parseInt(mk_url.split(":")[1])));
            //.Socket

            reader=connection.getInputStream();
            writer=connection.getOutputStream();

            String magic="conn:foo bar\r\n";
            writer.write(magic.getBytes());
            writer.flush();

//#else

//#         connection = (StreamConnection) Connector.open(mk_url);

//#         reader=connection.openInputStream();
//#         writer=connection.openOutputStream();

//#endif
            connection_start_time=System.currentTimeMillis();
            connected=true; // if we get here everything seems to be OK

            stats.reset();

            log("connecting OK");
        }
        catch (Exception ex)
            {
                // TODO difference fatal errors from those which will lead to reconnection
                log("Problem connecting" + "\n" + ex);
            }  
    }

    public int[] Decode64(byte[] in_arr, int offset,int len)
    {
        int ptrIn=offset;      
        int a,b,c,d,x,y,z;
        int ptr=0;
       
        int[] out_arr=new int[len];

        while(len!=0)
            {
                a=0;
                b=0;
                c=0;
                d=0;
                try {
                a = in_arr[ptrIn++] - '=';
                b = in_arr[ptrIn++] - '=';
                c = in_arr[ptrIn++] - '=';
                d = in_arr[ptrIn++] - '=';
                }
                catch (Exception e) {}
                //if(ptrIn > max - 2) break;     // nicht mehr Daten verarbeiten, als empfangen wurden

                x = (a << 2) | (b >> 4);
                y = ((b & 0x0f) << 4) | (c >> 2);
                z = ((c & 0x03) << 6) | d;

                if((len--)!=0) out_arr[ptr++] = x; else break;
                if((len--)!=0) out_arr[ptr++] = y; else break;
                if((len--)!=0) out_arr[ptr++] = z; else break;
            }
       
        return out_arr;

    }

    public void wait4send()
    {
        while(sending) //||recieving)
            sleep(50);
    }


    public void sleep(int time)
    {
        try { Thread.sleep(time); }
        catch (Exception e)  {   }
    }

    // FC - Function Mappers

    // send a version Request to the FC - the reply to this request will be processed in process_data when it arrives
    public void get_version()
    {
        stats.version_data_request_count++;
        send_command(0,'v');
    }

    public void set_gps_target(int longitude,int latitude)
    {
        int[] target=new int[8];
        target[0]= (0xFF)&(longitude<<24);
        target[1]= (0xFF)&(longitude<<16);
        target[2]= (0xFF)&(longitude<<8);
        target[3]= (0xFF)&(longitude);
        //      send_command(0,'s',target);
    }

    // send a MotorTest request - params are the speed for each Motor
    public void motor_test(int[] params)
    {
        stats.motortest_request_count++;
        send_command(FC_SLAVE_ADDR,'t',params);
    }

    public void send_keys(int[] params)
    {
        send_command(FC_SLAVE_ADDR,'k',params);
    }

    // get params
    public void get_params(int id)
    {
        wait4send();
        send_command(FC_SLAVE_ADDR,'q',id+1);
        stats.params_data_request_count++;
    }

   public void get_debug_name(int id)
    {

        wait4send();
        send_command(0,'a',id);
    }

 
    public void trigger_LCD_by_page(int page)
    {
        wait4send();
        send_command(0,'l',page);
        stats.lcd_data_request_count++;
    }

    public void trigger_debug()
    {
        if (sending||recieving) return; // its not that important - can be dropped
        send_command(0,'c');
    }



    public void switch_to_fc()
    {
        wait4send();
        send_command(NAVI_SLAVE_ADDR,'u',0);
        sleep(50);
        version=new MKVersion();
        LCD= new MKLCD(this);
    }


    public void switch_to_mk3mag()
    {
        wait4send();
        send_command(NAVI_SLAVE_ADDR   ,'u',1);
        sleep(50);
        version=new MKVersion();
        LCD= new MKLCD(this);
    }

    public final static byte[] navi_switch_magic={27,27,0x55,(byte)0xAA,0,(byte)'\r'};
    public void switch_to_navi()
    {
        wait4send();
        sending=true;
        try
            {
                writer.write(navi_switch_magic);
                stats.bytes_out+=6;
                writer.flush();
            }
        catch (Exception e)  {   }
        sending=false;
       
        sleep(50);
        version=new MKVersion();
        LCD= new MKLCD(this);
    }

    public String[] flash_msgs;
        int msg_pos=0;


    public boolean bootloader_intension_flash=false;
    public void jump_bootloader()
    {

        msg_pos=0;
        bootloader_stage= BOOTLOADER_STAGE_NONE;
        flash_msgs=new String[100];
        flash_msgs[msg_pos++]="Initiializing Bootloader";
        wait4send();
        sending=true;
       
        try
            {
                int attempt=0;

                while(bootloader_stage!= BOOTLOADER_STAGE_GOT_MKBL)
                    {
                        flash_msgs[msg_pos]="attempt "+attempt;
                        attempt++;
                        send_command_nocheck((byte)FC_SLAVE_ADDR,'R',new int[0]);
                        writer.write( 27);
                        writer.flush();

                        sleep(20);
                       
                        writer.write( 0xAA);
                        writer.flush();

                        sleep((attempt%2==0)?80:800); //800
                    }
                msg_pos++;
            }

        catch (Exception e)  {  
                flash_msgs[msg_pos++]="Exception:" +e.getMessage() ;
                flash_msgs[msg_pos++]=e.toString() ;
        }

        new Thread( this ).start(); // fire up main Thread
    }


    public void get_error_str()
    {
        send_command(NAVI_SLAVE_ADDR,'e');
    }

    public void trigger_rcdata()
    {
        send_command(FC_SLAVE_ADDR,'p');
    }


    public void write_params()
    {
        params.update_backup();
        wait4send();
        send_command(FC_SLAVE_ADDR,'s',params.field_bak[params.act_paramset]);
    }

    public void send_command(int modul,char cmd)
    {
        send_command(modul,cmd,new int[0]);
    }

    public void send_command(int modul,char cmd,int param)
    {
        int[] params=new int[1];
        params[0]=param;
        send_command(modul,cmd,params);
    }

    public void send_command_nocheck(byte modul,char cmd,int[] params)
    {
//      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

        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
        send_buff[0]='#';
        send_buff[1]=modul;
        send_buff[2]=(byte)cmd;
       
        for(int param_pos=0;param_pos<(params.length/3 + (params.length%3==0?0:1)) ;param_pos++)
            {
                int a = (param_pos*3<params.length)?params[param_pos*3]:0;
                int b = ((param_pos*3+1)<params.length)?params[param_pos*3+1]:0;
                int c = ((param_pos*3+2)<params.length)?params[param_pos*3+2]:0;

                send_buff[3+param_pos*4] =  (byte)((a >> 2)+'=' );
                send_buff[3+param_pos*4+1] = (byte)('=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)));
                send_buff[3+param_pos*4+2] = (byte)('=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)));
                send_buff[3+param_pos*4+3] = (byte)('=' + ( c & 0x3f));

                //send_buff[3+foo]='=';
            }

        /*      for(int foo=0;foo<(params.length/3 + (params.length%3==0?0:1) )*4;foo++)
                {
                int a = (foo<params.length) params[foo];
                int a = params[foo];
               
                //send_buff[3+foo]='=';
                }
        */

        try
            {
                int tmp_crc=0;
                for ( int tmp_i=0; tmp_i<send_buff.length;tmp_i++)
                    tmp_crc+=(int)send_buff[tmp_i];
                       
                writer.write(send_buff,0,send_buff.length);
                tmp_crc%=4096;

                writer.write( (char)(tmp_crc/64 + '='));
                writer.write( (char)(tmp_crc%64 + '='));
                writer.write('\r');
                stats.bytes_out+=send_buff.length+3;
                writer.flush();
            }
        catch (Exception e)
            { // problem sending data to FC
            }

    }
    // send command to FC ( add crc and pack into pseudo Base64
    public void send_command(int modul,char cmd,int[] params)
    {

        //      if (modul==0) return;
        sending=true;
        send_command_nocheck((byte)modul,cmd,params);
        sending=false;
    }


    public int slave_addr=-1;


    public int UBatt()
    {
        if (ufo_prober.is_mk())
            return debug_data.UBatt();
        else  if (ufo_prober.is_navi())
            return gps_position.UBatt;
       
        return -1;

    }



    public int SenderOkay()
    {
        if (ufo_prober.is_mk())
            return debug_data.SenderOkay();
        else  if (ufo_prober.is_navi())
            return gps_position.SenderOkay;
       
        return -1;

    }


    public void process_data(byte[] data,int len)
    {


        log("command " +(char)data[2] );               
        switch((char)data[2])
            {

            case 'A': // debug Data Names
                stats.debug_names_count++;
                debug_data.set_names_by_mk_data(Decode64(data,3,len-3));
                break;

            case 'L': // LCD Data
                stats.lcd_data_count++;
                LCD.handle_lcd_data(Decode64(data,3,len-3));

                break;

            case 'D': // debug Data
                log("got debug data");
                stats.debug_data_count++;
                debug_data.set_by_mk_data(Decode64(data,3,len-3),version);
                log("processed debug data");
                break;
               
            case 'V': // Version Info
                stats.version_data_count++;
                slave_addr=data[1];

                switch(slave_addr)
                    {
                    case FC_SLAVE_ADDR:
                        ufo_prober.set_to_mk();
                        break;

                    case NAVI_SLAVE_ADDR:
                        ufo_prober.set_to_navi();
                        break;

                    case MK3MAG_SLAVE_ADDR:
                        //                      ufo_prober.set_to_mk();
                        ufo_prober.set_to_mk3mag();
                        break;

                    default:
                        ufo_prober.set_to_incompatible();
                        break;
                    }


                version.set_by_mk_data(Decode64(data,3,len-3));
                break;

            case 'w':
                stats.angle_data_count++;

                break;


            case 'Q':
                if (ufo_prober.is_mk())
                    {
                        stats.params_data_count++;
                        params.set_by_mk_data(Decode64(data,3,len-3));
                    }
                break;
               
            case 'P':
                stats.stick_data_count++;
                stick_data.set_by_mk_data(Decode64(data,3,20));
                break;


            case 'E':
                int[] dec_data=Decode64(data,3,len-3);
                error_str="";
                for(int foo=0;foo<20;foo++)
                    if (dec_data[foo]!=0)
                        error_str+=(char)dec_data[foo];
                break;


            case 'O':
                stats.navi_data_count++;
                log("got navi data(" + len +"):");
               
                gps_position.set_by_mk_data(Decode64(data,3,len-3),version);

                log("long:" + gps_position.Longitude);
                log("lat:" + gps_position.Latitude);

                break;


                // Error from Navi


            default:
                stats.other_data_count++;
                break;

            }
       
    }

    public boolean force_disconnect=true;

    public void close_connections(boolean force)
    {
        //      if ((!force)&&root.canvas.do_vibra) root.vibrate(500);
        force_disconnect=force;
        try{ reader.close(); }
        catch (Exception inner_ex) { }

        try{ writer.close(); }
        catch (Exception inner_ex) { }
       
//#ifdef j2me
//#     try{ connection.close(); }
//#     catch (Exception inner_ex) { }
//#endif
        ufo_prober.set_to_none();
        stats.reset();
        connected=false;
        version=new MKVersion();
    }

    // Thread to recieve data from Connection
    public void run()
    {

        if (bootloader_stage==BOOTLOADER_STAGE_GOT_MKBL)
            {
            try {
                        flash_msgs[msg_pos++]="reading avr_sig";

                        writer.write( 't');
                        writer.flush();
               
                        int avr_sig=reader.read();
                        flash_msgs[msg_pos++]="got avr sig " + avr_sig;


                        if (reader.read()!=0)
                            throw new Exception("val after avrsig isnt 0");

                        if ((avr_sig!=0x74)&&(avr_sig!=224)&&(avr_sig!=120))
                            throw new Exception("avr sig" + avr_sig + " unknown");
                   
                        writer.write('T');
                        //              writer.flush();
                        writer.write(avr_sig);   // set devicetyp = 0x74 oder 0x76  
                        writer.flush();

                        if (reader.read()!=0x0d)
                            throw new Exception("cant get buffer size");
                       
                        writer.write('V');
                        writer.flush();

                        int bl_version_major=reader.read();
                        int bl_version_minor=reader.read();

                        flash_msgs[msg_pos++]="BL Version " + bl_version_major+"."+bl_version_minor;
                       
                       
                        writer.write('b');
                        writer.flush();

                        if (reader.read()!='Y')
                            throw new Exception("cant get buffer size");

                        int send_buff_size1=reader.read();
                        int send_buff_size2=reader.read();
                        int send_buff_size=send_buff_size1*0x100+send_buff_size2;
                       
                        flash_msgs[msg_pos++]="BUFF Size:" + send_buff_size;
                        //                      if (send_buff_size>128)
                        //    send_buff_size=128;
                if (bootloader_intension_flash)
                    {          

                        byte[] flash_buff =new byte[send_buff_size]; ///!!


                        flash_msgs[msg_pos++]="Opening firmware ..";


                        InputStream in;
                        try {
                            in=this.getClass().getResourceAsStream((avr_sig==224)?"/navi.bin":((avr_sig==120)?"mk3.bin":"/fc.bin"));       
                        }
                       
                        catch (Exception e) {               throw new Exception(" cant open firmware");                 }


                        int firmware_size= ((int)in.read()<<24) |((int)in.read()<<16) | ((int)in.read()<<8) | ((int)in.read()&0xff) ;
                       

                        flash_msgs[msg_pos++]=".. open with " + firmware_size + "bytes";



               
                       
                        //      if (true) throw new Exception("before erasing" );              

                        flash_msgs[msg_pos++]="Erasing Flash ..";
                        writer.write('e');
                        writer.flush();

                        if (reader.read()!=0x0d)
                            throw new Exception("cant erase flash");

                        flash_msgs[msg_pos]+="OK";


                        writer.write('A');
                        writer.write(0);
                        writer.write(0);
                        writer.flush();

                        if (reader.read()!=0x0d)
                            throw new Exception("cant set addr");
                       
                        flash_msgs[msg_pos++]="addr set";


                        int blocks2write=((firmware_size/send_buff_size));
                        if ((firmware_size%send_buff_size)>0)
                            blocks2write++;

                        for ( int block=0; block<blocks2write; block ++)
                            {
                                int hex_bytes_read=in.read(flash_buff,0,send_buff_size);
                               
                                flash_msgs[msg_pos]="bl:" + block + "/" + blocks2write + " si:"+hex_bytes_read ;


                                writer.write('B');
                                writer.write((hex_bytes_read>>8)& 0xFF);
                                writer.write((hex_bytes_read)& 0xFF);
                                writer.write('F');
                                writer.flush();

                                                                       
                                writer.write(flash_buff,0,hex_bytes_read);
                                writer.flush();                                
                               

                                if (avr_sig==224)
                                    {
                                        int crc=0xFFFF;
                                        for (int crc_pos=0;crc_pos<hex_bytes_read;crc_pos++)
                                            crc=CRC16(flash_buff[crc_pos],crc);
                                        writer.write(crc>>8);
                                        writer.write(crc&0xff);
                                        writer.flush();
                                    }
                                        //  flash_msgs[msg_pos]+="ok";
                                        //                              writer.flush();
                                       


                                if (reader.read()!=0x0d)
                                        throw new Exception("abort write at block"+block);
                                       
                                       
                               
                                //                             sleep(1000);
                                                    }
                        //              flash_msgs[msg_pos]="bl:" + block + "/" + blocks2write + " si:"+hex_bytes_read ;
                        /*

                        int inp=0;
                        int block=0;
                        while (inp!=-1)
                            {
                                int flash_buff_pos=0;
                                int crc=0xFFFF;
                               
                                while ((flash_buff_pos<send_buff_size)&&(inp!=-1))
                                    {
                                        inp=in.read();
                                        if (inp!=-1)
                                            {
                                                crc=CRC16(inp,crc);
                                                flash_buff[flash_buff_pos++]=(byte)inp;
                                            }
                                    }
                                //                              flash_msgs[msg_pos]="block" + block + "size:"+flash_buff_pos;
                               
                                block++;        
                       
                                boolean block_fin=false;


                                while(!block_fin)
                                    {
                                       
                                        writer.write('B');
                                        writer.write((flash_buff_pos>>8)& 0xFF);
                                        writer.write((flash_buff_pos)& 0xFF);
                                        writer.write('F');
                                        writer.flush();

                                        //                                      int ret_v=-1;
                                       
                                        writer.write(flash_buff,0,flash_buff_pos);
                                        flash_msgs[msg_pos]="bl:" + block + "si:"+flash_buff_pos ;
                                       
                                        writer.flush();                                
                                        //                                  flash_msgs[msg_pos]+="wtc";
                                       
                                       
                                        // append crc if navi
                                        if (avr_sig==224)
                                            {
                                                writer.write(crc>>8);
                                                writer.write(crc&0xff);
                                                writer.flush();
                                            }
                                        //  flash_msgs[msg_pos]+="ok";
                                        //                              writer.flush();
                                        //                      if (reader.read()!=0x0d)
                                        //                                  throw new Exception("abort write at block"+block);
                                       
                                       
                                        //ret_v=reader.read();
                                        //                                  flash_msgs[msg_pos]="ret"+ret_v + "crc"+crc;
                                       
                                        if (reader.read()==0x0d)
                                            block_fin=true;
                                       
                                    }

                            }
                        */

                        flash_msgs[++msg_pos]="written last block ";
                        msg_pos++;
                        flash_buff=null;

                        ufo_prober.set_to_none();
                        stats.reset();
                        version=new MKVersion();
                        System.gc();
                    }
                else // bootloader intension clear settings
                    {

                        flash_msgs[msg_pos]="reset params ..";
                        writer.write('B');
                        writer.write(0);
                        writer.write(4);
                        writer.write('E');
                        writer.flush();
                       
                        writer.write(0xFF);
                        writer.write(0xFF);
                        writer.write(0xFF);
                        writer.write(0xFF);
                        writer.flush();
                        flash_msgs[msg_pos++]+=" done";
                    }

            flash_msgs[msg_pos++]="Exiting Bootloader" ;
            params=new MKParamsParser();
            try{
                writer.write('E');
                writer.flush();
            }
            catch (Exception e)  {  
                flash_msgs[msg_pos++]="cant exit bootloader" ;
            }
            flash_msgs[msg_pos++]="Exit BL done" ;         


            }
           
            catch (Exception e)  {  
                flash_msgs[msg_pos++]="Fail:" +e.getMessage() ;


            flash_msgs[msg_pos++]="Exiting Bootloader" ;
            params=new MKParamsParser();
            try{
                writer.write('E');
                writer.flush();
            }
            catch (Exception e2)  {  
                flash_msgs[msg_pos++]="cant exit bootloader" ;
            }
            flash_msgs[msg_pos++]="Exit BL done" ;



                close_connections(false);
            }


            sending=false;
            }


        byte[] data_set=new byte[1024];
        int data_set_pos=0;
        byte[] data_in_buff=new byte[2048];
       
        int input;
        int pos=0;





        log("Thread started");
        while(true)
            {
               
                if (!connected)
                    {
                        if (!force_disconnect) connect();
                        sleep(100);
                    }
                else
                    try{
                       
                        /*             
                                while(sending)
                                {try { Thread.sleep(50); }
                                catch (Exception e)  {   }
                                }
                        */

                       
                        recieving=true;
                        int read_count =reader.read(data_in_buff,0,reader.available());
                        log("Connected - reading data " + read_count);         
                        //      pos=0;
                        input=0;
                        //data_buff[data_buff_pos]="";
                        // recieve data-set
                        if (read_count==0) sleep(50);

                        //                      int read_count =reader.read(data_in_buff,0,reader.available());
                        stats.bytes_in+=read_count;
                        if (read_count>0)
                            {
                                log("read" + read_count + " ds_pos" + data_set_pos);           
                       
                                for ( pos=0;pos<read_count;pos++)
                                    {
                                        if (data_in_buff[pos]==13)
                                            {
                                                data_buff[data_buff_pos]=""+data_buff_pos+ "--"+new String(data_set, 0, data_set_pos);
                                                data_buff_pos++;
                                                data_buff_pos%=DATA_BUFF_LEN;

                                               
                                                try{process_data(data_set,data_set_pos); }
                                                catch (Exception e)
                                                    {                  
                                                        log(".. problem processing");
                                                        log(e.toString());
                                                        }




                                                proxy.write(data_set,0,data_set_pos);
                                                //                                                      proxy.writer.write('\r');
                                                //proxy.writer.write('\n');
                                                //proxy.writer.flush();
                                                /*
                                                if (proxy!=null)
                                                    {
                                                       


                                                    }
                                                */

                                                data_set_pos=0;

                                            }
                                        else
                                            {
                                                data_set[data_set_pos++]=data_in_buff[pos];
                                               
                                               

                                                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'))
                                                    {
                                                        bootloader_stage= BOOTLOADER_STAGE_GOT_MKBL;
                                                        return;
                                                    }
                                               
                                            }

                                    }
                                       
                       
                            }
                        else
                            sleep(50);
                        /*
                        while ((input != 13)) //&&(input!=-1))
                            {
                                {
                                    //log("pre read");         
                                    log(""+reader.available());
                                    input = reader.read() ;
                                    log("Byte rcv" + input +"pos"+ pos);               
                                   
                                    proxy.write(input);
                                   
                                    data_buff[data_buff_pos]+=(char)input;
                                   
                                    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")))
                                        {
                                            bootloader_stage= BOOTLOADER_STAGE_GOT_MKBL;
                                            return;
                                        }
                                    if (input==-1) throw new Exception("disconnect");
                                    else
                                        {
                                            stats.bytes_in++;
                                            data_set[pos]=input;
                                            pos++;
                                        }
                                }
                               
                            }
                       
                       

                        data_buff_pos++;
                        data_buff_pos%=DATA_BUFF_LEN;
                        recieving=false;
                        log("Data recieved (" + pos + "Bytes)");               
                        log("processing ..");          
                        */


                        /*
                          if (proxy!=null)
                          {
                          proxy.writer.write('\r');
                          proxy.writer.write('\n');
                          proxy.writer.flush();
                          }
                        */

                        /*if (pos>5)
                            {
                                try{process_data(data_set,pos); }
                                catch (Exception e)
                                    {                  
                                        log(".. problem processing");
                                        log(e.toString());
                                    }
                               
                                log(".. processing done");             
                            }
                        */

                    }
                    catch (Exception ex)
                        {
                            log("Problem reading from MK -> closing conn");
                            log(ex.toString());
                            // close the connection
                            close_connections(false);
                        }      
               
                // sleep a bit to  get someting more done
                //              sleep(5); //50
               
            } // while
        //      log("Leaving Communicator thread");
    } // run()
}