29,19 → 29,29 |
public class MKCommunicator |
implements Runnable,DUBwiseDefinitions |
{ |
public byte slave_addr=-1; |
|
public int primary_abo; |
public int secondary_abo; |
public int default_abo; |
|
public int angle_nick=-4242; |
public int angle_roll=-4242; |
public byte bl_retrys=0; |
public boolean init_bootloader=false; |
|
public boolean disconnect_notify=false; |
|
public byte lib_version_major=0; |
public byte lib_version_minor=7; |
public byte lib_version_minor=9; |
|
public byte last_navi_error=0; |
|
public boolean mixer_change_notify=false; |
public boolean mixer_change_success=false; |
|
public boolean change_notify=false; |
public boolean thread_running=true; |
|
public String lib_version_str() |
{ |
return "V"+lib_version_major+"."+lib_version_minor; |
50,9 → 60,9 |
|
public int AngleNick() |
{ |
if (ufo_prober.is_mk()) |
if (is_mk()) |
return angle_nick; |
else if (ufo_prober.is_navi()) |
else if (is_navi()) |
return debug_data.analog[0]; |
return -1; |
|
62,10 → 72,10 |
public int Alt() // in dm |
{ |
int alt=0; |
if (ufo_prober.is_mk()) |
if (is_mk()) |
alt=debug_data.analog[5]/3; |
else |
if (ufo_prober.is_navi()) |
if (is_navi()) |
alt=gps_position.Altimeter/3; |
if ( alt<0) alt=0; // mk |
if ( alt>20000) alt=0; // navi |
80,9 → 90,10 |
} |
public int AngleRoll() |
{ |
if (ufo_prober.is_mk()) |
if (is_mk()) |
return angle_roll; |
else if (ufo_prober.is_navi()) |
else |
if (is_navi()) |
|
return debug_data.analog[1]; |
return -1; |
110,40 → 121,6 |
// public final static int DATA_IN_BUFF_SIZE=4096; |
|
public byte user_intent=0; |
public final static int[] crc16_table = { |
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, |
0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, |
0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, |
0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, |
0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, |
0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, |
0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, |
0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, |
0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, |
0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, |
0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, |
0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, |
0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, |
0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, |
0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, |
0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, |
0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, |
0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, |
0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, |
0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, |
0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, |
0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, |
0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, |
0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, |
0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, |
0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, |
0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, |
0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, |
0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, |
0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, |
0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, |
0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0 |
}; |
|
public void log(String str) |
{ |
154,12 → 131,6 |
// System.out.println(str); |
} |
|
public int CRC16(int ch, int crc) |
{ |
return crc16_table[((crc >> 8) ^ (ch)) & 0xFF] ^ (crc << 8); |
} |
|
|
public int conn_time_in_s() |
{ |
if (connected) |
167,11 → 138,10 |
else |
return 0; |
} |
public final static byte BOOTLOADER_STAGE_NONE=0; |
public final static byte BOOTLOADER_STAGE_GOT_MKBL=1; |
|
byte bootloader_stage= BOOTLOADER_STAGE_NONE; |
|
// byte bootloader_stage= BOOTLOADER_STAGE_NONE; |
|
public MKLCD LCD; |
public MKVersion version; |
public MKDebugData debug_data; |
187,7 → 157,7 |
public MKStatistics stats ; |
// public DUBwiseDebug debug; |
|
public UFOProber ufo_prober; |
// public UFOProber ufo_prober; |
public long connection_start_time=-1; |
|
|
194,13 → 164,84 |
public String error_str = null; |
|
|
public final static int FC_SLAVE_ADDR = 'a'+1; |
public final static int NAVI_SLAVE_ADDR = 'a'+2; |
public final static int MK3MAG_SLAVE_ADDR = 'a'+3; |
public final static byte FC_SLAVE_ADDR = 1; |
public final static byte NAVI_SLAVE_ADDR = 2; |
public final static byte MK3MAG_SLAVE_ADDR = 3; |
public final static byte FOLLOWME_SLAVE_ADDR = 10; |
public final static byte RE_SLAVE_ADDR = 23; |
|
|
|
|
public boolean is_navi() |
{ |
return (slave_addr==NAVI_SLAVE_ADDR); |
} |
|
|
public boolean is_mk() |
{ |
return (slave_addr==FC_SLAVE_ADDR); |
} |
|
public boolean is_mk3mag() |
{ |
return (slave_addr==MK3MAG_SLAVE_ADDR); |
} |
|
public boolean is_rangeextender() |
{ |
return (slave_addr==RE_SLAVE_ADDR); |
} |
|
public boolean is_followme() |
{ |
return (slave_addr==FOLLOWME_SLAVE_ADDR); |
} |
|
|
|
public boolean is_incompatible() |
{ |
switch (slave_addr) |
{ |
case FC_SLAVE_ADDR: |
case NAVI_SLAVE_ADDR: |
case MK3MAG_SLAVE_ADDR: |
case FOLLOWME_SLAVE_ADDR: |
case RE_SLAVE_ADDR: |
return false; |
default: |
return true; |
} |
} |
|
public String extended_name() |
{ |
switch (slave_addr) |
{ |
case -1: |
return "No Device"; |
|
case FC_SLAVE_ADDR: |
return "MK-Connection"; |
|
case NAVI_SLAVE_ADDR: |
return "Navi-Connection"; |
|
case MK3MAG_SLAVE_ADDR: |
return "MK3MAG-Connection"; |
|
case FOLLOWME_SLAVE_ADDR: |
return "FollowMe Connection"; |
case RE_SLAVE_ADDR: |
return "RangeExtender Connection"; |
|
default: |
return "Incompatible Device"; |
} |
} |
|
/****************** Section: private Attributes **********************************************/ |
//#ifdef j2me |
//# private javax.microedition.io.StreamConnection connection; |
248,7 → 289,7 |
gps_position=new MKGPSPosition(); |
stats = new MKStatistics(); |
proxy =new MKProxy(this); |
ufo_prober=new UFOProber(); |
// ufo_prober=new UFOProber(); |
new Thread( this ).start(); // fire up main Thread |
} |
|
381,6 → 422,8 |
|
} |
|
|
|
public void wait4send() |
{ |
while(sending) //||recieving) |
420,7 → 463,52 |
send_command(FC_SLAVE_ADDR,'t',params); |
} |
|
public void set_mixer_table(int[] params) |
{ |
|
send_command(FC_SLAVE_ADDR,'m',params); |
} |
|
|
public long lon; |
public long lat; |
|
public void send_follow_me(int time) |
{ |
long alt=0; |
|
int[] params=new int[29]; |
|
|
params[0]=(int)((lon)&0xFF) ; |
params[1]=(int)((lon>>8)&0xFF) ; |
params[2]=(int)((lon>>16)&0xFF) ; |
params[3]=(int)((lon>>24)&0xFF) ; |
|
params[4]=(int)((lat)&0xFF) ; |
params[5]=(int)((lat>>8)&0xFF) ; |
params[6]=(int)((lat>>16)&0xFF) ; |
params[7]=(int)((lat>>24)&0xFF) ; |
|
params[8]=(int)((alt)&0xFF ); |
params[9]=(int)((alt>>8)&0xFF ); |
params[10]=(int)((alt>>16)&0xFF); |
params[11]=(int)((alt>>24)&0xFF ); |
|
params[12]=2; // newdata |
|
params[13]=0; // heading |
params[14]=0; // heading |
|
params[15]=1; // tolerance |
|
params[16]=time; // time |
params[17]=0; // event |
|
send_command(NAVI_SLAVE_ADDR,'s',params); |
} |
|
|
public void send_extern_control() |
{ |
|
506,22 → 594,23 |
|
} |
|
public String[] flash_msgs; |
|
int msg_pos=0; |
|
|
public boolean bootloader_intension_flash=false; |
// public boolean bootloader_intension_flash=false; |
|
public boolean bootloader_finish_ok=false; |
// public boolean bootloader_finish_ok=false; |
|
public void jump_bootloader() |
{ |
|
// public void jump_bootloader() |
// { |
/* |
bootloader_finish_ok=false; |
msg_pos=0; |
bootloader_stage= BOOTLOADER_STAGE_NONE; |
flash_msgs=new String[100]; |
flash_msgs[msg_pos++]="Initiializing Bootloader"; |
// flash_msgs=new String[100]; |
// flash_msgs[msg_pos++]="Initiializing Bootloader"; |
wait4send(); |
sending=true; |
|
556,7 → 645,8 |
} |
|
new Thread( this ).start(); // fire up main Thread |
} |
*/ |
// } |
|
|
public void get_error_str() |
615,7 → 705,7 |
|
byte[] send_buff=new byte[3 + (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]=modul; |
send_buff[1]=(byte)(modul+'a'); |
send_buff[2]=(byte)cmd; |
|
for(int param_pos=0;param_pos<(params.length/3 + (params.length%3==0?0:1)) ;param_pos++) |
670,31 → 760,63 |
} |
|
|
public int slave_addr=-1; |
|
|
|
public int UBatt() |
{ |
if (ufo_prober.is_mk()) |
return debug_data.UBatt(); |
else if (ufo_prober.is_navi()) |
return gps_position.UBatt; |
|
return -1; |
|
switch (slave_addr) |
{ |
case FC_SLAVE_ADDR: |
return debug_data.analog[8]; |
|
case NAVI_SLAVE_ADDR: |
return gps_position.UBatt; |
|
case FOLLOWME_SLAVE_ADDR: |
return debug_data.analog[8]; |
|
default: |
return -1; // No Info |
} |
|
} |
|
|
|
public int SatsInUse() |
{ |
|
switch (slave_addr) |
{ |
case NAVI_SLAVE_ADDR: |
return gps_position.SatsInUse; |
|
case FOLLOWME_SLAVE_ADDR: |
return debug_data.analog[12]; |
|
default: |
return -1; // No Info |
} |
|
} |
|
|
|
public int SenderOkay() |
{ |
if (ufo_prober.is_mk()) |
return debug_data.SenderOkay(); |
else if (ufo_prober.is_navi()) |
return gps_position.SenderOkay; |
|
return -1; |
switch (slave_addr) |
{ |
case FC_SLAVE_ADDR: |
return debug_data.analog[10]; |
|
case NAVI_SLAVE_ADDR: |
return gps_position.SenderOkay; |
|
default: |
return -1; // No Info |
} |
} |
|
|
738,14 → 860,34 |
public void process_data(byte[] data,int len) |
{ |
|
slave_addr=data[1]; |
// check crc |
int tmp_crc=0; |
for ( int i=0 ; i<len-2 ; i++) |
tmp_crc+=(int)data[i]; |
|
tmp_crc%=4096; |
|
if (!((data[len-2]==(char)(tmp_crc/64 + '=')) |
&& |
(data[len-1]==(char)(tmp_crc%64 + '=')))) |
{ |
stats.crc_fail++; |
return; |
} |
// end of c |
|
// slave_addr=data[1]; |
log("command " +(char)data[2] ); |
|
|
int[] decoded_data=Decode64(data,3,len-5); |
|
switch((char)data[2]) |
{ |
|
case 'A': // debug Data Names |
stats.debug_names_count++; |
debug_data.set_names_by_mk_data(Decode64(data,3,len-3)); |
debug_data.set_names_by_mk_data(decoded_data); |
break; |
|
case 'B': // external_control confirm frames |
754,7 → 896,7 |
|
case 'L': // LCD Data |
stats.lcd_data_count++; |
LCD.handle_lcd_data(Decode64(data,3,len-3)); |
LCD.handle_lcd_data(decoded_data); |
|
break; |
|
761,9 → 903,9 |
case 'D': // debug Data |
log("got debug data"); |
stats.debug_data_count++; |
debug_data.set_by_mk_data(Decode64(data,3,len-3),version); |
debug_data.set_by_mk_data(decoded_data,version); |
|
if (ufo_prober.is_mk()) |
if (is_mk()) |
{ |
stats.process_mkflags(debug_data.motor_val(0)); // TODO remove dirty hack |
stats.process_alt(Alt()); |
783,8 → 925,13 |
|
case 'V': // Version Info |
stats.version_data_count++; |
slave_addr=data[1]; |
if (slave_addr!=data[1]-'a') |
{ |
slave_addr=(byte)(data[1]-'a'); |
change_notify=true; |
} |
|
/* |
switch(slave_addr) |
{ |
case FC_SLAVE_ADDR: |
796,70 → 943,69 |
break; |
|
case MK3MAG_SLAVE_ADDR: |
// ufo_prober.set_to_mk(); |
ufo_prober.set_to_mk3mag(); |
break; |
|
case RE_SLAVE_ADDR: |
ufo_prober.set_to_rangeextender(); |
break; |
|
default: |
ufo_prober.set_to_incompatible(); |
break; |
} |
*/ |
|
|
version.set_by_mk_data(Decode64(data,3,len-3)); |
version.set_by_mk_data(decoded_data); |
break; |
|
case 'w': |
int[] dec=Decode64(data,3,len-3); |
angle_nick=MKHelper.parse_signed_int_2(dec[0],dec[1]); |
angle_roll=MKHelper.parse_signed_int_2(dec[2],dec[3]); |
angle_nick=MKHelper.parse_signed_int_2(decoded_data[0],decoded_data[1]); |
angle_roll=MKHelper.parse_signed_int_2(decoded_data[2],decoded_data[3]); |
stats.angle_data_count++; |
|
break; |
|
|
case 'Q': |
if (ufo_prober.is_mk()) |
if (is_mk()) |
{ |
stats.params_data_count++; |
params.set_by_mk_data(Decode64(data,3,len-3)); |
params.set_by_mk_data(decoded_data); |
} |
break; |
|
|
case 'M': |
mixer_change_notify=true; |
mixer_change_success=(decoded_data[0]==1); |
break; |
case 'P': |
stats.stick_data_count++; |
stick_data.set_by_mk_data(Decode64(data,3,20)); |
stick_data.set_by_mk_data(decoded_data); |
break; |
|
|
case 'E': |
int[] dec_data=Decode64(data,3,len-3); |
case 'E': // Error Str from Navi |
error_str=""; |
for(int foo=0;foo<20;foo++) |
if (dec_data[foo]!=0) |
error_str+=(char)dec_data[foo]; |
if (decoded_data[foo]!=0) |
error_str+=(char)decoded_data[foo]; |
break; |
|
|
case 'O': |
|
case 'O': // OSD Values Str from Navi |
stats.navi_data_count++; |
log("got navi data(" + len +"):"); |
|
gps_position.set_by_mk_data(Decode64(data,3,len-3),version); |
gps_position.set_by_mk_data(decoded_data,version); |
|
stats.process_mkflags(gps_position.MKFlags); |
stats.process_compas(gps_position.CompasHeading); |
stats.process_speed(gps_position.GroundSpeed); |
stats.process_alt(Alt()); |
log("long:" + gps_position.Longitude); |
log("lat:" + gps_position.Latitude); |
|
break; |
|
|
// Error from Navi |
|
|
default: |
stats.other_data_count++; |
break; |
876,7 → 1022,7 |
{ |
|
// if ((!force)&&root.canvas.do_vibra) root.vibrate(500); |
force_disconnect=force; |
force_disconnect|=force; |
try{ reader.close(); } |
catch (Exception inner_ex) { } |
|
887,75 → 1033,30 |
//# try{ connection.close(); } |
//# catch (Exception inner_ex) { } |
//#endif |
ufo_prober.set_to_none(); |
slave_addr=-1; |
// ufo_prober.set_to_none(); |
stats.reset(); |
connected=false; |
version.reset(); |
// if (bootloader_stage==BOOTLOADER_STAGE_NONE) |
disconnect_notify=true; |
} |
|
|
int last_avr_sig=-1; |
// Thread to recieve data from Connection |
public void run() |
{ |
boolean sigfail=false; |
if (bootloader_stage==BOOTLOADER_STAGE_GOT_MKBL) |
// boolean sigfail=false; |
/* if (bootloader_stage==BOOTLOADER_STAGE_GOT_MKBL) |
{ |
try { |
flash_msgs[msg_pos++]="reading avr_sig"; |
|
writer.write( 't'); |
writer.flush(); |
|
int avr_sig=reader.read(); |
|
//while (avr_sig==63) |
// avr_sig=reader.read(); |
|
flash_msgs[msg_pos++]="got avr sig " + avr_sig; |
|
int avrsig_suff=reader.read(); |
if (avrsig_suff!=0) |
throw new Exception("val after avrsig is" +avrsig_suff +"should b 0"); |
|
if ((avr_sig!=0x74)&&(avr_sig!=224)&&(avr_sig!=120)) |
{ |
sigfail=true; |
throw new Exception("avr sig" + avr_sig + " unknown"); |
} |
|
writer.write('T'); |
// writer.flush(); |
writer.write(avr_sig); // set devicetyp = 0x74 oder 0x76 |
writer.flush(); |
|
if (reader.read()!=0x0d) |
throw new Exception("cant get buffer size"); |
|
writer.write('V'); |
writer.flush(); |
|
int bl_version_major=reader.read(); |
int bl_version_minor=reader.read(); |
|
flash_msgs[msg_pos++]="BL Version " + bl_version_major+"."+bl_version_minor; |
|
|
writer.write('b'); |
writer.flush(); |
|
if (reader.read()!='Y') |
throw new Exception("cant get buffer size"); |
|
int send_buff_size1=reader.read(); |
int send_buff_size2=reader.read(); |
int send_buff_size=send_buff_size1*0x100+send_buff_size2; |
|
flash_msgs[msg_pos++]="BUFF Size:" + send_buff_size; |
// if (send_buff_size>128) |
// send_buff_size=128; |
|
// if (!bootloader_intension |
if (bootloader_intension_flash) |
if (bootloader_intension_flash) |
{ |
|
byte[] flash_buff =new byte[send_buff_size]; ///!! |
977,13 → 1078,10 |
} |
catch (Exception e) { throw new Exception(" .. cant read size"); } |
|
|
|
int blocks2write=((firmware_size/send_buff_size)); |
flash_msgs[msg_pos++]=".. open("+blocks2write+" blocks," + firmware_size + "bytes)"; |
|
|
|
// if (true) throw new Exception("before erasing"); |
|
// if (true) throw new Exception("before erasing" ); |
1052,6 → 1150,9 |
|
// sleep(1000); |
} |
|
|
*/ |
// flash_msgs[msg_pos]="bl:" + block + "/" + blocks2write + " si:"+hex_bytes_read ; |
/* |
|
1119,6 → 1220,10 |
|
} |
*/ |
|
|
|
/** |
flash_msgs[++msg_pos]="written last block "; |
msg_pos++; |
flash_buff=null; |
1186,30 → 1291,24 |
sending=false; |
} |
|
**/ |
|
byte[] data_set=new byte[1024]; |
int data_set_pos=0; |
|
|
|
|
byte[] data_in_buff=new byte[DATA_IN_BUFF_SIZE]; |
|
int input; |
int pos=0; |
|
|
|
|
|
log("Thread started"); |
while(true) |
while(thread_running) |
{ |
|
if (!connected) |
{ |
sleep(10); |
if (!force_disconnect) connect(); |
sleep(100); |
} |
else |
try{ |
1275,18 → 1374,20 |
|
} |
else |
{ |
// { |
data_set[data_set_pos++]=data_in_buff[pos]; |
|
|
/* |
|
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')) |
|
{ |
|
bootloader_stage= BOOTLOADER_STAGE_GOT_MKBL; |
return; |
} |
|
} |
}*/ |
|
} |
|