@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; }
@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(); }
@Override public void reset(SystemPositionAndAttitude state) { model.setState(state); }