Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 91 → Rev 92

/DUBwise/src/Android/src/org/ligi/MKCommunicator.java
0,0 → 1,457
/********************************************************************************************************************************
*
* 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
*
*
*******************************************************************************************************************************/
package org.ligi;
//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.net.Socket 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_)
{
 
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;
}
 
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);
 
connection = new java.net.Socket("10.0.2.2",54321);
reader=connection.getInputStream();
writer=connection.getOutputStream();
 
//
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]+" ";
root.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++;
root.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];
root.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;
}
 
public boolean run=true;
// Thread to recieve data from Connection
public void run()
{
int[] data_set=new int[1024];
int input;
int pos=0;
msg+="!!run started!!";
while(run)
{
if (!connected)
{
if (!force_disconnect) connect();
}
else
try{
pos=0;
input=0;
// recieve data-set
while ((input != 13)&&(input != 10)) // &&(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++;
}
// root.log("p "+pos + " i:" + (char)input + " ii: " + input);
}
/*
if (proxy!=null)
{
proxy.writer.write('\r');
proxy.writer.write('\n');
proxy.writer.flush();
}
*/
try
{
if (pos>5)
process_data(data_set,pos);
}
catch (Exception ex)
{
root.log("Problem Parsinf data");
root.log(ex.toString());
// close the connection
// close_connections(false);
}
}
catch (Exception ex)
{
root.log("Problem reading from MK -> closing conn");
root.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()
 
 
}