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;
}