public ManeuverLocation clone() {
ManeuverLocation loc = new ManeuverLocation();
loc.setLocation(this);
loc.setZ(getZ());
loc.setZUnits(getZUnits());
return loc;
}
public ManeuverLocation(LocationType loc) {
super(loc);
if (loc instanceof ManeuverLocation) {
ManeuverLocation mloc = (ManeuverLocation) loc;
this.setZ(mloc.getZ());
this.setZUnits(mloc.getZUnits());
}
}
public void setHorizontalLocation(LocationType newLoc) {
newLoc.convertToAbsoluteLatLonDepth();
loc.setLatitude(newLoc.getLatitudeAsDoubleValue());
loc.setLongitude(newLoc.getLongitudeAsDoubleValue());
reference.setLat(newLoc.getLatitudeAsDoubleValueRads());
reference.setLon(newLoc.getLongitudeAsDoubleValueRads());
latitude = newLoc.getLatitudeAsDoubleValue();
longitude = newLoc.getLongitudeAsDoubleValue();
}
public ReferenceWaypoint(Reference ref) {
try {
this.reference = Reference.clone(ref);
} catch (Exception e) {
e.printStackTrace();
}
loc = new ManeuverLocation();
loc.setLatitudeRads(ref.getLat());
loc.setLongitudeRads(ref.getLon());
if (ref.getZ() != null) {
loc.setZ(ref.getZ().getValue());
loc.setZUnits(ManeuverLocation.Z_UNITS.valueOf(ref.getZ().getZUnits().name()));
}
boolean defineZ = (ref.getFlags() & Reference.FLAG_Z) != 0;
if (!defineZ) loc.setZUnits(ManeuverLocation.Z_UNITS.NONE);
loiter = (ref.getFlags() & Reference.FLAG_RADIUS) != 0;
this.loiterRadius = ref.getRadius();
this.latitude = loc.getLatitudeAsDoubleValue();
this.longitude = loc.getLongitudeAsDoubleValue();
this.z = loc.getZ();
this.zUnits = loc.getZUnits();
if (ref.getSpeed() != null) {
this.speedUnits = ref.getSpeed().getSpeedUnits();
this.speed = ref.getSpeed().getValue();
}
defineSpeed = (ref.getFlags() & Reference.FLAG_SPEED) != 0;
}
public void setZ(DesiredZ desiredZ) {
if (desiredZ == null) {
reference.setFlags((short) (reference.getFlags() ^ Reference.FLAG_Z));
reference.setZ(null);
loc.setZUnits(ManeuverLocation.Z_UNITS.NONE);
zUnits = ManeuverLocation.Z_UNITS.NONE;
} else {
reference.setZ(new DesiredZ((float) desiredZ.getValue(), desiredZ.getZUnits()));
reference.setFlags((short) (reference.getFlags() | Reference.FLAG_Z));
loc.setZ(desiredZ.getValue());
loc.setZUnits(ManeuverLocation.Z_UNITS.valueOf(desiredZ.getZUnits().name()));
zUnits = loc.getZUnits();
z = loc.getZ();
}
}
public ReferenceWaypoint(ManeuverLocation loc, double speed) {
loc.convertToAbsoluteLatLonDepth();
this.loc = loc.clone();
this.latitude = loc.getLatitudeAsDoubleValue();
this.longitude = loc.getLongitudeAsDoubleValue();
this.speed = speed;
this.speedUnits = SPEED_UNITS.METERS_PS;
this.z = loc.getZ();
this.zUnits = loc.getZUnits();
reference = new Reference();
reference.setLat(loc.getLatitudeAsDoubleValueRads());
reference.setLon(loc.getLongitudeAsDoubleValueRads());
reference.setZ(new DesiredZ((float) loc.getZ(), Z_UNITS.valueOf(loc.getZUnits().name())));
reference.setSpeed(new DesiredSpeed(speed, SPEED_UNITS.METERS_PS));
reference.setFlags((short) (Reference.FLAG_LOCATION | Reference.FLAG_SPEED | Reference.FLAG_Z));
}
@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;
}
}
/* (non-Javadoc)
* @see pt.lsts.neptus.mp.Maneuver#loadFromXML(java.lang.String)
*/
@Override
public void loadFromXML(String xml) {
try {
Document doc = DocumentHelper.parseText(xml);
Node node = doc.selectSingleNode(DEFAULT_ROOT_ELEMENT + "/finalPoint/point");
if (node == null)
node =
doc.selectSingleNode(
DEFAULT_ROOT_ELEMENT + "/initialPoint/point"); // to read old elevator specs
ManeuverLocation loc = new ManeuverLocation();
loc.load(node.asXML());
setManeuverLocation(loc);
Node speedNode = doc.selectSingleNode(DEFAULT_ROOT_ELEMENT + "/speed");
setSpeed(Double.parseDouble(speedNode.getText()));
setSpeedUnits(speedNode.valueOf("@unit"));
Node sz = doc.selectSingleNode(DEFAULT_ROOT_ELEMENT + "/startZ");
if (sz == null)
doc.selectSingleNode(DEFAULT_ROOT_ELEMENT + "/endZ"); // to read old elevator specs
setStartZ(sz == null ? 0 : Float.parseFloat(sz.getText()));
Node szu = doc.selectSingleNode(DEFAULT_ROOT_ELEMENT + "/startZUnits");
setStartZUnits(
szu == null
? ManeuverLocation.Z_UNITS.NONE
: ManeuverLocation.Z_UNITS.valueOf(szu.getText()));
setRadius(Float.parseFloat(doc.selectSingleNode(DEFAULT_ROOT_ELEMENT + "/radius").getText()));
Element flags = (Element) doc.selectSingleNode(DEFAULT_ROOT_ELEMENT + "/flags");
if (flags == null) {
setStartFromCurrentPosition(false);
} else {
Node ucl = flags.selectSingleNode("@useCurrentLocation");
if (ucl == null) setStartFromCurrentPosition(false);
else setStartFromCurrentPosition(Boolean.parseBoolean(ucl.getText()));
}
} catch (Exception e) {
NeptusLog.pub().error(this, e);
return;
}
}
public static void main(String[] args) {
ManeuverLocation loc = new ManeuverLocation();
loc.setZ(10);
loc.setZUnits(Z_UNITS.ALTITUDE);
String xml = loc.asXML();
ManeuverLocation loc2 = new ManeuverLocation();
loc2.load(xml);
NeptusLog.pub().info("<###> " + loc2.asXML());
}
@Override
public void propertiesChanged() {
loc.setZ(z);
loc.setZUnits(zUnits);
reference = new Reference();
if (loiter) reference.setRadius(loiterRadius);
reference.setLat(loc.getLatitudeAsDoubleValueRads());
reference.setLon(loc.getLongitudeAsDoubleValueRads());
if (loc.getZUnits() != ManeuverLocation.Z_UNITS.NONE)
reference.setZ(new DesiredZ((float) loc.getZ(), Z_UNITS.valueOf(loc.getZUnits().name())));
if (defineSpeed) reference.setSpeed(new DesiredSpeed(speed, speedUnits));
reference.setFlags(
(short)
(Reference.FLAG_LOCATION
| (defineSpeed ? Reference.FLAG_SPEED : 0)
| (loc.getZUnits() != ManeuverLocation.Z_UNITS.NONE ? Reference.FLAG_Z : 0)
| (loiter ? Reference.FLAG_RADIUS : 0)));
}
/** @param endZ the endZ to set */
public void setEndZ(float endZ) {
location.setZ(endZ);
}
@Override
public void setManeuverLocation(ManeuverLocation loc) {
location = loc.clone();
setStartZ((float) loc.getZ());
}
@Override
public ManeuverLocation getManeuverLocation() {
return location.clone();
}