/********************************************************************************************************************************
*
* 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.ufo;
//#ifdef j2me
//# import javax.microedition.io.*;
//#endif
//#ifdef android
import android.util.Log;
//#endif
import java.io.*;
public class MKCommunicator
implements Runnable,DUBwiseDefinitions
{
public int angle_nick=-
4242;
public int angle_roll=-
4242;
public byte bl_retrys=
0;
public boolean init_bootloader=
false;
public byte lib_version_major=
0;
public byte lib_version_minor=
4;
public String lib_version_str
()
{
return "V"+lib_version_major+
"."+lib_version_minor
;
}
/***************** Section: public Attributes **********************************************/
public boolean connected=
false; // flag for the connection state
public String mk_url=
""; // buffer the url which is given in the constuctor for reconnectin purposes
public final static int DATA_BUFF_LEN =
20; // in lines
public String[] data_buff
;
// boolean do_log=false;
boolean do_log=
true;
int data_buff_pos=
0;
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
)
{
//#ifdef android
if (do_log
) Log.
d("MK-Comm",str
);
//#endif
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
)
return (int)((System.
currentTimeMillis()-connection_start_time
)/
1000);
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
;
public MKLCD LCD
;
public MKVersion version
;
public MKDebugData debug_data
;
public int[] extern_control
;
public MKGPSPosition gps_position
;
public MKStickData stick_data
;
public MKParamsParser params
;
public MKWatchDog watchdog
;
public MKProxy proxy=
null;
public MKStatistics stats
;
// public DUBwiseDebug debug;
public UFOProber ufo_prober
;
public long connection_start_time=-
1;
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;
/****************** Section: private Attributes **********************************************/
//#ifdef j2me
//# private javax.microedition.io.StreamConnection connection;
//#endif
//#ifdef android
// java.net.Socket connection;
java.
net.
Socket connection
;
//#endif
private java.
io.
InputStream reader
;
private java.
io.
OutputStream writer
;
public String name
;
// DUBwise root;
private boolean sending=
false;
private boolean recieving=
false;
/****************** Section: public Methods ************************************************/
public MKCommunicator
()
{
data_buff=
new String[DATA_BUFF_LEN
];
for (int i=
0;i
<DATA_BUFF_LEN
;i++
)
data_buff
[i
]=
"";
// debug=debug_;
// root=root_;
version=
new MKVersion
();
debug_data=
new MKDebugData
();
stick_data=
new MKStickData
();
params=
new MKParamsParser
();
extern_control=
new int[EXTERN_CONTROL_LENGTH
];
extern_control
[EXTERN_CONTROL_CONFIG
]=
1;
extern_control
[EXTERN_CONTROL_FRAME
]=
1;
LCD=
new MKLCD
(this);
watchdog=
new MKWatchDog
(this);
gps_position=
new MKGPSPosition
();
stats =
new MKStatistics
();
proxy =
new MKProxy
(this);
ufo_prober=
new UFOProber
();
new Thread( this ).
start(); // fire up main Thread
}
public void write_raw
(byte[] _data
)
{
wait4send
();
sending=
true;
try {
writer.
write(_data,
0,_data.
length);
writer.
flush();
stats.
bytes_out+=_data.
length;
}
catch ( Exception e
){}
sending=
false;
}
public void do_proxy
(String proxy_url
)
{
proxy.
connect(proxy_url
);
}
// int port;
// 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
)
{
// port=_port;
mk_url=_url
; // remember URL for connecting / reconnecting later
name=_name
;
force_disconnect=
false;
connected=
false;
}
public boolean ready
()
{
return (connected
&&(version.
major!=-
1));
}
public String get_buff
(int age
)
{
age
%=DATA_BUFF_LEN
;
if (age
<=data_buff_pos
)
return ""+data_buff
[data_buff_pos-age
];
else
return ""+data_buff
[DATA_BUFF_LEN+data_buff_pos-age
];
}
/****************** Section: private Methods ************************************************/
private void connect
()
{
log
("trying to connect to" + mk_url
);
try{
// old call
// connection = (StreamConnection) Connector.open(mk_url, Connector.READ_WRITE);
//#ifdef android
connection =
(new java.
net.
Socket(mk_url.
split(":")[0],
Integer.
parseInt(mk_url.
split(":")[1])));
//.Socket
reader=connection.
getInputStream();
writer=connection.
getOutputStream();
String magic=
"conn:foo bar\r\n";
writer.
write(magic.
getBytes());
writer.
flush();
//#else
//# connection = (StreamConnection) Connector.open(mk_url);
//# reader=connection.openInputStream();
//# writer=connection.openOutputStream();
//#endif
connection_start_time=
System.
currentTimeMillis();
connected=
true; // if we get here everything seems to be OK
stats.
reset();
log
("connecting OK");
}
catch (Exception ex
)
{
// TODO difference fatal errors from those which will lead to reconnection
log
("Problem connecting" +
"\n" + ex
);
}
}
public int[] Decode64
(byte[] 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=
0;
b=
0;
c=
0;
d=
0;
try {
a = in_arr
[ptrIn++
] -
'=';
b = in_arr
[ptrIn++
] -
'=';
c = in_arr
[ptrIn++
] -
'=';
d = in_arr
[ptrIn++
] -
'=';
}
catch (Exception e
) {}
//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
;
}
public void wait4send
()
{
while(sending
) //||recieving)
sleep
(50);
}
public void sleep
(int time
)
{
try { Thread.
sleep(time
); }
catch (Exception e
) { }
}
// 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
()
{
stats.
version_data_request_count++
;
send_command
(0,
'v');
}
public void set_gps_target
(int longitude,
int latitude
)
{
int[] target=
new int[8];
target
[0]=
(0xFF
)&(longitude
<<24);
target
[1]=
(0xFF
)&(longitude
<<16);
target
[2]=
(0xFF
)&(longitude
<<8);
target
[3]=
(0xFF
)&(longitude
);
// send_command(0,'s',target);
}
// send a MotorTest request - params are the speed for each Motor
public void motor_test
(int[] params
)
{
stats.
motortest_request_count++
;
send_command
(FC_SLAVE_ADDR,
't',params
);
}
public void send_extern_control
()
{
stats.
external_control_request_count++
;
send_command
(FC_SLAVE_ADDR,
'b',extern_control
);
}
/* public void send_keys(int[] params)
{
send_command(FC_SLAVE_ADDR,'k',params);
}*/
// get params
public void get_params
(int id
)
{
wait4send
();
send_command
(FC_SLAVE_ADDR,
'q',id+
1);
stats.
params_data_request_count++
;
}
public void get_debug_name
(int id
)
{
wait4send
();
send_command
(0,
'a',id
);
}
public void trigger_LCD_by_page
(int page
)
{
wait4send
();
send_command
(0,
'l',page
);
stats.
lcd_data_request_count++
;
}
public void trigger_debug
()
{
if (sending||recieving
) return; // its not that important - can be dropped
send_command
(0,
'c');
}
public void switch_todo
()
{
sleep
(50);
version=
new MKVersion
();
LCD=
new MKLCD
(this);
debug_data=
new MKDebugData
();
}
public void switch_to_fc
()
{
wait4send
();
send_command
(NAVI_SLAVE_ADDR,
'u',
0);
switch_todo
();
}
public void switch_to_mk3mag
()
{
wait4send
();
send_command
(NAVI_SLAVE_ADDR ,
'u',
1);
switch_todo
();
}
public final static byte[] navi_switch_magic=
{27,
27,0x55,
(byte)0xAA,
0,
(byte)'\r'};
public void switch_to_navi
()
{
wait4send
();
sending=
true;
try
{
writer.
write(navi_switch_magic
);
stats.
bytes_out+=
6;
writer.
flush();
}
catch (Exception e
) { }
sending=
false;
switch_todo
();
}
public String[] flash_msgs
;
int msg_pos=
0;
public boolean bootloader_intension_flash=
false;
public boolean bootloader_finish_ok=
false;
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";
wait4send
();
sending=
true;
try
{
int attempt=
0;
while(bootloader_stage
!= BOOTLOADER_STAGE_GOT_MKBL
)
{
flash_msgs
[msg_pos
]=
"attempt "+attempt
;
attempt++
;
send_command_nocheck
((byte)FC_SLAVE_ADDR,
'R',
new int[0]);
try{
writer.
write( 27);
writer.
flush();
sleep
(20);
writer.
write( 0xAA
);
writer.
flush();
}
catch (Exception e
) { }
sleep
((attempt
%2==
0)?80:
800); //800
}
msg_pos++
;
}
catch (Exception e
) {
flash_msgs
[msg_pos++
]=
"Exception:" +e.
getMessage() ;
flash_msgs
[msg_pos++
]=e.
toString() ;
}
new Thread( this ).
start(); // fire up main Thread
}
public void get_error_str
()
{
send_command
(NAVI_SLAVE_ADDR,
'e');
}
public void trigger_rcdata
()
{
send_command
(FC_SLAVE_ADDR,
'p');
}
public void write_params
(int to
)
{
params.
update_backup(to
);
wait4send
();
params.
active_paramset=to
;
send_command
(FC_SLAVE_ADDR,
's',params.
field_bak[to
]);
}
public void send_command
(int modul,
char cmd
)
{
send_command
(modul,cmd,
new int[0]);
}
public void send_command
(int modul,
char cmd,
int param
)
{
int[] params=
new int[1];
params
[0]=param
;
send_command
(modul,cmd,params
);
}
public void send_command_nocheck
(byte 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
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
[2]=
(byte)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] =
(byte)((a
>> 2)+
'=' );
send_buff
[3+param_pos
*4+
1] =
(byte)('=' +
(((a
& 0x03
) << 4) |
((b
& 0xf0
) >> 4)));
send_buff
[3+param_pos
*4+
2] =
(byte)('=' +
(((b
& 0x0f
) << 2) |
((c
& 0xc0
) >> 6)));
send_buff
[3+param_pos
*4+
3] =
(byte)('=' +
( 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,
0,send_buff.
length);
tmp_crc
%=
4096;
writer.
write( (char)(tmp_crc/
64 +
'='));
writer.
write( (char)(tmp_crc
%64 +
'='));
writer.
write('\r');
stats.
bytes_out+=send_buff.
length+
3;
writer.
flush();
}
catch (Exception e
)
{ // problem sending data to FC
}
}
// send command to FC ( add crc and pack into pseudo Base64
public void send_command
(int modul,
char cmd,
int[] params
)
{
// if (modul==0) return;
sending=
true;
send_command_nocheck
((byte)modul,cmd,params
);
sending=
false;
}
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;
}
public int SenderOkay
()
{
if (ufo_prober.
is_mk())
return debug_data.
SenderOkay();
else if (ufo_prober.
is_navi())
return gps_position.
SenderOkay;
return -
1;
}
public void process_data
(byte[] data,
int len
)
{
log
("command " +
(char)data
[2] );
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));
break;
case 'B':
// external_control confirm frames
stats.
external_control_confirm_frame_count++
;
break;
case 'L':
// LCD Data
stats.
lcd_data_count++
;
LCD.
handle_lcd_data(Decode64
(data,
3,len-
3));
break;
case 'D':
// debug Data
log
("got debug data");
stats.
debug_data_count++
;
debug_data.
set_by_mk_data(Decode64
(data,
3,len-
3),version
);
log
("processed debug data");
break;
case 'V':
// Version Info
stats.
version_data_count++
;
slave_addr=data
[1];
switch(slave_addr
)
{
case FC_SLAVE_ADDR:
ufo_prober.
set_to_mk();
break;
case NAVI_SLAVE_ADDR:
ufo_prober.
set_to_navi();
break;
case MK3MAG_SLAVE_ADDR:
// ufo_prober.set_to_mk();
ufo_prober.
set_to_mk3mag();
break;
default:
ufo_prober.
set_to_incompatible();
break;
}
version.
set_by_mk_data(Decode64
(data,
3,len-
3));
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]);
stats.
angle_data_count++
;
break;
case 'Q':
if (ufo_prober.
is_mk())
{
stats.
params_data_count++
;
params.
set_by_mk_data(Decode64
(data,
3,len-
3));
}
break;
case 'P':
stats.
stick_data_count++
;
stick_data.
set_by_mk_data(Decode64
(data,
3,
20));
break;
case 'E':
int[] dec_data=Decode64
(data,
3,len-
3);
error_str=
"";
for(int foo=
0;foo
<20;foo++
)
if (dec_data
[foo
]!=
0)
error_str+=
(char)dec_data
[foo
];
break;
case 'O':
stats.
navi_data_count++
;
log
("got navi data(" + len +
"):");
gps_position.
set_by_mk_data(Decode64
(data,
3,len-
3),version
);
log
("long:" + gps_position.
Longitude);
log
("lat:" + gps_position.
Latitude);
break;
// Error from Navi
default:
stats.
other_data_count++
;
break;
}
}
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
) { }
//#ifdef j2me
//# try{ connection.close(); }
//# catch (Exception inner_ex) { }
//#endif
ufo_prober.
set_to_none();
stats.
reset();
connected=
false;
version=
new MKVersion
();
}
// Thread to recieve data from Connection
public void run
()
{
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
)
{
byte[] flash_buff =
new byte[send_buff_size
]; ///!!
String firmware_filename=
(avr_sig==
224)?"/navi.bin":
((avr_sig==
120)?"/mk3.bin":
"/fc.bin");
flash_msgs
[msg_pos++
]=
"Opening firmware " + firmware_filename +
"..";
InputStream in
;
try {
in=
this.
getClass().
getResourceAsStream(firmware_filename
);
}
catch (Exception e
) { throw new Exception(" .. cant open firmware"); }
int firmware_size=-
1;
try {
firmware_size=
((int)in.
read()<<24) |
((int)in.
read()<<16) |
((int)in.
read()<<8) |
((int)in.
read()&0xff
) ;
}
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" );
flash_msgs
[msg_pos++
]=
"Erasing Flash ..";
writer.
write('e');
writer.
flush();
if (reader.
read()!=0x0d
)
throw new Exception("cant erase flash");
flash_msgs
[msg_pos
]+=
"OK";
writer.
write('A');
writer.
write(0);
writer.
write(0);
writer.
flush();
if (reader.
read()!=0x0d
)
throw new Exception("cant set addr");
flash_msgs
[msg_pos++
]=
"addr set";
// int blocks2write=((firmware_size/send_buff_size));
if ((firmware_size
%send_buff_size
)>0)
blocks2write++
;
for ( int block=
0; block
<blocks2write
; block ++
)
{
int hex_bytes_read=in.
read(flash_buff,
0,send_buff_size
);
flash_msgs
[msg_pos
]=
"bl:" + block +
"/" + blocks2write +
" si:"+hex_bytes_read
;
writer.
write('B');
writer.
write((hex_bytes_read
>>8)& 0xFF
);
writer.
write((hex_bytes_read
)& 0xFF
);
writer.
write('F');
writer.
flush();
writer.
write(flash_buff,
0,hex_bytes_read
);
writer.
flush();
if (avr_sig==
224)
{
int crc=0xFFFF
;
for (int crc_pos=
0;crc_pos
<hex_bytes_read
;crc_pos++
)
crc=CRC16
(flash_buff
[crc_pos
],crc
);
writer.
write(crc
>>8);
writer.
write(crc
&0xff
);
writer.
flush();
}
// flash_msgs[msg_pos]+="ok";
// writer.flush();
if (reader.
read()!=0x0d
)
throw new Exception("abort write at block"+block
);
// sleep(1000);
}
// flash_msgs[msg_pos]="bl:" + block + "/" + blocks2write + " si:"+hex_bytes_read ;
/*
int inp=0;
int block=0;
while (inp!=-1)
{
int flash_buff_pos=0;
int crc=0xFFFF;
while ((flash_buff_pos<send_buff_size)&&(inp!=-1))
{
inp=in.read();
if (inp!=-1)
{
crc=CRC16(inp,crc);
flash_buff[flash_buff_pos++]=(byte)inp;
}
}
// flash_msgs[msg_pos]="block" + block + "size:"+flash_buff_pos;
block++;
boolean block_fin=false;
while(!block_fin)
{
writer.write('B');
writer.write((flash_buff_pos>>8)& 0xFF);
writer.write((flash_buff_pos)& 0xFF);
writer.write('F');
writer.flush();
// int ret_v=-1;
writer.write(flash_buff,0,flash_buff_pos);
flash_msgs[msg_pos]="bl:" + block + "si:"+flash_buff_pos ;
writer.flush();
// flash_msgs[msg_pos]+="wtc";
// append crc if navi
if (avr_sig==224)
{
writer.write(crc>>8);
writer.write(crc&0xff);
writer.flush();
}
// flash_msgs[msg_pos]+="ok";
// writer.flush();
// if (reader.read()!=0x0d)
// throw new Exception("abort write at block"+block);
//ret_v=reader.read();
// flash_msgs[msg_pos]="ret"+ret_v + "crc"+crc;
if (reader.read()==0x0d)
block_fin=true;
}
}
*/
flash_msgs
[++msg_pos
]=
"written last block ";
msg_pos++
;
flash_buff=
null;
ufo_prober.
set_to_none();
stats.
reset();
version=
new MKVersion
();
System.
gc();
}
else // bootloader intension clear settings
{
flash_msgs
[msg_pos
]=
"reset params ..";
writer.
write('B');
writer.
write(0);
writer.
write(4);
writer.
write('E');
writer.
flush();
writer.
write(0xFF
);
writer.
write(0xFF
);
writer.
write(0xFF
);
writer.
write(0xFF
);
writer.
flush();
flash_msgs
[msg_pos++
]+=
" done";
}
flash_msgs
[msg_pos++
]=
"Exiting Bootloader" ;
params=
new MKParamsParser
();
try{
writer.
write('E');
writer.
flush();
}
catch (Exception e
) {
flash_msgs
[msg_pos++
]=
"cant exit bootloader" ;
}
flash_msgs
[msg_pos++
]=
"Exit BL done" ;
bootloader_finish_ok=
true;
}
catch (Exception e
) {
flash_msgs
[msg_pos++
]=
"Fail:" +e.
getMessage() ;
flash_msgs
[msg_pos++
]=
"Exiting Bootloader" ;
params=
new MKParamsParser
();
try{
writer.
write('E');
writer.
flush();
}
catch (Exception e2
) {
flash_msgs
[msg_pos++
]=
"cant exit bootloader" ;
}
flash_msgs
[msg_pos++
]=
"Exit BL done" ;
if (sigfail
&&(bl_retrys
<3))
{
bl_retrys++
;
init_bootloader=
true;
}
close_connections
(false);
}
sending=
false;
}
byte[] data_set=
new byte[1024];
int data_set_pos=
0;
byte[] data_in_buff=
new byte[2048];
int input
;
int pos=
0;
log
("Thread started");
while(true)
{
if (!connected
)
{
if (!force_disconnect
) connect
();
sleep
(100);
}
else
try{
/*
while(sending)
{try { Thread.sleep(50); }
catch (Exception e) { }
}
*/
recieving=
true;
int read_count =reader.
read(data_in_buff,
0,reader.
available());
log
("Connected - reading data " + read_count
);
// pos=0;
input=
0;
//data_buff[data_buff_pos]="";
// recieve data-set
if (read_count==
0) sleep
(50);
// int read_count =reader.read(data_in_buff,0,reader.available());
stats.
bytes_in+=read_count
;
if (read_count
>0)
{
log
("read" + read_count +
" ds_pos" + data_set_pos
);
for ( pos=
0;pos
<read_count
;pos++
)
{
if (data_in_buff
[pos
]==
13)
{
data_buff
[data_buff_pos
]=
new String(data_set,
0, data_set_pos
);
data_buff_pos++
;
data_buff_pos
%=DATA_BUFF_LEN
;
try{process_data
(data_set,data_set_pos
); }
catch (Exception e
)
{
log
(".. problem processing");
log
(e.
toString());
}
proxy.
write(data_set,
0,data_set_pos
);
// proxy.writer.write('\r');
//proxy.writer.write('\n');
//proxy.writer.flush();
/*
if (proxy!=null)
{
}
*/
data_set_pos=
0;
}
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;
}
}
}
}
else
sleep
(50);
/*
while ((input != 13)) //&&(input!=-1))
{
{
//log("pre read");
log(""+reader.available());
input = reader.read() ;
log("Byte rcv" + input +"pos"+ pos);
proxy.write(input);
data_buff[data_buff_pos]+=(char)input;
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")))
{
bootloader_stage= BOOTLOADER_STAGE_GOT_MKBL;
return;
}
if (input==-1) throw new Exception("disconnect");
else
{
stats.bytes_in++;
data_set[pos]=input;
pos++;
}
}
}
data_buff_pos++;
data_buff_pos%=DATA_BUFF_LEN;
recieving=false;
log("Data recieved (" + pos + "Bytes)");
log("processing ..");
*/
/*
if (proxy!=null)
{
proxy.writer.write('\r');
proxy.writer.write('\n');
proxy.writer.flush();
}
*/
/*if (pos>5)
{
try{process_data(data_set,pos); }
catch (Exception e)
{
log(".. problem processing");
log(e.toString());
}
log(".. processing done");
}
*/
}
catch (Exception ex
)
{
log
("Problem reading from MK -> closing conn");
log
(ex.
toString());
// close the connection
close_connections
(false);
}
// sleep a bit to get someting more done
// sleep(5); //50
} // while
// log("Leaving Communicator thread");
} // run()
}