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.euclidean.twod.Vector2D;
import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
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;

/**
 * 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 double currentAngleOfAttack;
	private double velocity;
	
	private double targetVelocity;	
	
	private LinkedList<PositionVector> route = new LinkedList<>();
	private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>();  // TODO callback interface
		
	private StatelessActuatorComponent motor;
	
	private double mass; // kg
	private double areaTop; // m^2
	private double areaFront; // m^2
	private double dragCoefficient;
	private double maximumPitchAngleAllowed; // ° max angle
	private double maximumDecentVelocityAllowed; // m/s
	
	// FIXME currently not used
	private double maximumTurnAngle; // 90° per second turn angle
	
	
	public MulticopterMovement(UAVTopologyComponent topologyComponent, double massTotal, 
			double areaTop, double areaFront, double UAVDragCoefficient, double maximumPitchAngleAllowed, 
			double maximumDecentVelocityAllowed, double maximumTurnAngle) {
		this.topologyComponent = topologyComponent;	
		this.mass = massTotal;
		this.areaTop = areaTop;
		this.areaFront = areaFront;
		this.dragCoefficient = UAVDragCoefficient;
		this.maximumPitchAngleAllowed = maximumPitchAngleAllowed;
		this.maximumDecentVelocityAllowed = maximumDecentVelocityAllowed;
		this.maximumTurnAngle = maximumTurnAngle;
	}
	
	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 < velocity)
			{						
				target = route.removeFirst();
								
				if(route.isEmpty()) {	
					
					// go to hover mode					
					topologyComponent.updateCurrentLocation(target.clone()); 
					
					velocity = 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(targetVelocity > 0 && targetVelocity < getHorizontalMaxVelocity()) {
						motor.requestThrust(estimateRequiredThrust(targetVelocity));
						velocity = targetVelocity;
					}
					else {
						motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust());
						velocity = getHorizontalMaxVelocity();
					}							
			
					long timeUntilReachedLocation = (long) (distanceToTargetPosition / velocity) * 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(velocity * 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(targetVelocity > 0 && targetVelocity < getHorizontalMaxVelocity()) {
					motor.requestThrust(estimateRequiredThrust(targetVelocity));
					velocity = targetVelocity;
				}
				else {
					motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust());
					velocity = getHorizontalMaxVelocity();
				}				
				
				PositionVector directionToTarget = new PositionVector(target);
				directionToTarget.subtract(position);
				
				directionToTarget.normalize();
				
				if(directionToTarget.getX() != 0 || directionToTarget.getY() != 0) {
					topologyComponent.updateCurrentDirection(directionToTarget.clone());
				}				
				
				directionToTarget.multiplyScalar(velocity * timefactor);

				PositionVector newPosition = new PositionVector(position);
				newPosition.add(directionToTarget);		
				
				topologyComponent.updateCurrentLocation(newPosition);
			}
			
		}
		else if(motor.isOn()) {
			
			if(velocity != 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());
			}
		}
	}
	
	/*
	 * 
	 */
	
	protected double verticalDescentMaxThrust() {		
		// m * g - 0.5 * p * C * A * v^2
		return hoverThrustRequired() - 0.5 * bodyDrag(0, new PositionVector(0,0,1)) * maximumDecentVelocityAllowed * maximumDecentVelocityAllowed;
	}
	
	protected double verticalAscentMaxAcceleration() {
		return (motor.getMaxThrust() - hoverThrustRequired()) / mass;
	}
	
	@Override
	public double getVerticalAscentMaxVelocity() {		
		double maxThrust = motor.getMaxThrust();		
		return Math.sqrt(2.0 * (maxThrust - hoverThrustRequired()) / bodyDrag(0, new PositionVector(0,0,1)));		
	}
	
	
	protected double hoverThrustRequired()  {		
		return mass * GRAVITY;		
	}
	
	@Override
	public double getHorizontalMaxVelocity() {
		
		double horizontalThrust = getHorizontalComponentMaxThrust();
				
		double maxVelocity = Math.sqrt( (2.0 * horizontalThrust) / bodyDrag(maximumPitchAngleAllowed, new PositionVector(1,0,0)));
				
		return maxVelocity;
	}
	
	protected double getHorizontalComponentMaxThrust() {
		
		// hoverthrust / cos => amount of thrust in horizonal direction with °angle		
		double stableAltitudeMaximumTotalThrust = getHorizontalMaxVelocityRequiredTotalThrust();
	
		// fraction of total thrust in horizonal (forward) direction with °angle
		double maximumHorizontalThrustStableAltitude = stableAltitudeMaximumTotalThrust * Math.sin(maximumPitchAngleAllowed);
		
		return maximumHorizontalThrustStableAltitude;
	}

	protected double getHorizontalMaxVelocityRequiredTotalThrust() {
		return hoverThrustRequired() / Math.cos(maximumPitchAngleAllowed);
	}
	
	protected double bodyDrag(double angleRadians, PositionVector direction) {
		return AIRDENSITY * dragCoefficient * areaExposedToDrag(angleRadians, direction);
	}
	
	protected 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) * areaTop + Math.cos(angleRadians) * areaFront );
		double areaExposedTop = v.getY() * (Math.cos(angleRadians) * areaTop + Math.sin(angleRadians) * areaFront);
		
		return areaExposedFront + areaExposedTop;
	}
	
	/*
	 * F_drag [N] = 0.5 * p * C_drag * A * v^2
	 */
	protected double currentDrag() {
		return 0.5 * bodyDrag(currentAngleOfAttack, topologyComponent.getCurrentDirection()) * velocity * velocity;
	}
	
	/**
	 * 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
	 */
	protected double forwardDrag(double velocity, double angleInRadians) {
		return 0.5 * bodyDrag(angleInRadians, new PositionVector(1,0,0)) * velocity * velocity;
	}
	
	
	/*
	 *
	 */
	
	@Override
	public void setMotorControl(ActuatorComponent motor) {
		this.motor = (StatelessActuatorComponent) motor;	
	}
	
	@Override
	public void setTargetVelocity(double v_pref) {
		this.targetVelocity = v_pref;		
	}
	
	@Override
	public double getCurrentVelocity() {
		return velocity;
	}

	/**
	 * 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 getHorizontalMinVelocity() {
		return Math.sqrt(2 * hoverThrustRequired() * Math.tan(Math.toRadians(0.25)) / bodyDrag(Math.toRadians(0.25), new PositionVector(1,0,0)));
	}

	@Override
	public double estimatePowerConsumptionWatt(double velocity) {
		
		if(velocity == 0) {
			return motor.estimatePowerConsumptionWatt(hoverThrustRequired());
		}
		else if(velocity > getHorizontalMaxVelocity()) {
			return -1;
		}
		else if(velocity < getHorizontalMinVelocity()) {
			return -1;
		}
		else {			
			
			double requiredThrust = estimateRequiredThrust(velocity);
			double wattage = motor.estimatePowerConsumptionWatt(requiredThrust);		
			return wattage;
		}				
	}
	
	protected double estimateRequiredThrust(double velocity) {
		if(velocity == 0) {
			return motor.estimatePowerConsumptionWatt(hoverThrustRequired());
		}
		else if(velocity > getHorizontalMaxVelocity()) {
			return -1;
		}
		else if(velocity < getHorizontalMinVelocity()) {
			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
	 */
	protected double estimatePitchAngleForVelocity(double velocity) {
		
		int low = 0;
		int high = Integer.MAX_VALUE;
		
		double vsquared = (velocity * velocity);
		
		for(int i = 0; i <= ((int) Math.toDegrees(maximumPitchAngleAllowed)); 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 maximumPitchAngleAllowed;	
	}	
	
	/**
	 * Factory for this movement model
	 * 
	 * @author Julian Zobel
	 * @version 1.0, 14.01.2020
	 */
	public static class Factory implements AerialMovementModelFactory {
		
		private double massTotal = 1.465; // kg		
		private double areaTop = 0.245; // m^2
		private double areaFront = 0.1; // m^2
		private double UAVDragCoefficient = 0.7;
		private double maximumPitchAngleAllowed = Math.toRadians(60); // ° max angle
		private double maximumDecentVelocityAllowed = 5; // m/s
		private double maximumTurnAngle = Math.toRadians(90); // 90° per second turn angle
		
		public void setMassTotal(double massTotal) {
			this.massTotal = massTotal;
		}

		public void setAreaTop(double areaTop) {
			this.areaTop = areaTop;
		}

		public void setAreaFront(double areaFront) {
			this.areaFront = areaFront;
		}

		public void setUAVDragCoefficient(double uAVDragCoefficient) {
			UAVDragCoefficient = uAVDragCoefficient;
		}

		public void setMaximumPitchAngleAllowed(double maximumPitchAngleAllowed) {
			this.maximumPitchAngleAllowed = Math.toRadians(maximumPitchAngleAllowed);
		}

		public void setMaximumDecentVelocityAllowed(
				double maximumDecentVelocityAllowed) {
			this.maximumDecentVelocityAllowed = maximumDecentVelocityAllowed;
		}

		public void setMaximumTurnAngle(double maximumTurnAngle) {
			this.maximumTurnAngle = Math.toRadians(maximumTurnAngle);
		}

		public UAVMovementModel createComponent(UAVTopologyComponent topologyComponent) {
			return new MulticopterMovement(topologyComponent, massTotal, areaTop, areaFront, 
					UAVDragCoefficient, maximumPitchAngleAllowed, maximumDecentVelocityAllowed, maximumTurnAngle);
		}		
	}
}