Ejemplo n.º 1
0
  @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();
        }
      }
    }
  }
Ejemplo n.º 2
0
  @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;
  }
Ejemplo n.º 3
0
  @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"));
  }
Ejemplo n.º 4
0
 @Override
 public void parseIMCMessage(IMCMessage message) {
   setCustomSettings(message.getTupleList("custom"));
 }
Ejemplo n.º 5
0
  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";
  }