/* * (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 public void parseIMCMessage(IMCMessage message) { if (!DEFAULT_ROOT_ELEMENT.equalsIgnoreCase(message.getAbbrev())) return; pt.lsts.imc.Elevator elev = null; try { elev = pt.lsts.imc.Elevator.clone(message); } catch (Exception e) { e.printStackTrace(); return; } setMaxTime(elev.getTimeout()); ManeuverLocation loc = new ManeuverLocation(); loc.setLatitudeRads(elev.getLat()); loc.setLongitudeRads(elev.getLon()); loc.setZ(elev.getEndZ()); NeptusLog.pub().info("<###> " + elev.getEndZUnits()); // // loc.setZUnits(pt.lsts.neptus.mp.ManeuverLocation.Z_UNITS.valueOf(elev.getEndZUnits().toString())); loc.setZUnits(ManeuverLocation.Z_UNITS.valueOf(message.getString("end_z_units").toString())); setManeuverLocation(loc); startZ = (float) elev.getStartZ(); startZUnits = ManeuverLocation.Z_UNITS.valueOf(message.getString("start_z_units").toString()); setRadius((float) elev.getRadius()); setSpeed(elev.getSpeed()); setStartFromCurrentPosition((elev.getFlags() & pt.lsts.imc.Elevator.FLG_CURR_POS) != 0); setCustomSettings(elev.getCustom()); switch (elev.getSpeedUnits()) { case RPM: speedUnits = "RPM"; break; case METERS_PS: speedUnits = "m/s"; break; case PERCENTAGE: speedUnits = "%"; break; default: break; } }
@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; }