Subversion Repositories Projects

/********************************************************************************************************************************

 *                                                                    

 * 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

 *

 *

 *******************************************************************************************************************************/

import javax.microedition.io.*;

import java.io.*;

public class MKCommunicator

    implements Runnable

{

    /***************** Section: public Attributes **********************************************/

    public boolean connected=false; // flag for the connection state

    public boolean fatal=false; // flag which is set when an error is so fatal that reconnecting won't be tried - e.g. unknown version number.

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

    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 long connection_start_time=-1;

    /****************** Section: private Attributes **********************************************/

    private javax.microedition.io.StreamConnection connection;

    private java.io.InputStream reader;    

    private java.io.OutputStream writer;    

    // temp - to be removed

    String p_msg="--";

    public String msg="BT_INIT";

    // for statistics

    public int debug_data_count=0;

    public int debug_names_count=0;

    public int angle_data_count=0;

    public int version_data_count=0;

    public int other_data_count=0;

    public int lcd_data_count=0;

    public int params_data_count=0;

    public int navi_data_count=0;

    public int bytes_in_count=0;

    public int bytes_out_count=0;

    String name;

    DUBwise root;

    DUBwiseDebug debug;

    /******************  Section: public Methods ************************************************/

    public MKCommunicator(DUBwise root_,DUBwiseDebug debug_)  

    {

        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();

        proxy =new MKProxy();

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

    }

    public void do_proxy(String proxy_url)

    {

        proxy.connect(proxy_url);

    }

    //  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)

    {

        mk_url=_url; // remember URL for connecting / reconnecting later

        name=_name;

        force_disconnect=false;

        connected=false;

    }

    public boolean ready()

    {

        return (connected&&(version.major!=-1));

    }

    /******************  Section: private Methods ************************************************/

    private void connect()

    {

        System.out.println("trying to connect to" + mk_url);

        try{

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

            // old call

            // connection = (StreamConnection) Connector.open(mk_url, Connector.READ_WRITE);

            reader=connection.openInputStream();

            writer=connection.openOutputStream();

            //

            //String magic="\rmk-mode\r";

            //writer.write(magic.getBytes());

            //writer.flush();

            //

            connection_start_time=System.currentTimeMillis();

            connected=true; // if we get here everything seems to be OK

            get_version();

            lcd_data_count=0;

            debug_data_count=0;

            version_data_count=0;

        }

        catch (Exception ex)

            {

                // TODO difference fatal errors from those which will lead to reconnection

                msg="Problem connecting" + "\n" + ex;

                System.out.println("problem connecting " + ex);

            }  

    }

    public int[] Decode64(int[] 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;

    }

    // 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 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);

    }

    public void get_version()

    {

        send_command(0,'v',new int[0]);

    }

    // send a MotorTest request - params are the speed for each Motor

    public void motor_test(int[] params)

    {

        send_command(0,'t',params);

    }

    public void send_keys(int[] params)

    {

        send_command(0,'k',params);

    }

    // get params

    public void get_params(int id)

    {

        int[] params=new int[1];

        params[0]=id;

        while(sending||recieving)

            {try { Thread.sleep(50); }

            catch (Exception e)  {   }

            }

        send_command(0,'q',params);

    }

    public void get_debug_name(int id)

    {

        int[] params=new int[1];

        params[0]=id;

        while(sending||recieving)

            {try { Thread.sleep(50); }

            catch (Exception e)  {   }

            }

        send_command(0,'a',params);

    }

    public void trigger_debug()

    {

        if (sending||recieving) return; // its not that important - can be dropped

        int[] params=new int[0];

        send_command(0,'c',params);

    }

    public void switch_to_fc()

    {

        while(sending||recieving)

            {

                try { Thread.sleep(50); }

                catch (Exception e)  {   }

            }

        int[] params=new int[1];

        params[0]=0;

        send_command(0,'u',params);

        try { Thread.sleep(50); }

        catch (Exception e)  {   }

        version=new MKVersion();

        LCD= new MKLCD(this);

    }

    public void switch_to_navi()

    {

        while(sending||recieving)

            {try { Thread.sleep(50); }

            catch (Exception e)  {   }

            }

        sending=true;

        try

            {

        writer.write( 27);

        writer.write( 27);

        writer.write( 0x55);

        writer.write( 0xaa);

        writer.write( 0);

        writer.write('\r');

        bytes_out_count+=6;

        writer.flush();

            }

        catch (Exception e)  {   }

        sending=false;

        try { Thread.sleep(50); }

        catch (Exception e)  {   }

        version=new MKVersion();

        LCD= new MKLCD(this);

    }

    public void trigger_LCD(int key)

    {

        while(sending||recieving) // check if not only sending matters

            {try { Thread.sleep(50); }

            catch (Exception e)  {   }

            }

        //if (sending||recieving) return;

        int[] params=new int[3];

        params[0]=key;

        params[1]=0;

        params[2]=0;

        send_command(0,'h',params);

    }

    public void write_params()

    {

        params.update_backup();

        while(sending||recieving)

            {try { Thread.sleep(50); }

            catch (Exception e)  {   }

            }

        send_command(0,(char)('l'+params.act_paramset),params.field[params.act_paramset]);

    }

    boolean sending=false;

    boolean recieving=false;

    // 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;

        //      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

        char[] send_buff=new char[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]=(char)modul;

        send_buff[2]=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] =  (char)((a >> 2)+'=' );

                send_buff[3+param_pos*4+1] = (char)('=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)));

                send_buff[3+param_pos*4+2] = (char)('=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)));

                send_buff[3+param_pos*4+3] = (char)('=' + ( 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[tmp_i]);

                        bytes_out_count++;

                    }

                tmp_crc%=4096;

                writer.write( (char)(tmp_crc/64 + '='));

                writer.write( (char)(tmp_crc%64 + '='));

                writer.write('\r');

                bytes_out_count+=3;

                writer.flush();

            }

        catch (Exception e)

            { // problem sending data to FC

            }

        sending=false;

    }

    public int slave_addr=-1;

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

    {

        slave_addr=data[1];

        switch((char)data[2])

            {

            case 'D': // debug Data

                debug_data_count++;

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

                break;

            case 'A': // debug Data Names

                debug_names_count++;

                debug_data.set_names_by_mk_data(data[1]-'0',Decode64(data,3,len-3));

                break;

            case 'V': // Version Info

                version_data_count++;

                version.set_by_mk_data(Decode64(data,3,6));

                if (version.minor>60) // FIXME - NO good detection

                    root.canvas.ufo_prober.set_to_mk();

                break;

            case '0':

            case '1':

            case '2':

            case '3':

                lcd_data_count++;

                LCD.handle_lcd_data(Decode64(data,3,20),data[2]-(int)'0');

                break;

            case '4':

                stick_data.set_by_mk_data(Decode64(data,3,20));

                String tmp_s="";

                for (int tmp_c=0;tmp_c<10;tmp_c++)

                    tmp_s+="s"+tmp_c+"v"+stick_data.stick[tmp_c]+" ";

                debug.log(tmp_s);

                break;

            case 'L':

            case 'M':

            case 'N':

            case 'O':

            case 'P':

                //int[] foo=Decode64(data,3,len-3);

                params.set_by_mk_data((int)(data[2]-'L'),Decode64(data,3,len-3),version);

                params_data_count++;

                break;

            // data from navi

            case 'Q':

                navi_data_count++;

                debug.log("got navi data(" + len +"):");

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

                debug.log("long:" + gps_position.Longitude);

                debug.log("lat:" + gps_position.Latitude);

                root.canvas.ufo_prober.set_to_navi();

                break;

            case 'w':

                angle_data_count++;

                //              root.canvas.ufo_prober.set_to_mk();

                debug.log("got angle data");

                break;

            default:

                other_data_count++;

                debug.log("got other data:"+ (char)data[2] + "=>" + (byte)data[2]);

                /*

                String tmp_str="";

                for (int tmp_i=0;tmp_i<len;tmp_i++)

                    tmp_str+=(char)data[tmp_i];

                    debug.log(tmp_str);*/

                break;

            }

    }

    String o_msg="";

    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) { }

        try{ connection.close(); }

        catch (Exception inner_ex) { }

        connected=false;

    }

    // Thread to recieve data from Connection

    public void run()

    {

        int[] data_set=new int[300];

        int input;

        int pos=0;

        debug.log("Thread started");

        while(true)

            {

                debug.log("Connection Thread run");

                if (!connected)

                    {

                        if (!force_disconnect) connect();

                    }

                else

                    try{

                        /*             

                        while(sending)

                            {try { Thread.sleep(50); }

                            catch (Exception e)  {   }

                            }

                        */

                        recieving=true;

                        debug.log("Connected - reading data");         

                        pos=0;

                        input=0;

                        // recieve data-set

                        while ((input != 13)) //&&(input!=-1))

                            {

                                debug.log("pre read");         

                                input = reader.read() ;

                                debug.log("Byte rcv" + input);         

                                proxy.write(input);

                                if (input==-1) throw new Exception("disconnect");

                                if (input!=-1)

                                    {

                                        bytes_in_count++;

                                        data_set[pos]=input;

                                        pos++;

                                    }

                            }

                        recieving=false;

                        debug.log("Data recieved (" + pos + "Bytes) - processing ..");         

                        /*

                        if (proxy!=null)

                            {

                                proxy.writer.write('\r');

                                proxy.writer.write('\n');

                                proxy.writer.flush();

                            }

                        */

                        if (pos>5)

                            process_data(data_set,pos);

                        debug.log("Processing done");          

                    }

                    catch (Exception ex)

                        {

                            debug.log("Problem reading from MK -> closing conn");

                            debug.log(ex.toString());

                            // close the connection 

                            close_connections(false);

                        }      

                // sleep a bit to  get someting more done

                try { Thread.sleep(50); }

                catch (Exception e)  {   }

            } // while

        //      debug.log("Leaving Communicator thread");

    } // run()

}