/*
  * (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);
           }
         }
       }
     }
   }
 }
예제 #2
0
파일: Elevator.java 프로젝트: kcisek/neptus
  @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;
  }