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