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                                                                                                     *

 * ChangeLog:                                                                                                                     *

 *              0.01 - initial Version ( initialize connection / main Thread with reading data from MK)                           *

 *              0.02 - reconnect after connection loss ( e.g. switching on/off )                                                  *

 *              0.03 - added send_command ( with CRC )                                                                            *

 *              0.04 - added decode64 to decode 'pseudo' BASE64                                                                   *

 *              0.05 - added get_version                                                                                          *

 *              0.06 - added parsing of DebugData                                                                                 *

 *              0.07 - Code-(Doc&&Cleanup) && initial svn commit                                                                  *                  

 *                                                                                                                                *

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

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

    // version Info from Flight Control

    public int version_major=-1;

    public int version_minor=-1;

    public int version_compatible=-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";

    int data_count=0;

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

    public MKCommunicator(String url)     // Constructor with URL string e.g. "btspp://XXXXXXXXXXXX:1" - the X-Part is the MAC-Adress of the Bluetooth-Device connected to the Fligth-Control

    {

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

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

    }

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

    private void connect()

    {

        try{

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

                reader=connection.openInputStream();

                writer=connection.openOutputStream();

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

           }

        catch (Exception ex)

            {

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

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

            }  

        get_version();

    }

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

        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

            }

    }

    MKDebugData debug_data;

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

    {

        int[] decoded_data;

        switch((char)data[2])

            {

            case 'D': // debug Data

                decoded_data=Decode64(data,3,50);

                debug_data=new MKDebugData(decoded_data);

                msg="Got Debug Data s:" + debug_data.sekunden + "zy:"+debug_data.zyklen + "ze:" + debug_data.zeit + connected + ">" + data_count;

                for (int ff=0;ff<8;ff++)

                    msg+="\n  d:" + (ff*2+1) + " " + debug_data.analog[ff*2] + " d2:" + (ff*2+2) + " "  + debug_data.analog[ff*2+1] ;

                break;

            case 'V': // Version Info

                decoded_data=Decode64(data,3,6);

                version_major=decoded_data[0];

                version_minor=decoded_data[1];

                version_compatible=decoded_data[2];

                break;

            default:

                break;

            }

        msg+=p_msg;

        msg+="OK";

        data_count++;

    }

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

                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 

                try{ reader.close(); connection.close(); }

                catch (Exception inner_ex) { }

                connected=false;

            }  

            // sleep a bit to get someting more done

            try { Thread.sleep(50); }

            catch (Exception e)  {   }

            } // while

    } // run()

}