Save stash

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

@@ -30,7 +30,7 @@ public interface UAVMovementModel {
 	
 	public void setMotorControl(StatelessMotorComponent motor);
 	
-	public void setPreferredCruiseSpeed(double v_pref);
+	public void setPreferredSpeed(double v_pref);
 	
 	public double getMaxCruiseSpeed();
 	public double getMinCruiseSpeed();

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

@@ -137,7 +137,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
 
 	@Override
 	public void setMovementSpeed(double speed) {
-		movement.setPreferredCruiseSpeed(speed);
+		movement.setPreferredSpeed(speed);
 	}
 
 	@Override

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

@@ -24,6 +24,10 @@ 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;
@@ -33,28 +37,113 @@ import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocation
 
 public class MulticopterMovement implements UAVMovementModel {
 
-	//public static enum FlightState { OFF, HOVER, 
-	
+		
 	private UAVTopologyComponent topologyComponent;
 
-	private final double maxCruiseSpeed;
-	private final double minCruiseSpeed;
-	private double preferredCruiseSpeed;
+	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;
 	
-	public MulticopterMovement(UAVTopologyComponent topologyComponent, double maxCruiseSpeed, double minCruiseSpeed) {
+	
+	private double mass; // 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 maxAngle = Math.toRadians(45); // 45° max angle
+	private double descentVelocityMax = 5; // m/s
+	
+	
+	
+	
+	
+	public MulticopterMovement(UAVTopologyComponent topologyComponent) {
 		this.topologyComponent = topologyComponent;
-		this.maxCruiseSpeed = maxCruiseSpeed;
-		this.minCruiseSpeed = minCruiseSpeed;
-		this.preferredCruiseSpeed = this.maxCruiseSpeed;
-					
+
+		
+	}
+	
+	
+	/*
+	 * 
+	 */
+	
+	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(maxAngle, 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(maxAngle);
+		
+		// fraction of total thrust in horizonal (forward) direction with °angle
+		double maximumHorizontalThrustStableAltitude = stableAltitudeMaximumTotalThrust * Math.sin(maxAngle);
+		
+		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;
@@ -103,8 +192,8 @@ public class MulticopterMovement implements UAVMovementModel {
 
 	
 	@Override
-	public void setPreferredCruiseSpeed(double v_pref) {
-		this.preferredCruiseSpeed = v_pref;		
+	public void setPreferredSpeed(double v_pref) {
+		this.preferredSpeed = v_pref;		
 	}
 
 	@Override

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

@@ -69,7 +69,7 @@ public class SimpleMutlicopterMovement implements UAVMovementModel  {
 	}
 	
 	@Override
-	public void setPreferredCruiseSpeed(double v_pref) {
+	public void setPreferredSpeed(double v_pref) {
 		this.preferredCruiseSpeed = v_pref;		
 	}