0,0 → 1,437 |
/******************************************************************************************************************************** |
* |
* 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 |
* |
* |
*******************************************************************************************************************************/ |
|
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 MKStickData stick_data; |
public MKParamsParser params; |
public MKWatchDog watchdog; |
public MKProxy proxy=null; |
|
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; |
DUBwise root; |
DUBwiseDebug debug; |
|
/****************** Section: public Methods ************************************************/ |
public MKCommunicator(DUBwise root_,DUBwiseDebug debug_) |
{ |
debug=debug_; |
root=root_; |
version=new MKVersion(); |
debug_data=new MKDebugData(); |
stick_data=new MKStickData(); |
params=new MKParamsParser(); |
LCD= new MKLCD(this); |
watchdog=new MKWatchDog(this); |
new Thread( this ).start(); // fire up main Thread |
} |
|
|
public void do_proxy(String proxy_url) |
{ |
proxy=new MKProxy(proxy_url); |
} |
|
// 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; // remember URL for connecting / reconnecting later |
name=_name; |
force_disconnect=false; |
connected=false; |
} |
|
/****************** Section: private Methods ************************************************/ |
private void connect() |
{ |
System.out.println("trying to connect to" + mk_url); |
try{ |
connection = (StreamConnection) Connector.open(mk_url); |
// old call |
// connection = (StreamConnection) Connector.open(mk_url, Connector.READ_WRITE); |
reader=connection.openInputStream(); |
writer=connection.openOutputStream(); |
|
|
// |
String magic="\rmk-mode\r"; |
writer.write(magic.getBytes()); |
writer.flush(); |
// |
|
|
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; |
System.out.println("problem connecting " + 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; |
|
} |
|
// 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() |
{ |
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); |
} |
|
public void send_keys(int[] params) |
{ |
send_command(0,'k',params); |
} |
|
// get params |
public void get_params(int id) |
{ |
int[] params=new int[1]; |
params[0]=id; |
|
while(sending) |
{try { Thread.sleep(50); } |
catch (Exception e) { } |
} |
|
send_command(0,'q',params); |
} |
|
|
public void get_debug_name(int id) |
{ |
int[] params=new int[1]; |
params[0]=id; |
|
while(sending) |
{try { Thread.sleep(50); } |
catch (Exception e) { } |
} |
|
send_command(0,'a',params); |
} |
|
|
|
|
public void trigger_LCD(int key) |
{ |
if (sending) return; |
|
|
int[] params=new int[3]; |
params[0]=key; |
params[1]=0; |
params[2]=0; |
|
send_command(0,'h',params); |
|
} |
|
|
public void write_params() |
{ |
while(sending) |
{try { Thread.sleep(50); } |
catch (Exception e) { } |
} |
|
send_command(0,(char)('l'+params.act_paramset),params.field[params.act_paramset]); |
} |
|
|
boolean sending=false; |
|
// send command to FC ( add crc and pack into pseudo Base64 |
public void send_command(int modul,char cmd,int[] params) |
{ |
sending=true; |
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 |
} |
|
sending=false; |
} |
|
|
public void process_data(int[] data,int len) |
{ |
|
switch((char)data[2]) |
{ |
|
case 'D': // debug Data |
debug_data_count++; |
debug_data.set_by_mk_data(Decode64(data,3,len-3),version); |
break; |
|
case 'A': // debug Data Names |
//debug_data_count++; |
debug_data.set_names_by_mk_data(data[1]-'0',Decode64(data,3,len-3)); |
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_data_count++; |
LCD.handle_lcd_data(Decode64(data,3,20),data[2]-(int)'0'); |
|
break; |
case '4': |
stick_data.set_by_mk_data(Decode64(data,3,20)); |
String tmp_s=""; |
for (int tmp_c=0;tmp_c<10;tmp_c++) |
tmp_s+="s"+tmp_c+"v"+stick_data.stick[tmp_c]+" "; |
debug.log(tmp_s); |
break; |
|
case 'L': |
case 'M': |
case 'N': |
case 'O': |
case 'P': |
params.set_by_mk_data((int)(data[2]-'L'),Decode64(data,3,len-3),version); |
params_data_count++; |
break; |
|
default: |
other_data_count++; |
debug.log("got other data:" + (char)data[2] + "=>" + (byte)data[2]); |
|
|
String tmp_str=""; |
for (int tmp_i=0;tmp_i<len;tmp_i++) |
tmp_str+=(char)data[tmp_i]; |
debug.log(tmp_str); |
break; |
|
} |
|
|
|
|
} |
|
String o_msg=""; |
|
public boolean force_disconnect=true; |
|
public void close_connections(boolean force) |
{ |
if ((!force)&&root.canvas.do_vibra) root.vibrate(500); |
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 (proxy!=null) |
proxy.writer.write(input); |
// if (input==-1) throw new Exception("test"); |
if (input!=-1) |
{ |
data_set[pos]=input; |
pos++; |
} |
|
} |
|
if (proxy!=null) |
{ |
proxy.writer.write('\r'); |
proxy.writer.write('\n'); |
proxy.writer.flush(); |
} |
|
if (pos>5) |
process_data(data_set,pos); |
|
} |
catch (Exception ex) |
{ |
debug.log("Problem reading from MK -> closing conn"); |
debug.log(ex.toString()); |
// close the connection |
close_connections(false); |
|
|
} |
|
// sleep a bit to get someting more done |
try { Thread.sleep(50); } |
catch (Exception e) { } |
|
} // while |
|
|
} // run() |
|
|
} |