Skip to content

GitLab

Commit a860aadf authored by Julian Zobel's avatar Julian Zobel
Browse files

More renaming to make energy and movement moidels for UAVs more lenient.

parent 5813d44d
Hide whitespace changes
Inline Side-by-side
src/de/tud/kom/p2psim/api/energy/Battery.java
View file @ a860aadf
......@@ -38,7 +38,7 @@ public interface Battery extends Cloneable {
*
* @return the maximum capacity of the battery in uJ
*/
public double getMaximumEnergyLevel();
public double getMaximumEnergy();
/**
* Returns the current status of the Battery as a Percentage-Value
......@@ -52,7 +52,7 @@ public interface Battery extends Cloneable {
*
* @return the current capacity of the battery in uJ
*/
public double getCurrentEnergyLevel();
public double getCurrentEnergy();
/**
* Returns the consumed energy in uJoule
......
src/de/tud/kom/p2psim/api/energy/EnergyComponent.java
View file @ a860aadf
......@@ -28,8 +28,8 @@ import de.tudarmstadt.maki.simonstrator.api.Time;
* State-switching is event-based in this new version of the Model to allow more
* fine-grained analyzing.
*
* @author Bjoern Richerzhagen
* @version 1.0, 21.02.2012
* @author Bjoern Richerzhagen, Julian Zobel
* @version 1.1, 20.01.2020
*/
public interface EnergyComponent extends EventHandler {
......
src/de/tud/kom/p2psim/api/topology/component/ControllableLocationActuator.java
View file @ a860aadf
......@@ -87,5 +87,7 @@ public interface ControllableLocationActuator extends Actuator {
public double getMovementSpeed();
public double estimatePowerConsumption(double velocity);
public double estimatePowerConsumptionWatt(double velocity);
public double estimateFlightDistance(double velocity, double batterylevel, double batterythreshold);
}
src/de/tud/kom/p2psim/api/topology/movement/UAVMovementModel.java
View file @ a860aadf
......@@ -21,24 +21,42 @@
package de.tud.kom.p2psim.api.topology.movement;
import java.util.LinkedList;
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.util.PositionVector;
import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;
/**
* UAV Movement Models provide basic functionality for the simulation of flying UAVs.
*
* @author Julian Zobel
* @version 1.0, 20.01.2020
*/
public interface UAVMovementModel {
// NO - NO CHANGY
public final double AIRDENSITY = 1.2045; // kg/m^3
public final double GRAVITY = 9.807; // m/s^2
public void setMotorControl(ActuatorComponent motor);
public void setPreferredSpeed(double v_pref);
/**
* Set the preferred/target speed of this UAV.
*
* @param targetVelocity
*/
public void setTargetVelocity(double targetVelocity);
public double verticalAscentMaxVelocity();
public double horizontalMaxVelocity();
public double getCurrentSpeed();
public double horizontalMinVelocity();
public double getVerticalAscentMaxVelocity();
public double getHorizontalMaxVelocity();
public double getCurrentVelocity();
public double getHorizontalMinVelocity();
public double estimatePowerConsumption(double velocity);
/**
* Estimate the power consumption for a given velocity
* @param velocity
* @return The power consumption in W for the given velocity.
*/
public double estimatePowerConsumptionWatt(double velocity);
public void move(long timeBetweenMovementOperations);
......
src/de/tud/kom/p2psim/impl/energy/EnergyModelFactory.java
View file @ a860aadf
......@@ -94,12 +94,12 @@ public class EnergyModelFactory implements HostComponentFactory {
double percentageOfFullBattery = (1 - random.nextDouble());
if(percentageOfFullBattery < minimumStartEnergyLevel)
percentageOfFullBattery = minimumStartEnergyLevel;
double initialEnergy = batteryModel.getMaximumEnergyLevel() * percentageOfFullBattery;
double initialEnergy = batteryModel.getMaximumEnergy() * percentageOfFullBattery;
bat = batteryModel.copy(initialEnergy);
}
else {
bat = batteryModel.copy(batteryModel.getMaximumEnergyLevel());
bat = batteryModel.copy(batteryModel.getMaximumEnergy());
}
EnergyModel em = new ComponentBasedEnergyModel(host, bat);
......
src/de/tud/kom/p2psim/impl/energy/RechargeableBattery.java
View file @ a860aadf
......@@ -63,6 +63,6 @@ public class RechargeableBattery extends SimpleBattery {
@Override
public String toString() {
return "Rechargeable Battery ["+getCurrentPercentage()+"%] ["+getCurrentEnergyLevel()+"/"+getMaximumEnergyLevel()+"]";
return "Rechargeable Battery ["+getCurrentPercentage()+"%] ["+getCurrentEnergy()+"/"+getMaximumEnergy()+"]";
}
}
src/de/tud/kom/p2psim/impl/energy/SimpleBattery.java
View file @ a860aadf
......@@ -64,7 +64,7 @@ public class SimpleBattery implements Battery {
}
@Override
public double getCurrentEnergyLevel() {
public double getCurrentEnergy() {
if (isEmpty()) {
return 0.0;
}
......@@ -104,7 +104,7 @@ public class SimpleBattery implements Battery {
}
@Override
public double getMaximumEnergyLevel() {
public double getMaximumEnergy() {
return initialEnergy;
}
......
src/de/tud/kom/p2psim/impl/energy/components/ActuatorComponent.java
View file @ a860aadf
......@@ -24,5 +24,6 @@ import de.tud.kom.p2psim.api.energy.EnergyComponent;
// Interface Tag
public interface ActuatorComponent extends EnergyComponent {
}
src/de/tud/kom/p2psim/impl/energy/components/StatefulActuatorComponent.java
View file @ a860aadf
......@@ -21,6 +21,7 @@
package de.tud.kom.p2psim.impl.energy.components;
import de.tud.kom.p2psim.api.energy.Battery;
import de.tud.kom.p2psim.api.energy.ComponentType;
import de.tud.kom.p2psim.api.energy.EnergyEventListener;
import de.tud.kom.p2psim.api.energy.EnergyState;
......@@ -54,8 +55,8 @@ public class StatefulActuatorComponent implements ActuatorComponent {
public StatefulActuatorComponent(int numberOfActuators, double volt, double hoverAmp, double maxAmp) {
OFF = new DefaultEnergyState("OFF", 0);
FLY = new DefaultEnergyState("FLY", numberOfActuators * (hoverAmp * volt) * 1000000);
MAX = new DefaultEnergyState("MAX", numberOfActuators * (maxAmp * volt) * 1000000);
FLY = new DefaultEnergyState("FLY", numberOfActuators * (hoverAmp * volt) * Battery.uJconverison);
MAX = new DefaultEnergyState("MAX", numberOfActuators * (maxAmp * volt) * Battery.uJconverison);
this.currentState = OFF;
this.lastEnergyConsumationEvent = Time.getCurrentTime();
......@@ -92,11 +93,20 @@ public class StatefulActuatorComponent implements ActuatorComponent {
}
}
public double estimatePowerConsumption(double load) {
/**
* Estimates the energy consumption based on a given load.
* @param load: Actuator load between 0.0 and 1.0
* @return Energy consumption in J/s
*/
public double estimateEnergyConsumptionWatt(double load) {
double consumationDelta = MAX.getEnergyConsumption() - FLY.getEnergyConsumption();
double estimation = FLY.getEnergyConsumption() + consumationDelta * load;
return estimation;
System.out.println("MAX " + ((MAX.getEnergyConsumption() / 14.8) / Battery.uJconverison));
System.out.println("MIN" + ((FLY.getEnergyConsumption() / 14.8) / Battery.uJconverison));
System.out.println(load);
// this estimation is in uJ, but has to return J, thus, do conversion
return estimation / Battery.uJconverison;
}
@Override
......
src/de/tud/kom/p2psim/impl/energy/components/StatelessActuatorComponent.java
View file @ a860aadf
......@@ -129,7 +129,7 @@ public class StatelessActuatorComponent implements ActuatorComponent {
/**
*
* @param targetThrust
* @return The power consumption for the target thrust in uW (micro watt)
* @return The power consumption for the target thrust in Watt
*/
public double estimatePowerConsumptionWatt(double targetThrust) {
if(targetThrust == 0 || targetThrust <= numberOfActuators * characteristics.getFirst().getThrust()) {
......@@ -143,6 +143,8 @@ public class StatelessActuatorComponent implements ActuatorComponent {
else {
double amps = approximateAmpereDraw(targetThrust);
System.out.println(amps);
return numberOfActuators * amps * volts;
}
}
......
src/de/tud/kom/p2psim/impl/energy/models/AbstractEnergyModel.java
View file @ a860aadf
......@@ -173,7 +173,7 @@ public abstract class AbstractEnergyModel implements EnergyModel, BatterySensor,
@Override
public double getCurrentEnergyLevel() {
return bat.getCurrentEnergyLevel();
return bat.getCurrentEnergy();
}
@Override
......
src/de/tud/kom/p2psim/impl/topology/component/UAVTopologyComponent.java
View file @ a860aadf
......@@ -23,6 +23,7 @@ package de.tud.kom.p2psim.impl.topology.component;
import java.util.LinkedList;
import java.util.Set;
import de.tud.kom.p2psim.api.common.SimHost;
import de.tud.kom.p2psim.api.energy.Battery;
import de.tud.kom.p2psim.api.energy.ComponentType;
import de.tud.kom.p2psim.api.energy.EnergyModel;
import de.tud.kom.p2psim.api.network.SimNetInterface;
......@@ -38,6 +39,7 @@ import de.tud.kom.p2psim.impl.energy.models.AbstractEnergyModel;
import de.tud.kom.p2psim.impl.topology.placement.UAVBasePlacement;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;
import de.tudarmstadt.maki.simonstrator.api.Monitor;
import de.tudarmstadt.maki.simonstrator.api.Time;
import de.tudarmstadt.maki.simonstrator.api.component.ComponentNotAvailableException;
import de.tudarmstadt.maki.simonstrator.api.component.overlay.OverlayComponent;
import de.tudarmstadt.maki.simonstrator.api.component.sensor.location.AttractionPoint;
......@@ -137,12 +139,12 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
@Override
public double getMovementSpeed() {
return movement.getCurrentSpeed();
return movement.getCurrentVelocity();
}
@Override
public void setMovementSpeed(double speed) {
movement.setPreferredSpeed(speed);
movement.setTargetVelocity(speed);
}
@Override
......@@ -197,13 +199,18 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
@Override
public double getCurrentBatteryEnergy() {
return battery.getCurrentEnergyLevel();
return battery.getCurrentEnergy();
}
public RechargeableBattery getBattery() {
return battery;
}
@Override
public double getMaximumBatteryCapacity() {
return battery.getMaximumEnergy();
}
@Override
public UAVMovementModel getUAVMovement() {
return movement;
......@@ -278,7 +285,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
}
};
movement.setPreferredSpeed(movement.horizontalMaxVelocity());
movement.setTargetVelocity(movement.horizontalMaxVelocity());
movement.setTargetLocation(baseLocation, returnCallback);
}
......@@ -339,17 +346,26 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
public void updateCurrentDirection(PositionVector direction) {
this.direction.set(direction);
}
@Override
public double getMaximumBatteryCapacity() {
return battery.getMaximumEnergyLevel();
public double estimatePowerConsumptionWatt(double velocity) {
return movement.estimatePowerConsumptionWatt(velocity);
}
@Override
public double estimatePowerConsumption(double velocity) {
return movement.estimatePowerConsumption(velocity);
public double estimateFlightDistance(double velocity, double batterylevel, double batterythreshold) {
assert batterylevel > batterythreshold;
assert batterylevel <= 1.0 && batterylevel >= 0.0;
assert batterythreshold <= 1.0 && batterythreshold >= 0.0;
double availableEnergy = (battery.getMaximumEnergy() * (batterylevel - batterythreshold)) / Battery.uJconverison; // since battery energy is in uJ, conversion in J is required
double powerconsumption = estimatePowerConsumptionWatt(velocity); // J/s (or Watt)
double distance = (availableEnergy / powerconsumption) * velocity; // d = (E/P)* v [m]
return distance;
}
@Override
public PositionVector getBaseLocation() {
return baseLocation.clone();
......
src/de/tud/kom/p2psim/impl/topology/movement/aerial/MulticopterMovement.java
View file @ a860aadf
......@@ -45,32 +45,41 @@ import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocation
* @version 1.0, 11.09.2018
*/
public class MulticopterMovement implements UAVMovementModel {
private UAVTopologyComponent topologyComponent;
private double currentAngleOfAttack;
private double currentSpeed;
private double velocity;
private double targetSpeed;
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 = 1.465; // kg
private final double airdensity = 1.2045; // 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.1; // m^2
private double dragCoefficient = 0.7;
private double maxPitchAngle = Math.toRadians(60); // 45° max angle
private double descentVelocityMax = 5; // m/s
private double maxTurnAngle = Math.toRadians(90); // 90° per second turn angle
public MulticopterMovement(UAVTopologyComponent topologyComponent) {
this.topologyComponent = topologyComponent;
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;
......@@ -87,7 +96,7 @@ public class MulticopterMovement implements UAVMovementModel {
// If target point is reached within a 1 meter margin, we remove that point from the list
if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < currentSpeed)
if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < velocity)
{
target = route.removeFirst();
......@@ -96,7 +105,7 @@ public class MulticopterMovement implements UAVMovementModel {
// go to hover mode
topologyComponent.updateCurrentLocation(target.clone());
currentSpeed = 0;
velocity = 0;
motor.requestThrust(hoverThrustRequired());
PositionVector direction = topologyComponent.getCurrentDirection().clone();
......@@ -109,16 +118,16 @@ public class MulticopterMovement implements UAVMovementModel {
else {
// get to speed
if(targetSpeed > 0 && targetSpeed < horizontalMaxVelocity()) {
motor.requestThrust(estimateRequiredThrust(targetSpeed));
currentSpeed = targetSpeed;
if(targetVelocity > 0 && targetVelocity < getHorizontalMaxVelocity()) {
motor.requestThrust(estimateRequiredThrust(targetVelocity));
velocity = targetVelocity;
}
else {
motor.requestThrust(horizontalMaxVelocityRequiredTotalThrust());
currentSpeed = horizontalMaxVelocity();
motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust());
velocity = getHorizontalMaxVelocity();
}
long timeUntilReachedLocation = (long) (distanceToTargetPosition / currentSpeed) * Time.SECOND;
long timeUntilReachedLocation = (long) (distanceToTargetPosition / velocity) * Time.SECOND;
target = route.getFirst();
PositionVector directionToTarget = new PositionVector(target);
......@@ -128,7 +137,7 @@ public class MulticopterMovement implements UAVMovementModel {
directionToTarget.normalize();
topologyComponent.updateCurrentDirection(directionToTarget.clone());
directionToTarget.multiplyScalar(currentSpeed * timefactor);
directionToTarget.multiplyScalar(velocity * timefactor);
PositionVector newPosition = new PositionVector(position);
newPosition.add(directionToTarget);
......@@ -146,13 +155,13 @@ public class MulticopterMovement implements UAVMovementModel {
double timefactor = timeBetweenMovementOperations / Time.SECOND;
// get to speed
if(targetSpeed > 0 && targetSpeed < horizontalMaxVelocity()) {
motor.requestThrust(estimateRequiredThrust(targetSpeed));
currentSpeed = targetSpeed;
if(targetVelocity > 0 && targetVelocity < getHorizontalMaxVelocity()) {
motor.requestThrust(estimateRequiredThrust(targetVelocity));
velocity = targetVelocity;
}
else {
motor.requestThrust(horizontalMaxVelocityRequiredTotalThrust());
currentSpeed = horizontalMaxVelocity();
motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust());
velocity = getHorizontalMaxVelocity();
}
PositionVector directionToTarget = new PositionVector(target);
......@@ -164,7 +173,7 @@ public class MulticopterMovement implements UAVMovementModel {
topologyComponent.updateCurrentDirection(directionToTarget.clone());
}
directionToTarget.multiplyScalar(currentSpeed * timefactor);
directionToTarget.multiplyScalar(velocity * timefactor);
PositionVector newPosition = new PositionVector(position);
newPosition.add(directionToTarget);
......@@ -175,7 +184,7 @@ public class MulticopterMovement implements UAVMovementModel {
}
else if(motor.isOn()) {
if(currentSpeed != 0) {
if(velocity != 0) {
throw new UnsupportedOperationException("no route but speed not 0?");
}
......@@ -194,61 +203,62 @@ public class MulticopterMovement implements UAVMovementModel {
*
*/
public double verticalDescentMaxThrust() {
protected double verticalDescentMaxThrust() {
// m * g - 0.5 * p * C * A * v^2
return hoverThrustRequired() - 0.5 * bodyDrag(0, new PositionVector(0,0,1)) * descentVelocityMax * descentVelocityMax;
return hoverThrustRequired() - 0.5 * bodyDrag(0, new PositionVector(0,0,1)) * maximumDecentVelocityAllowed * maximumDecentVelocityAllowed;
}
public double verticalAscentMaxAcceleration() {
protected double verticalAscentMaxAcceleration() {
return (motor.getMaxThrust() - hoverThrustRequired()) / mass;
}
public double verticalAscentMaxVelocity() {
@Override
public double getVerticalAscentMaxVelocity() {
double maxThrust = motor.getMaxThrust();
return Math.sqrt(2.0 * (maxThrust - hoverThrustRequired()) / bodyDrag(0, new PositionVector(0,0,1)));
}
public double hoverThrustRequired() {
return mass * gravity;
protected double hoverThrustRequired() {
return mass * GRAVITY;
}
public double horizontalMaxVelocity() {
@Override
public double getHorizontalMaxVelocity() {
double horizontalThrust = horizontalComponentMaxThrust();
double horizontalThrust = getHorizontalComponentMaxThrust();
double maxVelocity = Math.sqrt( (2.0 * horizontalThrust) / bodyDrag(maxPitchAngle, new PositionVector(1,0,0)));
double maxVelocity = Math.sqrt( (2.0 * horizontalThrust) / bodyDrag(maximumPitchAngleAllowed, new PositionVector(1,0,0)));
return maxVelocity;
}
public double horizontalComponentMaxThrust() {
protected double getHorizontalComponentMaxThrust() {
// hoverthrust / cos => amount of thrust in horizonal direction with °angle
double stableAltitudeMaximumTotalThrust = horizontalMaxVelocityRequiredTotalThrust();
double stableAltitudeMaximumTotalThrust = getHorizontalMaxVelocityRequiredTotalThrust();
// fraction of total thrust in horizonal (forward) direction with °angle
double maximumHorizontalThrustStableAltitude = stableAltitudeMaximumTotalThrust * Math.sin(maxPitchAngle);
double maximumHorizontalThrustStableAltitude = stableAltitudeMaximumTotalThrust * Math.sin(maximumPitchAngleAllowed);
return maximumHorizontalThrustStableAltitude;
}
public double horizontalMaxVelocityRequiredTotalThrust() {
return hoverThrustRequired() / Math.cos(maxPitchAngle);
protected double getHorizontalMaxVelocityRequiredTotalThrust() {
return hoverThrustRequired() / Math.cos(maximumPitchAngleAllowed);
}
public double bodyDrag(double angleRadians, PositionVector direction) {
return airdensity * dragCoefficient * areaExposedToDrag(angleRadians, direction);
protected double bodyDrag(double angleRadians, PositionVector direction) {
return AIRDENSITY * dragCoefficient * areaExposedToDrag(angleRadians, direction);
}
public double areaExposedToDrag(double angleRadians, PositionVector 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) * A_top + Math.cos(angleRadians) * A_front );
double areaExposedTop = v.getY() * (Math.cos(angleRadians) * A_top + Math.sin(angleRadians) * A_front);
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;
}
......@@ -256,8 +266,8 @@ public class MulticopterMovement implements UAVMovementModel {
/*
* F_drag [N] = 0.5 * p * C_drag * A * v^2
*/
public double currentDrag() {
return 0.5 * bodyDrag(currentAngleOfAttack, topologyComponent.getCurrentDirection()) * currentSpeed * currentSpeed;
protected double currentDrag() {
return 0.5 * bodyDrag(currentAngleOfAttack, topologyComponent.getCurrentDirection()) * velocity * velocity;
}
/**
......@@ -267,7 +277,7 @@ public class MulticopterMovement implements UAVMovementModel {
* @param angleInRadians
* @return
*/
private double forwardDrag(double velocity, double angleInRadians) {
protected double forwardDrag(double velocity, double angleInRadians) {
return 0.5 * bodyDrag(angleInRadians, new PositionVector(1,0,0)) * velocity * velocity;
}
......@@ -278,20 +288,17 @@ public class MulticopterMovement implements UAVMovementModel {
@Override
public void setMotorControl(ActuatorComponent motor) {
this.motor = (StatelessActuatorComponent) motor;
this.motor = (StatelessActuatorComponent) motor;
}
@Override
public void setPreferredSpeed(double v_pref) {
this.targetSpeed = v_pref;
public void setTargetVelocity(double v_pref) {
this.targetVelocity = v_pref;
}
@Override
public double getCurrentSpeed() {
return currentSpeed;
public double getCurrentVelocity() {
return velocity;
}
/**
......@@ -350,43 +357,38 @@ public class MulticopterMovement implements UAVMovementModel {
}
@Override
public double horizontalMinVelocity() {
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 estimatePowerConsumption(double velocity) {
public double estimatePowerConsumptionWatt(double velocity) {
if(velocity == 0) {
return motor.estimatePowerConsumptionWatt(hoverThrustRequired());
}
else if(velocity > horizontalMaxVelocity()) {
else if(velocity > getHorizontalMaxVelocity()) {
return -1;
}
else if(velocity < horizontalMinVelocity()) {
else if(velocity < getHorizontalMinVelocity()) {
return -1;
}
else {
double estimateAngle = estimatePitchAngleForVelocity(velocity);
double estimatedDrag = forwardDrag(velocity, estimateAngle);
double requiredThrust = Math.sqrt(hoverThrustRequired() * hoverThrustRequired() + estimatedDrag * estimatedDrag);
double wattage = motor.estimatePowerConsumptionWatt(requiredThrust);
double requiredThrust = estimateRequiredThrust(velocity);
double wattage = motor.estimatePowerConsumptionWatt(requiredThrust);
return wattage;
}
}
}
public double estimateRequiredThrust(double velocity) {
protected double estimateRequiredThrust(double velocity) {
if(velocity == 0) {
return motor.estimatePowerConsumptionWatt(hoverThrustRequired());
}
else if(velocity > horizontalMaxVelocity()) {
else if(velocity > getHorizontalMaxVelocity()) {
return -1;
}
else if(velocity < horizontalMinVelocity()) {
else if(velocity < getHorizontalMinVelocity()) {
return -1;
}
else {
......@@ -404,14 +406,14 @@ public class MulticopterMovement implements UAVMovementModel {
* @param velocity
* @return
*/
private double estimatePitchAngleForVelocity(double velocity) {
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(maxPitchAngle)); i++) {
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));
......@@ -453,7 +455,7 @@ public class MulticopterMovement implements UAVMovementModel {
return Math.toRadians(nearest);
}
return maxPitchAngle;
return maximumPitchAngleAllowed;
}
/**
......@@ -463,8 +465,47 @@ public class MulticopterMovement implements UAVMovementModel {
* @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);
return new MulticopterMovement(topologyComponent, massTotal, areaTop, areaFront,
UAVDragCoefficient, maximumPitchAngleAllowed, maximumDecentVelocityAllowed, maximumTurnAngle);
}
}
}
src/de/tud/kom/p2psim/impl/topology/movement/aerial/SimpleMulticopterMovement.java
View file @ a860aadf
......@@ -46,19 +46,22 @@ import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocation
public class SimpleMulticopterMovement implements UAVMovementModel {
private UAVTopologyComponent topologyComponent;
private double speed;
private double targetSpeed;
private double velocity;
private double targetVelocity;
private double maximumHorizontalSpeed = 15;
private double maximumVerticalSpeed = 5;
private double maximumHorizontalVelocity;
private double maximumVerticalVelocity;
private LinkedList<PositionVector> route = new LinkedList<>();
private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>();
private StatefulActuatorComponent motor;
public SimpleMulticopterMovement(UAVTopologyComponent topologyComponent) {
this.topologyComponent = topologyComponent;
public SimpleMulticopterMovement(UAVTopologyComponent topologyComponent, double maximumHorizontalVelocity,
double maximumVerticalVelocity) {
this.topologyComponent = topologyComponent;
this.maximumHorizontalVelocity = maximumHorizontalVelocity;
this.maximumVerticalVelocity = maximumVerticalVelocity;
}
@Override
......@@ -72,12 +75,12 @@ public class SimpleMulticopterMovement implements UAVMovementModel {
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 < speed)
if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < velocity)
{
route.removeFirst();
topologyComponent.updateCurrentLocation(targetPosition); // triggers energy consumption for last distance
speed = 0;
velocity = 0;
motor.useActuator(0);
locationReached(targetPosition);
return;
......@@ -85,12 +88,12 @@ public class SimpleMulticopterMovement implements UAVMovementModel {
double timefactor = timeBetweenMovementOperations / Time.SECOND;
speed = Math.min(distanceToTargetPosition, targetSpeed);
velocity = Math.min(distanceToTargetPosition, targetVelocity);
PositionVector direction = new PositionVector(targetPosition);
direction.subtract(currentPosition);
direction.normalize();
direction.multiplyScalar(speed * timefactor);
direction.multiplyScalar(velocity * timefactor);
PositionVector newPosition = new PositionVector(currentPosition);
newPosition.add(direction);
......@@ -100,7 +103,7 @@ public class SimpleMulticopterMovement implements UAVMovementModel {
}
else if(motor.isOn()) {
if(speed != 0) {
if(velocity != 0) {
throw new UnsupportedOperationException("no route but speed not 0?");
}
......@@ -116,13 +119,13 @@ public class SimpleMulticopterMovement implements UAVMovementModel {
}
@Override
public void setPreferredSpeed(double v_pref) {
this.targetSpeed = v_pref;
public void setTargetVelocity(double v_pref) {
this.targetVelocity = v_pref;
}
@Override
public double getCurrentSpeed() {
return speed;
public double getCurrentVelocity() {
return velocity;
}
/**
......@@ -181,15 +184,18 @@ public class SimpleMulticopterMovement implements UAVMovementModel {
}
@Override
public double estimatePowerConsumption(double velocity) {
public double estimatePowerConsumptionWatt(double velocity) {
double wattage = 0;
if(velocity == 0) {
return motor.estimatePowerConsumption(0);
wattage = motor.estimateEnergyConsumptionWatt(0);
}
else {
return motor.estimatePowerConsumption(1.0);
else {
wattage = motor.estimateEnergyConsumptionWatt(1.0);
}
return wattage;
}
@Override
......@@ -198,17 +204,17 @@ public class SimpleMulticopterMovement implements UAVMovementModel {
}
@Override
public double verticalAscentMaxVelocity() {
return maximumVerticalSpeed;
public double getVerticalAscentMaxVelocity() {
return maximumVerticalVelocity;
}
@Override
public double horizontalMaxVelocity() {
return maximumHorizontalSpeed;
public double getHorizontalMaxVelocity() {
return maximumHorizontalVelocity;
}
@Override
public double horizontalMinVelocity() {
public double getHorizontalMinVelocity() {
return 0;
}
......@@ -219,8 +225,21 @@ public class SimpleMulticopterMovement implements UAVMovementModel {
* @version 1.0, 14.01.2020
*/
public static class Factory implements AerialMovementModelFactory {
private double maximumHorizontalVelocity = 15;
public void setMaximumHorizontalVelocity(double maximumHorizontalVelocity) {
this.maximumHorizontalVelocity = maximumHorizontalVelocity;
}
private double maximumVerticalVelocity = 5;
public void setMaximumVerticalVelocity(double maximumVerticalVelocity) {
this.maximumVerticalVelocity = maximumVerticalVelocity;
}
public UAVMovementModel createComponent(UAVTopologyComponent topologyComponent) {
return new SimpleMulticopterMovement(topologyComponent);
}
return new SimpleMulticopterMovement(topologyComponent, maximumHorizontalVelocity, maximumVerticalVelocity);
}
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment