154
2016-10-03 22:21:36
0
recv_length = inputStream.read(buf, last_offset, BUF_SIZE-last_offset) + last_offset;
strfrom = 0;
totaldata = new String(buf, 0, recv_length);
while((strto = totaldata.indexOf("rn", strfrom)) != -1){
data = totaldata.substring(strfrom, strto);
data_length = data.length();
strfrom = strto + 2;
if((pchksum = data.lastIndexOf('*')) != -1){
chksum_recv = ByteHelper.fromHexToByte(data.substring(pchksum+1, data_length));
chksum_calc = getNmeaChecksum(data.getBytes(), 1, pchksum-1);
}else{
continue;
}
//chk checksum and start code
if((data.indexOf('$') != 0) || (chksum_recv != chksum_calc))
continue;
params = data.substring(1, pchksum).split(",", -1);
try{
if("GPGGA".equals(params[0]) && params.length > 11){
double oldlat = Double.parseDouble(params[2]);
double oldlon = Double.parseDouble(params[4]);
latitude = (int)(oldlat / 100) + ((oldlat % 100.0f) / 60.0f);
longitude = (int)(oldlon / 100) + ((oldlon % 100.0f) / 60.0f);
geoid_separation = Double.parseDouble(params[11]);
altitude_msl = Double.parseDouble(params[9]);
altitude_ellipsoid = altitude_msl + geoid_separation;
double[] geo = new double[3];
double[] ecef = new double[3];
geo[0] = latitude;
geo[1] = longitude;
geo[2] = altitude_ellipsoid;
Coord.geo_to_ecef(geo, ecef);
ecefX = ecef[0];
ecefY = ecef[1];
ecefZ = ecef[2];
}else if("GPVTG".equals(params[0])){
heading_degree = Double.parseDouble(params[1]);
horizonal_speed_knots = Double.parseDouble(params[5]);
horizonal_speed_kmh = Double.parseDouble(params[7]);
double radLat = latitude * Math.PI / 180.0;
double radLon = longitude * Math.PI / 180.0;
double sinLat = Math.sin(radLat);
double cosLat = Math.cos(radLat);
double sinLon = Math.sin(radLon);
double cosLon = Math.cos(radLon);
double radHead = heading_degree * Math.PI / 180.0;
double x = 0.0;
double y = Math.sin(radHead) * horizonal_speed_kmh;
double z = Math.cos(radHead) * horizonal_speed_kmh;
//Rotate by asix Y
x = x * cosLat + z * sinLat;
z = x *-sinLat + z * cosLat;
//Rotate by asix Z
x = x * cosLon + y *-sinLon;
y = x * sinLon + y * cosLon;
ecefXV = x;
ecefYV = y;
ecefZV = z;
velN = Math.cos(radHead) * horizonal_speed_kmh;
velE = Math.sin(radHead) * horizonal_speed_kmh;
velD = -vertical_speed_kmh;
}else if("GPRMC".equals(params[0])){
isVaild = "A".equals(params[2]);
rawtime = Double.parseDouble(params[1]);
rawdate = Integer.parseInt(params[9]);
int year, month, day, hour, min, sec, mill, dayofweek;
year = 2000 + (rawdate / 10000);
month = (rawdate / 100) % 100;
day = rawdate % 100;
hour = (int) (rawtime / 10000.0f);
min = (int) (rawtime / 100.0f) % 100;
sec = (int) (rawtime) % 100;
mill = (int) (rawtime * 1000) % 1000;
//CalcDate;
Calendar c = Calendar.getInstance(TimeZone.getTimeZone("GMT"));
c.set(Calendar.YEAR, year );
c.set(Calendar.MONTH, month );
c.set(Calendar.DAY_OF_MONTH, day );
c.set(Calendar.HOUR_OF_DAY, hour );
c.set(Calendar.MINUTE, min );
c.set(Calendar.SECOND, sec );
c.set(Calendar.MILLISECOND, mill );
dayofweek = c.get(Calendar.DAY_OF_WEEK);
iTOW = (float)(dayofweek * 24 * 60 * 60
+ hour * 60 * 60
+ min * 60
+ sec) + mill / 1000.0f;
}else if("GPGSA".equals(params[0])){
for(int i=0; i<12; i++){
String val = params[i+3];
if(val.isEmpty())
break;
sat_used[i] = Integer.parseInt(val);
sat_used_len = i+1;
}
pdop = Double.parseDouble(params[15]);
hdop = Double.parseDouble(params[16]);
vdop = Double.parseDouble(params[17]);
}else if("GPGSV".equals(params[0])){
sat_inview_len = Integer.parseInt(params[3]);
int totpage = Integer.parseInt(params[1]);
int curpage = Integer.parseInt(params[2]);
int id = (curpage - 1) * 4;
int len = 4;
if(totpage == curpage)
len = sat_inview_len - (totpage-1) * 4;
for(int i=0; i<len; i++, id++){
sat_inview_visible[id] = !params[i*4 + 7].isEmpty();
sat_inview_id[id] = Integer.parseInt(params[i*4 + 4]);
sat_inview_ele[id] = Integer.parseInt(params[i*4 + 5]);
sat_inview_azi[id] = Integer.parseInt(params[i*4 + 6]);
if(sat_inview_visible[id])
sat_inview_snr[id] = Integer.parseInt(params[i*4 + 7]);
else
sat_inview_snr[id] = 0;
}
}else{
System.out.println("NOT_DEF>" + data);
}
}catch(NumberFormatException e){}
}