Power estimation on UAV Movement models.

Updated calculations in motor components

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

@@ -37,6 +37,8 @@ public interface UAVMovementModel {
 	public double getCurrentSpeed();
 	public double minimumVelocity();
 	
+	public double estimatePowerConsumption(double velocity);
+	
 	public void move(long timeBetweenMovementOperations);
 	
 	public void setTargetLocation(PositionVector target, ReachedLocationCallback reachedLocationCallback);

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

@@ -65,7 +65,7 @@ public class StatelessMotorComponent implements EnergyComponent {
 		this.numberOfActuators = numberOfActuators;				
 		this.state = componentState.OFF;
 		this.lastEnergyConsumationTime = Time.getCurrentTime();
-			
+		
 		thrust = 0;
 		amps = 0;
 		energyState = new DefaultEnergyState("OFF", 0);
@@ -122,20 +122,43 @@ public class StatelessMotorComponent implements EnergyComponent {
 			setLoad(characteristics.getLast());
 		}
 		else {
-			approximateThrustCurrentRelation(targetThrust);
+			calculateAndSetThrustRelatedAmpereDraw(targetThrust);
 		}
 				
 		return this.thrust;
 	}
 	
+	/**
+	 * 
+	 * @param targetThrust
+	 * @return The power consumption for the target thrust in uW (micro watt)
+	 */
+	public double estimatePowerConsumptionWatt(double targetThrust) {
+		if(targetThrust == 0 || targetThrust <= characteristics.getFirst().getThrust()) {
+			// not allowed
+			return Double.NaN;
+		}
+		else if(targetThrust > characteristics.getLast().getThrust()) {
+			// not allowed
+			return Double.NaN;
+		}
+		else {			
+			double amps = approximateAmpereDraw(targetThrust);
+			
+			return numberOfActuators * amps * volts;
+		}
+	}
+	
+	
 	/**
 	 * 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
+	 * @return the approximated ampere draw
 	 */
-	private void approximateThrustCurrentRelation(double targetThrust) {
+	private double approximateAmpereDraw(double targetThrust) {
 		
 		MotorCharacteristic lower = null, upper = null;
 		
@@ -143,8 +166,7 @@ public class StatelessMotorComponent implements EnergyComponent {
 		for (MotorCharacteristic ch : characteristics) {
 			//
 			if(ch.getThrust() == targetThrust) {
-				setLoad(ch);
-				return;
+				return ch.getCurrent();
 			}
 			else {
 				// list is sorted, lower bound is the biggest that is lower
@@ -172,8 +194,17 @@ public class StatelessMotorComponent implements EnergyComponent {
 		 * 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());
-				
+		return lower.getCurrent() + delta * (upper.getCurrent() - lower.getCurrent());
+	}
+	
+	/**
+	 * Approximates the ampere draw required forthe requested thrust
+	 * 
+	 * Target thrust should be strictly within the possible thrust limits
+	 * 
+	 */
+	private void calculateAndSetThrustRelatedAmpereDraw(double targetThrust) {				
+		double calculatedAmps = approximateAmpereDraw(targetThrust);			
 		setLoad(targetThrust, calculatedAmps);
 	}
 	

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

@@ -311,5 +311,12 @@ public class MulticopterMovement implements UAVMovementModel {
 	}
 
 
+	@Override
+	public double estimatePowerConsumption(double velocity) {
+		// TODO Auto-generated method stub
+		return 0;
+	}
+
+
 
 }

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

@@ -25,6 +25,8 @@ import java.util.LinkedHashMap;
 import java.util.LinkedList;
 import java.util.Map;
 
+import javax.persistence.criteria.CriteriaBuilder.In;
+
 import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
 import org.joda.time.tz.ZoneInfoProvider;
 
@@ -49,8 +51,7 @@ public class SimpleMulticopterMovement implements UAVMovementModel  {
 	private double currentAngleOfAttack;
 	private double currentSpeed;
 	
-	private double preferredSpeed;
-	
+	private double targetedSpeed;	
 	
 	private LinkedList<PositionVector> route = new LinkedList<>();
 	private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>();  // TODO callback interface
@@ -58,6 +59,7 @@ public class SimpleMulticopterMovement implements UAVMovementModel  {
 	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
@@ -68,15 +70,51 @@ public class SimpleMulticopterMovement implements UAVMovementModel  {
 	private double descentVelocityMax = 5; // m/s
 	private double maxTurnAngle = Math.toRadians(90); // 90° per second turn angle
 	
-		
+	
+	// FIXME remove only for testing
+	private static boolean first = true;	
+	
 	public SimpleMulticopterMovement(UAVTopologyComponent topologyComponent) {
-		this.topologyComponent = topologyComponent;		
+		this.topologyComponent = topologyComponent;	
+		
+		
 	}
 	
 	
 	@Override
 	public void move(long timeBetweenMovementOperations) {
 		
+		if(first) {
+			
+			System.out.println("***MOVEMENT MODEL****");
+			
+			System.out.println("Hover thrust: " + hoverThrustRequired() + "N");
+			
+			System.out.println("Maximum thrust: " + motor.getMaxThrust() + "N");
+			
+			System.out.println("Vertical ascension: " + verticalAscentMaxAcceleration() + " m/s^2 >> max velocity" + verticalAscentMaxVelocity() + " m/s at maximum thrust");
+			
+			System.out.println("Horizontal Flight: " + horizontalMaxVelocityRequiredTotalThrust() + " N >> max velocity" + horizontalMaxVelocity() + " m/s @ 45° pitch angle, minimum velocity " + this.minimumHorizontalVelocity() + " m/s @ 0.25° pitch angle");
+					
+			System.out.println("************");
+			
+			
+			
+			
+			for (int i = (int) Math.ceil(minimumHorizontalVelocity()); i <= horizontalMaxVelocity(); i++) {
+				double est = estimatePowerConsumption(i);
+				
+				System.out.println(" v = " + i + " m/s ==> estimated power consumption = " + est + " J/s");
+				System.out.println("=== === === === === ===");
+			}
+			
+			System.out.println(" v = " + horizontalMaxVelocity() + " m/s ==> estimated power consumption = " + estimatePowerConsumption(horizontalMaxVelocity()) + " J/s");
+			System.out.println("=== === === === === ===");
+			
+			first = false;
+		}
+		
+		
 		if(motor.isOn() && !route.isEmpty()) {
 			PositionVector position = new PositionVector(topologyComponent.getRealPosition());			
 						
@@ -105,7 +143,7 @@ public class SimpleMulticopterMovement implements UAVMovementModel  {
 				}
 				else {
 					
-					motor.requestThrust(horizontalMaxThrust());
+					motor.requestThrust(horizontalMaxVelocityRequiredTotalThrust());
 					currentSpeed = horizontalMaxVelocity();
 					
 					long timeUntilReachedLocation = (long) (distanceToTargetPosition / currentSpeed) * Time.SECOND;
@@ -135,7 +173,7 @@ public class SimpleMulticopterMovement implements UAVMovementModel  {
 			else {
 				double timefactor = timeBetweenMovementOperations / Time.SECOND;
 				
-				motor.getMaxThrust();
+				motor.requestThrust(horizontalMaxVelocityRequiredTotalThrust());
 				currentSpeed = horizontalMaxVelocity();
 				
 				PositionVector directionToTarget = new PositionVector(target);
@@ -189,24 +227,28 @@ public class SimpleMulticopterMovement 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);
 	}
@@ -229,6 +271,16 @@ public class SimpleMulticopterMovement implements UAVMovementModel  {
 		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;
+	}
 	
 	
 	/*
@@ -245,7 +297,7 @@ public class SimpleMulticopterMovement implements UAVMovementModel  {
 	
 	@Override
 	public void setPreferredSpeed(double v_pref) {
-		this.preferredSpeed = v_pref;		
+		this.targetedSpeed = v_pref;		
 	}
 	
 	@Override
@@ -313,5 +365,110 @@ public class SimpleMulticopterMovement implements UAVMovementModel  {
 	public double minimumVelocity() {
 		return 0;
 	}
+
+	
+	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;
+		}
+		else if(velocity < minimumHorizontalVelocity()) {
+			return -1;
+		}
+		else {
+			System.out.println("===========================");
+			double estimateAngle = estimatePitchAngleForVelocity(velocity);
+			double estimatedDrag = forwardDrag(velocity, estimateAngle);			
+			
+			double requiredThrust = Math.sqrt(hoverThrustRequired() * hoverThrustRequired() + estimatedDrag * estimatedDrag);			
+			double wattage = motor.estimatePowerConsumptionWatt(requiredThrust);
+			
+			System.out.println("Motor requires " + wattage + " J/s at velocity " + velocity + " (Thrust: " + requiredThrust + " N, angle = " + Math.toDegrees(estimateAngle)  + ", drag = "+ estimatedDrag+" N )");
+			
+			
+			return wattage;
+		}
+		
+		
+	}
+	
+	
+	/**
+	 * 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;
+				}			
+			}
+			
+			double n = Math.sqrt(2 * hoverThrustRequired() * Math.tan(Math.toRadians(nearest)) / bodyDrag(Math.toRadians(nearest), new PositionVector(1,0,0)));
+			
+			System.out.println("target velocity = " + velocity + " m/s ==> AoA = " + nearest + "° // v_max = " + n + " m/s");
+			
+			
+			
+			return Math.toRadians(nearest);
+		}
+		
+		
+		return maxPitchAngle;
+	
+	}
+	
+	
+	
 	
 }