Multiple adaptions of movement models for UAVs. Made possible to switch...

Multiple adaptions of movement models for UAVs. Made possible to switch between stateless actuator model and stateful actuator model for UAVs.

src/de/tud/kom/p2psim/api/topology/movement/SimUAVLocationActuator.java

@@ -21,8 +21,8 @@
 package de.tud.kom.p2psim.api.topology.movement;
 
 import de.tud.kom.p2psim.api.topology.component.ControllableLocationActuator;
-import de.tud.kom.p2psim.impl.energy.components.StateActuatorEnergyComponent;
-import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
+import de.tud.kom.p2psim.impl.energy.components.StatefulActuatorComponent;
+import de.tud.kom.p2psim.impl.energy.components.StatelessActuatorComponent;
 
 
 public interface SimUAVLocationActuator extends SimLocationActuator, ControllableLocationActuator {

src/de/tud/kom/p2psim/api/topology/movement/UAVMovementModel.java

@@ -22,20 +22,21 @@ package de.tud.kom.p2psim.api.topology.movement;
 
 import java.util.LinkedList;
 
-import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
+import de.tud.kom.p2psim.impl.energy.components.ActuatorComponent;
+import de.tud.kom.p2psim.impl.energy.components.StatelessActuatorComponent;
 import de.tud.kom.p2psim.impl.topology.util.PositionVector;
 import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;
 
 public interface UAVMovementModel {
 	
-	public void setMotorControl(StatelessMotorComponent motor);
+	public void setMotorControl(ActuatorComponent motor);
 	
 	public void setPreferredSpeed(double v_pref);
 	
 	public double verticalAscentMaxVelocity();
 	public double horizontalMaxVelocity();
 	public double getCurrentSpeed();
-	public double minimumVelocity();
+	public double horizontalMinVelocity();
 	
 	public double estimatePowerConsumption(double velocity);
 	

src/de/tud/kom/p2psim/impl/energy/components/ActuatorComponent.java

+/*
+ * Copyright (c) 2005-2010 KOM – Multimedia Communications Lab
+ *
+ * This file is part of PeerfactSim.KOM.
+ * 
+ * PeerfactSim.KOM is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * any later version.
+ * 
+ * PeerfactSim.KOM is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ * 
+ * You should have received a copy of the GNU General Public License
+ * along with PeerfactSim.KOM.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+package de.tud.kom.p2psim.impl.energy.components;
+
+import de.tud.kom.p2psim.api.energy.EnergyComponent;
+
+// Interface Tag
+public interface ActuatorComponent extends EnergyComponent {
+
+}

src/de/tud/kom/p2psim/impl/energy/components/StateActuatorEnergyComponent.java → src/de/tud/kom/p2psim/impl/energy/components/StatefulActuatorComponent.java

@@ -22,7 +22,6 @@ package de.tud.kom.p2psim.impl.energy.components;
 
 
 import de.tud.kom.p2psim.api.energy.ComponentType;
-import de.tud.kom.p2psim.api.energy.EnergyComponent;
 import de.tud.kom.p2psim.api.energy.EnergyEventListener;
 import de.tud.kom.p2psim.api.energy.EnergyState;
 import de.tud.kom.p2psim.impl.energy.DefaultEnergyState;
@@ -31,12 +30,12 @@ import de.tudarmstadt.maki.simonstrator.api.Time;
 
 /**
  * Energy component for actuators, representing the electrical consumers of actuators.
- *  This models several states, where energy is consumed differently. 
+ * This models uses several states, that define the used energy. 
  * 
  * @author Julian Zobel
  * @version 1.0, 11.09.2018
  */
-public class StateActuatorEnergyComponent implements EnergyComponent {
+public class StatefulActuatorComponent implements ActuatorComponent {
 	
 	/**
 	 * States supported by this energy component
@@ -53,7 +52,7 @@ public class StateActuatorEnergyComponent implements EnergyComponent {
 	
 	private double actuatorLoad;
 		
-	public StateActuatorEnergyComponent(int numberOfActuators, double volt, double hoverAmp, double maxAmp) {
+	public StatefulActuatorComponent(int numberOfActuators, double volt, double hoverAmp, double maxAmp) {
 		OFF = new DefaultEnergyState("OFF", 0);		
 		FLY = new DefaultEnergyState("FLY", numberOfActuators * (hoverAmp * volt) * 1000000);		
 		MAX = new DefaultEnergyState("MAX", numberOfActuators * (maxAmp * volt) * 1000000);
@@ -63,15 +62,13 @@ public class StateActuatorEnergyComponent implements EnergyComponent {
 		this.actuatorLoad = 0;
 	}
 	
-	public void doStateChange(EnergyState newState) {
-		
+	public void doStateChange(EnergyState newState) {		
 		long timeSpentInState = Time.getCurrentTime() - lastEnergyConsumationEvent;
 		
 		energyModel.componentConsumedEnergy(this, calculateEnergyConsumation(currentState, timeSpentInState));			
 		
 		currentState = newState;		
-		lastEnergyConsumationEvent = Time.getCurrentTime();
-				
+		lastEnergyConsumationEvent = Time.getCurrentTime();				
 	}
 	
 	@Override
@@ -95,10 +92,16 @@ public class StateActuatorEnergyComponent implements EnergyComponent {
 		}
 	}
 	
+	public double estimatePowerConsumption(double load) {
+		double consumationDelta = MAX.getEnergyConsumption() - FLY.getEnergyConsumption();			
+		double estimation = FLY.getEnergyConsumption() + consumationDelta * load;
+		
+		return estimation;
+	}
+	
 	@Override
 	public void eventOccurred(Object content, int type) {
-		// TODO Auto-generated method stub
-		
+		// TODO Auto-generated method stub		
 	}
 
 	@Override
@@ -133,7 +136,6 @@ public class StateActuatorEnergyComponent implements EnergyComponent {
 	public boolean isOn() {
 		if(!currentState.equals(OFF) && isAvailable()) 
 		{
-			doStateChange(FLY);
 			return true;			
 		}
 		return false;

src/de/tud/kom/p2psim/impl/energy/components/StatelessMotorComponent.java → src/de/tud/kom/p2psim/impl/energy/components/StatelessActuatorComponent.java

@@ -24,7 +24,6 @@ package de.tud.kom.p2psim.impl.energy.components;
 import java.util.Comparator;
 import java.util.LinkedList;
 import de.tud.kom.p2psim.api.energy.ComponentType;
-import de.tud.kom.p2psim.api.energy.EnergyComponent;
 import de.tud.kom.p2psim.api.energy.EnergyEventListener;
 import de.tud.kom.p2psim.api.energy.EnergyState;
 import de.tud.kom.p2psim.impl.energy.DefaultEnergyState;
@@ -36,11 +35,10 @@ import de.tudarmstadt.maki.simonstrator.api.Time;
  * Is configured by {@link MotorCharacteristic}s with a given thrust (N) and a given current (A). 
  * Values in between given characteristics are calculated by linear interpolation.
  * 
- * 
  * @author Julian Zobel
  * @version 1.0, 05.03.2019
  */
-public class StatelessMotorComponent implements EnergyComponent {
+public class StatelessActuatorComponent implements ActuatorComponent {
 
 	public enum componentState {OFF, ON}
 	
@@ -60,7 +58,7 @@ public class StatelessMotorComponent implements EnergyComponent {
 	private LinkedList<MotorCharacteristic> characteristics = new LinkedList<>();
 	
 		
-	public StatelessMotorComponent(int numberOfActuators, double volt) {			
+	public StatelessActuatorComponent(int numberOfActuators, double volt) {			
 		this.volts = volt;		
 		this.numberOfActuators = numberOfActuators;				
 		this.state = componentState.OFF;

src/de/tud/kom/p2psim/impl/energy/configs/ActuatorEnergyConsumptionConfig.java → src/de/tud/kom/p2psim/impl/energy/configs/StatefulActuatorConfiguration.java

@@ -22,15 +22,15 @@ package de.tud.kom.p2psim.impl.energy.configs;
 
 import de.tud.kom.p2psim.api.common.SimHost;
 import de.tud.kom.p2psim.api.energy.EnergyConfiguration;
-import de.tud.kom.p2psim.impl.energy.components.StateActuatorEnergyComponent;
+import de.tud.kom.p2psim.impl.energy.components.StatefulActuatorComponent;
 
 /**
- * Energy Configuration for {@link StateActuatorEnergyComponent}s.
+ * Energy Configuration for {@link StatefulActuatorComponent}s.
  * 
  * @author Julian Zobel
  * @version 1.0, 11.09.2018
  */
-public class ActuatorEnergyConsumptionConfig implements EnergyConfiguration<StateActuatorEnergyComponent> {
+public class StatefulActuatorConfiguration implements EnergyConfiguration<StatefulActuatorComponent> {
 
 	private int numberOfActuators;
 	private double volt; 	
@@ -38,8 +38,8 @@ public class ActuatorEnergyConsumptionConfig implements EnergyConfiguration<Stat
 	private double flyAmp; // in ampere
 	
 	@Override
-	public StateActuatorEnergyComponent getConfiguredEnergyComponent(SimHost host) {
-		return new StateActuatorEnergyComponent(numberOfActuators, volt, hoverAmp, flyAmp);		
+	public StatefulActuatorComponent getConfiguredEnergyComponent(SimHost host) {
+		return new StatefulActuatorComponent(numberOfActuators, volt, hoverAmp, flyAmp);		
 	}
 
 	@Override

src/de/tud/kom/p2psim/impl/energy/configs/ActuatorConfiguration.java → src/de/tud/kom/p2psim/impl/energy/configs/StatelessActuatorConfiguration.java

@@ -24,7 +24,7 @@ import java.util.LinkedList;
 import de.tud.kom.p2psim.api.common.SimHost;
 import de.tud.kom.p2psim.api.energy.EnergyConfiguration;
 import de.tud.kom.p2psim.impl.energy.components.MotorCharacteristic;
-import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
+import de.tud.kom.p2psim.impl.energy.components.StatelessActuatorComponent;
 
 
 /**
@@ -33,7 +33,7 @@ import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
  * @author Julian Zobel
  * @version 1.0, 05.03.2019
  */
-public class ActuatorConfiguration implements EnergyConfiguration<StatelessMotorComponent> {
+public class StatelessActuatorConfiguration implements EnergyConfiguration<StatelessActuatorComponent> {
 
 	private int numberOfActuators;
 	private double volt; 	
@@ -41,8 +41,8 @@ public class ActuatorConfiguration implements EnergyConfiguration<StatelessMotor
 	private LinkedList<MotorCharacteristic> characteristics = new LinkedList<>();
 	
 	@Override
-	public StatelessMotorComponent getConfiguredEnergyComponent(SimHost host) {
-		StatelessMotorComponent comp = new StatelessMotorComponent(numberOfActuators, volt);
+	public StatelessActuatorComponent getConfiguredEnergyComponent(SimHost host) {
+		StatelessActuatorComponent comp = new StatelessActuatorComponent(numberOfActuators, volt);
 		
 		for (MotorCharacteristic c : characteristics) {
 			comp.addChar(c);

src/de/tud/kom/p2psim/impl/energy/models/ComponentBasedEnergyModel.java

@@ -25,7 +25,7 @@ import de.tud.kom.p2psim.api.energy.Battery;
 import de.tud.kom.p2psim.api.energy.EnergyComponent;
 import de.tud.kom.p2psim.api.energy.EnergyEventListener;
 import de.tud.kom.p2psim.api.network.SimNetInterface;
-import de.tud.kom.p2psim.impl.energy.components.StateActuatorEnergyComponent;
+import de.tud.kom.p2psim.impl.energy.components.StatefulActuatorComponent;
 
 /**
  * Energy Model based on multiple exchangeable components. Each component states the amount of consumed energy,
@@ -60,8 +60,8 @@ public class ComponentBasedEnergyModel extends AbstractEnergyModel implements En
 				
 				monitorEmptyBattery();
 				
-				if(component instanceof StateActuatorEnergyComponent) {
-					((StateActuatorEnergyComponent) component).turnOff();
+				if(component instanceof StatefulActuatorComponent) {
+					((StatefulActuatorComponent) component).turnOff();
 				}
 				
 				/*

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

@@ -32,7 +32,8 @@ import de.tud.kom.p2psim.api.topology.movement.SimUAVLocationActuator;
 import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
 import de.tud.kom.p2psim.api.topology.placement.PlacementModel;
 import de.tud.kom.p2psim.impl.energy.RechargeableBattery;
-import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
+import de.tud.kom.p2psim.impl.energy.components.ActuatorComponent;
+import de.tud.kom.p2psim.impl.energy.components.StatelessActuatorComponent;
 import de.tud.kom.p2psim.impl.energy.models.AbstractEnergyModel;
 import de.tud.kom.p2psim.impl.topology.placement.UAVBasePlacement;
 import de.tud.kom.p2psim.impl.topology.util.PositionVector;
@@ -63,7 +64,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
 	
 	protected PositionVector direction;
 	
-	private StatelessMotorComponent actuator;
+	private ActuatorComponent actuator;
 	private RechargeableBattery battery;
 	
 	private UAVstate state = UAVstate.OFFLINE;
@@ -90,7 +91,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
 		
 		try {
 			actuator = getHost().getComponent(EnergyModel.class)
-					.getComponent(ComponentType.ACTUATOR, StatelessMotorComponent.class);	
+					.getComponent(ComponentType.ACTUATOR, ActuatorComponent.class);	
 			movement.setMotorControl(actuator);
 		} catch (ComponentNotAvailableException e) {
 			System.err.println("No Acutator Energy Component was found!");
@@ -126,7 +127,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
 
 	@Override
 	public double getMinMovementSpeed() {
-		return movement.minimumVelocity();
+		return movement.horizontalMinVelocity();
 	}
 
 	@Override

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

@@ -20,56 +20,175 @@
 
 package de.tud.kom.p2psim.impl.topology.movement.aerial;
 
+
 import java.util.LinkedHashMap;
 import java.util.LinkedList;
 import java.util.Map;
-
-import org.apache.commons.math3.geometry.Vector;
-import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
 import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
-
 import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
-import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
+import de.tud.kom.p2psim.impl.energy.components.ActuatorComponent;
+import de.tud.kom.p2psim.impl.energy.components.StatelessActuatorComponent;
 import de.tud.kom.p2psim.impl.topology.component.UAVTopologyComponent;
 import de.tud.kom.p2psim.impl.topology.util.PositionVector;
 import de.tudarmstadt.maki.simonstrator.api.Time;
 import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;
 
-public class MulticopterMovement implements UAVMovementModel {
+/**
+ * Simplified thrust-based local movement model based on the Intel Aero UAV. 
+ * 
+ * The movement logic uses only straight forward movement with the maximum speed available. 
+ * 
+ * TODO Acceleration
+ * TODO Movement model for plane-like UAVs 
+ * 
+ * @author Julian Zobel
+ * @version 1.0, 11.09.2018
+ */
+public class MulticopterMovement implements UAVMovementModel  {
 
-		
 	private UAVTopologyComponent topologyComponent;
-
-	private PositionVector direction;
+	
 	private double currentAngleOfAttack;
 	private double currentSpeed;
 	
-	private double preferredSpeed;
-	
+	private double targetSpeed;	
 	
 	private LinkedList<PositionVector> route = new LinkedList<>();
 	private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>();  // TODO callback interface
 		
-	private StatelessMotorComponent motor;
-	
+	private StatelessActuatorComponent motor;
 	
 	private double mass = 1.465; // kg
-	private final double airdensity = 1.2255; // kg/m^3
+	private final double airdensity = 1.2045; // kg/m^3
 	private final double gravity = 9.807; // m/s^2
 	private double A_top = 0.245; // m^2
-	private double A_front = 0.04; // m^2
-	private double dragCoefficient = 0.5;
-	private double maxPitchAngle = Math.toRadians(45); // 45° max angle
+	private double A_front = 0.1; // m^2
+	private double dragCoefficient = 0.7;
+	private double maxPitchAngle = Math.toRadians(60); // 45° max angle
 	private double descentVelocityMax = 5; // m/s
 	private double maxTurnAngle = Math.toRadians(90); // 90° per second turn angle
 	
 	
 	public MulticopterMovement(UAVTopologyComponent topologyComponent) {
-		this.topologyComponent = topologyComponent;
-		this.direction = new PositionVector(0,0);
-		
+		this.topologyComponent = topologyComponent;			
 	}
 	
+	boolean first = true;
+	
+	@Override
+	public void move(long timeBetweenMovementOperations) {
+				
+		if(motor.isOn() && !route.isEmpty()) {
+			PositionVector position = new PositionVector(topologyComponent.getRealPosition());			
+						
+			PositionVector target = route.getFirst();
+			
+			double distanceToTargetPosition = position.distanceTo(target);
+			
+			
+			// If target point is reached within a 1 meter margin, we remove that point from the list 
+			if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < currentSpeed)
+			{						
+				target = route.removeFirst();
+								
+				if(route.isEmpty()) {	
+					
+					// go to hover mode					
+					topologyComponent.updateCurrentLocation(target.clone()); 
+					
+					currentSpeed = 0;					
+					motor.requestThrust(hoverThrustRequired());
+					
+					PositionVector direction = topologyComponent.getCurrentDirection().clone();
+					direction.setEntry(2, 0);
+					topologyComponent.updateCurrentDirection(direction);
+					
+					locationReached(topologyComponent.getCurrentLocation());
+					return;
+				}
+				else {
+					
+					// get to speed	
+					if(targetSpeed > 0 && targetSpeed < horizontalMaxVelocity()) {
+						motor.requestThrust(estimateRequiredThrust(targetSpeed));
+						currentSpeed = targetSpeed;
+					}
+					else {
+						motor.requestThrust(horizontalMaxVelocityRequiredTotalThrust());
+						currentSpeed = horizontalMaxVelocity();
+					}							
+			
+					long timeUntilReachedLocation = (long) (distanceToTargetPosition / currentSpeed) * Time.SECOND;
+					
+					target = route.getFirst();					
+					PositionVector directionToTarget = new PositionVector(target);
+					directionToTarget.subtract(position);
+					
+					double timefactor = timeUntilReachedLocation / Time.SECOND;
+					
+					directionToTarget.normalize();
+					topologyComponent.updateCurrentDirection(directionToTarget.clone());
+					directionToTarget.multiplyScalar(currentSpeed * timefactor);
+					
+					PositionVector newPosition = new PositionVector(position);
+					newPosition.add(directionToTarget);		
+					
+					topologyComponent.updateCurrentLocation(newPosition);
+					
+					if(timeUntilReachedLocation < timeBetweenMovementOperations) {
+						this.move(timeBetweenMovementOperations - timeUntilReachedLocation);					
+					}
+					
+				}			
+				
+			}
+			else {
+				double timefactor = timeBetweenMovementOperations / Time.SECOND;
+				
+				// get to speed	
+				if(targetSpeed > 0 && targetSpeed < horizontalMaxVelocity()) {
+					motor.requestThrust(estimateRequiredThrust(targetSpeed));
+					currentSpeed = targetSpeed;
+				}
+				else {
+					motor.requestThrust(horizontalMaxVelocityRequiredTotalThrust());
+					currentSpeed = horizontalMaxVelocity();
+				}				
+				
+				PositionVector directionToTarget = new PositionVector(target);
+				directionToTarget.subtract(position);
+				
+				directionToTarget.normalize();
+				
+				if(directionToTarget.getX() != 0 || directionToTarget.getY() != 0) {
+					topologyComponent.updateCurrentDirection(directionToTarget.clone());
+				}				
+				
+				directionToTarget.multiplyScalar(currentSpeed * timefactor);
+
+				PositionVector newPosition = new PositionVector(position);
+				newPosition.add(directionToTarget);		
+				
+				topologyComponent.updateCurrentLocation(newPosition);
+			}
+			
+		}
+		else if(motor.isOn()) {
+			
+			if(currentSpeed != 0) {
+				throw new UnsupportedOperationException("no route but speed not 0?");
+			}
+			
+			PositionVector position = new PositionVector(topologyComponent.getRealPosition());	
+			
+			if(position.getAltitude() == 0) {
+				motor.requestThrust(0);
+			}
+			else {
+				motor.requestThrust(hoverThrustRequired());
+			}
+		}
+	}
 	
 	/*
 	 * 
@@ -97,24 +216,28 @@ public class MulticopterMovement implements UAVMovementModel {
 	
 	public double horizontalMaxVelocity() {
 		
-		double horizontalThrust = horizontalMaxThrust();
+		double horizontalThrust = horizontalComponentMaxThrust();
 				
 		double maxVelocity = Math.sqrt( (2.0 * horizontalThrust) / bodyDrag(maxPitchAngle, new PositionVector(1,0,0)));
 				
 		return maxVelocity;
 	}
 	
-	public double horizontalMaxThrust() {
+	public double horizontalComponentMaxThrust() {
 		
 		// hoverthrust / cos => amount of thrust in horizonal direction with °angle		
-		double stableAltitudeMaximumTotalThrust = hoverThrustRequired() / Math.cos(maxPitchAngle);
-		
+		double stableAltitudeMaximumTotalThrust = horizontalMaxVelocityRequiredTotalThrust();
+	
 		// fraction of total thrust in horizonal (forward) direction with °angle
 		double maximumHorizontalThrustStableAltitude = stableAltitudeMaximumTotalThrust * Math.sin(maxPitchAngle);
 		
 		return maximumHorizontalThrustStableAltitude;
 	}
 
+	public double horizontalMaxVelocityRequiredTotalThrust() {
+		return hoverThrustRequired() / Math.cos(maxPitchAngle);
+	}
+	
 	public double bodyDrag(double angleRadians, PositionVector direction) {
 		return airdensity * dragCoefficient * areaExposedToDrag(angleRadians, direction);
 	}
@@ -124,7 +247,7 @@ public class MulticopterMovement implements UAVMovementModel {
 		Vector2D v = new Vector2D(Math.abs(direction.getX()) + Math.abs(direction.getY()), Math.abs(direction.getZ()));
 		v = v.normalize();
 		
-		double areaExposedFront = v.getX() * (Math.sin(angleRadians) * A_top + Math.cos(angleRadians) * A_front);
+		double areaExposedFront = v.getX() * (Math.sin(angleRadians) * A_top + Math.cos(angleRadians) * A_front );
 		double areaExposedTop = v.getY() * (Math.cos(angleRadians) * A_top + Math.sin(angleRadians) * A_front);
 		
 		return areaExposedFront + areaExposedTop;
@@ -134,9 +257,19 @@ public class MulticopterMovement implements UAVMovementModel {
 	 * F_drag [N] = 0.5 * p * C_drag * A * v^2
 	 */
 	public double currentDrag() {
-		return 0.5 * bodyDrag(currentAngleOfAttack, direction) * currentSpeed * currentSpeed;
+		return 0.5 * bodyDrag(currentAngleOfAttack, topologyComponent.getCurrentDirection()) * currentSpeed * currentSpeed;
 	}
 	
+	/**
+	 * Calculate the drag induced on the UAV with a given velocity and an angle of attack (in radians) moving forward horizontally.
+	 * 
+	 * @param velocity
+	 * @param angleInRadians
+	 * @return
+	 */
+	private double forwardDrag(double velocity, double angleInRadians) {
+		return 0.5 * bodyDrag(angleInRadians, new PositionVector(1,0,0)) * velocity * velocity;
+	}
 	
 	
 	/*
@@ -144,104 +277,16 @@ public class MulticopterMovement implements UAVMovementModel {
 	 */
 	
 	@Override
-	public void setMotorControl(StatelessMotorComponent motor) {
-		this.motor = motor;
+	public void setMotorControl(ActuatorComponent motor) {
+		this.motor = (StatelessActuatorComponent) motor;
 	}
 
-	@Override
-	public void move(long timeBetweenMovementOperations) {
-		
-		if(motor.isOn() && !route.isEmpty()) {
-			PositionVector position = new PositionVector(topologyComponent.getRealPosition());			
-			
-			double thrust = 0;
-			direction = new PositionVector(0,0,1);
-			
-			double distz = 100 - position.getZ();
-			
-			if(distz < 100) {
-				
-				double time = ((double) timeBetweenMovementOperations) / ((double) Time.SECOND);
-				
-				// ASCENSION TO SAFETY HEIGHT
-				thrust = motor.requestThrust(motor.getMaxThrust());
-												
-				double acc = ((thrust - hoverThrustRequired()) / mass ) - currentDrag();
-				double speed = 0;
-				
-				if(acc < 0.1) {
-					// normal flight
-					speed = verticalAscentMaxVelocity();
-				}
-				else {
-					// acceleration
-					speed = currentSpeed + acc * time; 
-				}
-				
-				
-				System.out.println("ACCELERATION : " + acc);			
-				System.out.println("SPEED : " + speed);
-				
-				//double dist = 0.5 * acc * (timeBetweenMovementOperations / Time.SECOND) * (timeBetweenMovementOperations / Time.SECOND);
-				double dist = currentSpeed * time;
-				
-				//double safetydistance = speed
-				
-				//System.out.println();
-				
-				position.add(new PositionVector(0,0,dist));
-				
-				System.out.println("NEW POSITION : " + position);
-
-				topologyComponent.updateCurrentLocation(position);
-			}
-			else {
-				thrust = motor.requestThrust(hoverThrustRequired());
-				topologyComponent.updateCurrentLocation(position);
-			}
-			
-			
-			
-			
-//			PositionVector directionToTarget = new PositionVector(route.getFirst());
-//			directionToTarget.subtract(position);
-//			
-//			double distanceToTargetPosition = directionToTarget.getLength();
-			
-							
-//			// If target point is reached within a 1 meter margin, we remove that point from the list 
-//			if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < currentSpeed)
-//			{						
-//				route.removeFirst();
-//		
-//				topologyComponent.updateCurrentLocation(position); 
-//				currentSpeed = 0;
-//			
-//				locationReached(position);
-//				return;
-//			}
-//			
-//			double timefactor = timeBetweenMovementOperations / Time.SECOND;
-//			
-//			currentSpeed = Math.min(distanceToTargetPosition, preferredSpeed);
-//				
-//			direction.normalize();
-//			direction.multiplyScalar(currentSpeed * timefactor);
-//
-//			PositionVector newPosition = new PositionVector(position);
-//			newPosition.add(direction);		
-//			
-//			topologyComponent.updateCurrentLocation(newPosition);
-			
-		}
-		
-		//System.out.println(Simulator.getFormattedTime(Simulator.getCurrentTime()) + " | " +  topologyComponent);		
-	}
+	
 
 	
 	@Override
 	public void setPreferredSpeed(double v_pref) {
-		this.preferredSpeed = v_pref;		
+		this.targetSpeed = v_pref;		
 	}
 	
 	@Override
@@ -302,21 +347,112 @@ public class MulticopterMovement implements UAVMovementModel {
 	public void removeTargetLocations() {
 		route.clear();
 		locationCallbacks.clear();
-	}
-
+	}	
 
 	@Override
-	public double minimumVelocity() {
-		return 0;
+	public double horizontalMinVelocity() {
+		return Math.sqrt(2 * hoverThrustRequired() * Math.tan(Math.toRadians(0.25)) / bodyDrag(Math.toRadians(0.25), new PositionVector(1,0,0)));
 	}
 
-
 	@Override
 	public double estimatePowerConsumption(double velocity) {
-		// TODO Auto-generated method stub
-		return 0;
+		
+		if(velocity == 0) {
+			return motor.estimatePowerConsumptionWatt(hoverThrustRequired());
+		}
+		else if(velocity > horizontalMaxVelocity()) {
+			return -1;
+		}
+		else if(velocity < horizontalMinVelocity()) {
+			return -1;
+		}
+		else {			
+			double estimateAngle = estimatePitchAngleForVelocity(velocity);
+			double estimatedDrag = forwardDrag(velocity, estimateAngle);			
+			
+			double requiredThrust = Math.sqrt(hoverThrustRequired() * hoverThrustRequired() + estimatedDrag * estimatedDrag);			
+			double wattage = motor.estimatePowerConsumptionWatt(requiredThrust);
+						
+			return wattage;
+		}
+		
+		
 	}
-
-
-
+	
+	public double estimateRequiredThrust(double velocity) {
+		if(velocity == 0) {
+			return motor.estimatePowerConsumptionWatt(hoverThrustRequired());
+		}
+		else if(velocity > horizontalMaxVelocity()) {
+			return -1;
+		}
+		else if(velocity < horizontalMinVelocity()) {
+			return -1;
+		}
+		else {
+			double estimateAngle = estimatePitchAngleForVelocity(velocity);
+			double estimatedDrag = forwardDrag(velocity, estimateAngle);					
+			double requiredThrust = Math.sqrt(hoverThrustRequired() * hoverThrustRequired() + estimatedDrag * estimatedDrag);
+			return requiredThrust;
+		}
+	}
+	
+	/**
+	 * Estimate the pitch angle (angle of attack) required to get the target velocity. 
+	 * Angle precision is 1/4 degree. 
+	 * 
+	 * @param velocity
+	 * @return
+	 */
+	private double estimatePitchAngleForVelocity(double velocity) {
+		
+		int low = 0;
+		int high = Integer.MAX_VALUE;
+		
+		double vsquared = (velocity * velocity);
+		
+		for(int i = 0; i <= ((int) Math.toDegrees(maxPitchAngle)); i++) {
+			
+			double v2 = 2 * hoverThrustRequired() * Math.tan(Math.toRadians(i)) / bodyDrag(Math.toRadians(i), new PositionVector(1,0,0));
+						
+			if(v2 > vsquared && i < high) {				
+				high = i;
+			}
+			else if(v2 < vsquared && i >= low) {
+				low = i;				
+			}
+			else if(v2 == vsquared ) {
+				return Math.toRadians(i);
+			}
+			
+		}
+		
+		if(high < Integer.MAX_VALUE) {
+			double lo = low;
+			double hi = high;
+			
+			double nearest = -1;
+			double nearestDiff = Double.MAX_VALUE;
+			
+			double step = (hi - lo) / 4;
+			
+			for(int i = 0; i < 4; i++) {
+			
+				double d = lo + i * step;
+				
+				double v2 = 2 * hoverThrustRequired() * Math.tan(Math.toRadians(d)) / bodyDrag(Math.toRadians(d), new PositionVector(1,0,0));
+				
+				double diff = Math.abs(((velocity * velocity) - v2));
+				
+				if(diff < nearestDiff || (lo == 0 && i == 1)) {
+					nearestDiff = diff;
+					nearest = d;
+				}			
+			}
+				
+			return Math.toRadians(nearest);
+		}
+			
+		return maxPitchAngle;	
+	}	
 }

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

@@ -24,265 +24,94 @@ package de.tud.kom.p2psim.impl.topology.movement.aerial;
 import java.util.LinkedHashMap;
 import java.util.LinkedList;
 import java.util.Map;
-import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
-
-import de.tud.kom.p2psim.api.energy.Battery;
 import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
-import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
+import de.tud.kom.p2psim.impl.energy.components.ActuatorComponent;
+import de.tud.kom.p2psim.impl.energy.components.StatefulActuatorComponent;
 import de.tud.kom.p2psim.impl.topology.component.UAVTopologyComponent;
 import de.tud.kom.p2psim.impl.topology.util.PositionVector;
 import de.tudarmstadt.maki.simonstrator.api.Time;
 import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;
 
 /**
- * Local movement logic specifically designs the movement for multicopter UAVs. 
- * This simple movement logic uses straight forward movement with the maximum speed available.
+ * Local movement for UAVs. 
+ * This simple movement logic uses straight forward movement with the set speed (up to a max).
+ * Energy consumption is based on state actuators.
  * 
  * @author Julian Zobel
- * @version 1.0, 11.09.2018
+ * @version 1.1, 14.01.2020
  */
 public class SimpleMulticopterMovement implements UAVMovementModel  {
 
-	private UAVTopologyComponent topologyComponent;
-	
-	private double currentAngleOfAttack;
-	private double currentSpeed;
+	private UAVTopologyComponent topologyComponent;	
+	private double speed;	
+	private double targetSpeed;
 	
-	private double targetSpeed;	
+	private double maximumHorizontalSpeed = 15;
+	private double maximumVerticalSpeed = 5;
 	
 	private LinkedList<PositionVector> route = new LinkedList<>();
-	private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>();  // TODO callback interface
-		
-	private StatelessMotorComponent motor;
-	
-	private double mass = 1.465; // kg
-	private final double airdensity = 1.2045; // kg/m^3
-	private final double gravity = 9.807; // m/s^2
-	private double A_top = 0.245; // m^2
-	private double A_front = 0.1; // m^2
-	private double dragCoefficient = 0.7;
-	private double maxPitchAngle = Math.toRadians(60); // 45° max angle
-	private double descentVelocityMax = 5; // m/s
-	private double maxTurnAngle = Math.toRadians(90); // 90° per second turn angle
-	
-	
-	public SimpleMulticopterMovement(UAVTopologyComponent topologyComponent) {
-		this.topologyComponent = topologyComponent;	
+	private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>();  
 		
+	private StatefulActuatorComponent motor;
 		
-	}
-	
-	boolean first = true;
+	public SimpleMulticopterMovement(UAVTopologyComponent topologyComponent) {
+		this.topologyComponent = topologyComponent;			
+	}	
 	
 	@Override
 	public void move(long timeBetweenMovementOperations) {
 				
-		if(motor.isOn() && !route.isEmpty()) {
-			PositionVector position = new PositionVector(topologyComponent.getRealPosition());			
-						
-			PositionVector target = route.getFirst();
-			
-			double distanceToTargetPosition = position.distanceTo(target);
-			
+		if(motor.isOn() && !route.isEmpty()) {		
+		
+			PositionVector currentPosition = topologyComponent.getRealPosition();
 			
+			PositionVector targetPosition = route.getFirst();
+			Double distanceToTargetPosition = targetPosition.distanceTo(currentPosition);
+							
 			// If target point is reached within a 1 meter margin, we remove that point from the list 
-			if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < currentSpeed)
+			if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < speed)
 			{						
-				target = route.removeFirst();
-								
-				if(route.isEmpty()) {	
-					
-					// go to hover mode					
-					topologyComponent.updateCurrentLocation(target.clone()); 
-					
-					currentSpeed = 0;					
-					motor.requestThrust(hoverThrustRequired());
-					
-					PositionVector direction = topologyComponent.getCurrentDirection().clone();
-					direction.setEntry(2, 0);
-					topologyComponent.updateCurrentDirection(direction);
-					
-					locationReached(topologyComponent.getCurrentLocation());
-					return;
-				}
-				else {
-					
-					// get to speed	
-					if(targetSpeed > 0 && targetSpeed < horizontalMaxVelocity()) {
-						motor.requestThrust(estimateRequiredThrust(targetSpeed));
-						currentSpeed = targetSpeed;
-					}
-					else {
-						motor.requestThrust(horizontalMaxVelocityRequiredTotalThrust());
-						currentSpeed = horizontalMaxVelocity();
-					}							
-			
-					long timeUntilReachedLocation = (long) (distanceToTargetPosition / currentSpeed) * Time.SECOND;
-					
-					target = route.getFirst();					
-					PositionVector directionToTarget = new PositionVector(target);
-					directionToTarget.subtract(position);
-					
-					double timefactor = timeUntilReachedLocation / Time.SECOND;
-					
-					directionToTarget.normalize();
-					topologyComponent.updateCurrentDirection(directionToTarget.clone());
-					directionToTarget.multiplyScalar(currentSpeed * timefactor);
-					
-					PositionVector newPosition = new PositionVector(position);
-					newPosition.add(directionToTarget);		
-					
-					topologyComponent.updateCurrentLocation(newPosition);
-					
-					if(timeUntilReachedLocation < timeBetweenMovementOperations) {
-						this.move(timeBetweenMovementOperations - timeUntilReachedLocation);					
-					}
-					
-				}			
+				route.removeFirst();
 				
+				topologyComponent.updateCurrentLocation(targetPosition); // triggers energy consumption for last distance
+				speed = 0;
+				motor.useActuator(0);
+				locationReached(targetPosition);
+				return;
 			}
-			else {
-				double timefactor = timeBetweenMovementOperations / Time.SECOND;
-				
-				// get to speed	
-				if(targetSpeed > 0 && targetSpeed < horizontalMaxVelocity()) {
-					motor.requestThrust(estimateRequiredThrust(targetSpeed));
-					currentSpeed = targetSpeed;
-				}
-				else {
-					motor.requestThrust(horizontalMaxVelocityRequiredTotalThrust());
-					currentSpeed = horizontalMaxVelocity();
-				}				
-				
-				PositionVector directionToTarget = new PositionVector(target);
-				directionToTarget.subtract(position);
-				
-				directionToTarget.normalize();
-				
-				if(directionToTarget.getX() != 0 || directionToTarget.getY() != 0) {
-					topologyComponent.updateCurrentDirection(directionToTarget.clone());
-				}				
-				
-				directionToTarget.multiplyScalar(currentSpeed * timefactor);
+			
+			double timefactor = timeBetweenMovementOperations / Time.SECOND;
+			
+			speed = Math.min(distanceToTargetPosition, targetSpeed);					
+						
+			PositionVector direction = new PositionVector(targetPosition);
+			direction.subtract(currentPosition);
+			direction.normalize();
+			direction.multiplyScalar(speed * timefactor);
 
-				PositionVector newPosition = new PositionVector(position);
-				newPosition.add(directionToTarget);		
-				
-				topologyComponent.updateCurrentLocation(newPosition);
-			}
+			PositionVector newPosition = new PositionVector(currentPosition);
+			newPosition.add(direction);		
 			
+			motor.useActuator(1);
+			topologyComponent.updateCurrentLocation(newPosition);					
 		}
 		else if(motor.isOn()) {
 			
-			if(currentSpeed != 0) {
+			if(speed != 0) {
 				throw new UnsupportedOperationException("no route but speed not 0?");
 			}
 			
 			PositionVector position = new PositionVector(topologyComponent.getRealPosition());	
 			
 			if(position.getAltitude() == 0) {
-				motor.requestThrust(0);
+				motor.turnOff();
 			}
 			else {
-				motor.requestThrust(hoverThrustRequired());
+				motor.useActuator(0);
 			}
 		}
 	}
-	
-	/*
-	 * 
-	 */
-	
-	public double verticalDescentMaxThrust() {		
-		// m * g - 0.5 * p * C * A * v^2
-		return hoverThrustRequired() - 0.5 * bodyDrag(0, new PositionVector(0,0,1)) * descentVelocityMax * descentVelocityMax;
-	}
-	
-	public double verticalAscentMaxAcceleration() {
-		return (motor.getMaxThrust() - hoverThrustRequired()) / mass;
-	}
-	
-	public double verticalAscentMaxVelocity() {		
-		double maxThrust = motor.getMaxThrust();		
-		return Math.sqrt(2.0 * (maxThrust - hoverThrustRequired()) / bodyDrag(0, new PositionVector(0,0,1)));		
-	}
-	
-	
-	public double hoverThrustRequired()  {		
-		return mass * gravity;		
-	}
-	
-	
-	public double horizontalMaxVelocity() {
 		
-		double horizontalThrust = horizontalComponentMaxThrust();
-				
-		double maxVelocity = Math.sqrt( (2.0 * horizontalThrust) / bodyDrag(maxPitchAngle, new PositionVector(1,0,0)));
-				
-		return maxVelocity;
-	}
-	
-	public double horizontalComponentMaxThrust() {
-		
-		// hoverthrust / cos => amount of thrust in horizonal direction with °angle		
-		double stableAltitudeMaximumTotalThrust = horizontalMaxVelocityRequiredTotalThrust();
-	
-		// fraction of total thrust in horizonal (forward) direction with °angle
-		double maximumHorizontalThrustStableAltitude = stableAltitudeMaximumTotalThrust * Math.sin(maxPitchAngle);
-		
-		return maximumHorizontalThrustStableAltitude;
-	}
-
-	public double horizontalMaxVelocityRequiredTotalThrust() {
-		return hoverThrustRequired() / Math.cos(maxPitchAngle);
-	}
-	
-	public double bodyDrag(double angleRadians, PositionVector direction) {
-		return airdensity * dragCoefficient * areaExposedToDrag(angleRadians, direction);
-	}
-	
-	public double areaExposedToDrag(double angleRadians, PositionVector direction) {
-			
-		Vector2D v = new Vector2D(Math.abs(direction.getX()) + Math.abs(direction.getY()), Math.abs(direction.getZ()));
-		v = v.normalize();
-		
-		double areaExposedFront = v.getX() * (Math.sin(angleRadians) * A_top + Math.cos(angleRadians) * A_front );
-		double areaExposedTop = v.getY() * (Math.cos(angleRadians) * A_top + Math.sin(angleRadians) * A_front);
-		
-		return areaExposedFront + areaExposedTop;
-	}
-	
-	/*
-	 * F_drag [N] = 0.5 * p * C_drag * A * v^2
-	 */
-	public double currentDrag() {
-		return 0.5 * bodyDrag(currentAngleOfAttack, topologyComponent.getCurrentDirection()) * currentSpeed * currentSpeed;
-	}
-	
-	/**
-	 * Calculate the drag induced on the UAV with a given velocity and an angle of attack (in radians) moving forward horizontally.
-	 * 
-	 * @param velocity
-	 * @param angleInRadians
-	 * @return
-	 */
-	private double forwardDrag(double velocity, double angleInRadians) {
-		return 0.5 * bodyDrag(angleInRadians, new PositionVector(1,0,0)) * velocity * velocity;
-	}
-	
-	
-	/*
-	 *
-	 */
-	
-	@Override
-	public void setMotorControl(StatelessMotorComponent motor) {
-		this.motor = motor;
-	}
-
-	
-
-	
 	@Override
 	public void setPreferredSpeed(double v_pref) {
 		this.targetSpeed = v_pref;		
@@ -290,7 +119,7 @@ public class SimpleMulticopterMovement implements UAVMovementModel  {
 	
 	@Override
 	public double getCurrentSpeed() {
-		return currentSpeed;
+		return speed;
 	}
 
 	/**
@@ -348,116 +177,35 @@ public class SimpleMulticopterMovement implements UAVMovementModel  {
 		locationCallbacks.clear();
 	}
 
-
-	@Override
-	public double minimumVelocity() {
-		return minimumHorizontalVelocity();
-	}
-
-	
-	public double minimumHorizontalVelocity() {
-		return Math.sqrt(2 * hoverThrustRequired() * Math.tan(Math.toRadians(0.25)) / bodyDrag(Math.toRadians(0.25), new PositionVector(1,0,0)));
-	}
-
 	@Override
 	public double estimatePowerConsumption(double velocity) {
 		
 		if(velocity == 0) {
-			return motor.estimatePowerConsumptionWatt(hoverThrustRequired());
-		}
-		else if(velocity > horizontalMaxVelocity()) {
-			return -1;
+			return motor.estimatePowerConsumption(0);
 		}
-		else if(velocity < minimumHorizontalVelocity()) {
-			return -1;
-		}
-		else {			
-			double estimateAngle = estimatePitchAngleForVelocity(velocity);
-			double estimatedDrag = forwardDrag(velocity, estimateAngle);			
-			
-			double requiredThrust = Math.sqrt(hoverThrustRequired() * hoverThrustRequired() + estimatedDrag * estimatedDrag);			
-			double wattage = motor.estimatePowerConsumptionWatt(requiredThrust);
-						
-			return wattage;
+		else { 
+			return motor.estimatePowerConsumption(1.0);
 		}
 		
-		
 	}
-	
-	public double estimateRequiredThrust(double velocity) {
-		if(velocity == 0) {
-			return motor.estimatePowerConsumptionWatt(hoverThrustRequired());
-		}
-		else if(velocity > horizontalMaxVelocity()) {
-			return -1;
-		}
-		else if(velocity < minimumHorizontalVelocity()) {
-			return -1;
-		}
-		else {
-			double estimateAngle = estimatePitchAngleForVelocity(velocity);
-			double estimatedDrag = forwardDrag(velocity, estimateAngle);					
-			double requiredThrust = Math.sqrt(hoverThrustRequired() * hoverThrustRequired() + estimatedDrag * estimatedDrag);
-			return requiredThrust;
-		}
+
+	@Override
+	public void setMotorControl(ActuatorComponent motor) {
+		this.motor = (StatefulActuatorComponent) motor;
+	}
+
+	@Override
+	public double verticalAscentMaxVelocity() {
+		return maximumVerticalSpeed;
+	}
+
+	@Override
+	public double horizontalMaxVelocity() {
+		return maximumHorizontalSpeed;
 	}
 	
-	/**
-	 * Estimate the pitch angle (angle of attack) required to get the target velocity. 
-	 * Angle precision is 1/4 degree. 
-	 * 
-	 * @param velocity
-	 * @return
-	 */
-	private double estimatePitchAngleForVelocity(double velocity) {
-		
-		int low = 0;
-		int high = Integer.MAX_VALUE;
-		
-		double vsquared = (velocity * velocity);
-		
-		for(int i = 0; i <= ((int) Math.toDegrees(maxPitchAngle)); i++) {
-			
-			double v2 = 2 * hoverThrustRequired() * Math.tan(Math.toRadians(i)) / bodyDrag(Math.toRadians(i), new PositionVector(1,0,0));
-						
-			if(v2 > vsquared && i < high) {				
-				high = i;
-			}
-			else if(v2 < vsquared && i >= low) {
-				low = i;				
-			}
-			else if(v2 == vsquared ) {
-				return Math.toRadians(i);
-			}
-			
-		}
-		
-		if(high < Integer.MAX_VALUE) {
-			double lo = low;
-			double hi = high;
-			
-			double nearest = -1;
-			double nearestDiff = Double.MAX_VALUE;
-			
-			double step = (hi - lo) / 4;
-			
-			for(int i = 0; i < 4; i++) {
-			
-				double d = lo + i * step;
-				
-				double v2 = 2 * hoverThrustRequired() * Math.tan(Math.toRadians(d)) / bodyDrag(Math.toRadians(d), new PositionVector(1,0,0));
-				
-				double diff = Math.abs(((velocity * velocity) - v2));
-				
-				if(diff < nearestDiff || (lo == 0 && i == 1)) {
-					nearestDiff = diff;
-					nearest = d;
-				}			
-			}
-				
-			return Math.toRadians(nearest);
-		}
-			
-		return maxPitchAngle;	
-	}	
+	@Override	
+	public double horizontalMinVelocity() {
+		return 0;
+	}
 }