Extended simple UAV movement

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

@@ -48,6 +48,10 @@ public interface ControllableLocationActuator extends Actuator {
 	public boolean deactivate();
 
 	public PositionVector getCurrentLocation();
+	
+	public PositionVector getCurrentDirection();
+	
+	public void updateCurrentDirection(PositionVector direction);
 
 	public double getCurrentBatteryLevel();
 	

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

@@ -65,6 +65,8 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
 	
 	private OverlayComponent uavOverlayComponent;
 	
+	protected PositionVector direction;
+	
 	private StatelessMotorComponent actuator;
 	private RechargeableBattery battery;
 	
@@ -83,6 +85,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
 	public UAVTopologyComponent(SimHost host, Topology topology,
 			MovementModel movementModel, PlacementModel placementModel, boolean registerAsInformationProviderInSiS) {
 		super(host, topology, movementModel, placementModel, registerAsInformationProviderInSiS);		
+		direction = new PositionVector(0,-1,0);
 	}
 
 	@Override
@@ -328,6 +331,16 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
 			net.goOnline();
 	}
 
+	@Override
+	public PositionVector getCurrentDirection() {
+		return direction;
+	}
+
+	@Override
+	public void updateCurrentDirection(PositionVector direction) {
+		this.direction.set(direction);
+	}
+
 	
 
 }

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

@@ -25,6 +25,7 @@ import java.util.LinkedHashMap;
 import java.util.LinkedList;
 import java.util.Map;
 
+import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
 import org.joda.time.tz.ZoneInfoProvider;
 
 import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
@@ -41,84 +42,215 @@ import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocation
  * @author Julian Zobel
  * @version 1.0, 11.09.2018
  */
-public class SimpleMutlicopterMovement implements UAVMovementModel  {
+public class SimpleMulticopterMovement implements UAVMovementModel  {
 
 	private UAVTopologyComponent topologyComponent;
-
-	private final double maxCruiseSpeed;
-	private final double minCruiseSpeed;
-	private double preferredCruiseSpeed;
+	
+	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;
 	
-	public SimpleMutlicopterMovement(UAVTopologyComponent topologyComponent, double maxCruiseSpeed, double minCruiseSpeed) {
-		this.topologyComponent = topologyComponent;
-		this.maxCruiseSpeed = maxCruiseSpeed;
-		this.minCruiseSpeed = minCruiseSpeed;
-		this.preferredCruiseSpeed = this.maxCruiseSpeed;
-					
-	}
-		
-	@Override
-	public void setMotorControl(StatelessMotorComponent motor) {
-		this.motor = motor;
-	}
 	
-	@Override
-	public void setPreferredSpeed(double v_pref) {
-		this.preferredCruiseSpeed = v_pref;		
-	}
-
-	@Override
-	public double getCurrentSpeed() {
-		return currentSpeed;
+	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 SimpleMulticopterMovement(UAVTopologyComponent topologyComponent) {
+		this.topologyComponent = topologyComponent;		
 	}
 	
 	
-
 	@Override
 	public void move(long timeBetweenMovementOperations) {
 		
-		if(topologyComponent.isActive() && !route.isEmpty()) {
-			PositionVector currentPosition= topologyComponent.getRealPosition();
+		if(motor.isOn() && !route.isEmpty()) {
+			PositionVector position = new PositionVector(topologyComponent.getRealPosition());			
+						
+			PositionVector target = route.getFirst();
+			
+			double distanceToTargetPosition = position.distanceTo(target);
+			
 			
-			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)
 			{						
-				route.removeFirst();
-				motor.requestThrust(16.3);
-				topologyComponent.updateCurrentLocation(targetPosition); // triggers energy consumption for last distance
-				currentSpeed = 0;
-				motor.requestThrust(14.4); // now hover
-				locationReached(targetPosition);
-				return;
+				target = route.removeFirst();
+				
+				if(route.isEmpty()) {
+					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 {
+					
+					motor.requestThrust(horizontalMaxThrust());
+					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;
+				
+				motor.getMaxThrust();
+				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);
 			}
 			
-			double timefactor = timeBetweenMovementOperations / Time.SECOND;
 			
-			currentSpeed = Math.min(distanceToTargetPosition, preferredCruiseSpeed);					
-						
-			PositionVector direction = new PositionVector(targetPosition);
-			direction.subtract(currentPosition);
-			direction.normalize();
-			direction.multiplyScalar(currentSpeed * timefactor);
-
-			PositionVector newPosition = new PositionVector(currentPosition);
-			newPosition.add(direction);		
 			
-			motor.requestThrust(22);
-			topologyComponent.updateCurrentLocation(newPosition);
+			
+	
 			
 		}
+	}
+	
+	/*
+	 * 
+	 */
+	
+	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() {
 		
-		//System.out.println(Simulator.getFormattedTime(Simulator.getCurrentTime()) + " | " +  topologyComponent);		
+		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, topologyComponent.getCurrentDirection()) * currentSpeed * currentSpeed;
+	}
+	
+	
+	
+	/*
+	 *
+	 */
+	
+	@Override
+	public void setMotorControl(StatelessMotorComponent motor) {
+		this.motor = motor;
+	}
+
+	
+
+	
+	@Override
+	public void setPreferredSpeed(double v_pref) {
+		this.preferredSpeed = v_pref;		
+	}
+	
+	@Override
+	public double getCurrentSpeed() {
+		return currentSpeed;
 	}
 
 	/**
@@ -176,22 +308,10 @@ public class SimpleMutlicopterMovement implements UAVMovementModel  {
 		locationCallbacks.clear();
 	}
 
-	@Override
-	public double verticalAscentMaxVelocity() {
-		return maxCruiseSpeed;
-	}
-
-	@Override
-	public double horizontalMaxVelocity() {
-		return maxCruiseSpeed;
-	}
 
 	@Override
-	public double minimumVelocity() {		
+	public double minimumVelocity() {
 		return 0;
 	}
-
-	
-	
 	
 }