Error Fix Movement

src/de/tud/kom/p2psim/api/topology/component/ControllableLocationActuator.java

@@ -90,6 +90,13 @@ public interface ControllableLocationActuator extends Actuator, LocationSensor {
 	
 	public double estimatePowerConsumptionWatt(double velocity);
 	
+	/**
+	 * 
+	 * @param velocity 
+	 * @param batterylevel [0.0, 1.0]
+	 * @param batterythreshold [0.0, 1.0]
+	 * @return
+	 */
 	public double estimateFlightDistance(double velocity, double batterylevel, double batterythreshold);	
 
 	public double getOptimalMovementSpeed();

src/de/tud/kom/p2psim/impl/topology/component/UAVTopologyComponent.java

@@ -303,9 +303,8 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
 	public void returnToBase(double velocity, ReachedLocationCallback cb) {
 		if(!isActive()) 
 			return;
-		this.setState(UAVstate.RETURN);
-		movement.setTargetVelocity(velocity);
-			
+		this.setState(UAVstate.RETURN);		
+		movement.setTargetVelocity(velocity);			
 		movement.setTargetLocationRoute(getBaseReturnRoute(), cb);
 	}
 
@@ -458,6 +457,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
 		return distance;
 	}
 	
+	@Override
 	public double getOptimalMovementSpeed() {
 		return movement.optimalSpeed();
 	}

src/de/tud/kom/p2psim/impl/topology/movement/aerial/MulticopterMovement.java

@@ -284,6 +284,10 @@ public class MulticopterMovement implements UAVMovementModel  {
 		return uav.powerConsumptionWatt(velocity);
 	}
 	
+	public MulticopterModel getUAVModel() {
+		return this.uav;
+	}
+	
 		
 	/**
 	 * Factory for this movement model
@@ -345,10 +349,10 @@ public class MulticopterMovement implements UAVMovementModel  {
 
 		}
 		
-		public static EnergyModelPropertyMeasurement getPropoertyMeasurement(StateMulticopterMovement m, double velocity, double thrust, double watt) {		
-			// FIXME 
-			double amp = watt / 14.8;			
-			return new EnergyModelPropertyMeasurement(velocity, thrust, amp, watt);
+		public static EnergyModelPropertyMeasurement getPropoertyMeasurement(MulticopterMovement m, double velocity) {		
+			// FIXME only useful with UAVs that use 14.8V
+			double amp = m.getUAVModel().powerConsumptionWatt(velocity) / 14.8;			
+			return new EnergyModelPropertyMeasurement(velocity, m.getUAVModel().thrustForHorizontalVelocity(velocity), amp, m.getUAVModel().powerConsumptionWatt(velocity));
 		}
 
 	}

src/de/tud/kom/p2psim/impl/topology/movement/aerial/StateMulticopterMovement.java

@@ -27,12 +27,16 @@ import java.util.Map;
 import javax.persistence.Column;
 import javax.persistence.Entity;
 import javax.persistence.Table;
+
+import org.apache.logging.log4j.Level;
+
 import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
 import de.tud.kom.p2psim.api.uav.MulticopterModel;
 import de.tud.kom.p2psim.impl.energy.components.ActuatorComponent;
 import de.tud.kom.p2psim.impl.topology.component.UAVTopologyComponent;
 import de.tud.kom.p2psim.impl.topology.util.PositionVector;
 import de.tud.kom.p2psim.impl.util.db.metric.CustomMeasurement;
+import de.tudarmstadt.maki.simonstrator.api.Monitor;
 import de.tudarmstadt.maki.simonstrator.api.Time;
 import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;
 
@@ -172,6 +176,11 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 	 */
 	private long approachTarget(long time) {
 		
+		if(state != MobilityState.TARGET) {
+			Monitor.log(getClass(), Monitor.Level.ERROR, "Wrong State", state);
+			return time;
+		}
+		
 		switch(intrastate) {
 			case STOP:
 			case ADJUST:
@@ -294,10 +303,9 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 		}
 		
 		double v = flightVelocity();
-		if(v != velocityVector.getLength()) {
-			if( Math.abs(v - velocityVector.getLength()) > MARGIN) {
-				throw new AssertionError("Not a rounding error!");
-			}
+		if(v != velocityVector.getLength() && Math.abs(v - velocityVector.getLength()) > MARGIN) {				
+					Monitor.log(StateMulticopterMovement.class, Monitor.Level.WARN, 
+							"The Multicopter velocity is not within the margin to the target velocity. Deviation: " +Math.abs(v - velocityVector.getLength())+"m/s", velocityVector);		
 		}
 				
 		double distanceToTarget = positionVector.distanceTo(currentTargetPosition);
@@ -365,7 +373,11 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 	 */
 	private long horizontalDeceleration(long time) {
 		assert velocityVector.getZ() == 0;		
-		assert horizontalAngleToPosition(currentTargetPosition) == 0;
+		
+		if( horizontalAngleToPosition(currentTargetPosition) != 0) {
+			Monitor.log(StateMulticopterMovement.class, Monitor.Level.WARN, 
+					"The Multicopter is not properly aligned to the target. Deviation: " + Math.toDegrees(horizontalAngleToPosition(currentTargetPosition))+"°", positionVector);	
+		}		
 		
 		double v = velocityVector.getLength();
 		
@@ -988,7 +1000,14 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 	private void locationReached(PositionVector position) {
 		
 		assert position == currentTargetPosition;
-		assert position == route.getFirst();
+		try {
+			assert position == route.getFirst();
+		}
+		catch (AssertionError e) {
+			// TODO: handle exception
+			System.out.println(position);
+			System.out.println();
+		}
 		
 		// remove current target position when reached			
 		route.removeFirst();
@@ -1005,7 +1024,11 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 	}
 	
 	@Override
-	public void removeTargetLocations() {
+	public void removeTargetLocations() {		
+		Monitor.log(getClass(), Monitor.Level.INFO, "All targets removed, abort current action", 0);
+		
+		intrastate = IntraState.STOP;
+		currentTargetPosition = null;
 		route.clear();
 		locationCallbacks.clear();
 	}		
@@ -1049,7 +1072,10 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 	@Override
 	public void setTargetLocation(PositionVector target,
 			ReachedLocationCallback reachedLocationCallback) {
-		route.clear();
+		
+		if(currentTargetPosition != null || this.route.size() > 0 || intrastate != IntraState.STOP) {
+			removeTargetLocations();
+		}		
 		route.add(target);
 		if(reachedLocationCallback != null)
 			locationCallbacks.put(target, reachedLocationCallback);		
@@ -1058,7 +1084,9 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 	@Override
 	public void setTargetLocationRoute(LinkedList<PositionVector> route,
 			ReachedLocationCallback reachedLocationCallback) {
-		this.route.clear();
+		if(currentTargetPosition != null || this.route.size() > 0 || intrastate != IntraState.STOP) {
+			removeTargetLocations();
+		}
 		this.route.addAll(route);
 		if(reachedLocationCallback != null)
 			locationCallbacks.put(route.getLast(), reachedLocationCallback);