Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 380 → Rev 381

/DUBwise/trunk/shared/src/MKCommunicator.java
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;
}
}
}*/
 
}