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

 *

 *

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

package org.ligi.ufo;

//#ifdef j2me

//# import javax.microedition.io.*;

//#endif

//#ifdef android

import android.util.Log;

//#endif

import java.io.*;

public class MKCommunicator

    implements Runnable,DUBwiseDefinitions

{

    public int angle_nick=-4242;

    public int angle_roll=-4242;

    public byte bl_retrys=0;

    public boolean init_bootloader=false;

    public byte lib_version_major=0;

    public byte lib_version_minor=6;

    public String lib_version_str()

    {

        return "V"+lib_version_major+"."+lib_version_minor;

    }

    public int AngleNick()

    {

        if (ufo_prober.is_mk())

            return angle_nick;

        else    if (ufo_prober.is_navi())

            return debug_data.analog[0];

        return -1;

    }

    public int Alt() // in dm

    {

        int alt=0;

        if (ufo_prober.is_mk())

            alt=debug_data.analog[5]/3;

        else   

            if (ufo_prober.is_navi())

                alt=gps_position.Altimeter/3;

        if ( alt<0) alt=0;  // mk

        if ( alt>20000) alt=0; // navi

        return alt;

    }

    public String Alt_formated() // in dm

    {

        return "" + Alt()/10 + "m";

    }

    public int AngleRoll()

    {

        if (ufo_prober.is_mk())

            return angle_roll;

        else    if (ufo_prober.is_navi())

            return debug_data.analog[1];

        return -1;

    }

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

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

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

    public final static int DATA_BUFF_LEN = 20; // in lines

    public String[] data_buff;

    //    boolean do_log=false;

    boolean do_log=true;

    int data_buff_pos=0;

    //    public final static int DATA_IN_BUFF_SIZE=512;

    public final static int DATA_IN_BUFF_SIZE=2048;

    //    public final static int DATA_IN_BUFF_SIZE=4096;

    public byte user_intent=0;

    public final static int[] crc16_table = {

        0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,

        0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,

        0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,

        0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,

        0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,

        0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,

        0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,

        0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,

        0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,

        0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,

        0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,

        0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,

        0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,

        0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,

        0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,

        0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,

        0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,

        0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,

        0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,

        0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,

        0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,

        0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,

        0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,

        0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,

        0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,

        0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,

        0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,

        0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,

        0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,

        0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,

        0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,

        0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0

    };

    public void log(String str)

    {

//#ifdef android

        if (do_log)     Log.d("MK-Comm",str);

//#endif

//      canvas.debug.log(str);

        //      System.out.println(str);

    }

    public int CRC16(int ch, int crc)

    {

        return crc16_table[((crc >> 8) ^ (ch)) & 0xFF] ^ (crc << 8);

    }

    public int conn_time_in_s()

    {

        if (connected)

            return (int)((System.currentTimeMillis()-connection_start_time)/1000);

        else

            return 0;

    }

    public final static byte BOOTLOADER_STAGE_NONE=0;

    public final static byte BOOTLOADER_STAGE_GOT_MKBL=1;

    byte bootloader_stage= BOOTLOADER_STAGE_NONE;

    public MKLCD LCD;

    public MKVersion version;

    public MKDebugData debug_data;

    public int[] extern_control;

    public MKGPSPosition gps_position;

    public MKStickData stick_data;

    public MKParamsParser params;

    public MKWatchDog watchdog;

    public MKProxy proxy=null;

    public MKStatistics stats ;

    //    public DUBwiseDebug debug;

    public UFOProber    ufo_prober;

    public long connection_start_time=-1;

    public String error_str = null;

    public final static int FC_SLAVE_ADDR              = 'a'+1;

    public final static int NAVI_SLAVE_ADDR            = 'a'+2;

    public final static int MK3MAG_SLAVE_ADDR          = 'a'+3;

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

//#ifdef j2me

//#    private javax.microedition.io.StreamConnection connection;

//#endif

//#ifdef android

//    java.net.Socket connection;

    java.net.Socket connection;

//#endif

    private java.io.InputStream reader;    

    private java.io.OutputStream writer;    

    public String name;

    //    DUBwise root;

    private boolean sending=false;

    private boolean recieving=false;

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

    public MKCommunicator()  

    {

        data_buff=new String[DATA_BUFF_LEN];

        for (int i=0;i<DATA_BUFF_LEN;i++)

            data_buff[i]="";

        //      debug=debug_;

        //      root=root_;

        version=new MKVersion();

        debug_data=new MKDebugData();

        stick_data=new MKStickData();

        params=new MKParamsParser();

        extern_control=new int[EXTERN_CONTROL_LENGTH];

        extern_control[EXTERN_CONTROL_CONFIG]=1;

        extern_control[EXTERN_CONTROL_FRAME]=1;

        LCD= new MKLCD(this);

        watchdog=new MKWatchDog(this);

        gps_position=new MKGPSPosition();

        stats = new MKStatistics();

        proxy =new MKProxy(this);

        ufo_prober=new UFOProber();

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

    }

    public void write_raw(byte[] _data)

    {

        wait4send();

        sending=true;

        try {

        writer.write(_data,0,_data.length);

        writer.flush();

        stats.bytes_out+=_data.length;

        }

        catch ( Exception e){}

        sending=false;

    }

    public void do_proxy(String proxy_url)

    {

        proxy.connect(proxy_url);

    }

    //    int port;

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

    {

        //      port=_port;

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

        name=_name;

        force_disconnect=false;

        connected=false;

    }

    public boolean ready()

    {

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

    }

    public String get_buff(int age)

    {

        age%=DATA_BUFF_LEN;

        if (age<=data_buff_pos)

            return ""+data_buff[data_buff_pos-age];

        else

            return ""+data_buff[DATA_BUFF_LEN+data_buff_pos-age];

    }

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

    private void connect()

    {

        log("trying to connect to" + mk_url);

        try{

            // old call

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

//#ifdef android

            connection = (new java.net.Socket(mk_url.split(":")[0],Integer.parseInt(mk_url.split(":")[1])));

            //.Socket 

            reader=connection.getInputStream();

            writer=connection.getOutputStream();

            String magic="conn:foo bar\r\n";

            writer.write(magic.getBytes());

            writer.flush();

//#else

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

//#         reader=connection.openInputStream();

//#         writer=connection.openOutputStream();

//#endif

            connection_start_time=System.currentTimeMillis();

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

            stats.reset();

            log("connecting OK");

        }

        catch (Exception ex)

            {

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

                log("Problem connecting" + "\n" + ex);

            }  

    }

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

    }

    public void wait4send()

    {

        while(sending) //||recieving)

            sleep(50);

    }

    public void sleep(int time)

    {

        try { Thread.sleep(time); }

        catch (Exception e)  {   }

    }

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

    {

        stats.version_data_request_count++;

        send_command(0,'v');

    }

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

    }

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

    public void motor_test(int[] params)

    {

        stats.motortest_request_count++;

        send_command(FC_SLAVE_ADDR,'t',params);

    }

    public void send_extern_control()

    {

        stats.external_control_request_count++;

        send_command(FC_SLAVE_ADDR,'b',extern_control);

    }

    /*    public void send_keys(int[] params)

          {

          send_command(FC_SLAVE_ADDR,'k',params);

          }*/

    // get params

    public void get_params(int id)

    {

        wait4send();

        send_command(FC_SLAVE_ADDR,'q',id+1);

        stats.params_data_request_count++;

    }

   public void get_debug_name(int id)

    {

        wait4send();

        send_command(0,'a',id);

    }

    public void trigger_LCD_by_page(int page)

    {

        wait4send();

        send_command(0,'l',page);

        stats.lcd_data_request_count++;

    }

    public void trigger_debug()

    {

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

        send_command(0,'c');

    }

    public void switch_todo()

    {

        sleep(150);

        version=new MKVersion();

        LCD= new MKLCD(this);

        debug_data=new MKDebugData();

    }

    public void switch_to_fc()

    {

        wait4send();

        send_command(NAVI_SLAVE_ADDR,'u',0);

        switch_todo();

    }

    public void switch_to_mk3mag()

    {

        wait4send();

        send_command(NAVI_SLAVE_ADDR   ,'u',1);

        switch_todo();

    }

    public final static byte[] navi_switch_magic={27,27,0x55,(byte)0xAA,0,(byte)'\r'};

    public void switch_to_navi()

    {

        wait4send();

        sending=true;

        try

            {

                writer.write(navi_switch_magic);

                stats.bytes_out+=6;

                writer.flush();

            }

        catch (Exception e)  {   }

        sending=false;

        switch_todo();

    }

    public String[] flash_msgs;

        int msg_pos=0;

    public boolean bootloader_intension_flash=false;

    public boolean bootloader_finish_ok=false;

    public void jump_bootloader()

    {

        bootloader_finish_ok=false;

        msg_pos=0;

        bootloader_stage= BOOTLOADER_STAGE_NONE;

        flash_msgs=new String[100];

        flash_msgs[msg_pos++]="Initiializing Bootloader";

        wait4send();

        sending=true;

        try

            {

                int attempt=0;

                while(bootloader_stage!= BOOTLOADER_STAGE_GOT_MKBL)

                    {

                        flash_msgs[msg_pos]="attempt "+attempt;

                        attempt++;

                        send_command_nocheck((byte)FC_SLAVE_ADDR,'R',new int[0]);

                        try{

                        writer.write( 27);

                        writer.flush();

                        sleep(20);

                        writer.write( 0xAA);

                        writer.flush();

                        }

                        catch (Exception e)  { }

                        sleep((attempt%2==0)?80:800); //800

                    }

                msg_pos++;

            }

        catch (Exception e)  {  

                flash_msgs[msg_pos++]="Exception:" +e.getMessage() ;

                flash_msgs[msg_pos++]=e.toString() ;

        }

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

    }

    public void get_error_str()

    {

        send_command(NAVI_SLAVE_ADDR,'e');

    }

    public void trigger_rcdata()

    {

        send_command(FC_SLAVE_ADDR,'p');

    }

    public void write_params(int to)

    {

        params.update_backup(to);

        write_params_(to) ;

    }

    public void write_params_(int to)

    {

        wait4send();

        params.active_paramset=to;

        send_command(FC_SLAVE_ADDR,'s',params.field_bak[to]);

    }

    public void set_debug_interval(int interval)

    {

        send_command(2,'d',interval);

    }

    public void set_gpsosd_interval(int interval)

    {

        send_command(NAVI_SLAVE_ADDR,'o',interval);

    }

    public void send_command(int modul,char cmd)

    {

        send_command(modul,cmd,new int[0]);

    }

    public void send_command(int modul,char cmd,int param)

    {

        int[] params=new int[1];

        params[0]=param;

        send_command(modul,cmd,params);

    }

    public void send_command_nocheck(byte 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

        byte[] send_buff=new byte[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]=modul;

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

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

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

                send_buff[3+param_pos*4+3] = (byte)('=' + ( 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,0,send_buff.length);

                tmp_crc%=4096;

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

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

                writer.write('\r');

                stats.bytes_out+=send_buff.length+3;

                writer.flush();

            }

        catch (Exception e)

            { // problem sending data to FC

            }

    }

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

        send_command_nocheck((byte)modul,cmd,params);

        sending=false;

    }

    public int slave_addr=-1;

    public int UBatt()

    {

        if (ufo_prober.is_mk())

            return debug_data.UBatt();

        else  if (ufo_prober.is_navi())

            return gps_position.UBatt;

        return -1;

    }

    public int SenderOkay()

    {

        if (ufo_prober.is_mk())

            return debug_data.SenderOkay();

        else  if (ufo_prober.is_navi())

            return gps_position.SenderOkay;

        return -1;

    }

    public    int[][] debug_buff=null;

    public    int     debug_buff_off=0;

    public    int     debug_buff_len=0;

    public    int    debug_buff_interval=0;

    public    int    debug_buff_lastset=0;

    public    int    debug_buff_max=1;

    public    int[] debug_buff_targets=null;

    public void setup_debug_buff(int[] targets,int len,int interval)

    {

        debug_buff=new int[len][targets.length];

        debug_buff_off=0;

        debug_buff_len=len;

        debug_buff_interval=interval;

        if (debug_buff_interval<2)debug_buff_interval=2;

        debug_buff_targets=targets;

        debug_buff_max=1;

        debug_buff_lastset=0;

    }

    public int chg_debug_max(int val)

    {

        if (val>debug_buff_max)

            debug_buff_max=val;

        if (-val>debug_buff_max)

            debug_buff_max=-val;

        return val;

    }

    public void destroy_debug_buff()

    {

        debug_buff_targets=null;

    }

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

    {

        slave_addr=data[1];

        log("command " +(char)data[2] );               

        switch((char)data[2])

            {

            case 'A': // debug Data Names

                stats.debug_names_count++;

                debug_data.set_names_by_mk_data(Decode64(data,3,len-3));

                break;

            case 'B': // external_control confirm frames

                stats.external_control_confirm_frame_count++;

                break;

            case 'L': // LCD Data

                stats.lcd_data_count++;

                LCD.handle_lcd_data(Decode64(data,3,len-3));

                break;

            case 'D': // debug Data

                log("got debug data");

                stats.debug_data_count++;

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

                if (ufo_prober.is_mk())

                    stats.process_mkflags(debug_data.motor_val(0)); // TODO remove dirty hack

                if (debug_buff_targets!=null)

                    {

                        for (int sp=0;sp<debug_buff_targets.length;sp++)

                            debug_buff[debug_buff_off][sp]=chg_debug_max(debug_data.analog[debug_buff_targets[sp]]);

                        if (debug_buff_off>debug_buff_lastset)

                            debug_buff_lastset=debug_buff_off;

                        debug_buff_off=(debug_buff_off+1)%debug_buff_len;

                    }

                log("processed debug data");

                break;

            case 'V': // Version Info

                stats.version_data_count++;

                slave_addr=data[1];

                switch(slave_addr)

                    {

                    case FC_SLAVE_ADDR:

                        ufo_prober.set_to_mk();

                        break;

                    case NAVI_SLAVE_ADDR:

                        ufo_prober.set_to_navi();

                        break;

                    case MK3MAG_SLAVE_ADDR:

                        //                      ufo_prober.set_to_mk();

                        ufo_prober.set_to_mk3mag();

                        break;

                    default:

                        ufo_prober.set_to_incompatible();

                        break;

                    }

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

                break;

            case 'w':

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