@Subscribe
public void handleEstimatedState(IMCMessage msg) {
if (msg.getSrc() == systemIdToListen && msg.getAbbrev().equals(messageToListen)) {
int srcEnt = msg.getHeader().getInteger("src_ent");
if (entities.containsKey(srcEnt)) {
entities.get(srcEnt).setMessage(msg);
} else {
String title = "";
if (srcEnt != 0xff) {
title = EntitiesResolver.resolveName(systemField.getText(), srcEnt);
} else {
title = "N/A";
}
// Ignore messages relative to an Entity that is still unknown
if (title == null) return;
InspectorMessagePanel imp = new InspectorMessagePanel();
imp.setTitle("Source Entity: " + title);
// pane.add(new JLabel("\u2206t " + (msg.getTimestamp() -
// lastMessageTimeByEntity.get(srcEnt))));
if (!title.equals("N/A")) pane.add(imp, "wrap, w 600, h 200!");
else pane.add(imp, "wrap");
entities.put(srcEnt, imp);
imp.setMessage(msg);
try {
SwingUtilities.invokeAndWait(
new Runnable() {
@Override
public void run() {
jsp.invalidate();
jsp.validate();
jsp.repaint();
}
});
} catch (Exception e) {
e.printStackTrace();
}
}
}
}
@Override
public boolean canBeApplied(IMraLogGroup source) {
IMraLog log = source.getLog("SonarData");
if (log == null) return false;
IMCMessage first = log.firstLogEntry();
IMCMessage msg = first;
while (msg != null) {
// Wait 2 seconds for the first valid multibeam SonarData message //FIXME
if (msg.getTimestampMillis() > first.getTimestampMillis() + 2000) break;
if (msg.getLong("type") == SonarData.TYPE.MULTIBEAM.value()) {
return true;
}
msg = log.nextLogEntry();
}
return false;
}
@Override
public void parseIMCMessage(IMCMessage message) {
setMaxTime((int) message.getDouble("timeout"));
setSpeed(message.getDouble("speed"));
ManeuverLocation pos = new ManeuverLocation();
pos.setLatitude(Math.toDegrees(message.getDouble("lat")));
pos.setLongitude(Math.toDegrees(message.getDouble("lon")));
pos.setZ(message.getDouble("z"));
String zunits = message.getString("z_units");
if (zunits != null)
pos.setZUnits(pt.up.fe.dceg.neptus.mp.ManeuverLocation.Z_UNITS.valueOf(zunits));
setManeuverLocation(pos);
String speed_units = message.getString("speed_units");
if (speed_units.equals("METERS_PS")) setSpeedUnits("m/s");
else if (speed_units.equals("RPM")) setSpeedUnits("RPM");
else setSpeedUnits("%");
setDuration((int) message.getDouble("duration"));
setRadius(message.getDouble("radius"));
setCustomSettings(message.getTupleList("custom"));
}
@Override
public void parseIMCMessage(IMCMessage message) {
setCustomSettings(message.getTupleList("custom"));
}
public String process() {
try {
File outFile = new File(log.getFile("Data.lsf").getParentFile() + "/multibeam.837");
os = new DataOutputStream(new FileOutputStream(outFile));
pingLog = log.getLog("SonarData");
esLog = log.getLog("EstimatedState");
} catch (FileNotFoundException e) {
e.printStackTrace();
return e.getClass().getSimpleName() + " while exporting to 837: " + e.getMessage();
}
IMCMessage pingMsg = pingLog.firstLogEntry();
byte[] buffer;
byte[] prevBuffer;
byte[] zeroFill = new byte[236];
prevBuffer = new byte[16000];
short pitch;
short roll;
short heading;
String lat = "";
String lon = "";
String sTime = "";
String sMillis = "";
double res[] = new double[2];
Calendar cal = Calendar.getInstance();
cal.setTimeZone(TimeZone.getTimeZone("UTC"));
new LocationType();
// Build zeroFill padding
for (int i = 0; i < zeroFill.length; i++) {
zeroFill[i] = 0;
}
try {
while (pingMsg != null) {
// Check for Sidescan message and multibeam entity
if (pingMsg.getInteger("type") == SonarData.TYPE.MULTIBEAM.value()) {
IMCMessage esMsg = esLog.getEntryAtOrAfter(pingLog.currentTimeMillis());
if (esMsg == null) {
roll = 900;
pitch = 900;
heading = 900;
} else {
roll = (short) (Math.toDegrees(esMsg.getDouble("phi")));
pitch = (short) (Math.toDegrees(esMsg.getDouble("theta")));
heading = (short) (Math.toDegrees(esMsg.getDouble("psi")));
res =
CoordinateUtil.latLonAddNE2(
Math.toDegrees(esMsg.getDouble("lat")),
Math.toDegrees(esMsg.getDouble("lon")),
esMsg.getDouble("x"),
esMsg.getDouble("y"));
int d = (int) res[0];
double m = ((res[0] - d) * 60);
lat = String.format(" %02d.%.5f", Math.abs(d), Math.abs(m)) + (d > 0 ? " N" : " S");
d = (int) res[1];
m = ((res[1] - d) * 60);
lon = String.format("%03d.%.5f", Math.abs(d), Math.abs(m)) + (d > 0 ? " E" : " W");
//
NeptusLog.pub().info("<###> " + lat);
NeptusLog.pub().info("<###> " + lon);
// if(heading < 0)
// heading = (short) (360 + heading);
}
long timestamp = esLog.currentTimeMillis();
cal.setTimeInMillis(timestamp);
String month = "";
switch (cal.get(Calendar.MONTH)) {
case 0:
month = "JAN";
break;
case 1:
month = "FEB";
break;
case 2:
month = "MAR";
break;
case 3:
month = "APR";
break;
case 4:
month = "MAY";
break;
case 5:
month = "JUN";
break;
case 6:
month = "JUL";
break;
case 7:
month = "AUG";
break;
case 8:
month = "SEP";
break;
case 9:
month = "OCT";
break;
case 10:
month = "NOV";
break;
case 11:
month = "DEC";
break;
}
sTime =
String.format(
"%02d-%s-%d\0%02d:%02d:%02d\0.00\0",
cal.get(Calendar.DAY_OF_MONTH),
month,
cal.get(Calendar.YEAR),
cal.get(Calendar.HOUR_OF_DAY),
cal.get(Calendar.MINUTE),
cal.get(Calendar.SECOND));
sMillis = String.format(".%03d\0", cal.get(Calendar.MILLISECOND));
buffer = new byte[pingMsg.getRawData("data").length];
// Header
os.write("837".getBytes());
os.writeByte((buffer.length == 8000 ? 10 : 11)); // 10 = 8000(IUX), 11 = 16000(IVX)
os.writeShort(buffer.length == 8000 ? 8192 : 16384); // Number of total bytes to read
os.writeShort(buffer.length + 13); // Number of bytes only for data points
os.write(sTime.getBytes()); // TIMESTAMP
os.writeInt(0);
os.writeByte(0x83); // 11000011 = 1 Reserved, 1 Xcdr Up, 000 Reserved and 011 Profile
os.writeByte(1); // Start gain
os.write(new byte[] {0, 0}); // Tilt Angle
os.write(new byte[] {0, 0, 7}); // Reserved, Reserved, Pings Averaged
os.writeByte(18); // Pulse length in us/10
os.writeByte(0); // User defined byte
os.writeShort(0); // sound speed short ( 0 = 1500ms )
os.write(lat.getBytes()); // Lat and Lon NMEA style
os.write(lon.getBytes());
os.writeByte(0); // Speed
os.writeShort(0); // Course
os.writeByte(0); // and a Reserved byte as 0
os.writeShort(260); // 260Hz operating frequency
os.writeShort((pitch * 10 + 900) + 32768); // Pitch = 0
os.writeShort((roll * 10 + 900) + 32768); // Roll
os.writeShort(0x8000); // Heading = 0
os.writeShort(97); // Repetition rate in ms
os.writeByte(50);
os.writeShort(0); // 2 reserved bytes 0
os.write(sMillis.getBytes());
os.writeShort(0);
// Sonar return header
os.write(buffer.length == 8000 ? "IUX".getBytes() : "IVX".getBytes());
os.write(
new byte[] {
16, // Head ID
0, // Serial status
7, // Packet Number
36, // Version
(byte) pingMsg.getInteger("max_range"), // Range
0, // reserved
0, // reserved
// 3,
// -24
});
os.writeShort(buffer.length); // data bytes
System.arraycopy(pingMsg.getRawData("data"), 0, buffer, 0, buffer.length);
// Echo values
os.write(buffer);
os.writeByte(0xFC); // Trailing value always 0xFC
// Exta bytes and zero-fill
os.writeFloat(0); // Offset X
os.writeFloat(0); // Offset Y
os.writeFloat(0); // Offset Z
os.writeByte(1); // Sensor type (?)
os.writeShort(pitch); // Pitch
os.writeShort(roll); // Roll
os.writeShort(heading); // Heading
os.writeShort(0); // Timer Ticks
os.writeShort(0); // Azimuth Head Position
os.writeByte(1); // Azimuth Up/Down
os.writeFloat(0); // Heave
os.write(new byte[] {0, 0, 0, 0, 0, 0, 0}); // 7 reserved bytes
if (buffer.length == 8000)
os.write(zeroFill, 0, 44); // in case we have only 8000 bytes
else os.write(zeroFill);
System.arraycopy(buffer, 0, prevBuffer, 0, buffer.length);
}
pingMsg = pingLog.nextLogEntry();
}
} catch (Exception e) {
e.printStackTrace();
return e.getClass().getSimpleName() + " while exporting to 837: " + e.getMessage();
}
NeptusLog.pub().info("<###>end");
return "Export to 837 completed successfully";
}
