@Override
public IMCMessage serializeToIMC() {
pt.up.fe.dceg.neptus.imc.StationKeeping message = new pt.up.fe.dceg.neptus.imc.StationKeeping();
double[] latLonDepth = this.getManeuverLocation().getAbsoluteLatLonDepth();
message.setLat(Math.toRadians(latLonDepth[0]));
message.setLon(Math.toRadians(latLonDepth[1]));
message.setZ(getManeuverLocation().getZ());
message.setZUnits(getManeuverLocation().getZUnits().toString());
message.setDuration(getDuration());
message.setSpeed(this.getSpeed());
String speedU = this.getSpeedUnits();
try {
if ("m/s".equalsIgnoreCase(speedU))
message.setSpeedUnits(pt.up.fe.dceg.neptus.imc.StationKeeping.SPEED_UNITS.METERS_PS);
else if ("RPM".equalsIgnoreCase(speedU))
message.setSpeedUnits(pt.up.fe.dceg.neptus.imc.StationKeeping.SPEED_UNITS.RPM);
else if ("%".equalsIgnoreCase(speedU))
message.setSpeedUnits(pt.up.fe.dceg.neptus.imc.StationKeeping.SPEED_UNITS.PERCENTAGE);
else if ("percentage".equalsIgnoreCase(speedU))
message.setSpeedUnits(pt.up.fe.dceg.neptus.imc.StationKeeping.SPEED_UNITS.PERCENTAGE);
} catch (Exception ex) {
NeptusLog.pub().error(this, ex);
}
message.setRadius(this.getRadius());
message.setCustom(getCustomSettings());
return message;
}
@Override
protected Vector<DefaultProperty> additionalProperties() {
Vector<DefaultProperty> props = new Vector<DefaultProperty>();
DefaultProperty duration =
PropertiesEditor.getPropertyInstance("Duration", Integer.class, this.duration, true);
duration.setShortDescription("The Station Keeping's duration, in seconds (0 means +Infinity)");
props.add(duration);
DefaultProperty speed =
PropertiesEditor.getPropertyInstance("Speed", Double.class, this.speed, true);
speed.setShortDescription("The vehicle's desired speed when Station Keeping");
props.add(speed);
DefaultProperty speedUnits =
PropertiesEditor.getPropertyInstance("Speed Units", String.class, this.speedUnits, true);
speedUnits.setShortDescription("The units to consider in the speed parameters");
PropertiesEditor.getPropertyEditorRegistry().registerEditor(speedUnits, new SpeedUnitsEditor());
PropertiesEditor.getPropertyRendererRegistry()
.registerRenderer(speedUnits, new I18nCellRenderer());
props.add(speedUnits);
DefaultProperty radius =
PropertiesEditor.getPropertyInstance("Radius", Double.class, this.radius, true);
radius.setShortDescription("Sets the radius of the trajectory");
props.add(radius);
for (DefaultProperty p : props) {
NeptusLog.pub().info("<###>* " + p.getName() + "=" + p.getValue());
}
return props;
}
@Override
public void loadFromXML(String XML) {
try {
Document doc = DocumentHelper.parseText(XML);
// basePoint
Node node = doc.selectSingleNode("StationKeeping/basePoint/point");
ManeuverLocation loc = new ManeuverLocation();
loc.load(node.asXML());
setManeuverLocation(loc);
// Speed
Node speedNode = doc.selectSingleNode("StationKeeping/speed");
setSpeed(Double.parseDouble(speedNode.getText()));
setSpeedUnits(speedNode.valueOf("@unit"));
// Duration
setDuration(Integer.parseInt(doc.selectSingleNode("StationKeeping/duration").getText()));
// Trajectory
setRadius(
Double.parseDouble(doc.selectSingleNode("StationKeeping/trajectory/radius").getText()));
} catch (Exception e) {
NeptusLog.pub().error(this, e);
return;
}
}
public static void main(String[] args) {
dumpAsHex(
"Teste", "Isto é de facto \0 um teste para ver se imprime bem Hex!".getBytes(), System.out);
dumpAsHex(FileUtil.getFileAsString("legal/Neptus-LICENSE.txt").getBytes(), System.out);
dumpAsHex(FileUtil.getFileAsString("src/images/auto-pilot.png").getBytes(), System.out);
String tt = "\0\1Isto é de facto um teste para ver se imprime bem Hex!";
tt = FileUtil.getFileAsString("legal/Neptus-LICENSE.txt");
NeptusLog.pub().info("<###> " + new String(Base64.encodeBase64Chunked(tt.getBytes())));
// dumpAsHex(tt.getBytes(), System.out);
NeptusLog.pub()
.info(
"<###> " + new String(Base64.decodeBase64(Base64.encodeBase64Chunked(tt.getBytes()))));
System.err.println(
dumpAsHexToString(FileUtil.getFileAsString("legal/Neptus-LICENSE.txt").getBytes()));
}
public static void main(String[] args) {
ImcId16Editor test = new ImcId16Editor();
test.imcId16Editor.addKeyListener(
new KeyAdapter() {
@Override
public void keyPressed(KeyEvent e) {
NeptusLog.pub().info("<###> " + e.getKeyCode());
}
});
test.setValue(new ImcId16("ed:01"));
GuiUtils.testFrame(test.imcId16Editor, "sdsd");
NeptusLog.pub().info("<###> " + test.getValue());
}
public void download(String destPath, FTPClient client) {
byte[] buffer = new byte[10240];
try {
new File(destPath + "/" + sourcePath).getParentFile().mkdirs();
FileOutputStream out = new FileOutputStream(destPath + "/" + sourcePath);
InputStream in = client.retrieveFileStream(sourcePath);
int counter = 0;
while (true) {
int bytes = in.read(buffer);
NeptusLog.pub().info("<###> " + bytes);
if (bytes < 0) break;
out.write(buffer, 0, bytes);
counter += bytes;
NeptusLog.pub().info("<###> " + counter);
}
NeptusLog.pub().info("<###>Finished Transfering");
out.close();
in.close();
} catch (Exception ex) {
ex.printStackTrace();
}
}
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";
}
