Go to most recent revision |
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 *
* 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()
}