Beispiel #1
0
  @Override
  public boolean init(
      String vehicleId, Loiter man, SystemPositionAndAttitude state, Object manState) {
    enteredLoiter = false;
    destination = new LocationType(man.getManeuverLocation());
    if (man.getManeuverLocation().getZUnits() == ManeuverLocation.Z_UNITS.DEPTH)
      destination.setDepth(man.getManeuverLocation().getZ());
    else if (man.getManeuverLocation().getZUnits() == ManeuverLocation.Z_UNITS.ALTITUDE)
      destination.setDepth(-man.getManeuverLocation().getZ());
    else destination.setDepth(0);

    clockwise = man.getDirection().equalsIgnoreCase("Clockwise");
    radius = man.getRadius();
    loiterType = man.getLoiterType();
    duration = man.getLoiterDuration();
    speed = man.getSpeed();

    if (man.getSpeedUnits().equals("RPM")) speed = SpeedConversion.convertRpmtoMps(speed);
    else if (man.getSpeedUnits().equals("%")) // convert to RPM and then to m/s
    speed = SpeedConversion.convertPercentageToMps(speed);

    speed = Math.min(speed, SpeedConversion.MAX_SPEED);

    if (manState != null && manState instanceof Double) {
      loiterTime = (Double) manState;
    }
    model.setState(state);
    return true;
  }
Beispiel #2
0
  @Override
  public SystemPositionAndAttitude step(SystemPositionAndAttitude state, double timestep) {
    model.setState(state);
    double distToDestination = state.getPosition().getDistanceInMeters(destination);
    if (loiterType.equalsIgnoreCase("circular")) {

      if (distToDestination - 2 > radius) {
        model.guide(
            destination, speed, destination.getDepth() >= 0 ? null : -destination.getDepth());
        if (!enteredLoiter) loiterTime = 0;
      } else {
        enteredLoiter = true;
        double perpend = state.getPosition().getXYAngle(destination) + Math.PI / 2;
        LocationType loc = new LocationType(state.getPosition());
        loc.setDepth(destination.getDepth());
        if (clockwise) loc.translatePosition(Math.cos(perpend) * -20, Math.sin(perpend) * -20, 0);
        else loc.translatePosition(Math.cos(perpend) * 20, Math.sin(perpend) * 20, 0);
        model.guide(loc, speed, destination.getDepth() >= 0 ? null : -destination.getDepth());
        loiterTime += timestep;
      }
    } else {
      if (distToDestination < speed * 2) loiterTime += timestep;
      else
        model.guide(
            destination, speed, destination.getDepth() >= 0 ? null : -destination.getDepth());
    }

    model.advance(timestep);

    if (loiterTime > duration) finished = true;

    return model.getState();
  }