Subversion Repositories FlightCtrl

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

    public int debug_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;



    String name;

    /******************  Section: public Methods ************************************************/
    public MKCommunicator()  
    {
        version=new MKVersion();
        debug_data=new MKDebugData();
        params=new MKParamsParser();
        //      mk_url=url; // remember URL for connecting / reconnecting later

                LCD= new MKLCD(this);
                watchdog=new MKWatchDog(this);
        new Thread( this ).start(); // fire up main Thread
    }


    public void do_proxy(String proxy_url)
    {
        proxy=new MKProxy(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;
        name=_name;
        force_disconnect=false;
        connected=false;
    }

    /******************  Section: private Methods ************************************************/
    private void connect()
    {
       
        try{
                connection = (StreamConnection) Connector.open(mk_url, Connector.READ_WRITE);
                reader=connection.openInputStream();
                writer=connection.openOutputStream();
               
                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;
            }  



    }

   

    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 = in_arr[ptrIn++] - '=';
                b = in_arr[ptrIn++] - '=';
                c = in_arr[ptrIn++] - '=';
                d = in_arr[ptrIn++] - '=';
                //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 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);
    }

    // get params
    public void get_params(int id)
    {
        int[] params=new int[1];
        params[0]=id;

        while(sending)
            {try { Thread.sleep(50); }
            catch (Exception e)  {   }
            }

        send_command(0,'q',params);
    }

    public void trigger_LCD(int key)
    {
        if (sending) return;

       
        int[] params=new int[3];
        params[0]=key;
        params[1]=0;
        params[2]=0;
       
        send_command(0,'h',params);
       
    }


    public void write_params()
    {
        while(sending)
            {try { Thread.sleep(50); }
            catch (Exception e)  {   }
            }

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


    boolean sending=false;

    // send command to FC ( add crc and pack into pseudo Base64
    public void send_command(int modul,char cmd,int[] params)
    {
        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
        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]);
                    }
                tmp_crc%=4096;
                writer.write( (char)(tmp_crc/64 + '='));
                writer.write( (char)(tmp_crc%64 + '='));
                writer.write('\r');
                writer.flush();
            }
        catch (Exception e)
            { // problem sending data to FC
            }

        sending=false;
    }


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

       
        switch((char)data[2])
            {
           
            case 'D': // debug Data
                debug_data_count++;
                debug_data.set_by_mk_data(Decode64(data,3,50),version);
                break;
               
            case 'V': // Version Info
                version_data_count++;
                version.set_by_mk_data(Decode64(data,3,6));
                break;
               
            case '0':
            case '1':
            case '2':
            case '3':
                LCD.handle_lcd_data(Decode64(data,3,20),data[2]-(int)'0');
                lcd_data_count++;
                break;

            case 'L':
            case 'M':
            case 'N':
            case 'O':
            case 'P':
                params.set_by_mk_data((int)(data[2]-'L'),Decode64(data,3,58),version);
                params_data_count++;
                break;

            default:
                other_data_count++;
                break;

            }



       
    }

    String o_msg="";

    public boolean force_disconnect=true;

    public void close_connections(boolean force)
    {
        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[150];
        int input;
        int pos=0;
        msg+="!!run started!!";
        while(true)
            {
            if (!connected)
                {
                   if (!force_disconnect) connect();
                }
            else
                try{
               
                    pos=0;
                    input=0;
                    // recieve data-set
                    while ((input != 13)) // &&(input!=-1))
                       {

                           input = reader.read() ;
                           if (proxy!=null)
                               proxy.writer.write(input);
                           if (input==-1) throw new Exception("test");
                           data_set[pos]=input;
                           pos++;
     
                       }

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

                    process_data(data_set,pos);
                   
                   }
        catch (Exception ex)
            {
                msg="Problem reading from MK";
                // close the connection
                close_connections(false);


            }  
   
            // sleep a bit to get someting more done
            try { Thread.sleep(50); }
            catch (Exception e)  {   }
       
            } // while


    } // run()


}