Rev 82 |
Blame |
Last modification |
View Log
| RSS feed
/********************************************************************************************************************************
*
* 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 MKGPSPosition gps_position
;
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";
// for statistics
public int debug_data_count=
0;
public int debug_names_count=
0;
public int angle_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;
public int navi_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);
gps_position=
new MKGPSPosition
();
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;
}
public boolean ready
()
{
return (connected
&&(version.
major!=-
1));
}
/****************** 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=
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
;
}
// 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 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
);
}
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||recieving
)
{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||recieving
)
{try { Thread.
sleep(50); }
catch (Exception e
) { }
}
send_command
(0,
'a',params
);
}
public void trigger_LCD
(int key
)
{
while(sending||recieving
)
{try { Thread.
sleep(50); }
catch (Exception e
) { }
}
//if (sending||recieving) 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||recieving
)
{try { Thread.
sleep(50); }
catch (Exception e
) { }
}
send_command
(0,
(char)('l'+params.
act_paramset),params.
field[params.
act_paramset]);
}
boolean sending=
false;
boolean recieving=
false;
// 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;
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 int slave_addr=-
1;
public void process_data
(int[] data,
int len
)
{
slave_addr=data
[1];
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_names_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':
//int[] foo=Decode64(data,3,len-3);
params.
set_by_mk_data((int)(data
[2]-
'L'),Decode64
(data,
3,len-
3),version
);
params_data_count++
;
break;
// data from navi
case 'Q':
navi_data_count++
;
debug.
log("got navi data(" + len +
"):");
gps_position.
set_by_mk_data(Decode64
(data,
3,len-
3),version
);
debug.
log("long:" + gps_position.
Longitude);
debug.
log("lat:" + gps_position.
Latitude);
root.
canvas.
ufo_prober.
set_to_navi();
break;
case 'w':
angle_data_count++
;
root.
canvas.
ufo_prober.
set_to_mk();
debug.
log("got angle data:");
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[300];
int input
;
int pos=
0;
debug.
log("Thread started");
while(true)
{
debug.
log("Connection Thread run");
if (!connected
)
{
if (!force_disconnect
) connect
();
}
else
try{
while(sending
)
{try { Thread.
sleep(50); }
catch (Exception e
) { }
}
recieving=
true;
debug.
log("Connected - reading data");
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++
;
}
}
recieving=
false;
debug.
log("Data recieved (" + pos +
"Bytes) - processing ..");
/*
if (proxy!=null)
{
proxy.writer.write('\r');
proxy.writer.write('\n');
proxy.writer.flush();
}
*/
if (pos
>5)
process_data
(data_set,pos
);
debug.
log("Processing done");
}
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
// debug.log("Leaving Communicator thread");
} // run()
}