Rev 303 |
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
* Project-Start: 9/2007
* Version: 0.07
* Mailto: ligi@smart4mobile.de
* Licence: Creative Commons / Non Commercial
* Big Up: Holger&Ingo
*
************************************************************************************************/
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 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
;
/****************** Section: public Methods ************************************************/
public MKCommunicator
()
{
version=
new MKVersion
();
debug_data=
new MKDebugData
();
params=
new MKParamsParser
();
// mk_url=url; // remember URL for connecting / reconnecting later
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
;
name=_name
;
force_disconnect=
false;
connected=
false;
}
/****************** Section: private Methods ************************************************/
private void connect
()
{
try{
connection =
(StreamConnection
) Connector.
open(mk_url, Connector.
READ_WRITE);
reader=connection.
openInputStream();
writer=connection.
openOutputStream();
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
;
}
}
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 a MotorTest request - params are the speed for each Motor
public void motor_test
(int[] params
)
{
send_command
(0,
't',params
);
}
// get params
public void get_params
(int id
)
{
int[] params=
new int[1];
params
[0]=id
;
send_command
(0,
'q',params
);
}
// 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
;
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
}
}
public void process_data
(int[] data,
int len
)
{
int[] decoded_data
;
switch((char)data
[2])
{
case 'D':
// debug Data
debug_data_count++
;
debug_data.
set_by_mk_data(Decode64
(data,
3,
50),version
);
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.
handle_lcd_data(Decode64
(data,
3,
20),data
[2]-
(int)'0');
lcd_data_count++
;
break;
case 'L':
case 'M':
case 'N':
case 'O':
case 'P':
params.
set_by_mk_data((int)(data
[2]-
'L'),Decode64
(data,
3,
58),version
);
params_data_count++
;
break;
default:
other_data_count++
;
break;
}
}
String o_msg=
"";
public boolean force_disconnect=
true;
public void close_connections
(boolean force
)
{
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");
data_set
[pos
]=input
;
pos++
;
}
if (proxy
!=
null)
{
proxy.
writer.
write('\r');
proxy.
writer.
write('\n');
proxy.
writer.
flush();
}
process_data
(data_set,pos
);
}
catch (Exception ex
)
{
msg=
"Problem reading from MK";
// close the connection
close_connections
(false);
}
// sleep a bit to get someting more done
try { Thread.
sleep(50); }
catch (Exception e
) { }
} // while
} // run()
}