@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();
}