WebSVN - Projects - Blame - Rev 381 - /DUBwise/trunk/shared/src/MKCommunicator.java

Rev	Author	Line No.	Line
206	ligi	1	/********************************************************************************************************************************
		2	*
		3	* Abstaction Layer to Communicate via J2ME and Bluetooth with the FlightCtrl of the MikroKopter Project (www.mikrokopter.de )
		4	*
		5	* Author: Marcus -LiGi- Bueschleb
		6	*
		7	* see README for further Infos
		8	*
		9	*
		10	*******************************************************************************************************************************/
		11
		12
		13	package org.ligi.ufo;
		14
		15
		16	//#ifdef j2me
		17	//# import javax.microedition.io.*;
		18	//#endif
		19
		20	//#ifdef android
		21	import android.util.Log;
		22	//#endif
		23
		24
		25
		26	import java.io.*;
		27
		28
		29	public class MKCommunicator
219	ligi	30	implements Runnable,DUBwiseDefinitions
206	ligi	31	{
381	ligi	32	public byte slave_addr=-1;
222	ligi	33
381	ligi	34	public int primary_abo;
		35	public int secondary_abo;
		36	public int default_abo;
		37
222	ligi	38	public int angle_nick=-4242;
		39	public int angle_roll=-4242;
219	ligi	40	public byte bl_retrys=0;
213	ligi	41
266	ligi	42	public boolean disconnect_notify=false;
		43
213	ligi	44	public byte lib_version_major=0;
381	ligi	45	public byte lib_version_minor=9;
213	ligi	46
266	ligi	47	public byte last_navi_error=0;
		48
381	ligi	49	public boolean mixer_change_notify=false;
		50	public boolean mixer_change_success=false;
		51
		52	public boolean change_notify=false;
		53	public boolean thread_running=true;
		54
213	ligi	55	public String lib_version_str()
		56	{
		57	return "V"+lib_version_major+"."+lib_version_minor;
		58	}
		59
		60
264	ligi	61	public int AngleNick()
		62	{
381	ligi	63	if (is_mk())
264	ligi	64	return angle_nick;
381	ligi	65	else if (is_navi())
264	ligi	66	return debug_data.analog[0];
		67	return -1;
		68
		69	}
		70
		71
265	ligi	72	public int Alt() // in dm
		73	{
		74	int alt=0;
381	ligi	75	if (is_mk())
265	ligi	76	alt=debug_data.analog[5]/3;
		77	else
381	ligi	78	if (is_navi())
265	ligi	79	alt=gps_position.Altimeter/3;
		80	if ( alt<0) alt=0; // mk
		81	if ( alt>20000) alt=0; // navi
		82
		83	return alt;
		84
		85	}
		86
		87	public String Alt_formated() // in dm
		88	{
		89	return "" + Alt()/10 + "m";
		90	}
264	ligi	91	public int AngleRoll()
		92	{
381	ligi	93	if (is_mk())
264	ligi	94	return angle_roll;
381	ligi	95	else
		96	if (is_navi())
265	ligi	97
264	ligi	98	return debug_data.analog[1];
		99	return -1;
		100
		101	}
		102
		103
206	ligi	104	/*************** Section: public Attributes ********************************************/
		105	public boolean connected=false; // flag for the connection state
		106
		107	public String mk_url=""; // buffer the url which is given in the constuctor for reconnectin purposes
		108
		109	public final static int DATA_BUFF_LEN = 20; // in lines
		110
		111	public String[] data_buff;
		112
		113	// boolean do_log=false;
		114	boolean do_log=true;
		115
		116	int data_buff_pos=0;
		117
264	ligi	118
		119	// public final static int DATA_IN_BUFF_SIZE=512;
		120	public final static int DATA_IN_BUFF_SIZE=2048;
		121	// public final static int DATA_IN_BUFF_SIZE=4096;
		122
206	ligi	123	public byte user_intent=0;
		124
		125	public void log(String str)
		126	{
		127	//#ifdef android
219	ligi	128	if (do_log) Log.d("MK-Comm",str);
206	ligi	129	//#endif
264	ligi	130	// canvas.debug.log(str);
		131	// System.out.println(str);
206	ligi	132	}
		133
		134	public int conn_time_in_s()
		135	{
		136	if (connected)
		137	return (int)((System.currentTimeMillis()-connection_start_time)/1000);
		138	else
		139	return 0;
		140	}
		141
		142
381	ligi	143	// byte bootloader_stage= BOOTLOADER_STAGE_NONE;
		144
206	ligi	145	public MKLCD LCD;
		146	public MKVersion version;
		147	public MKDebugData debug_data;
		148
219	ligi	149	public int[] extern_control;
		150
206	ligi	151	public MKGPSPosition gps_position;
		152
		153	public MKStickData stick_data;
		154	public MKParamsParser params;
		155	public MKWatchDog watchdog;
		156	public MKProxy proxy=null;
		157	public MKStatistics stats ;
		158	// public DUBwiseDebug debug;
		159
381	ligi	160	// public UFOProber ufo_prober;
206	ligi	161	public long connection_start_time=-1;
		162
		163
		164	public String error_str = null;
		165
		166
381	ligi	167	public final static byte FC_SLAVE_ADDR = 1;
		168	public final static byte NAVI_SLAVE_ADDR = 2;
		169	public final static byte MK3MAG_SLAVE_ADDR = 3;
		170	public final static byte FOLLOWME_SLAVE_ADDR = 10;
		171	public final static byte RE_SLAVE_ADDR = 23;
206	ligi	172
		173
		174
		175
381	ligi	176	public boolean is_navi()
		177	{
		178	return (slave_addr==NAVI_SLAVE_ADDR);
		179	}
		180
		181
		182	public boolean is_mk()
		183	{
		184	return (slave_addr==FC_SLAVE_ADDR);
		185	}
		186
		187	public boolean is_mk3mag()
		188	{
		189	return (slave_addr==MK3MAG_SLAVE_ADDR);
		190	}
		191
		192	public boolean is_rangeextender()
		193	{
		194	return (slave_addr==RE_SLAVE_ADDR);
		195	}
		196
		197	public boolean is_followme()
		198	{
		199	return (slave_addr==FOLLOWME_SLAVE_ADDR);
		200	}
		201
		202
		203
		204	public boolean is_incompatible()
		205	{
		206	switch (slave_addr)
		207	{
		208	case FC_SLAVE_ADDR:
		209	case NAVI_SLAVE_ADDR:
		210	case MK3MAG_SLAVE_ADDR:
		211	case FOLLOWME_SLAVE_ADDR:
		212	case RE_SLAVE_ADDR:
		213	return false;
		214	default:
		215	return true;
		216	}
		217	}
		218
		219	public String extended_name()
		220	{
		221	switch (slave_addr)
		222	{
		223	case -1:
		224	return "No Device";
		225
		226	case FC_SLAVE_ADDR:
		227	return "MK-Connection";
		228
		229	case NAVI_SLAVE_ADDR:
		230	return "Navi-Connection";
		231
		232	case MK3MAG_SLAVE_ADDR:
		233	return "MK3MAG-Connection";
		234
		235	case FOLLOWME_SLAVE_ADDR:
		236	return "FollowMe Connection";
		237	case RE_SLAVE_ADDR:
		238	return "RangeExtender Connection";
		239
		240	default:
		241	return "Incompatible Device";
		242	}
		243	}
		244
206	ligi	245	/**************** Section: private Attributes ********************************************/
		246	//#ifdef j2me
		247	//# private javax.microedition.io.StreamConnection connection;
		248	//#endif
		249
		250	//#ifdef android
		251	// java.net.Socket connection;
		252	java.net.Socket connection;
		253	//#endif
		254
		255
		256	private java.io.InputStream reader;
		257	private java.io.OutputStream writer;
		258
		259
		260
		261	public String name;
		262	// DUBwise root;
		263
		264
		265	private boolean sending=false;
		266	private boolean recieving=false;
		267
		268
		269
		270	/**************** Section: public Methods **********************************************/
		271	public MKCommunicator()
		272	{
		273
		274	data_buff=new String[DATA_BUFF_LEN];
		275	for (int i=0;i<DATA_BUFF_LEN;i++)
		276	data_buff[i]="";
		277	// debug=debug_;
		278	// root=root_;
		279	version=new MKVersion();
		280	debug_data=new MKDebugData();
		281	stick_data=new MKStickData();
		282	params=new MKParamsParser();
219	ligi	283	extern_control=new int[EXTERN_CONTROL_LENGTH];
		284	extern_control[EXTERN_CONTROL_CONFIG]=1;
		285	extern_control[EXTERN_CONTROL_FRAME]=1;
		286
206	ligi	287	LCD= new MKLCD(this);
		288	watchdog=new MKWatchDog(this);
		289	gps_position=new MKGPSPosition();
		290	stats = new MKStatistics();
		291	proxy =new MKProxy(this);
381	ligi	292	// ufo_prober=new UFOProber();
206	ligi	293	new Thread( this ).start(); // fire up main Thread
		294	}
		295
		296
		297
		298	public void write_raw(byte[] _data)
		299	{
		300	wait4send();
		301	sending=true;
		302	try {
		303	writer.write(_data,0,_data.length);
		304	writer.flush();
		305
		306	stats.bytes_out+=_data.length;
		307	}
		308	catch ( Exception e){}
		309	sending=false;
		310	}
		311
		312	public void do_proxy(String proxy_url)
		313	{
		314	proxy.connect(proxy_url);
		315	}
		316
		317	// int port;
		318
		319	// URL string: "btspp://XXXXXXXXXXXX:1" - the X-Part is the MAC-Adress of the Bluetooth-Device connected to the Fligth-Control
		320	public void connect_to(String _url,String _name)
		321	{
		322	// port=_port;
		323	mk_url=_url; // remember URL for connecting / reconnecting later
		324	name=_name;
		325	force_disconnect=false;
		326	connected=false;
		327	}
		328
		329	public boolean ready()
		330	{
		331	return (connected&&(version.major!=-1));
		332	}
		333
		334
		335	public String get_buff(int age)
		336	{
		337
		338	age%=DATA_BUFF_LEN;
		339
219	ligi	340	if (age<=data_buff_pos)
206	ligi	341	return ""+data_buff[data_buff_pos-age];
		342	else
		343	return ""+data_buff[DATA_BUFF_LEN+data_buff_pos-age];
		344
		345
		346	}
		347	/**************** Section: private Methods **********************************************/
		348	private void connect()
		349	{
		350	log("trying to connect to" + mk_url);
		351	try{
		352
		353	// old call
		354	// connection = (StreamConnection) Connector.open(mk_url, Connector.READ_WRITE);
		355
		356	//#ifdef android
		357	connection = (new java.net.Socket(mk_url.split(":")[0],Integer.parseInt(mk_url.split(":")[1])));
		358	//.Socket
		359
		360	reader=connection.getInputStream();
		361	writer=connection.getOutputStream();
		362
		363	String magic="conn:foo bar\r\n";
		364	writer.write(magic.getBytes());
		365	writer.flush();
		366
		367	//#else
		368
		369	//# connection = (StreamConnection) Connector.open(mk_url);
		370
		371	//# reader=connection.openInputStream();
		372	//# writer=connection.openOutputStream();
		373
		374	//#endif
		375	connection_start_time=System.currentTimeMillis();
		376	connected=true; // if we get here everything seems to be OK
		377
		378	stats.reset();
		379
		380	log("connecting OK");
		381	}
		382	catch (Exception ex)
		383	{
		384	// TODO difference fatal errors from those which will lead to reconnection
		385	log("Problem connecting" + "\n" + ex);
		386	}
		387	}
		388
		389	public int[] Decode64(byte[] in_arr, int offset,int len)
		390	{
		391	int ptrIn=offset;
		392	int a,b,c,d,x,y,z;
		393	int ptr=0;
		394
		395	int[] out_arr=new int[len];
		396
		397	while(len!=0)
		398	{
		399	a=0;
		400	b=0;
		401	c=0;
		402	d=0;
		403	try {
		404	a = in_arr[ptrIn++] - '=';
		405	b = in_arr[ptrIn++] - '=';
		406	c = in_arr[ptrIn++] - '=';
		407	d = in_arr[ptrIn++] - '=';
		408	}
		409	catch (Exception e) {}
		410	//if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden
		411
		412	x = (a << 2) \| (b >> 4);
		413	y = ((b & 0x0f) << 4) \| (c >> 2);
		414	z = ((c & 0x03) << 6) \| d;
		415
		416	if((len--)!=0) out_arr[ptr++] = x; else break;
		417	if((len--)!=0) out_arr[ptr++] = y; else break;
		418	if((len--)!=0) out_arr[ptr++] = z; else break;
		419	}
		420
		421	return out_arr;
		422
		423	}
		424
381	ligi	425
		426
206	ligi	427	public void wait4send()
		428	{
		429	while(sending) //\|\|recieving)
		430	sleep(50);
		431	}
		432
		433
		434	public void sleep(int time)
		435	{
		436	try { Thread.sleep(time); }
		437	catch (Exception e) { }
		438	}
		439
		440	// FC - Function Mappers
		441
		442	// send a version Request to the FC - the reply to this request will be processed in process_data when it arrives
		443	public void get_version()
		444	{
		445	stats.version_data_request_count++;
		446	send_command(0,'v');
		447	}
		448
		449	public void set_gps_target(int longitude,int latitude)
		450	{
		451	int[] target=new int[8];
		452	target[0]= (0xFF)&(longitude<<24);
		453	target[1]= (0xFF)&(longitude<<16);
		454	target[2]= (0xFF)&(longitude<<8);
		455	target[3]= (0xFF)&(longitude);
		456	// send_command(0,'s',target);
		457	}
		458
		459	// send a MotorTest request - params are the speed for each Motor
		460	public void motor_test(int[] params)
		461	{
		462	stats.motortest_request_count++;
		463	send_command(FC_SLAVE_ADDR,'t',params);
		464	}
		465
381	ligi	466	public void set_mixer_table(int[] params)
		467	{
		468
		469	send_command(FC_SLAVE_ADDR,'m',params);
		470	}
219	ligi	471
381	ligi	472
		473	public long lon;
		474	public long lat;
		475
		476	public void send_follow_me(int time)
		477	{
		478	long alt=0;
		479
		480	int[] params=new int[29];
		481
		482
		483	params[0]=(int)((lon)&0xFF) ;
		484	params[1]=(int)((lon>>8)&0xFF) ;
		485	params[2]=(int)((lon>>16)&0xFF) ;
		486	params[3]=(int)((lon>>24)&0xFF) ;
		487
		488	params[4]=(int)((lat)&0xFF) ;
		489	params[5]=(int)((lat>>8)&0xFF) ;
		490	params[6]=(int)((lat>>16)&0xFF) ;
		491	params[7]=(int)((lat>>24)&0xFF) ;
		492
		493	params[8]=(int)((alt)&0xFF );
		494	params[9]=(int)((alt>>8)&0xFF );
		495	params[10]=(int)((alt>>16)&0xFF);
		496	params[11]=(int)((alt>>24)&0xFF );
		497
		498	params[12]=2; // newdata
		499
		500	params[13]=0; // heading
		501	params[14]=0; // heading
		502
		503	params[15]=1; // tolerance
		504
		505	params[16]=time; // time
		506	params[17]=0; // event
		507
		508	send_command(NAVI_SLAVE_ADDR,'s',params);
		509	}
		510
		511
219	ligi	512	public void send_extern_control()
206	ligi	513	{
219	ligi	514
		515	stats.external_control_request_count++;
		516	send_command(FC_SLAVE_ADDR,'b',extern_control);
206	ligi	517	}
		518
219	ligi	519	/* public void send_keys(int[] params)
		520	{
		521	send_command(FC_SLAVE_ADDR,'k',params);
		522	}*/
		523
206	ligi	524	// get params
		525	public void get_params(int id)
		526	{
		527	wait4send();
		528	send_command(FC_SLAVE_ADDR,'q',id+1);
		529	stats.params_data_request_count++;
		530	}
		531
		532	public void get_debug_name(int id)
		533	{
		534
		535	wait4send();
		536	send_command(0,'a',id);
		537	}
		538
		539
		540	public void trigger_LCD_by_page(int page)
		541	{
		542	wait4send();
		543	send_command(0,'l',page);
		544	stats.lcd_data_request_count++;
		545	}
		546
		547	public void trigger_debug()
		548	{
		549	if (sending\|\|recieving) return; // its not that important - can be dropped
		550	send_command(0,'c');
		551	}
		552
		553
216	ligi	554	public void switch_todo()
		555	{
264	ligi	556	sleep(150);
266	ligi	557	version.reset();
		558	// LCD= new MKLCD(this);
216	ligi	559	debug_data=new MKDebugData();
206	ligi	560
216	ligi	561	}
		562
206	ligi	563	public void switch_to_fc()
		564	{
		565	wait4send();
		566	send_command(NAVI_SLAVE_ADDR,'u',0);
216	ligi	567	switch_todo();
		568
206	ligi	569	}
		570
		571
		572	public void switch_to_mk3mag()
		573	{
		574	wait4send();
		575	send_command(NAVI_SLAVE_ADDR ,'u',1);
216	ligi	576	switch_todo();
206	ligi	577	}
		578
215	ligi	579	public final static byte[] navi_switch_magic={27,27,0x55,(byte)0xAA,0,(byte)'\r'};
206	ligi	580	public void switch_to_navi()
		581	{
		582	wait4send();
		583	sending=true;
		584	try
		585	{
215	ligi	586	writer.write(navi_switch_magic);
206	ligi	587	stats.bytes_out+=6;
		588	writer.flush();
		589	}
		590	catch (Exception e) { }
		591	sending=false;
		592
216	ligi	593	switch_todo();
		594
206	ligi	595	}
		596
381	ligi	597
206	ligi	598	int msg_pos=0;
		599
		600
381	ligi	601	// public boolean bootloader_intension_flash=false;
224	ligi	602
381	ligi	603	// public boolean bootloader_finish_ok=false;
224	ligi	604
206	ligi	605
381	ligi	606	// public void jump_bootloader()
		607	// {
		608	/*
224	ligi	609	bootloader_finish_ok=false;
206	ligi	610	msg_pos=0;
		611	bootloader_stage= BOOTLOADER_STAGE_NONE;
381	ligi	612	// flash_msgs=new String[100];
		613	// flash_msgs[msg_pos++]="Initiializing Bootloader";
206	ligi	614	wait4send();
		615	sending=true;
		616
		617	try
		618	{
		619	int attempt=0;
		620
		621	while(bootloader_stage!= BOOTLOADER_STAGE_GOT_MKBL)
		622	{
		623	flash_msgs[msg_pos]="attempt "+attempt;
		624	attempt++;
		625	send_command_nocheck((byte)FC_SLAVE_ADDR,'R',new int[0]);
219	ligi	626
		627	try{
206	ligi	628	writer.write( 27);
		629	writer.flush();
		630
		631	sleep(20);
		632
		633	writer.write( 0xAA);
		634	writer.flush();
219	ligi	635	}
		636	catch (Exception e) { }
206	ligi	637	sleep((attempt%2==0)?80:800); //800
		638	}
		639	msg_pos++;
		640	}
		641
		642	catch (Exception e) {
		643	flash_msgs[msg_pos++]="Exception:" +e.getMessage() ;
		644	flash_msgs[msg_pos++]=e.toString() ;
		645	}
		646
		647	new Thread( this ).start(); // fire up main Thread
381	ligi	648	*/
		649	// }
206	ligi	650
		651
		652	public void get_error_str()
		653	{
		654	send_command(NAVI_SLAVE_ADDR,'e');
		655	}
		656
		657	public void trigger_rcdata()
		658	{
		659	send_command(FC_SLAVE_ADDR,'p');
		660	}
		661
		662
223	ligi	663	public void write_params(int to)
206	ligi	664	{
223	ligi	665	params.update_backup(to);
242	ligi	666	write_params_(to) ;
		667	}
		668
		669	public void write_params_(int to)
		670	{
206	ligi	671	wait4send();
229	ligi	672	params.active_paramset=to;
223	ligi	673	send_command(FC_SLAVE_ADDR,'s',params.field_bak[to]);
262	ligi	674
206	ligi	675	}
		676
262	ligi	677	public void set_debug_interval(int interval)
		678	{
264	ligi	679	send_command(2,'d',interval);
262	ligi	680	}
264	ligi	681
		682
		683	public void set_gpsosd_interval(int interval)
		684	{
		685	send_command(NAVI_SLAVE_ADDR,'o',interval);
		686	}
		687
206	ligi	688	public void send_command(int modul,char cmd)
		689	{
		690	send_command(modul,cmd,new int[0]);
		691	}
		692
262	ligi	693
		694
206	ligi	695	public void send_command(int modul,char cmd,int param)
		696	{
		697	int[] params=new int[1];
		698	params[0]=param;
		699	send_command(modul,cmd,params);
		700	}
		701
		702	public void send_command_nocheck(byte modul,char cmd,int[] params)
		703	{
		704	// char[] send_buff=new char[5 + (params.length/3 + (params.length%3==0?0:1) )4]; // 5=1start_char+1addr+1cmd+2*crc
		705
		706	byte[] send_buff=new byte[3 + (params.length/3 + (params.length%3==0?0:1) )4]; // 5=1start_char+1addr+1cmd+2*crc
		707	send_buff[0]='#';
381	ligi	708	send_buff[1]=(byte)(modul+'a');
206	ligi	709	send_buff[2]=(byte)cmd;
		710
		711	for(int param_pos=0;param_pos<(params.length/3 + (params.length%3==0?0:1)) ;param_pos++)
		712	{
		713	int a = (param_pos3<params.length)?params[param_pos3]:0;
		714	int b = ((param_pos3+1)<params.length)?params[param_pos3+1]:0;
		715	int c = ((param_pos3+2)<params.length)?params[param_pos3+2]:0;
		716
		717	send_buff[3+param_pos*4] = (byte)((a >> 2)+'=' );
		718	send_buff[3+param_pos*4+1] = (byte)('=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4)));
		719	send_buff[3+param_pos*4+2] = (byte)('=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6)));
		720	send_buff[3+param_pos*4+3] = (byte)('=' + ( c & 0x3f));
		721
		722	//send_buff[3+foo]='=';
		723	}
		724
		725	/* for(int foo=0;foo<(params.length/3 + (params.length%3==0?0:1) )*4;foo++)
		726	{
		727	int a = (foo<params.length) params[foo];
		728	int a = params[foo];
		729
		730	//send_buff[3+foo]='=';
		731	}
		732	*/
		733	try
		734	{
		735	int tmp_crc=0;
		736	for ( int tmp_i=0; tmp_i<send_buff.length;tmp_i++)
		737	tmp_crc+=(int)send_buff[tmp_i];
		738
		739	writer.write(send_buff,0,send_buff.length);
		740	tmp_crc%=4096;
		741
		742	writer.write( (char)(tmp_crc/64 + '='));
		743	writer.write( (char)(tmp_crc%64 + '='));
		744	writer.write('\r');
		745	stats.bytes_out+=send_buff.length+3;
		746	writer.flush();
		747	}
		748	catch (Exception e)
		749	{ // problem sending data to FC
		750	}
		751
		752	}
		753	// send command to FC ( add crc and pack into pseudo Base64
		754	public void send_command(int modul,char cmd,int[] params)
		755	{
		756	// if (modul==0) return;
		757	sending=true;
		758	send_command_nocheck((byte)modul,cmd,params);
		759	sending=false;
		760	}
		761
		762
		763
		764
381	ligi	765
206	ligi	766	public int UBatt()
		767	{
		768
381	ligi	769	switch (slave_addr)
		770	{
		771	case FC_SLAVE_ADDR:
		772	return debug_data.analog[8];
		773
		774	case NAVI_SLAVE_ADDR:
		775	return gps_position.UBatt;
		776
		777	case FOLLOWME_SLAVE_ADDR:
		778	return debug_data.analog[8];
		779
		780	default:
		781	return -1; // No Info
		782	}
		783
206	ligi	784	}
		785
		786
		787
381	ligi	788	public int SatsInUse()
		789	{
		790
		791	switch (slave_addr)
		792	{
		793	case NAVI_SLAVE_ADDR:
		794	return gps_position.SatsInUse;
		795
		796	case FOLLOWME_SLAVE_ADDR:
		797	return debug_data.analog[12];
		798
		799	default:
		800	return -1; // No Info
		801	}
		802
		803	}
		804
		805
		806
206	ligi	807	public int SenderOkay()
		808	{
381	ligi	809	switch (slave_addr)
		810	{
		811	case FC_SLAVE_ADDR:
		812	return debug_data.analog[10];
206	ligi	813
381	ligi	814	case NAVI_SLAVE_ADDR:
		815	return gps_position.SenderOkay;
		816
		817	default:
		818	return -1; // No Info
		819	}
206	ligi	820	}
		821
		822
262	ligi	823	public int[][] debug_buff=null;
		824	public int debug_buff_off=0;
		825	public int debug_buff_len=0;
		826	public int debug_buff_interval=0;
		827	public int debug_buff_lastset=0;
		828	public int debug_buff_max=1;
		829
		830	public int[] debug_buff_targets=null;
		831
		832	public void setup_debug_buff(int[] targets,int len,int interval)
		833	{
		834	debug_buff=new int[len][targets.length];
		835
		836	debug_buff_off=0;
		837	debug_buff_len=len;
264	ligi	838
262	ligi	839	debug_buff_interval=interval;
264	ligi	840	if (debug_buff_interval<2)debug_buff_interval=2;
262	ligi	841	debug_buff_targets=targets;
		842	debug_buff_max=1;
		843	debug_buff_lastset=0;
		844	}
		845
		846	public int chg_debug_max(int val)
		847	{
		848	if (val>debug_buff_max)
		849	debug_buff_max=val;
		850	if (-val>debug_buff_max)
		851	debug_buff_max=-val;
		852	return val;
		853	}
		854
		855	public void destroy_debug_buff()
		856	{
		857	debug_buff_targets=null;
		858	}
		859
206	ligi	860	public void process_data(byte[] data,int len)
		861	{
		862
381	ligi	863	// check crc
		864	int tmp_crc=0;
		865	for ( int i=0 ; i<len-2 ; i++)
		866	tmp_crc+=(int)data[i];
		867
		868	tmp_crc%=4096;
		869
		870	if (!((data[len-2]==(char)(tmp_crc/64 + '='))
		871	&&
		872	(data[len-1]==(char)(tmp_crc%64 + '='))))
		873	{
		874	stats.crc_fail++;
		875	return;
		876	}
		877	// end of c
		878
		879	// slave_addr=data[1];
206	ligi	880	log("command " +(char)data[2] );
381	ligi	881
		882
		883	int[] decoded_data=Decode64(data,3,len-5);
		884
206	ligi	885	switch((char)data[2])
		886	{
		887
		888	case 'A': // debug Data Names
		889	stats.debug_names_count++;
381	ligi	890	debug_data.set_names_by_mk_data(decoded_data);
206	ligi	891	break;
		892
219	ligi	893	case 'B': // external_control confirm frames
		894	stats.external_control_confirm_frame_count++;
		895	break;
		896
206	ligi	897	case 'L': // LCD Data
		898	stats.lcd_data_count++;
381	ligi	899	LCD.handle_lcd_data(decoded_data);
206	ligi	900
		901	break;
		902
		903	case 'D': // debug Data
		904	log("got debug data");
		905	stats.debug_data_count++;
381	ligi	906	debug_data.set_by_mk_data(decoded_data,version);
262	ligi	907
381	ligi	908	if (is_mk())
266	ligi	909	{
		910	stats.process_mkflags(debug_data.motor_val(0)); // TODO remove dirty hack
		911	stats.process_alt(Alt());
		912	}
262	ligi	913	if (debug_buff_targets!=null)
		914	{
		915	for (int sp=0;sp<debug_buff_targets.length;sp++)
		916	debug_buff[debug_buff_off][sp]=chg_debug_max(debug_data.analog[debug_buff_targets[sp]]);
		917	if (debug_buff_off>debug_buff_lastset)
		918	debug_buff_lastset=debug_buff_off;
		919
		920	debug_buff_off=(debug_buff_off+1)%debug_buff_len;
		921
		922	}
206	ligi	923	log("processed debug data");
		924	break;
		925
		926	case 'V': // Version Info
		927	stats.version_data_count++;
381	ligi	928	if (slave_addr!=data[1]-'a')
		929	{
		930	slave_addr=(byte)(data[1]-'a');
		931	change_notify=true;
		932	}
206	ligi	933
381	ligi	934	/*
206	ligi	935	switch(slave_addr)
		936	{
		937	case FC_SLAVE_ADDR:
		938	ufo_prober.set_to_mk();
		939	break;
		940
		941	case NAVI_SLAVE_ADDR:
		942	ufo_prober.set_to_navi();
		943	break;
		944
		945	case MK3MAG_SLAVE_ADDR:
		946	ufo_prober.set_to_mk3mag();
		947	break;
		948
381	ligi	949	case RE_SLAVE_ADDR:
		950	ufo_prober.set_to_rangeextender();
		951	break;
		952
206	ligi	953	default:
		954	ufo_prober.set_to_incompatible();
		955	break;
		956	}
381	ligi	957	*/
206	ligi	958
381	ligi	959	version.set_by_mk_data(decoded_data);
206	ligi	960	break;
		961
		962	case 'w':
381	ligi	963	angle_nick=MKHelper.parse_signed_int_2(decoded_data[0],decoded_data[1]);
		964	angle_roll=MKHelper.parse_signed_int_2(decoded_data[2],decoded_data[3]);
206	ligi	965	stats.angle_data_count++;
		966	break;
		967
		968
		969	case 'Q':
381	ligi	970	if (is_mk())
206	ligi	971	{
		972	stats.params_data_count++;
381	ligi	973	params.set_by_mk_data(decoded_data);
206	ligi	974	}
		975	break;
381	ligi	976
		977	case 'M':
		978	mixer_change_notify=true;
		979	mixer_change_success=(decoded_data[0]==1);
		980	break;
206	ligi	981	case 'P':
		982	stats.stick_data_count++;
381	ligi	983	stick_data.set_by_mk_data(decoded_data);
206	ligi	984	break;
		985
		986
381	ligi	987	case 'E': // Error Str from Navi
206	ligi	988	error_str="";
		989	for(int foo=0;foo<20;foo++)
381	ligi	990	if (decoded_data[foo]!=0)
		991	error_str+=(char)decoded_data[foo];
206	ligi	992	break;
		993
381	ligi	994
		995	case 'O': // OSD Values Str from Navi
206	ligi	996	stats.navi_data_count++;
		997	log("got navi data(" + len +"):");
		998
381	ligi	999	gps_position.set_by_mk_data(decoded_data,version);
206	ligi	1000
265	ligi	1001	stats.process_mkflags(gps_position.MKFlags);
		1002	stats.process_compas(gps_position.CompasHeading);
266	ligi	1003	stats.process_speed(gps_position.GroundSpeed);
		1004	stats.process_alt(Alt());
206	ligi	1005
		1006	break;
		1007
		1008
		1009	default:
		1010	stats.other_data_count++;
		1011	break;
		1012
		1013	}
		1014
		1015	}
		1016
265	ligi	1017
		1018
206	ligi	1019	public boolean force_disconnect=true;
		1020
		1021	public void close_connections(boolean force)
		1022	{
266	ligi	1023
206	ligi	1024	// if ((!force)&&root.canvas.do_vibra) root.vibrate(500);
381	ligi	1025	force_disconnect\|=force;
206	ligi	1026	try{ reader.close(); }
		1027	catch (Exception inner_ex) { }
		1028
		1029	try{ writer.close(); }
		1030	catch (Exception inner_ex) { }
		1031
		1032	//#ifdef j2me
		1033	//# try{ connection.close(); }
		1034	//# catch (Exception inner_ex) { }
		1035	//#endif
381	ligi	1036	slave_addr=-1;
		1037	// ufo_prober.set_to_none();
219	ligi	1038	stats.reset();
206	ligi	1039	connected=false;
266	ligi	1040	version.reset();
381	ligi	1041	// if (bootloader_stage==BOOTLOADER_STAGE_NONE)
266	ligi	1042	disconnect_notify=true;
206	ligi	1043	}
		1044
381	ligi	1045
		1046	int last_avr_sig=-1;
206	ligi	1047	// Thread to recieve data from Connection
		1048	public void run()
		1049	{
381	ligi	1050	// boolean sigfail=false;
		1051	/* if (bootloader_stage==BOOTLOADER_STAGE_GOT_MKBL)
206	ligi	1052	{
		1053	try {
		1054
		1055	// if (send_buff_size>128)
		1056	// send_buff_size=128;
224	ligi	1057
		1058	// if (!bootloader_intension
381	ligi	1059	if (bootloader_intension_flash)
219	ligi	1060	{
206	ligi	1061
219	ligi	1062	byte[] flash_buff =new byte[send_buff_size]; ///!!
		1063
		1064	String firmware_filename=(avr_sig==224)?"/navi.bin":((avr_sig==120)?"/mk3.bin":"/fc.bin");
		1065	flash_msgs[msg_pos++]="Opening firmware " + firmware_filename + "..";
		1066
		1067
		1068	InputStream in;
		1069	try {
		1070	in=this.getClass().getResourceAsStream(firmware_filename);
		1071	}
		1072
		1073	catch (Exception e) { throw new Exception(" .. cant open firmware"); }
		1074	int firmware_size=-1;
		1075	try {
		1076
		1077	firmware_size= ((int)in.read()<<24) \|((int)in.read()<<16) \| ((int)in.read()<<8) \| ((int)in.read()&0xff) ;
		1078	}
		1079	catch (Exception e) { throw new Exception(" .. cant read size"); }
		1080
		1081	int blocks2write=((firmware_size/send_buff_size));
		1082	flash_msgs[msg_pos++]=".. open("+blocks2write+" blocks," + firmware_size + "bytes)";
		1083
		1084
		1085	// if (true) throw new Exception("before erasing");
		1086
		1087	// if (true) throw new Exception("before erasing" );
		1088
		1089	flash_msgs[msg_pos++]="Erasing Flash ..";
		1090	writer.write('e');
206	ligi	1091	writer.flush();
		1092
219	ligi	1093	if (reader.read()!=0x0d)
		1094	throw new Exception("cant erase flash");
		1095
		1096	flash_msgs[msg_pos]+="OK";
		1097
		1098
		1099	writer.write('A');
		1100	writer.write(0);
		1101	writer.write(0);
		1102	writer.flush();
		1103
		1104	if (reader.read()!=0x0d)
		1105	throw new Exception("cant set addr");
		1106
		1107	flash_msgs[msg_pos++]="addr set";
		1108
		1109
		1110	// int blocks2write=((firmware_size/send_buff_size));
		1111	if ((firmware_size%send_buff_size)>0)
		1112	blocks2write++;
		1113
		1114	for ( int block=0; block<blocks2write; block ++)
206	ligi	1115	{
219	ligi	1116	int hex_bytes_read=in.read(flash_buff,0,send_buff_size);
		1117
		1118	flash_msgs[msg_pos]="bl:" + block + "/" + blocks2write + " si:"+hex_bytes_read ;
		1119
		1120
		1121	writer.write('B');
		1122	writer.write((hex_bytes_read>>8)& 0xFF);
		1123	writer.write((hex_bytes_read)& 0xFF);
		1124	writer.write('F');
		1125	writer.flush();
		1126
		1127
		1128	writer.write(flash_buff,0,hex_bytes_read);
		1129	writer.flush();
		1130
		1131
		1132	if (avr_sig==224)
		1133	{
		1134	int crc=0xFFFF;
		1135	for (int crc_pos=0;crc_pos<hex_bytes_read;crc_pos++)
		1136	crc=CRC16(flash_buff[crc_pos],crc);
		1137	writer.write(crc>>8);
		1138	writer.write(crc&0xff);
		1139	writer.flush();
		1140	}
206	ligi	1141	// flash_msgs[msg_pos]+="ok";
		1142	// writer.flush();
		1143
		1144
		1145
219	ligi	1146	if (reader.read()!=0x0d)
		1147	throw new Exception("abort write at block"+block);
		1148
		1149
		1150
		1151	// sleep(1000);
		1152	}
381	ligi	1153
		1154
		1155	*/
219	ligi	1156	// flash_msgs[msg_pos]="bl:" + block + "/" + blocks2write + " si:"+hex_bytes_read ;
		1157	/*
		1158
206	ligi	1159	int inp=0;
		1160	int block=0;
		1161	while (inp!=-1)
		1162	{
		1163	int flash_buff_pos=0;
		1164	int crc=0xFFFF;
		1165
		1166	while ((flash_buff_pos<send_buff_size)&&(inp!=-1))
		1167	{
		1168	inp=in.read();
		1169	if (inp!=-1)
		1170	{
		1171	crc=CRC16(inp,crc);
		1172	flash_buff[flash_buff_pos++]=(byte)inp;
		1173	}
		1174	}
		1175	// flash_msgs[msg_pos]="block" + block + "size:"+flash_buff_pos;
		1176
		1177	block++;
		1178
		1179	boolean block_fin=false;
		1180
		1181
		1182	while(!block_fin)
		1183	{
		1184
		1185	writer.write('B');
		1186	writer.write((flash_buff_pos>>8)& 0xFF);
		1187	writer.write((flash_buff_pos)& 0xFF);
		1188	writer.write('F');
		1189	writer.flush();
		1190
		1191	// int ret_v=-1;
		1192
		1193	writer.write(flash_buff,0,flash_buff_pos);
		1194	flash_msgs[msg_pos]="bl:" + block + "si:"+flash_buff_pos ;
		1195
		1196	writer.flush();
		1197	// flash_msgs[msg_pos]+="wtc";
		1198
		1199
		1200	// append crc if navi
		1201	if (avr_sig==224)
		1202	{
		1203	writer.write(crc>>8);
		1204	writer.write(crc&0xff);
		1205	writer.flush();
		1206	}
		1207	// flash_msgs[msg_pos]+="ok";
		1208	// writer.flush();
		1209	// if (reader.read()!=0x0d)
		1210	// throw new Exception("abort write at block"+block);
		1211
		1212
		1213	//ret_v=reader.read();
		1214	// flash_msgs[msg_pos]="ret"+ret_v + "crc"+crc;
		1215
		1216	if (reader.read()==0x0d)
		1217	block_fin=true;
		1218
		1219	}
		1220
		1221	}
		1222	*/
381	ligi	1223
		1224
		1225
		1226	/**
206	ligi	1227	flash_msgs[++msg_pos]="written last block ";
		1228	msg_pos++;
		1229	flash_buff=null;
		1230
		1231	ufo_prober.set_to_none();
		1232	stats.reset();
		1233	version=new MKVersion();
		1234	System.gc();
		1235	}
		1236	else // bootloader intension clear settings
		1237	{
		1238
		1239	flash_msgs[msg_pos]="reset params ..";
		1240	writer.write('B');
		1241	writer.write(0);
		1242	writer.write(4);
		1243	writer.write('E');
		1244	writer.flush();
		1245
		1246	writer.write(0xFF);
		1247	writer.write(0xFF);
		1248	writer.write(0xFF);
		1249	writer.write(0xFF);
		1250	writer.flush();
		1251	flash_msgs[msg_pos++]+=" done";
		1252	}
		1253
		1254	flash_msgs[msg_pos++]="Exiting Bootloader" ;
		1255	params=new MKParamsParser();
		1256	try{
		1257	writer.write('E');
		1258	writer.flush();
		1259	}
		1260	catch (Exception e) {
		1261	flash_msgs[msg_pos++]="cant exit bootloader" ;
		1262	}
		1263	flash_msgs[msg_pos++]="Exit BL done" ;
		1264
224	ligi	1265	bootloader_finish_ok=true;
206	ligi	1266	}
		1267
		1268	catch (Exception e) {
		1269	flash_msgs[msg_pos++]="Fail:" +e.getMessage() ;
		1270
		1271
		1272	flash_msgs[msg_pos++]="Exiting Bootloader" ;
		1273	params=new MKParamsParser();
		1274	try{
		1275	writer.write('E');
		1276	writer.flush();
		1277	}
		1278	catch (Exception e2) {
		1279	flash_msgs[msg_pos++]="cant exit bootloader" ;
		1280	}
		1281	flash_msgs[msg_pos++]="Exit BL done" ;
219	ligi	1282	if (sigfail&&(bl_retrys<3))
		1283	{
		1284	bl_retrys++;
		1285	init_bootloader=true;
		1286	}
		1287	close_connections(false);
206	ligi	1288	}
		1289
		1290
		1291	sending=false;
		1292	}
		1293
381	ligi	1294	**/
206	ligi	1295
		1296	byte[] data_set=new byte[1024];
		1297	int data_set_pos=0;
264	ligi	1298
		1299	byte[] data_in_buff=new byte[DATA_IN_BUFF_SIZE];
206	ligi	1300
		1301	int input;
		1302	int pos=0;
		1303
		1304	log("Thread started");
381	ligi	1305	while(thread_running)
206	ligi	1306	{
264	ligi	1307
206	ligi	1308	if (!connected)
		1309	{
381	ligi	1310	sleep(10);
206	ligi	1311	if (!force_disconnect) connect();
		1312	}
		1313	else
		1314	try{
		1315
		1316	/*
		1317	while(sending)
		1318	{try { Thread.sleep(50); }
		1319	catch (Exception e) { }
		1320	}
		1321	*/
		1322	recieving=true;
264	ligi	1323	int read_count ;
		1324
		1325	if (reader.available()<DATA_IN_BUFF_SIZE)
		1326	read_count =reader.read(data_in_buff,0,reader.available());
		1327	else
		1328	read_count =reader.read(data_in_buff,0,DATA_IN_BUFF_SIZE);
		1329
262	ligi	1330	// log("Connected - reading data " + read_count);
206	ligi	1331	// pos=0;
		1332	input=0;
		1333	//data_buff[data_buff_pos]="";
		1334	// recieve data-set
		1335
		1336	// int read_count =reader.read(data_in_buff,0,reader.available());
		1337	stats.bytes_in+=read_count;
		1338	if (read_count>0)
		1339	{
		1340	log("read" + read_count + " ds_pos" + data_set_pos);
		1341
		1342	for ( pos=0;pos<read_count;pos++)
		1343	{
		1344	if (data_in_buff[pos]==13)
		1345	{
233	ligi	1346	data_buff[data_buff_pos]=new String(data_set, 0, data_set_pos);
206	ligi	1347	data_buff_pos++;
		1348	data_buff_pos%=DATA_BUFF_LEN;
		1349
		1350
		1351	try{process_data(data_set,data_set_pos); }
		1352	catch (Exception e)
		1353	{
		1354	log(".. problem processing");
		1355	log(e.toString());
		1356	}
		1357
		1358
		1359
		1360
		1361	proxy.write(data_set,0,data_set_pos);
		1362	// proxy.writer.write('\r');
		1363	//proxy.writer.write('\n');
		1364	//proxy.writer.flush();
		1365	/*
		1366	if (proxy!=null)
		1367	{
		1368
		1369
		1370
		1371	}
		1372	*/
		1373	data_set_pos=0;
		1374
		1375	}
		1376	else
381	ligi	1377	// {
206	ligi	1378	data_set[data_set_pos++]=data_in_buff[pos];
		1379
381	ligi	1380	/*
206	ligi	1381
		1382	if ( (data_set_pos>4) && (data_set[data_set_pos-4]==(byte)'M') && (data_set[data_set_pos-3]==(byte)'K') && (data_set[data_set_pos-2]==(byte)'B') && (data_set[data_set_pos-1]==(byte)'L'))
381	ligi	1383
206	ligi	1384	{
381	ligi	1385
206	ligi	1386	bootloader_stage= BOOTLOADER_STAGE_GOT_MKBL;
		1387	return;
		1388	}
		1389
381	ligi	1390	}*/
206	ligi	1391
		1392	}
		1393
		1394
		1395	}
		1396	else
266	ligi	1397	{
		1398	recieving=false;
		1399	sleep(20);
		1400	}
206	ligi	1401	/*
		1402	while ((input != 13)) //&&(input!=-1))
		1403	{
		1404	{
		1405	//log("pre read");
		1406	log(""+reader.available());
		1407	input = reader.read() ;
		1408	log("Byte rcv" + input +"pos"+ pos);
		1409
		1410	proxy.write(input);
		1411
		1412	data_buff[data_buff_pos]+=(char)input;
		1413
		1414	if ((data_buff[data_buff_pos].length()>3)&&(data_buff[data_buff_pos].substring(data_buff[data_buff_pos].length()-4,data_buff[data_buff_pos].length()).equals("MKBL")))
		1415	{
		1416	bootloader_stage= BOOTLOADER_STAGE_GOT_MKBL;
		1417	return;
		1418	}
		1419	if (input==-1) throw new Exception("disconnect");
		1420	else
		1421	{
		1422	stats.bytes_in++;
		1423	data_set[pos]=input;
		1424	pos++;
		1425	}
		1426	}
		1427
		1428	}
		1429
		1430
		1431
		1432	data_buff_pos++;
		1433	data_buff_pos%=DATA_BUFF_LEN;
		1434	recieving=false;
		1435	log("Data recieved (" + pos + "Bytes)");
		1436	log("processing ..");
		1437	*/
		1438
		1439	/*
		1440	if (proxy!=null)
		1441	{
		1442	proxy.writer.write('\r');
		1443	proxy.writer.write('\n');
		1444	proxy.writer.flush();
		1445	}
		1446	*/
		1447	/*if (pos>5)
		1448	{
		1449	try{process_data(data_set,pos); }
		1450	catch (Exception e)
		1451	{
		1452	log(".. problem processing");
		1453	log(e.toString());
		1454	}
		1455
		1456	log(".. processing done");
		1457	}
		1458	*/
		1459	}
		1460	catch (Exception ex)
		1461	{
		1462	log("Problem reading from MK -> closing conn");
		1463	log(ex.toString());
		1464	// close the connection
		1465	close_connections(false);
		1466	}
		1467
		1468	// sleep a bit to get someting more done
		1469	// sleep(5); //50
		1470
		1471	} // while
		1472	// log("Leaving Communicator thread");
		1473	} // run()
		1474	}

Subversion Repositories Projects

(root)/DUBwise/trunk/shared/src/MKCommunicator.java - Rev 381