@Override
protected boolean initSystemComms() {
VehicleType vehicleaux = VehiclesHolder.getVehicleWithImc(getSystemCommId());
ImcSystem resSys = ImcSystemsHolder.lookupSystem(getSystemCommId());
if (resSys == null) {
if (vehicleaux != null) {
resSys = new ImcSystem(vehicleaux);
ImcSystemsHolder.registerSystem(resSys);
}
}
if (resSys == null) {
NeptusLog.pub()
.error("Error creating private sending transports for " + getSystemCommId() + ".");
return false;
}
// CommMean commMean = CommUtil.getActiveCommMeanForProtocol(
// vehicleaux, "imc");
CommMean commMean = resSys.commsInfo;
if (commMean == null) {
NeptusLog.pub()
.error("Error creating private sending transports for " + getSystemCommId() + ".");
return false;
}
this.ipAddress = commMean.getHostAddress();
this.ipRemotePort = ((IMCArgs) (commMean.getProtocolsArgs().get(CommMean.IMC))).getPort();
return true;
}
/*
* (non-Javadoc)
*
* @see pt.lsts.neptus.util.comm.manager.CommonCommBaseImplementation#triggerExtraActionOnSetActive(boolean,
* pt.lsts.neptus.messages.listener.MessageInfo, pt.lsts.neptus.messages.IMessage)
*/
@Override
protected void triggerExtraActionOnSetActive(
boolean isActive, MessageInfo info, IMCMessage message) {
ImcSystem sys = ImcSystemsHolder.lookupSystem(getSystemCommId());
if (sys != null) {
if (message == null
|| !StringUtils.isTokenInList("Announce,EntityList", message.getAbbrev())) {
if (!useActivityCounter) {
sys.setActive(isActive);
// NeptusLog.pub().info("<###> "+sys.getName()+": "+isActive()+" "+(message !=
// null?message.getAbbrevName():""));
} else {
// If IMCAuthorityState.OFF then we consider not active
if (sys.getAuthorityState() == ImcSystem.IMCAuthorityState.OFF) {
sys.setActive(false);
} else if (!isActive) {
sys.setActive(false);
} else {
activityCounter.add(System.currentTimeMillis());
int vecSize = activityCounter.size();
if (vecSize > 3) {
activityCounter.remove(0);
}
vecSize = activityCounter.size();
if (vecSize == 3) {
if (activityCounter.get(2) - activityCounter.get(0) <= 3000) sys.setActive(true);
else sys.setActive(false);
} else {
sys.setActive(false);
}
}
}
}
}
}
@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;
}