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          

 * Project-Start: 9/2007                          

 * Version:       0.07                            

 * Mailto:        ligi@smart4mobile.de            

 * Licence:       Creative Commons / Non Commercial

 * Big Up:        Holger&Ingo      

 *

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

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

    }

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

    }

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

        send_command(0,'q',params);

    }

    // send command to FC ( add crc and pack into pseudo Base64

    public void send_command(int 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

        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

            }

    }

    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(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 (input==-1) throw new Exception("test");

                           data_set[pos]=input;

                           pos++;

                       }

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

}