/*
* 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 .
*
*/
package de.tud.kom.p2psim.impl.topology.movement.aerial;
import java.util.LinkedHashMap;
import java.util.LinkedList;
import java.util.Map;
import javax.persistence.Column;
import javax.persistence.Entity;
import javax.persistence.Table;
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.tud.kom.p2psim.impl.util.db.metric.CustomMeasurement;
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 route = new LinkedList<>();
private Map 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
private double optimalVelocity = -1;
// TODO 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("[MulticopterMovement] no route but speed not 0?");
}
PositionVector position = new PositionVector(topologyComponent.getRealPosition());
if(position.getAltitude() == 0) {
motor.requestThrust(0);
}
else {
motor.requestThrust(hoverThrustRequired());
}
topologyComponent.updateCurrentLocation(position);
}
}
/*
*
*/
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 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 getTargetLocations() {
LinkedList 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);
}
}
// For evaluation only!
@Entity
@Table(name = "energyModel")
public static class EnergyModelPropertyMeasurement extends CustomMeasurement {
/*
* For DB performance reasons, we only store up to 15 characters of the
* topic (longer topics are truncated)
*/
@Column(nullable = false, name = "[velocity]")
final float velocity;
@Column(nullable = false, name = "[thrust]")
final float thrust;
@Column(nullable = false, name = "[ampere]")
final float ampere;
@Column(nullable = false, name = "[watt]")
final float watt;
@Column(nullable = false, name = "[wattPerMeter]")
final float wattperdistance;
public EnergyModelPropertyMeasurement(double velocity, double thrust, double ampere, double watt) {
/*
* Store all relevant fields
*/
this.velocity = (float) velocity;
this.thrust = (float) thrust;
this.ampere = (float) ampere;
this.watt = (float) watt;
if (velocity == 0) {
this.wattperdistance = -1;
} else
this.wattperdistance = this.watt / this.velocity;
}
public static EnergyModelPropertyMeasurement getPropoertyMeasurement(MulticopterMovement m, double velocity) {
double th = m.estimateRequiredThrust(velocity);
if (th == -1) {
return null;
}
double w = m.estimatePowerConsumptionWatt(velocity);
double amp = w / 14.8;
return new EnergyModelPropertyMeasurement(velocity, th, amp, w);
}
}
@Override
public double estimateOptimalSpeed() {
if(optimalVelocity == -1) {
double MIN = Double.MAX_VALUE;
double opt = -1;
double min = getHorizontalMinVelocity();
double max = getHorizontalMaxVelocity();
int steps = 50;
double stepsize = ( max-min) / steps;
for (int i = 0; i < steps; i++) {
double speed = min + i * stepsize;
double energyPerMeter = estimatePowerConsumptionWatt(speed) / speed ;
if(energyPerMeter < MIN) {
MIN = energyPerMeter;
opt = speed;
}
}
optimalVelocity = opt;
}
return optimalVelocity;
}
}