Ejemplo n.º 1
0
  private DataDiscretizer parseLog(String entity) {
    DataDiscretizer dd = new DataDiscretizer(10);

    IMraLog parser = logSource.getLog(messageName);
    IMraLog stateParser = logSource.getLog("EstimatedState");

    if (parser == null || stateParser == null) {
      return dd;
    }

    IMCMessage entry = parser.nextLogEntry();
    IMCMessage stateEntry = stateParser.nextLogEntry();
    LocationType ref = IMCUtils.getLocation(stateEntry).convertToAbsoluteLatLonDepth();

    entityList.clear();
    entityList.add("ALL");
    while (entry != null) {
      parser.advance(1);
      entry = parser.getCurrentEntry();
      if (entry != null) {
        String entName = logSource.getEntityName(entry.getSrc(), entry.getSrcEnt());

        if (!entityList.contains(entName)) entityList.add(entName);

        if (!entity.equalsIgnoreCase("ALL") && !entName.equalsIgnoreCase(entity)) continue;

        stateEntry = stateParser.getEntryAtOrAfter(parser.currentTimeMillis());

        LocationType loc = new LocationType();
        loc.setLatitudeRads(stateEntry.getDouble("lat"));
        loc.setLongitudeRads(stateEntry.getDouble("lon"));
        loc.translatePosition(
            stateEntry.getDouble("x"), stateEntry.getDouble("y"), stateEntry.getDouble("z"));
        double[] offsets = loc.getOffsetFrom(ref);

        double[] vals = new double[4];

        vals[0] = Double.NaN;

        if (stateEntry != null) {
          vals[0] = entry.getDouble(varName);
          dd.addPoint(offsets[1], -offsets[0], vals);
        }
      }
    }
    entCombo.setModel(new DefaultComboBoxModel<>(entityList));

    return dd;
  }
  @Override
  protected boolean processMsgLocally(MessageInfo info, IMCMessage msg) {
    // msg.dump(System.out);
    // System.out.flush();

    ImcSystem resSys = ImcSystemsHolder.lookupSystem(systemCommId);

    if (resSys != null) {
      if (resSys.getAuthorityState() == ImcSystem.IMCAuthorityState.OFF) return false;
    }

    logMessage(info, msg);

    try {
      if (bus != null) bus.post(msg);
    } catch (Exception e1) {
      e1.printStackTrace();
    } catch (Error e1) {
      e1.printStackTrace();
    }

    imcState.setMessage(msg);

    if (resSys == null) return true;

    switch (msg.getMgid()) {
      case VehicleState.ID_STATIC:
        try {
          int errorCount = msg.getInteger("error_count");
          if (errorCount > 0) resSys.setOnErrorState(true);
          else resSys.setOnErrorState(false);

          Object errEntStr = msg.getValue("error_ents");
          if (errEntStr != null) resSys.setOnErrorStateStr(errEntStr.toString());
          else resSys.setOnErrorStateStr("");
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case PlanControlState.ID_STATIC:
        try {
          String planId = msg.getString("plan_id");
          String maneuver = msg.getString("man_id");
          String state = msg.getString("state");
          PlanType plan = new PlanType(null);
          plan.setId(planId + "|" + I18n.textc("Man", "Maneuver (short form)") + ":" + maneuver);
          if ("EXECUTING".equalsIgnoreCase(state)) resSys.setActivePlan(plan);
          else resSys.setActivePlan(null);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case EmergencyControlState.ID_STATIC:
        try {
          String planId;
          planId = msg.getString("plan_id");

          if (planId == null) planId = msg.getString("mission_id");

          String state = msg.getString("state");
          resSys.setEmergencyPlanId(planId);
          resSys.setEmergencyStatusStr(state);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case EstimatedState.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();

          double lat = msg.getDouble("lat");
          double lon = msg.getDouble("lon");
          double height = msg.getDouble("height");
          msg.getDouble("depth");
          msg.getDouble("altitude");
          double x = msg.getDouble("x");
          double y = msg.getDouble("y");
          double z = msg.getDouble("z");
          double phi = msg.getDouble("phi");
          double theta = msg.getDouble("theta");
          double psi = msg.getDouble("psi");

          LocationType loc = new LocationType();
          loc.setLatitudeRads(lat);
          loc.setLongitudeRads(lon);
          loc.setHeight(height);
          loc.setOffsetNorth(x);
          loc.setOffsetEast(y);
          loc.setOffsetDown(z);
          loc.convertToAbsoluteLatLonDepth();

          if (loc != null) {
            resSys.setLocation(loc, timeMillis);
          }

          resSys.setAttitudeDegrees(
              Math.toDegrees(phi), Math.toDegrees(theta), Math.toDegrees(psi), timeMillis);

          // double u = msg.getDouble("u");
          // double v = msg.getDouble("v");
          // double w = msg.getDouble("w");
          double vx = msg.getDouble("vx");
          double vy = msg.getDouble("vy");
          double vz = msg.getDouble("vz");

          double courseRad = AngleCalc.calcAngle(0, 0, vy, vx);
          double groundSpeed = Math.sqrt(vx * vx + vy * vy);
          double verticalSpeed = vz;

          resSys.storeData(
              ImcSystem.COURSE_KEY,
              (int)
                  AngleCalc.nomalizeAngleDegrees360(
                      MathMiscUtils.round(Math.toDegrees(courseRad), 0)),
              timeMillis,
              true);
          resSys.storeData(ImcSystem.GROUND_SPEED_KEY, groundSpeed, timeMillis, true);
          resSys.storeData(ImcSystem.VERTICAL_SPEED_KEY, verticalSpeed, timeMillis, true);

          double headingRad = msg.getDouble("psi");
          resSys.storeData(
              ImcSystem.HEADING_KEY,
              (int)
                  AngleCalc.nomalizeAngleDegrees360(
                      MathMiscUtils.round(Math.toDegrees(headingRad), 0)),
              timeMillis,
              true);

        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case SimulatedState.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();
          resSys.storeData(msg.getAbbrev(), msg, timeMillis, true);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case OperationalLimits.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();
          resSys.storeData(msg.getAbbrev(), msg, timeMillis, true);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case IndicatedSpeed.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();
          double value = msg.getDouble("value");
          resSys.storeData(ImcSystem.INDICATED_SPEED_KEY, value, timeMillis, true);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case TrueSpeed.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();
          double value = msg.getDouble("value");
          resSys.storeData(ImcSystem.TRUE_SPEED_KEY, value, timeMillis, true);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case PlanDB.ID_STATIC:
        try {
          resSys.getPlanDBControl().onMessage(info, msg);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case Rpm.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();
          int entityId = (Integer) msg.getHeaderValue("src_ent");
          final int value = msg.getInteger("value");
          if (entityId == 0xFF) {
            resSys.storeData(ImcSystem.RPM_MAP_ENTITY_KEY, value, timeMillis, true);
          } else {
            final String entityName = EntitiesResolver.resolveName(resSys.getName(), entityId);
            if (entityName != null) {
              Object obj = resSys.retrieveData(ImcSystem.RPM_MAP_ENTITY_KEY);
              if (obj == null) {
                Map<String, Integer> map =
                    (Map<String, Integer>)
                        Collections.synchronizedMap(new HashMap<String, Integer>());
                map.put(entityName, value);
                resSys.storeData(ImcSystem.RPM_MAP_ENTITY_KEY, map, timeMillis, true);
              } else {
                @SuppressWarnings("unchecked")
                Map<String, Integer> rpms =
                    (Map<String, Integer>) resSys.retrieveData(ImcSystem.RPM_MAP_ENTITY_KEY);
                rpms.put(entityName, value);
                resSys.storeData(ImcSystem.RPM_MAP_ENTITY_KEY, rpms, timeMillis, false);
              }
            }
          }
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case FuelLevel.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();
          FuelLevel fuelLevelMsg = (FuelLevel) msg;
          resSys.storeData(ImcSystem.FUEL_LEVEL_KEY, fuelLevelMsg, timeMillis, true);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case LblConfig.ID_STATIC:
        try {
          if (((LblConfig) msg).getOp() == OP.CUR_CFG)
            resSys.storeData(
                ImcSystem.LBL_CONFIG_KEY, (LblConfig) msg, msg.getTimestampMillis(), true);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case AcousticSystems.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();
          AcousticSystems acousticSystemsMsg = (AcousticSystems) msg;
          resSys.storeData(ImcSystem.ACOUSTIC_SYSTEMS, acousticSystemsMsg, timeMillis, true);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;
      default:
        break;
    }

    return true;
  }