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
......@@ -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
......
......@@ -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 {
......
......@@ -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);
}
......@@ -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);
......
......@@ -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);
......
......@@ -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()+"]";
}
}
......@@ -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;
}
......
......@@ -24,5 +24,6 @@ import de.tud.kom.p2psim.api.energy.EnergyComponent;
// Interface Tag
public interface ActuatorComponent extends EnergyComponent {
}
......@@ -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
......
......@@ -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;
}
}
......
......@@ -173,7 +173,7 @@ public abstract class AbstractEnergyModel implements EnergyModel, BatterySensor,
@Override
public double getCurrentEnergyLevel() {
return bat.getCurrentEnergyLevel();
return bat.getCurrentEnergy();
}
@Override
......
......@@ -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();
......
......@@ -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);
}
}
}
......@@ -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