Merge remote-tracking branch 'origin/jz/movementmodel-testbed' into jz/dev-metrics

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

@@ -21,12 +21,13 @@
 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.ActuatorEnergyComponent;
+import de.tud.kom.p2psim.impl.energy.components.StateActuatorEnergyComponent;
+import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
 
 
 public interface SimUAVLocationActuator extends SimLocationActuator, ControllableLocationActuator {
 	
 	public UAVMovementModel getUAVMovement();
 	public void setUAVMovement(UAVMovementModel uavMovement);
-	public ActuatorEnergyComponent getActuatorEnergyComponent();
+	
 }

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

@@ -21,16 +21,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.topology.util.PositionVector;
 import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;
 
 public interface UAVMovementModel {
 	
-	public void setPreferredCruiseSpeed(double v_pref);
+	public void setMotorControl(StatelessMotorComponent motor);
+	
+	public void setPreferredSpeed(double v_pref);
 	
-	public double getMaxCruiseSpeed();
-	public double getMinCruiseSpeed();
+	public double verticalAscentMaxVelocity();
+	public double horizontalMaxVelocity();
 	public double getCurrentSpeed();
+	public double minimumVelocity();
 	
 	public void move(long timeBetweenMovementOperations);
 	

src/de/tud/kom/p2psim/impl/energy/components/MotorCharacteristic.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.tudarmstadt.maki.simonstrator.api.util.XMLConfigurableConstructor;
+
+public class MotorCharacteristic {
+
+	private final double thrust;
+	private final double current;
+	
+	@XMLConfigurableConstructor({ "thrust", "current" })
+	public MotorCharacteristic(double thrust, double current) {
+		this.thrust = thrust;
+		this.current = current;			
+	}
+	
+	public double getCurrent() {
+		return current;
+	}
+	
+	public double getThrust() {
+		return thrust;
+	}
+	
+	@Override
+	public String toString() {
+		return "["+ thrust + "N @ " + current + "A]";
+	}
+}

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

@@ -36,7 +36,7 @@ import de.tudarmstadt.maki.simonstrator.api.Time;
  * @author Julian Zobel
  * @version 1.0, 11.09.2018
  */
-public class ActuatorEnergyComponent implements EnergyComponent {
+public class StateActuatorEnergyComponent implements EnergyComponent {
 	
 	/**
 	 * States supported by this energy component
@@ -53,7 +53,7 @@ public class ActuatorEnergyComponent implements EnergyComponent {
 	
 	private double actuatorLoad;
 		
-	public ActuatorEnergyComponent(int numberOfActuators, double volt, double hoverAmp, double maxAmp) {
+	public StateActuatorEnergyComponent(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);

src/de/tud/kom/p2psim/impl/energy/components/StatelessMotorComponent.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 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;
+import de.tudarmstadt.maki.simonstrator.api.Time;
+
+
+/**
+ * Component for devices that provide thrust, such as electrical motors for UAV propulsion. 
+ * 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 enum componentState {OFF, ON}
+	
+	private componentState state;
+	
+	private EnergyEventListener energyModel;
+	
+	private long lastEnergyConsumationTime;	
+	
+	private double volts;
+	private final double uJconversionFactor = 1000000;
+	private final int numberOfActuators;
+	
+	private double thrust;
+	private double amps;
+	private EnergyState energyState;
+	private LinkedList<MotorCharacteristic> characteristics = new LinkedList<>();
+	
+		
+	public StatelessMotorComponent(int numberOfActuators, double volt) {			
+		this.volts = volt;		
+		this.numberOfActuators = numberOfActuators;				
+		this.state = componentState.OFF;
+		this.lastEnergyConsumationTime = Time.getCurrentTime();
+			
+		thrust = 0;
+		amps = 0;
+		energyState = new DefaultEnergyState("OFF", 0);
+	}
+	
+	/**
+	 * Get the maximum thrust provided by this component.
+	 * @return
+	 */
+	public double getMaxThrust() {
+		return characteristics.getLast().getThrust();
+	}
+	
+	/**
+	 * Set the new energy state and calculate the energy consumption from the last state
+	 */
+	private void setEnergyState() {	
+		
+		// set the new energy state
+		EnergyState newState;
+		
+		if(state == componentState.ON) {
+			newState = new DefaultEnergyState("Actuator", numberOfActuators * (amps * volts) * uJconversionFactor);
+		}
+		else {
+			newState = new DefaultEnergyState("OFF", 0);
+		}
+				
+		// calculate energy consumption for the previous state
+		long timeSpentInState = Time.getCurrentTime() - lastEnergyConsumationTime;
+		
+		double cons =  calculateEnergyConsumation(energyState, timeSpentInState);
+		
+		System.out.println("consumed energy : " + (cons/uJconversionFactor) + " in " + Time.getFormattedTime(timeSpentInState));
+		
+		energyModel.componentConsumedEnergy(this, cons);			
+		
+		// set new state
+		energyState = newState;		
+		lastEnergyConsumationTime = Time.getCurrentTime();
+	}
+	
+	/**	
+	 * Request a given amount of thrust to be provided from this component. If the amount is less than the minimum 
+	 * or more than the maximum, the minimum or maximum thrust values, respectively, are enforced. 
+	 *  
+	 * @param targetThrust
+	 * @return The amount of thrust this component now generates.
+	 */
+	public double requestThrust(double targetThrust) {
+		
+		if(targetThrust == 0 || targetThrust <= characteristics.getFirst().getThrust()) {
+			setLoad(characteristics.getFirst());
+		}
+		else if(targetThrust >= characteristics.getLast().getThrust()) {
+			setLoad(characteristics.getLast());
+		}
+		else {
+			approximateThrustCurrentRelation(targetThrust);
+		}
+		
+		System.out.println("Now providing Thrust: " + this.thrust + " N");
+		
+		return this.thrust;
+	}
+	
+	/**
+	 * Given an amount of thrust between the minimum and maximum values, the required current 
+	 * to provide this amount of thrust is calculated by linear interpolation by the nearest lower
+	 * and upper {@link MotorCharacteristic}s. 
+	 * 
+	 * @param targetThrust
+	 */
+	private void approximateThrustCurrentRelation(double targetThrust) {
+		
+		MotorCharacteristic lower = null, upper = null;
+		
+		// find the lower and upper bounding characteristics
+		for (MotorCharacteristic ch : characteristics) {
+			//
+			if(ch.getThrust() == targetThrust) {
+				setLoad(ch);
+				return;
+			}
+			else {
+				// list is sorted, lower bound is the biggest that is lower
+				if(ch.getThrust() < targetThrust) {
+					lower = ch;
+				}
+				// the first that is greater is used as upper bound
+				else if(ch.getThrust() > targetThrust) {
+					upper = ch;
+					break;
+				}
+			}
+		}
+		
+		if(upper == null || lower == null) {
+			throw new UnsupportedOperationException("Lower or upper bounds cannot be null");
+		}
+		
+		if(upper.getThrust() < targetThrust || lower.getThrust() > targetThrust) {
+			throw new UnsupportedOperationException("Lower or upper bound do not match");
+		}
+		
+		/*
+		 * Calculate the approximated current with the upper and lower bounds:
+		 * Amp_approx = Amp_lower + (T_target - T_lower)/(T_upper - T_lower) * (Amp_upper - Amp_lower)
+		 */		
+		double delta = (targetThrust - lower.getThrust())/(upper.getThrust() - lower.getThrust());
+		
+		double calculatedAmps = lower.getCurrent() + delta * (upper.getCurrent() - lower.getCurrent());
+		
+		System.out.println(targetThrust + " N targeted => " + calculatedAmps + " A calculated | " + delta + " <> " + lower + " " + upper);
+		
+		setLoad(targetThrust, calculatedAmps);
+	}
+	
+	private void setLoad(double thrust, double amps) {
+		this.thrust = thrust;
+		this.amps = amps;
+		setEnergyState();
+	}
+	
+	private void setLoad(MotorCharacteristic ch) {
+		this.thrust = ch.getThrust();
+		this.amps = ch.getCurrent();
+		setEnergyState();
+	}
+	
+	/**
+	 * Add a {@link MotorCharacteristic} for this motor. 
+	 * 
+	 * @param c
+	 */
+	public void addChar(MotorCharacteristic c) {
+		
+		characteristics.add(c);
+	
+		// sort the characteristics starting from low to high thrust
+		characteristics.sort(new Comparator<MotorCharacteristic>() {
+			@Override
+			public int compare(MotorCharacteristic o1,
+					MotorCharacteristic o2) {
+				return (int) (o1.getThrust() - o2.getThrust());
+			}
+		});
+		
+	}
+	
+		
+	@Override
+	public void eventOccurred(Object content, int type) {
+		// TODO Auto-generated method stub
+		
+	}
+
+	@Override
+	public ComponentType getType() {	
+		return ComponentType.ACTUATOR;
+	}
+
+	@Override
+	public boolean turnOff() {
+		this.thrust = 0;
+		this.amps = 0;
+		this.state = componentState.OFF;
+		setEnergyState();
+		
+		return true;
+	}
+
+	@Override
+	public boolean turnOn() {
+		if (isAvailable()) {
+			if(this.state != componentState.ON) {
+				this.state = componentState.ON;
+				requestThrust(0);
+			}
+			return true;
+		}
+		return false;
+	}
+	
+	public boolean isAvailable() {
+		if (energyModel.componentCanBeActivated(this)) {	
+			return true;
+		}
+		return false;
+	}
+
+	@Override
+	public boolean isOn() {
+		if(this.state != componentState.OFF && isAvailable()) {			
+			return true;			
+		}
+		return false;
+	}
+	
+	@Override
+	public void setEnergyEventListener(EnergyEventListener listener) {
+		energyModel = listener;		
+	}
+
+	public EnergyState getCurrentState() {
+		return energyState;
+	}
+	
+	
+	
+}

src/de/tud/kom/p2psim/impl/energy/configs/ActuatorConfiguration.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.configs;
+
+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;
+
+
+/**
+ * 
+ * 
+ * @author Julian Zobel
+ * @version 1.0, 05.03.2019
+ */
+public class ActuatorConfiguration implements EnergyConfiguration<StatelessMotorComponent> {
+
+	private int numberOfActuators;
+	private double volt; 	
+	
+	private LinkedList<MotorCharacteristic> characteristics = new LinkedList<>();
+	
+	@Override
+	public StatelessMotorComponent getConfiguredEnergyComponent(SimHost host) {
+		StatelessMotorComponent comp = new StatelessMotorComponent(numberOfActuators, volt);
+		
+		for (MotorCharacteristic c : characteristics) {
+			comp.addChar(c);
+		}
+		
+		return comp;
+	}
+
+	@Override
+	public String getHelp() {		
+		return "Fix actuator energy consumption config";
+	}
+
+	@Override
+	public boolean isWellConfigured() {
+		if(numberOfActuators >= 1 && volt > 0 && characteristics.size() >= 2) {
+			return true;
+		}
+					
+		return false;
+	}
+
+	public void setNumberOfActuators(int num) {
+		numberOfActuators = num;
+	}
+		
+	public void setVolt(double voltage) {
+		this.volt = voltage;
+	}
+	
+	public void setMotorCharacteristic(MotorCharacteristic c) {
+		characteristics.add(c);
+	}
+
+}

src/de/tud/kom/p2psim/impl/energy/configs/ActuatorEnergyConsumptionConfig.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.ActuatorEnergyComponent;
+import de.tud.kom.p2psim.impl.energy.components.StateActuatorEnergyComponent;
 
 /**
- * Energy Configuration for {@link ActuatorEnergyComponent}s.
+ * Energy Configuration for {@link StateActuatorEnergyComponent}s.
  * 
  * @author Julian Zobel
  * @version 1.0, 11.09.2018
  */
-public class ActuatorEnergyConsumptionConfig implements EnergyConfiguration<ActuatorEnergyComponent> {
+public class ActuatorEnergyConsumptionConfig implements EnergyConfiguration<StateActuatorEnergyComponent> {
 
 	private int numberOfActuators;
 	private double volt; 	
@@ -38,8 +38,8 @@ public class ActuatorEnergyConsumptionConfig implements EnergyConfiguration<Actu
 	private double flyAmp; // in ampere
 	
 	@Override
-	public ActuatorEnergyComponent getConfiguredEnergyComponent(SimHost host) {
-		return new ActuatorEnergyComponent(numberOfActuators, volt, hoverAmp, flyAmp);		
+	public StateActuatorEnergyComponent getConfiguredEnergyComponent(SimHost host) {
+		return new StateActuatorEnergyComponent(numberOfActuators, volt, hoverAmp, flyAmp);		
 	}
 
 	@Override

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.ActuatorEnergyComponent;
+import de.tud.kom.p2psim.impl.energy.components.StateActuatorEnergyComponent;
 
 /**
  * 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 ActuatorEnergyComponent) {
-					((ActuatorEnergyComponent) component).turnOff();
+				if(component instanceof StateActuatorEnergyComponent) {
+					((StateActuatorEnergyComponent) component).turnOff();
 				}
 				
 				/*

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

@@ -35,7 +35,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.ActuatorEnergyComponent;
+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.models.AbstractEnergyModel;
 import de.tud.kom.p2psim.impl.topology.placement.UAVBasePlacement;
 import de.tud.kom.p2psim.impl.topology.util.PositionVector;
@@ -64,7 +65,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
 	
 	private OverlayComponent uavOverlayComponent;
 	
-	private ActuatorEnergyComponent actuator;
+	private StatelessMotorComponent actuator;
 	private RechargeableBattery battery;
 	
 	private UAVstate state = UAVstate.OFFLINE;
@@ -90,7 +91,8 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
 		
 		try {
 			actuator = getHost().getComponent(EnergyModel.class)
-					.getComponent(ComponentType.ACTUATOR, ActuatorEnergyComponent.class);			
+					.getComponent(ComponentType.ACTUATOR, StatelessMotorComponent.class);	
+			movement.setMotorControl(actuator);
 		} catch (ComponentNotAvailableException e) {
 			System.err.println("No Acutator Energy Component was found!");
 		}
@@ -120,12 +122,12 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
 
 	@Override
 	public double getMinMovementSpeed() {
-		return movement.getMinCruiseSpeed();
+		return movement.minimumVelocity();
 	}
 
 	@Override
 	public double getMaxMovementSpeed() {
-		return movement.getMaxCruiseSpeed();
+		return movement.horizontalMaxVelocity();
 	}
 
 	@Override
@@ -135,7 +137,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
 
 	@Override
 	public void setMovementSpeed(double speed) {
-		movement.setPreferredCruiseSpeed(speed);
+		movement.setPreferredSpeed(speed);
 	}
 
 	@Override
@@ -210,11 +212,6 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
 		this.movement = uavMovement;		
 	}
 
-	@Override
-	public ActuatorEnergyComponent getActuatorEnergyComponent() {
-		return actuator;
-	}
-
 	@Override
 	public Set<AttractionPoint> getAllAttractionPoints() {
 		throw new UnsupportedOperationException();

src/de/tud/kom/p2psim/impl/topology/movement/aerial/MulticopterMovement.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.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.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 {
+
+		
+	private UAVTopologyComponent topologyComponent;
+
+	private PositionVector direction;
+	private double currentAngleOfAttack;
+	private double currentSpeed;
+	
+	private double preferredSpeed;
+	
+	
+	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.2255; // 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 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);
+		
+	}
+	
+	
+	/*
+	 * 
+	 */
+	
+	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 = horizontalMaxThrust();
+				
+		double maxVelocity = Math.sqrt( (2.0 * horizontalThrust) / bodyDrag(maxPitchAngle, new PositionVector(1,0,0)));
+				
+		return maxVelocity;
+	}
+	
+	public double horizontalMaxThrust() {
+		
+		// hoverthrust / cos => amount of thrust in horizonal direction with °angle		
+		double stableAltitudeMaximumTotalThrust = hoverThrustRequired() / Math.cos(maxPitchAngle);
+		
+		// fraction of total thrust in horizonal (forward) direction with °angle
+		double maximumHorizontalThrustStableAltitude = stableAltitudeMaximumTotalThrust * Math.sin(maxPitchAngle);
+		
+		return maximumHorizontalThrustStableAltitude;
+	}
+
+	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, direction) * currentSpeed * currentSpeed;
+	}
+	
+	
+	
+	/*
+	 *
+	 */
+	
+	@Override
+	public void setMotorControl(StatelessMotorComponent motor) {
+		this.motor = 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;		
+	}
+	
+	@Override
+	public double getCurrentSpeed() {
+		return currentSpeed;
+	}
+
+	/**
+	 * Trigger the callback function, if there is a valid callback 
+	 * 
+	 * @param position
+	 */
+	private void locationReached(PositionVector position) {
+		if(locationCallbacks.containsKey(position)) {
+			locationCallbacks.get(position).reachedLocation();
+		}
+	}
+	
+	@Override
+	public void setTargetLocation(PositionVector target,
+			ReachedLocationCallback reachedLocationCallback) {
+		route.clear();
+		route.add(target);
+		if(reachedLocationCallback != null)
+			locationCallbacks.put(target, reachedLocationCallback);
+		
+	}
+
+	@Override
+	public void setTargetLocationRoute(LinkedList<PositionVector> route,
+			ReachedLocationCallback reachedLocationCallback) {
+		this.route.clear();
+		this.route.addAll(route);
+		if(reachedLocationCallback != null)
+			locationCallbacks.put(route.getLast(), reachedLocationCallback);
+	}
+
+	@Override
+	public void addTargetLocation(PositionVector target,
+			ReachedLocationCallback reachedLocationCallback) {
+		route.add(target);
+		if(reachedLocationCallback != null)
+			locationCallbacks.put(target, reachedLocationCallback);
+	}
+
+	@Override
+	public LinkedList<PositionVector> getTargetLocations() {
+		
+		LinkedList<PositionVector> copy = new LinkedList<>();
+		for (PositionVector pv : route) {
+			copy.add(pv.clone());		
+		}
+		
+		return copy;
+	}
+
+	@Override
+	public void removeTargetLocations() {
+		route.clear();
+		locationCallbacks.clear();
+	}
+
+
+	@Override
+	public double minimumVelocity() {
+		return 0;
+	}
+
+
+
+}

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

@@ -28,6 +28,7 @@ import java.util.Map;
 import org.joda.time.tz.ZoneInfoProvider;
 
 import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
+import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
 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;
@@ -52,6 +53,7 @@ public class SimpleMutlicopterMovement implements UAVMovementModel  {
 	private LinkedList<PositionVector> route = new LinkedList<>();
 	private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>();  // TODO callback interface
 		
+	private StatelessMotorComponent motor;
 	
 	public SimpleMutlicopterMovement(UAVTopologyComponent topologyComponent, double maxCruiseSpeed, double minCruiseSpeed) {
 		this.topologyComponent = topologyComponent;
@@ -62,18 +64,13 @@ public class SimpleMutlicopterMovement implements UAVMovementModel  {
 	}
 		
 	@Override
-	public void setPreferredCruiseSpeed(double v_pref) {
-		this.preferredCruiseSpeed = v_pref;		
-	}
-
-	@Override
-	public double getMaxCruiseSpeed() {
-		return maxCruiseSpeed;
+	public void setMotorControl(StatelessMotorComponent motor) {
+		this.motor = motor;
 	}
-
+	
 	@Override
-	public double getMinCruiseSpeed() {
-		return minCruiseSpeed;
+	public void setPreferredSpeed(double v_pref) {
+		this.preferredCruiseSpeed = v_pref;		
 	}
 
 	@Override
@@ -96,10 +93,10 @@ public class SimpleMutlicopterMovement implements UAVMovementModel  {
 			if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < currentSpeed)
 			{						
 				route.removeFirst();
-				topologyComponent.getActuatorEnergyComponent().useActuator(0.5);
+				motor.requestThrust(16.3);
 				topologyComponent.updateCurrentLocation(targetPosition); // triggers energy consumption for last distance
 				currentSpeed = 0;
-				topologyComponent.getActuatorEnergyComponent().useActuator(0); // now hover
+				motor.requestThrust(14.4); // now hover
 				locationReached(targetPosition);
 				return;
 			}
@@ -116,7 +113,7 @@ public class SimpleMutlicopterMovement implements UAVMovementModel  {
 			PositionVector newPosition = new PositionVector(currentPosition);
 			newPosition.add(direction);		
 			
-			topologyComponent.getActuatorEnergyComponent().useActuator(1);
+			motor.requestThrust(22);
 			topologyComponent.updateCurrentLocation(newPosition);
 			
 		}
@@ -179,6 +176,21 @@ public class SimpleMutlicopterMovement implements UAVMovementModel  {
 		locationCallbacks.clear();
 	}
 
+	@Override
+	public double verticalAscentMaxVelocity() {
+		return maxCruiseSpeed;
+	}
+
+	@Override
+	public double horizontalMaxVelocity() {
+		return maxCruiseSpeed;
+	}
+
+	@Override
+	public double minimumVelocity() {		
+		return 0;
+	}
+