| 0,0 → 1,233 |
|---|
| /********************************************************************************************************************************* |
| * * |
| * 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() |
| |
| |
| } |