Rev 181 |
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;
public MKLCD LCD
;
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;
/****************** 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
{
debug_data=
new MKDebugData
();
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();
connection_start_time=
System.
currentTimeMillis();
connected=
true; // if we get here everything seems to be OK
get_version
();
LCD=
new MKLCD
(this);
}
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 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
<params.
length)?params
[param_pos
]:
0;
int b =
((param_pos+
1)<params.
length)?params
[param_pos+
1]:
0;
int c =
((param_pos+
2)<params.
length)?params
[param_pos+
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
}
}
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
);
debug_data_count++
;
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];
version_data_count++
;
break;
case '0':
case '1':
case '2':
case '3':
LCD.
handle_lcd_data(Decode64
(data,
3,
20),data
[2]-
(int)'0');
lcd_data_count++
;
// decoded_data=
// if ((data[2]-(int)'0')!=3)
// get_LCD();
break;
default:
other_data_count++
;
break;
}
}
String o_msg=
"";
public boolean force_disconnect=
false;
public void close_connections
()
{
force_disconnect=
true;
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 (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
close_connections
();
}
// sleep a bit to get someting more done
try { Thread.
sleep(50); }
catch (Exception e
) { }
} // while
} // run()
}