WebSVN - Projects - Diff - Rev 135 and 149 - /DUBwise/src/J2ME/src/MKCommunicator.java


/********************************************************************************************************************************
 *                                                                    
 * 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()
 
 
}
 

Rev 135	Rev 149
1	/********************************************************************************************************************************	1	/********************************************************************************************************************************
2	*	2	*
3	* Abstaction Layer to Communicate via J2ME and Bluetooth with the FlightCtrl of the MikroKopter Project (www.mikrokopter.de )	3	* Abstaction Layer to Communicate via J2ME and Bluetooth with the FlightCtrl of the MikroKopter Project (www.mikrokopter.de )
4	*	4	*
5	* Author: Marcus -LiGi- Bueschleb	5	* Author: Marcus -LiGi- Bueschleb
6	*	6	*
7	* see README for further Infos	7	* see README for further Infos
8	*	8	*
9	*	9	*
10	*******************************************************************************************************************************/	10	*******************************************************************************************************************************/
11		11
12	import javax.microedition.io.*;	12	import javax.microedition.io.*;
13	import java.io.*;	13	import java.io.*;
14		14
15		15
16	public class MKCommunicator	16	public class MKCommunicator
17	implements Runnable	17	implements Runnable
18	{	18	{
19	/*************** Section: public Attributes ********************************************/	19	/*************** Section: public Attributes ********************************************/
20	public boolean connected=false; // flag for the connection state	20	public boolean connected=false; // flag for the connection state
21	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.	21	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.
22	public String mk_url=""; // buffer the url which is given in the constuctor for reconnectin purposes	22	public String mk_url=""; // buffer the url which is given in the constuctor for reconnectin purposes
23		23
24	public MKLCD LCD;	24	public MKLCD LCD;
25	public MKVersion version;	25	public MKVersion version;
26	public MKDebugData debug_data;	26	public MKDebugData debug_data;
27		27
28	public MKGPSPosition gps_position;	28	public MKGPSPosition gps_position;
29		29
30	public MKStickData stick_data;	30	public MKStickData stick_data;
31	public MKParamsParser params;	31	public MKParamsParser params;
32	public MKWatchDog watchdog;	32	public MKWatchDog watchdog;
33	public MKProxy proxy=null;	33	public MKProxy proxy=null;
34		34
35	public long connection_start_time=-1;	35	public long connection_start_time=-1;
36		36
37		37
38	/**************** Section: private Attributes ********************************************/	38	/**************** Section: private Attributes ********************************************/
39	private javax.microedition.io.StreamConnection connection;	39	private javax.microedition.io.StreamConnection connection;
40	private java.io.InputStream reader;	40	private java.io.InputStream reader;
41	private java.io.OutputStream writer;	41	private java.io.OutputStream writer;
42		42
43		43
44	// temp - to be removed	44	// temp - to be removed
45	String p_msg="--";	45	String p_msg="--";
46	public String msg="BT_INIT";	46	public String msg="BT_INIT";
47		-
48		-
49		47
50	// for statistics	48	// for statistics
51	public int debug_data_count=0;	49	public int debug_data_count=0;
52	public int debug_names_count=0;	50	public int debug_names_count=0;
53	public int angle_data_count=0;	51	public int angle_data_count=0;
54	public int version_data_count=0;	52	public int version_data_count=0;
55	public int other_data_count=0;	53	public int other_data_count=0;
56	public int lcd_data_count=0;	54	public int lcd_data_count=0;
57	public int params_data_count=0;	55	public int params_data_count=0;
58	public int navi_data_count=0;	56	public int navi_data_count=0;
59		57
-		58
-		59	public int bytes_in_count=0;
-		60	public int bytes_out_count=0;
-		61
60		62
61	String name;	63	String name;
62	DUBwise root;	64	DUBwise root;
63		65
64		66
65	DUBwiseDebug debug;	67	DUBwiseDebug debug;
66	/**************** Section: public Methods **********************************************/	68	/**************** Section: public Methods **********************************************/
67	public MKCommunicator(DUBwise root_,DUBwiseDebug debug_)	69	public MKCommunicator(DUBwise root_,DUBwiseDebug debug_)
68	{	70	{
69	debug=debug_;	71	debug=debug_;
70	root=root_;	72	root=root_;
71	version=new MKVersion();	73	version=new MKVersion();
72	debug_data=new MKDebugData();	74	debug_data=new MKDebugData();
73	stick_data=new MKStickData();	75	stick_data=new MKStickData();
74	params=new MKParamsParser();	76	params=new MKParamsParser();
75	LCD= new MKLCD(this);	77	LCD= new MKLCD(this);
76	watchdog=new MKWatchDog(this);	78	watchdog=new MKWatchDog(this);
77	gps_position=new MKGPSPosition();	79	gps_position=new MKGPSPosition();
-		80	proxy =new MKProxy();
78	new Thread( this ).start(); // fire up main Thread	81	new Thread( this ).start(); // fire up main Thread
79	}	82	}
80		83
-		84
81		85
82	public void do_proxy(String proxy_url)	86	public void do_proxy(String proxy_url)
83	{	87	{
84	proxy=new MKProxy(proxy_url);	88	proxy.connect(proxy_url);
85	}	89	}
86		90
87		91
88	// URL string: "btspp://XXXXXXXXXXXX:1" - the X-Part is the MAC-Adress of the Bluetooth-Device connected to the Fligth-Control	92	// URL string: "btspp://XXXXXXXXXXXX:1" - the X-Part is the MAC-Adress of the Bluetooth-Device connected to the Fligth-Control
89	public void connect_to(String _url,String _name)	93	public void connect_to(String _url,String _name)
90	{	94	{
91	mk_url=_url; // remember URL for connecting / reconnecting later	95	mk_url=_url; // remember URL for connecting / reconnecting later
92	name=_name;	96	name=_name;
93	force_disconnect=false;	97	force_disconnect=false;
94	connected=false;	98	connected=false;
95	}	99	}
96		100
97	public boolean ready()	101	public boolean ready()
98	{	102	{
99	return (connected&&(version.major!=-1));	103	return (connected&&(version.major!=-1));
100	}	104	}
101	/**************** Section: private Methods **********************************************/	105	/**************** Section: private Methods **********************************************/
102	private void connect()	106	private void connect()
103	{	107	{
104	System.out.println("trying to connect to" + mk_url);	108	System.out.println("trying to connect to" + mk_url);
105	try{	109	try{
106	connection = (StreamConnection) Connector.open(mk_url);	110	connection = (StreamConnection) Connector.open(mk_url);
107	// old call	111	// old call
108	// connection = (StreamConnection) Connector.open(mk_url, Connector.READ_WRITE);	112	// connection = (StreamConnection) Connector.open(mk_url, Connector.READ_WRITE);
109	reader=connection.openInputStream();	113	reader=connection.openInputStream();
110	writer=connection.openOutputStream();	114	writer=connection.openOutputStream();
111		115
112	//	116	//
113	String magic="\rmk-mode\r";	117	//String magic="\rmk-mode\r";
114	writer.write(magic.getBytes());	118	//writer.write(magic.getBytes());
115	writer.flush();	119	//writer.flush();
116	//	120	//
117		121
118		122
119	connection_start_time=System.currentTimeMillis();	123	connection_start_time=System.currentTimeMillis();
120	connected=true; // if we get here everything seems to be OK	124	connected=true; // if we get here everything seems to be OK
121	get_version();	125	get_version();
122	lcd_data_count=0;	126	lcd_data_count=0;
123	debug_data_count=0;	127	debug_data_count=0;
124	version_data_count=0;	128	version_data_count=0;
125		129
126		130
127	}	131	}
128	catch (Exception ex)	132	catch (Exception ex)
129	{	133	{
130	// TODO difference fatal errors from those which will lead to reconnection	134	// TODO difference fatal errors from those which will lead to reconnection
131	msg="Problem connecting" + "\n" + ex;	135	msg="Problem connecting" + "\n" + ex;
132	System.out.println("problem connecting " + ex);	136	System.out.println("problem connecting " + ex);
133	}	137	}
134		138
135		139
136		140
137	}	141	}
138		142
139	public int[] Decode64(int[] in_arr, int offset,int len)	143	public int[] Decode64(int[] in_arr, int offset,int len)
140	{	144	{
141	int ptrIn=offset;	145	int ptrIn=offset;
142	int a,b,c,d,x,y,z;	146	int a,b,c,d,x,y,z;
143	int ptr=0;	147	int ptr=0;
144		148
145	int[] out_arr=new int[len];	149	int[] out_arr=new int[len];
146		150
147	while(len!=0)	151	while(len!=0)
148	{	152	{
149	a=0;	153	a=0;
150	b=0;	154	b=0;
151	c=0;	155	c=0;
152	d=0;	156	d=0;
153	try {	157	try {
154	a = in_arr[ptrIn++] - '=';	158	a = in_arr[ptrIn++] - '=';
155	b = in_arr[ptrIn++] - '=';	159	b = in_arr[ptrIn++] - '=';
156	c = in_arr[ptrIn++] - '=';	160	c = in_arr[ptrIn++] - '=';
157	d = in_arr[ptrIn++] - '=';	161	d = in_arr[ptrIn++] - '=';
158	}	162	}
159	catch (Exception e) {}	163	catch (Exception e) {}
160	//if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden	164	//if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden
161		165
162	x = (a << 2) \| (b >> 4);	166	x = (a << 2) \| (b >> 4);
163	y = ((b & 0x0f) << 4) \| (c >> 2);	167	y = ((b & 0x0f) << 4) \| (c >> 2);
164	z = ((c & 0x03) << 6) \| d;	168	z = ((c & 0x03) << 6) \| d;
165		169
166	if((len--)!=0) out_arr[ptr++] = x; else break;	170	if((len--)!=0) out_arr[ptr++] = x; else break;
167	if((len--)!=0) out_arr[ptr++] = y; else break;	171	if((len--)!=0) out_arr[ptr++] = y; else break;
168	if((len--)!=0) out_arr[ptr++] = z; else break;	172	if((len--)!=0) out_arr[ptr++] = z; else break;
169	}	173	}
170		174
171	return out_arr;	175	return out_arr;
172		176
173	}	177	}
174		178
175	// FC - Function Mappers	179	// FC - Function Mappers
176		180
177	// send a version Request to the FC - the reply to this request will be processed in process_data when it arrives	181	// send a version Request to the FC - the reply to this request will be processed in process_data when it arrives
178	public void set_gps_target(int longitude,int latitude)	182	public void set_gps_target(int longitude,int latitude)
179	{	183	{
180	int[] target=new int[8];	184	int[] target=new int[8];
181	target[0]= (0xFF)&(longitude<<24);	185	target[0]= (0xFF)&(longitude<<24);
182	target[1]= (0xFF)&(longitude<<16);	186	target[1]= (0xFF)&(longitude<<16);
183	target[2]= (0xFF)&(longitude<<8);	187	target[2]= (0xFF)&(longitude<<8);
184	target[3]= (0xFF)&(longitude);	188	target[3]= (0xFF)&(longitude);
185		-
186	send_command(0,'s',target);	189	// send_command(0,'s',target);
187	}	190	}
188		191
189		192
190	public void get_version()	193	public void get_version()
191	{	194	{
192	send_command(0,'v',new int[0]);	195	send_command(0,'v',new int[0]);
193	}	196	}
194		197
195	// send a MotorTest request - params are the speed for each Motor	198	// send a MotorTest request - params are the speed for each Motor
196	public void motor_test(int[] params)	199	public void motor_test(int[] params)
197	{	200	{
198	send_command(0,'t',params);	201	send_command(0,'t',params);
199	}	202	}
200		203
201	public void send_keys(int[] params)	204	public void send_keys(int[] params)
202	{	205	{
203	send_command(0,'k',params);	206	send_command(0,'k',params);
204	}	207	}
205		208
206	// get params	209	// get params
207	public void get_params(int id)	210	public void get_params(int id)
208	{	211	{
209	int[] params=new int[1];	212	int[] params=new int[1];
210	params[0]=id;	213	params[0]=id;
211		214
212	while(sending\|\|recieving)	215	while(sending\|\|recieving)
213	{try { Thread.sleep(50); }	216	{try { Thread.sleep(50); }
214	catch (Exception e) { }	217	catch (Exception e) { }
215	}	218	}
216		219
217	send_command(0,'q',params);	220	send_command(0,'q',params);
218	}	221	}
219		222
220		223
221	public void get_debug_name(int id)	224	public void get_debug_name(int id)
222	{	225	{
223	int[] params=new int[1];	226	int[] params=new int[1];
224	params[0]=id;	227	params[0]=id;
225		228
226	while(sending\|\|recieving)	229	while(sending\|\|recieving)
227	{try { Thread.sleep(50); }	230	{try { Thread.sleep(50); }
228	catch (Exception e) { }	231	catch (Exception e) { }
229	}	232	}
230		233
231	send_command(0,'a',params);	234	send_command(0,'a',params);
232	}	235	}
233		236
-		237
-		238
-		239
-		240	public void trigger_debug()
-		241	{
-		242	if (sending\|\|recieving) return; // its not that important - can be dropped
-		243
-		244	int[] params=new int[0];
-		245	send_command(0,'c',params);
-		246	}
-		247
-		248
-		249
-		250	public void switch_to_fc()
-		251	{
-		252
-		253
-		254	while(sending\|\|recieving)
-		255	{
-		256	try { Thread.sleep(50); }
-		257	catch (Exception e) { }
-		258	}
-		259
-		260
-		261
-		262	int[] params=new int[1];
-		263	params[0]=0;
-		264	send_command(0,'u',params);
-		265
-		266	try { Thread.sleep(50); }
-		267	catch (Exception e) { }
-		268
-		269	version=new MKVersion();
-		270	LCD= new MKLCD(this);
-		271
-		272	}
-		273
-		274
-		275	public void switch_to_navi()
-		276
-		277	{
-		278
-		279	while(sending\|\|recieving)
-		280	{try { Thread.sleep(50); }
-		281	catch (Exception e) { }
-		282	}
-		283
-		284
-		285	sending=true;
-		286	try
-		287	{
-		288	writer.write( 27);
-		289	writer.write( 27);
-		290	writer.write( 0x55);
-		291	writer.write( 0xaa);
-		292	writer.write( 0);
-		293	writer.write('\r');
-		294	bytes_out_count+=6;
-		295	writer.flush();
-		296	}
-		297	catch (Exception e) { }
-		298	sending=false;
-		299
-		300	try { Thread.sleep(50); }
-		301	catch (Exception e) { }
-		302	version=new MKVersion();
-		303	LCD= new MKLCD(this);
-		304	}
234		305
235		306
236		307
237	public void trigger_LCD(int key)	308	public void trigger_LCD(int key)
238	{	309	{
239	while(sending\|\|recieving)	310	while(sending\|\|recieving) // check if not only sending matters
240	{try { Thread.sleep(50); }	311	{try { Thread.sleep(50); }
241	catch (Exception e) { }	312	catch (Exception e) { }
242	}	313	}
243		314
244	//if (sending\|\|recieving) return;	315	//if (sending\|\|recieving) return;
245		316
246	int[] params=new int[3];	317	int[] params=new int[3];
247	params[0]=key;	318	params[0]=key;
248	params[1]=0;	319	params[1]=0;
249	params[2]=0;	320	params[2]=0;
250		321
251	send_command(0,'h',params);	322	send_command(0,'h',params);
252	}	323	}
253		324
254		325
255	public void write_params()	326	public void write_params()
256	{	327	{
-		328	params.update_backup();
257	while(sending\|\|recieving)	329	while(sending\|\|recieving)
258	{try { Thread.sleep(50); }	330	{try { Thread.sleep(50); }
259	catch (Exception e) { }	331	catch (Exception e) { }
260	}	332	}
261		333
262	send_command(0,(char)('l'+params.act_paramset),params.field[params.act_paramset]);	334	send_command(0,(char)('l'+params.act_paramset),params.field[params.act_paramset]);
263	}	335	}
264		336
265		337
266	boolean sending=false;	338	boolean sending=false;
267	boolean recieving=false;	339	boolean recieving=false;
268		340
269	// send command to FC ( add crc and pack into pseudo Base64	341	// send command to FC ( add crc and pack into pseudo Base64
270	public void send_command(int modul,char cmd,int[] params)	342	public void send_command(int modul,char cmd,int[] params)
271	{	343	{
-		344
272	// if (modul==0) return;	345	// if (modul==0) return;
273	sending=true;	346	sending=true;
-		347	// char[] send_buff=new char[5 + (params.length/3 + (params.length%3==0?0:1) )4]; // 5=1start_char+1addr+1cmd+2*crc
-		348
274	char[] send_buff=new char[5 + (params.length/3 + (params.length%3==0?0:1) )4]; // 5=1start_char+1addr+1cmd+2*crc	349	char[] send_buff=new char[3 + (params.length/3 + (params.length%3==0?0:1) )4]; // 5=1start_char+1addr+1cmd+2*crc
275	send_buff[0]='#';	350	send_buff[0]='#';
276	send_buff[1]=(char)modul;	351	send_buff[1]=(char)modul;
277	send_buff[2]=cmd;	352	send_buff[2]=cmd;
278		353
279	for(int param_pos=0;param_pos<(params.length/3 + (params.length%3==0?0:1)) ;param_pos++)	354	for(int param_pos=0;param_pos<(params.length/3 + (params.length%3==0?0:1)) ;param_pos++)
280	{	355	{
281	int a = (param_pos3<params.length)?params[param_pos3]:0;	356	int a = (param_pos3<params.length)?params[param_pos3]:0;
282	int b = ((param_pos3+1)<params.length)?params[param_pos3+1]:0;	357	int b = ((param_pos3+1)<params.length)?params[param_pos3+1]:0;
283	int c = ((param_pos3+2)<params.length)?params[param_pos3+2]:0;	358	int c = ((param_pos3+2)<params.length)?params[param_pos3+2]:0;
284		359
285	send_buff[3+param_pos*4] = (char)((a >> 2)+'=' );	360	send_buff[3+param_pos*4] = (char)((a >> 2)+'=' );
286	send_buff[3+param_pos*4+1] = (char)('=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4)));	361	send_buff[3+param_pos*4+1] = (char)('=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4)));
287	send_buff[3+param_pos*4+2] = (char)('=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6)));	362	send_buff[3+param_pos*4+2] = (char)('=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6)));
288	send_buff[3+param_pos*4+3] = (char)('=' + ( c & 0x3f));	363	send_buff[3+param_pos*4+3] = (char)('=' + ( c & 0x3f));
289		364
290	//send_buff[3+foo]='=';	365	//send_buff[3+foo]='=';
291	}	366	}
292		367
293	/* for(int foo=0;foo<(params.length/3 + (params.length%3==0?0:1) )*4;foo++)	368	/* for(int foo=0;foo<(params.length/3 + (params.length%3==0?0:1) )*4;foo++)
294	{	369	{
295	int a = (foo<params.length) params[foo];	370	int a = (foo<params.length) params[foo];
296	int a = params[foo];	371	int a = params[foo];
297		372
298	//send_buff[3+foo]='=';	373	//send_buff[3+foo]='=';
299	}	374	}
300	*/	375	*/
301	try	376	try
302	{	377	{
303	int tmp_crc=0;	378	int tmp_crc=0;
304	for ( int tmp_i=0; tmp_i<send_buff.length;tmp_i++)	379	for ( int tmp_i=0; tmp_i<send_buff.length;tmp_i++)
305	{	380	{
306	tmp_crc+=(int)send_buff[tmp_i];	381	tmp_crc+=(int)send_buff[tmp_i];
307	writer.write(send_buff[tmp_i]);	382	writer.write(send_buff[tmp_i]);
-		383	bytes_out_count++;
308	}	384	}
309	tmp_crc%=4096;	385	tmp_crc%=4096;
310	writer.write( (char)(tmp_crc/64 + '='));	386	writer.write( (char)(tmp_crc/64 + '='));
311	writer.write( (char)(tmp_crc%64 + '='));	387	writer.write( (char)(tmp_crc%64 + '='));
312	writer.write('\r');	388	writer.write('\r');
-		389	bytes_out_count+=3;
313	writer.flush();	390	writer.flush();
314	}	391	}
315	catch (Exception e)	392	catch (Exception e)
316	{ // problem sending data to FC	393	{ // problem sending data to FC
317	}	394	}
318		395
319	sending=false;	396	sending=false;
320	}	397	}
321		398
322		399
323	public int slave_addr=-1;	400	public int slave_addr=-1;
324		401
325	public void process_data(int[] data,int len)	402	public void process_data(int[] data,int len)
326	{	403	{
327		404
328	slave_addr=data[1];	405	slave_addr=data[1];
329		406
330	switch((char)data[2])	407	switch((char)data[2])
331	{	408	{
332		409
333	case 'D': // debug Data	410	case 'D': // debug Data
334	debug_data_count++;	411	debug_data_count++;
335	debug_data.set_by_mk_data(Decode64(data,3,len-3),version);	412	debug_data.set_by_mk_data(Decode64(data,3,len-3),version);
336	break;	413	break;
337		414
338	case 'A': // debug Data Names	415	case 'A': // debug Data Names
339	debug_names_count++;	416	debug_names_count++;
340	debug_data.set_names_by_mk_data(data[1]-'0',Decode64(data,3,len-3));	417	debug_data.set_names_by_mk_data(data[1]-'0',Decode64(data,3,len-3));
341	break;	418	break;
342		419
343	case 'V': // Version Info	420	case 'V': // Version Info
344	version_data_count++;	421	version_data_count++;
345	version.set_by_mk_data(Decode64(data,3,6));	422	version.set_by_mk_data(Decode64(data,3,6));
-		423	if (version.minor>60) // FIXME - NO good detection
-		424	root.canvas.ufo_prober.set_to_mk();
346	break;	425	break;
347		426
348	case '0':	427	case '0':
349	case '1':	428	case '1':
350	case '2':	429	case '2':
351	case '3':	430	case '3':
352	lcd_data_count++;	431	lcd_data_count++;
353	LCD.handle_lcd_data(Decode64(data,3,20),data[2]-(int)'0');	432	LCD.handle_lcd_data(Decode64(data,3,20),data[2]-(int)'0');
354		433
355	break;	434	break;
356	case '4':	435	case '4':
357	stick_data.set_by_mk_data(Decode64(data,3,20));	436	stick_data.set_by_mk_data(Decode64(data,3,20));
358	String tmp_s="";	437	String tmp_s="";
359	for (int tmp_c=0;tmp_c<10;tmp_c++)	438	for (int tmp_c=0;tmp_c<10;tmp_c++)
360	tmp_s+="s"+tmp_c+"v"+stick_data.stick[tmp_c]+" ";	439	tmp_s+="s"+tmp_c+"v"+stick_data.stick[tmp_c]+" ";
361	debug.log(tmp_s);	440	debug.log(tmp_s);
362	break;	441	break;
363		442
364	case 'L':	443	case 'L':
365	case 'M':	444	case 'M':
366	case 'N':	445	case 'N':
367	case 'O':	446	case 'O':
368	case 'P':	447	case 'P':
369	//int[] foo=Decode64(data,3,len-3);	448	//int[] foo=Decode64(data,3,len-3);
370	params.set_by_mk_data((int)(data[2]-'L'),Decode64(data,3,len-3),version);	449	params.set_by_mk_data((int)(data[2]-'L'),Decode64(data,3,len-3),version);
371	params_data_count++;	450	params_data_count++;
372	break;	451	break;
373		452
374		453
375		454
376	// data from navi	455	// data from navi
377	case 'Q':	456	case 'Q':
378	navi_data_count++;	457	navi_data_count++;
379	debug.log("got navi data(" + len +"):");	458	debug.log("got navi data(" + len +"):");
380		459
381		460
382	gps_position.set_by_mk_data(Decode64(data,3,len-3),version);	461	gps_position.set_by_mk_data(Decode64(data,3,len-3),version);
383		462
384	debug.log("long:" + gps_position.Longitude);	463	debug.log("long:" + gps_position.Longitude);
385	debug.log("lat:" + gps_position.Latitude);	464	debug.log("lat:" + gps_position.Latitude);
386		465
387	root.canvas.ufo_prober.set_to_navi();	466	root.canvas.ufo_prober.set_to_navi();
388	break;	467	break;
389		468
390		469
391	case 'w':	470	case 'w':
392	angle_data_count++;	471	angle_data_count++;
393	root.canvas.ufo_prober.set_to_mk();	472	// root.canvas.ufo_prober.set_to_mk();
394	debug.log("got angle data:");	473	debug.log("got angle data");
395		474
396		475
397	break;	476	break;
398		477
399	default:	478	default:
400	other_data_count++;	479	other_data_count++;
401	debug.log("got other data:"+ (char)data[2] + "=>" + (byte)data[2]);	480	debug.log("got other data:"+ (char)data[2] + "=>" + (byte)data[2]);
402		481
403	/*	482	/*
404	String tmp_str="";	483	String tmp_str="";
405	for (int tmp_i=0;tmp_i<len;tmp_i++)	484	for (int tmp_i=0;tmp_i<len;tmp_i++)
406	tmp_str+=(char)data[tmp_i];	485	tmp_str+=(char)data[tmp_i];
407	debug.log(tmp_str);*/	486	debug.log(tmp_str);*/
408	break;	487	break;
409		488
410	}	489	}
411		490
412		491
413		492
414		493
415	}	494	}
416		495
417	String o_msg="";	496	String o_msg="";
418		497

Subversion Repositories Projects

(root)/DUBwise/src/J2ME/src/MKCommunicator.java - Rev 135 → 149