From 749d8f285413d394e7bfd7ebfe43287850fede29 Mon Sep 17 00:00:00 2001 From: Julian Zobel Date: Thu, 20 Jan 2022 16:32:39 +0100 Subject: [PATCH] Lots of documentation Adaption of movement models and UAV models --- .../topology/movement/UAVMovementModel.java | 7 +- .../kom/p2psim/api/uav/MulticopterModel.java | 168 +++- .../component/UAVTopologyComponent.java | 11 +- .../movement/aerial/MulticopterMovement.java | 343 +------ .../aerial/SimpleMulticopterMovement.java | 33 +- .../aerial/StateMulticopterMovement.java | 870 ++++++++++-------- .../tud/kom/p2psim/impl/uav/IntelAeroRTF.java | 153 ++- 7 files changed, 835 insertions(+), 750 deletions(-) diff --git a/src/de/tud/kom/p2psim/api/topology/movement/UAVMovementModel.java b/src/de/tud/kom/p2psim/api/topology/movement/UAVMovementModel.java index f6acb9cc..a329e5f2 100644 --- a/src/de/tud/kom/p2psim/api/topology/movement/UAVMovementModel.java +++ b/src/de/tud/kom/p2psim/api/topology/movement/UAVMovementModel.java @@ -43,7 +43,7 @@ public interface UAVMovementModel { public void setTargetVelocity(double targetVelocity); public double getCurrentVelocity(); public double getMinimumVelocity(); - public double getMaxiumumVelocity(); + public double getMaximumVelocity(); public void move(long timeBetweenMovementOperations); @@ -59,4 +59,9 @@ public interface UAVMovementModel { public LinkedList getTargetLocations(); public void removeTargetLocations(); + + public double powerConsumptionWatt(double velocity); + + public double optimalSpeed(); + } diff --git a/src/de/tud/kom/p2psim/api/uav/MulticopterModel.java b/src/de/tud/kom/p2psim/api/uav/MulticopterModel.java index 42cf9dbb..5380faf8 100644 --- a/src/de/tud/kom/p2psim/api/uav/MulticopterModel.java +++ b/src/de/tud/kom/p2psim/api/uav/MulticopterModel.java @@ -24,7 +24,7 @@ package de.tud.kom.p2psim.api.uav; import de.tud.kom.p2psim.impl.energy.components.ActuatorComponent; /** - * Interface for UAV models. Multicopter! + * Interface for thrust/acceleration-based UAV models. Multicopter! * * @author Julian Zobel * @version 1.0, 19.01.2022 @@ -34,27 +34,181 @@ public interface MulticopterModel { public final double AIRDENSITY = 1.2045; // kg/m^3 public final double GRAVITY = 9.807; // m/s^2 - public double hoverThrustRequired(); + /** + * Query if the multicopter's motors are active and usable. + * + * @return + */ + public boolean isOn(); + /** + * Maximum provided thrust by the multicopter motors (full throttle, maximum rotation). + * + * @return + */ + public double maxThrust(); + /** + * Minimum provided thrust by the multicopter motors (idle, minimum rotation) + * + * @return + */ + public double minThrust(); - // v - public double verticalAscentMaxVelocity(); + /** + * Thrust requires to hold the multicopter on a stable level in the air. + * + * @return + */ + public double hoverThrust(); + + /** + * Thrust required for a certain horizontal velocity. Includes vertical thrust component. + * + * @param velocity + * @return + */ + public double thrustForHorizontalVelocity(double velocity); + + /** + * Thrust required for the maximum possible horizontal velocity. Includes vertical + * thrust component. + * + * @return + */ + public double thrustForHorizontalMaxVelocity(); + + /** + * Thrust required for a controlled vertical descent. + * + * @param velocity + * @return + */ + public double verticalDescentThrust(double velocity); + + /** + * Thrust required for vertical ascent. + * + * @param velocity + * @return + */ + public double verticalAscentThrust(double velocity); + + /** + * Optimal horizontal speed of the multicopter w.r.t. energy consumption. + * @return + */ + public double optimalSpeed(); + /** + * Maximum horizontal velocity. + * @return + */ public double horizontalMaxVelocity(); + + /** + * Minimum horizontal velocity. + * @return + */ public double horizontalMinVelocity(); - // acc + /** + * Maximum velocity for vertical ascent. + * @return + */ + public double verticalAscentMaxVelocity(); + + /** + * Maximum velocity for vertical descent. + * @return + */ + public double verticalDescentMaxVelocity(); + + /** + * Optimal velocity when flying a curve. + * @return + */ + public double curveVelocity(); + + /** + * Maximum acceleration for horizontal flight. + * @return + */ public double horizontalMaxAcceleration(); - // curve + /** + * Maximum deceleration in horizontal flight. + * @return + */ + public double horizontalMaxDeceleration(); + + /** + * Maximum acceleration for vertical descent. + * @return + */ + public double verticalDescentMaxAcceleration(); + + /** + * Maximum acceleration for vertical ascent. + * @return + */ + public double verticalAscentMaxAcceleration(); + + /** + * Maximum turn angle (rad per second), that the multicopter can change its facing while hovering. + * @return + */ public double getMaximumTurnAngle(); + /** + * The radius of the curve/circle the multicopter can fly with a given velocity. + * @param curveVelocity + * @return + */ + public double getCurveRadius(double curveVelocity); + + /** + * The acceleration, the multicopter is subject to when flying a curve/in a circle with a given velocity. + * @param curveVelocity + * @return + */ + public double getCurveCentripedalAcceleration(double curveVelocity); + + /** + * Pitch angle (Angle of attack) required to achieve a given velocity. + * @param velocity + * @return + */ + public double pitchAngleForHorizontalVelocity(double velocity); + + /** + * Power consumption in watt, when flying a given velocity. + * @param velocity + * @return + */ public double powerConsumptionWatt(double velocity); - public double optimalSpeed(); + /** + * Sets the actuator of the multicopter. + * @param motor + */ public void setMotor(ActuatorComponent motor); + /** + * Puts load on the multicopter's motors for a given phase time, such that the + * given thrust is provided within that time. + * + * @param thrust + * @param phaseTime + */ + public void thrust(double thrust, long phaseTime); + + /** + * Hacky factory interface for multicopter model factories. + * + * @author Julian Zobel + * @version 1.0, 20.01.2022 + */ public interface Factory { public MulticopterModel createComponent(); } diff --git a/src/de/tud/kom/p2psim/impl/topology/component/UAVTopologyComponent.java b/src/de/tud/kom/p2psim/impl/topology/component/UAVTopologyComponent.java index 3597f327..8eb4cdf2 100644 --- a/src/de/tud/kom/p2psim/impl/topology/component/UAVTopologyComponent.java +++ b/src/de/tud/kom/p2psim/impl/topology/component/UAVTopologyComponent.java @@ -128,12 +128,12 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S @Override public double getMinMovementSpeed() { - return movement.horizontalMinVelocity(); + return movement.getMinimumVelocity(); } @Override public double getMaxMovementSpeed() { - return movement.getHorizontalMaxVelocity(); + return movement.getMaximumVelocity(); } @Override @@ -339,8 +339,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S && state == UAVstate.BASE_CONNECTION_READY && activate()) { disconnectFromBase(); - - movement.setTargetVelocity(movement.getVerticalAscentMaxVelocity()); + movement.setTargetLocation(getBaseHoverLocation(), cb); return true; @@ -442,7 +441,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S @Override public double estimatePowerConsumptionWatt(double velocity) { - return movement.estimatePowerConsumptionWatt(velocity); + return movement.powerConsumptionWatt(velocity); } @Override @@ -460,7 +459,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S } public double getOptimalMovementSpeed() { - return movement.estimateOptimalSpeed(); + return movement.optimalSpeed(); } @Override diff --git a/src/de/tud/kom/p2psim/impl/topology/movement/aerial/MulticopterMovement.java b/src/de/tud/kom/p2psim/impl/topology/movement/aerial/MulticopterMovement.java index 11c3be60..38f7d7de 100644 --- a/src/de/tud/kom/p2psim/impl/topology/movement/aerial/MulticopterMovement.java +++ b/src/de/tud/kom/p2psim/impl/topology/movement/aerial/MulticopterMovement.java @@ -28,10 +28,9 @@ 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.api.uav.MulticopterModel; 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; @@ -53,40 +52,19 @@ 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 MulticopterModel uav; - 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) { + public MulticopterMovement(UAVTopologyComponent topologyComponent, MulticopterModel uav) { this.topologyComponent = topologyComponent; - this.mass = massTotal; - this.areaTop = areaTop; - this.areaFront = areaFront; - this.dragCoefficient = UAVDragCoefficient; - this.maximumPitchAngleAllowed = maximumPitchAngleAllowed; - this.maximumDecentVelocityAllowed = maximumDecentVelocityAllowed; - this.maximumTurnAngle = maximumTurnAngle; + this.uav = uav; } boolean first = true; @@ -94,7 +72,7 @@ public class MulticopterMovement implements UAVMovementModel { @Override public void move(long timeBetweenMovementOperations) { - if(motor.isOn() && !route.isEmpty()) { + if(uav.isOn() && !route.isEmpty()) { PositionVector position = new PositionVector(topologyComponent.getRealPosition()); PositionVector target = route.getFirst(); @@ -112,8 +90,8 @@ public class MulticopterMovement implements UAVMovementModel { // go to hover mode topologyComponent.updateCurrentLocation(target.clone()); - velocity = 0; - motor.requestThrust(hoverThrustRequired(), timeBetweenMovementOperations); + velocity = 0; + uav.thrust(uav.hoverThrust(), timeBetweenMovementOperations); PositionVector direction = topologyComponent.getCurrentDirection().clone(); direction.setEntry(2, 0); @@ -125,13 +103,15 @@ public class MulticopterMovement implements UAVMovementModel { else { // get to speed - if(targetVelocity > 0 && targetVelocity < getHorizontalMaxVelocity()) { - motor.requestThrust(estimateRequiredThrust(targetVelocity), timeBetweenMovementOperations); + if(targetVelocity > 0 && targetVelocity < getMaximumVelocity()) { + + + uav.thrust(uav.thrustForHorizontalVelocity(targetVelocity), timeBetweenMovementOperations); velocity = targetVelocity; } else { - motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust(), timeBetweenMovementOperations); - velocity = getHorizontalMaxVelocity(); + uav.thrust(uav.thrustForHorizontalMaxVelocity(), timeBetweenMovementOperations); + velocity = getMaximumVelocity(); } long timeUntilReachedLocation = (long) (distanceToTargetPosition / velocity) * Time.SECOND; @@ -162,13 +142,13 @@ public class MulticopterMovement implements UAVMovementModel { double timefactor = timeBetweenMovementOperations / Time.SECOND; // get to speed - if(targetVelocity > 0 && targetVelocity < getHorizontalMaxVelocity()) { - motor.requestThrust(estimateRequiredThrust(targetVelocity), timeBetweenMovementOperations); + if(targetVelocity > 0 && targetVelocity < getMaximumVelocity()) { + uav.thrust(uav.thrustForHorizontalVelocity(targetVelocity), timeBetweenMovementOperations); velocity = targetVelocity; } else { - motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust(), timeBetweenMovementOperations); - velocity = getHorizontalMaxVelocity(); + uav.thrust(uav.thrustForHorizontalMaxVelocity(), timeBetweenMovementOperations); + velocity = getMaximumVelocity(); } PositionVector directionToTarget = new PositionVector(target); @@ -189,7 +169,7 @@ public class MulticopterMovement implements UAVMovementModel { } } - else if(motor.isOn()) { + else if(uav.isOn()) { if(velocity != 0) { throw new UnsupportedOperationException("[MulticopterMovement] no route but speed not 0?"); @@ -198,10 +178,10 @@ public class MulticopterMovement implements UAVMovementModel { PositionVector position = new PositionVector(topologyComponent.getRealPosition()); if(position.getAltitude() == 0) { - motor.requestThrust(0, timeBetweenMovementOperations); + uav.thrust(0, timeBetweenMovementOperations); } else { - motor.requestThrust(hoverThrustRequired(), timeBetweenMovementOperations); + uav.thrust(uav.hoverThrust(), timeBetweenMovementOperations); } topologyComponent.updateCurrentLocation(position); @@ -212,92 +192,9 @@ public class MulticopterMovement implements UAVMovementModel { * */ - 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; + uav.setMotor(motor); } @Override @@ -366,107 +263,28 @@ public class MulticopterMovement implements UAVMovementModel { } @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))); + public double getMinimumVelocity() { + return uav.horizontalMinVelocity(); } + @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; - } + public double optimalSpeed() { + return uav.optimalSpeed(); } - - 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; - } + + @Override + public double getMaximumVelocity() { + return uav.horizontalMaxVelocity(); + } + + + @Override + public double powerConsumptionWatt(double velocity) { + return uav.powerConsumptionWatt(velocity); } - /** - * 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 * @@ -474,47 +292,15 @@ public class MulticopterMovement implements UAVMovementModel { * @version 1.0, 14.01.2020 */ public static class Factory implements AerialMovementModelFactory { + + private MulticopterModel.Factory factory; - 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 void setUAVModel(MulticopterModel.Factory uavFactory) { + this.factory = uavFactory; } public UAVMovementModel createComponent(UAVTopologyComponent topologyComponent) { - return new MulticopterMovement(topologyComponent, massTotal, areaTop, areaFront, - UAVDragCoefficient, maximumPitchAngleAllowed, maximumDecentVelocityAllowed, maximumTurnAngle); + return new MulticopterMovement(topologyComponent, factory.createComponent()); } } @@ -559,49 +345,12 @@ public class MulticopterMovement implements UAVMovementModel { } - 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); + public static EnergyModelPropertyMeasurement getPropoertyMeasurement(StateMulticopterMovement m, double velocity, double thrust, double watt) { + // FIXME + double amp = watt / 14.8; + return new EnergyModelPropertyMeasurement(velocity, thrust, amp, watt); } } - @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; - } - } diff --git a/src/de/tud/kom/p2psim/impl/topology/movement/aerial/SimpleMulticopterMovement.java b/src/de/tud/kom/p2psim/impl/topology/movement/aerial/SimpleMulticopterMovement.java index 5a9b34a3..90ba02ae 100644 --- a/src/de/tud/kom/p2psim/impl/topology/movement/aerial/SimpleMulticopterMovement.java +++ b/src/de/tud/kom/p2psim/impl/topology/movement/aerial/SimpleMulticopterMovement.java @@ -47,9 +47,9 @@ public class SimpleMulticopterMovement implements UAVMovementModel { private double targetVelocity; private double maximumHorizontalVelocity; - private double maximumVerticalVelocity; // TODO currently not used + private double maximumVerticalVelocity; private double minimumHorizontalVelocity; private double minimumVerticalVelocity; @@ -198,7 +198,7 @@ public class SimpleMulticopterMovement implements UAVMovementModel { } @Override - public double estimatePowerConsumptionWatt(double velocity) { + public double powerConsumptionWatt(double velocity) { double wattage = 0; @@ -220,21 +220,7 @@ public class SimpleMulticopterMovement implements UAVMovementModel { this.motor = (StatefulActuatorComponent) motor; } - @Override - public double getVerticalAscentMaxVelocity() { - return maximumVerticalVelocity; - } - - @Override - public double getHorizontalMaxVelocity() { - return maximumHorizontalVelocity; - } - - @Override - public double getHorizontalMinVelocity() { - return 0; - } - + /** * Factory for this movement model * @@ -279,8 +265,19 @@ public class SimpleMulticopterMovement implements UAVMovementModel { } } + + @Override + public double getMinimumVelocity() { + return 0; + } + + @Override + public double getMaximumVelocity() { + return maximumHorizontalVelocity; + } + @Override - public double estimateOptimalSpeed() { + public double optimalSpeed() { return optimalHorizontalVelocity; } } diff --git a/src/de/tud/kom/p2psim/impl/topology/movement/aerial/StateMulticopterMovement.java b/src/de/tud/kom/p2psim/impl/topology/movement/aerial/StateMulticopterMovement.java index aed57b66..5f4e699c 100644 --- a/src/de/tud/kom/p2psim/impl/topology/movement/aerial/StateMulticopterMovement.java +++ b/src/de/tud/kom/p2psim/impl/topology/movement/aerial/StateMulticopterMovement.java @@ -20,7 +20,6 @@ package de.tud.kom.p2psim.impl.topology.movement.aerial; - import java.util.LinkedHashMap; import java.util.LinkedList; import java.util.Map; @@ -28,12 +27,9 @@ 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.api.uav.MulticopterModel; 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; @@ -41,15 +37,12 @@ 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. + * Complex thrust based movement model using specific models of a multicopter UAV for calculation. * - * The movement logic uses only straight forward movement with the maximum speed available. - * - * TODO Acceleration - * TODO Movement model for plane-like UAVs + * TODO Altitude changes while moving are not possible. * * @author Julian Zobel - * @version 1.0, 11.09.2018 + * @version 1.0, 20.02.2022 */ public class StateMulticopterMovement implements UAVMovementModel { @@ -59,53 +52,45 @@ public class StateMulticopterMovement implements UAVMovementModel { private MobilityState state = MobilityState.GROUND; private IntraState intrastate = IntraState.STOP; - private UAVTopologyComponent topologyComponent; - + private final double MARGIN = Math.pow(10, -2); // defines which resolution is enough to arrive at a target + private UAVTopologyComponent topologyComponent; private MulticopterModel uav; - private double currentAngleOfAttack; - - private double targetVelocity = -1; - - private PositionVector currentTargetPosition = null; + private double currentAngleOfAttack; + private double cruiseVelocity = -1; private LinkedList route = new LinkedList<>(); - private Map locationCallbacks = new LinkedHashMap<>(); // TODO callback interface - - - - - private final double MARGIN = Math.pow(10, -2); - - - ///// + private Map locationCallbacks = new LinkedHashMap<>(); + + private PositionVector currentTargetPosition = null; private PositionVector positionVector = null; private PositionVector directionVector = new PositionVector(0,0,0); private PositionVector velocityVector = new PositionVector(0,0,0); private PositionVector accelerationVector = new PositionVector(0,0,0); - + /** + * Constructor. + * + * @param topologyComponent + * @param uav + */ public StateMulticopterMovement(UAVTopologyComponent topologyComponent, MulticopterModel uav) { this.topologyComponent = topologyComponent; this.uav = uav; } - private void update() { - System.out.println("Updaing location: " + positionVector); - topologyComponent.updateCurrentLocation(new PositionVector(positionVector)); - topologyComponent.updateCurrentDirection(new PositionVector(directionVector)); - } - @Override public void move(long timeBetweenMovementOperations) { - + + // required for initial resolving this components position and direction if(positionVector == null) { positionVector = new PositionVector(topologyComponent.getRealPosition()); directionVector = new PositionVector(topologyComponent.getCurrentDirection()); } - if(motor.isOn() && !route.isEmpty()) { + // UAV only moves when the motor is online and there is a valid target position available. + if(uav.isOn() && !route.isEmpty()) { assert positionVector.distanceTo(topologyComponent.getRealPosition()) == 0; if(currentTargetPosition == null) { @@ -113,86 +98,128 @@ public class StateMulticopterMovement implements UAVMovementModel { } else { + // perform the next step based on the current state. switch( state) { - case ASCEND: - timeBetweenMovementOperations = ascend(timeBetweenMovementOperations); - break; - case DESCEND: - // TODO - timeBetweenMovementOperations = descend(timeBetweenMovementOperations); - break; - case HOVER: - case GROUND: - if(positionVector.distanceTo(currentTargetPosition) == 0) { - System.out.println("location reached"); - this.locationReached(currentTargetPosition); - } - else { - // new target, check - if(positionVector.getAltitude() < currentTargetPosition.getAltitude()) { - this.state = MobilityState.ASCEND; - } - else if(positionVector.getAltitude() > currentTargetPosition.getAltitude()) { - this.state = MobilityState.DESCEND; - } - else { - this.state = MobilityState.TARGET; + case ASCEND: + timeBetweenMovementOperations = ascend(timeBetweenMovementOperations); + break; + case DESCEND: + timeBetweenMovementOperations = descend(timeBetweenMovementOperations); + break; + case HOVER: + case GROUND: + if(positionVector.distanceTo(currentTargetPosition) == 0) { + this.locationReached(currentTargetPosition); } - - } - break; - case TARGET: - timeBetweenMovementOperations = approachTarget(timeBetweenMovementOperations); - break; - - default: throw new UnsupportedOperationException("Wrong State"); + else { + // new target, check if altitude must be adapted, otherwise approach + if(positionVector.getAltitude() < currentTargetPosition.getAltitude()) { + this.state = MobilityState.ASCEND; + } + else if(positionVector.getAltitude() > currentTargetPosition.getAltitude()) { + this.state = MobilityState.DESCEND; + } + else { + this.state = MobilityState.TARGET; + } + } + break; + case TARGET: + timeBetweenMovementOperations = approachTarget(timeBetweenMovementOperations); + break; + + default: throw new UnsupportedOperationException("Wrong State"); } - update(); + update(); // location & direction } - + + // if the performed movement calculations did not use all available time, + // we need to perform more calculations... if(timeBetweenMovementOperations != 0) { move(timeBetweenMovementOperations); return; - } - - + } } - else if(motor.isOn()) { + // when there is no target position but the motor is online, the UAV hovers (while in the air) or stays on the ground. + else if(uav.isOn()) { if(velocityVector.getLength() != 0) { throw new UnsupportedOperationException("[StateMulticopterMovement] no route but speed not 0?"); - } - - PositionVector position = new PositionVector(topologyComponent.getRealPosition()); + } - if(position.getAltitude() == 0) { + if(positionVector.getAltitude() == 0) { ground(timeBetweenMovementOperations); } else { hover(timeBetweenMovementOperations); } - topologyComponent.updateCurrentLocation(position); + update(); } } + + + /////////////////// + // TARGET FLIGHT + /////////////////// + /** - * Calculate the maximum length of the acceleration phase for limited distances to prevent overshooting the target. + * Approach the current target position. * - * @param acceleration - * @param deceleration - * @param distanceToTarget - * @param v0 + * @param time * @return */ - private double limitAccelerationTimeForShortDistances(double acceleration, double deceleration, double distanceToTarget, double v0) { - double A = 0.5 * acceleration + (acceleration * acceleration)/(2 * deceleration); - double B = v0 + (v0 * acceleration) / deceleration; - double C = (v0 * v0) / deceleration - distanceToTarget; + private long approachTarget(long time) { - return (-B + Math.sqrt(B * B - 4 * A * C))/(2*A); + switch(intrastate) { + case STOP: + case ADJUST: + // not directly in front of the UAV... turn or curve + if(horizontalAngleToPosition(currentTargetPosition) != 0) { + if(velocityVector.getLength() == 0) { + intrastate = IntraState.TURN; + } + else { + intrastate = IntraState.CURVE; + } + } + else { + intrastate = IntraState.ACCELERATION; + } + break; + case ACCELERATION: + time = horizontalAcceleration(time); + break; + case CONSTANT: + time = horizontalFlight(time); + break; + case DECELERATION: + time = horizontalDeceleration(time); + break; + case CURVE: + time = circularMovement(time); + break; + case TURN: + time = hoverTurn(time); + break; + default: throw new UnsupportedOperationException("Wrong State"); + } + + if(time > 0 && intrastate != IntraState.STOP) { + time = approachTarget(time); + } + + return time; } + + /** + * Accelerate the UAV, straight level flight. + * + * @param time + * @return + */ private long horizontalAcceleration(long time) { // acceleration assert velocityVector.getZ() == 0; @@ -202,8 +229,7 @@ public class StateMulticopterMovement implements UAVMovementModel { // acceleration is thrust from motors minus drag from speed // rough approximation for velocity induced drag when accelerating (half of maximum) - double a_acc = getHorizontalMaxAcceleration() - (forwardDrag(getHorizontalMaxVelocity() / 2, maximumPitchAngleAllowed) / mass); - + double a_acc = uav.horizontalMaxAcceleration(); double v0 = velocityVector.getLength(); double vMax = flightVelocity(); @@ -212,7 +238,7 @@ public class StateMulticopterMovement implements UAVMovementModel { if(route.size() == 1) { // deceleration is thrust from motors plus drag from speed - double a_dec = getHorizontalMaxAcceleration() + (forwardDrag(getHorizontalMaxVelocity() / 2, maximumPitchAngleAllowed) / mass); + double a_dec = uav.horizontalMaxDeceleration(); double decTime = vMax / a_dec; double decDist = -0.5 * a_dec * decTime * decTime + vMax * decTime; @@ -243,16 +269,22 @@ public class StateMulticopterMovement implements UAVMovementModel { directionVector.normalize(); // should be valid anyways? positionVector = positionVector.plus(directionVector.scale(accDist)); velocityVector = directionVector.scale(vint); - currentAngleOfAttack = estimatePitchAngleForVelocity(vint); + currentAngleOfAttack = uav.pitchAngleForHorizontalVelocity(vint); accelerationVector = directionVector.scale(a_acc); // accelerate towards target - motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust(), accelerationPhaseTime); + uav.thrust(uav.thrustForHorizontalMaxVelocity(), accelerationPhaseTime); // remaining time return time - accelerationPhaseTime; } + /** + * Horizontal straight level flight. + * + * @param time + * @return + */ private long horizontalFlight(long time) { assert velocityVector.getZ() == 0; @@ -277,7 +309,7 @@ public class StateMulticopterMovement implements UAVMovementModel { // if last given point, need to break! if(decelerationRequired) { // rough approximation for velocity induced drag when accelerating (half of maximum) - double a_dec = getHorizontalMaxAcceleration() + accelerationDrag(); + double a_dec = uav.horizontalMaxDeceleration(); // calculate brake time double decTime = v / a_dec; @@ -293,6 +325,7 @@ public class StateMulticopterMovement implements UAVMovementModel { constantPhaseTime = time; } else { + // reached end of phase if(!decelerationRequired) { intrastate = IntraState.ADJUST; } @@ -307,7 +340,7 @@ public class StateMulticopterMovement implements UAVMovementModel { directionVector.normalize(); // should be valid anyways? positionVector = positionVector.plus(directionVector.scale(constDist)); velocityVector = directionVector.scale(v); - currentAngleOfAttack = estimatePitchAngleForVelocity(v); + currentAngleOfAttack = uav.pitchAngleForHorizontalVelocity(v); accelerationVector = directionVector.scale(0); if(intrastate == IntraState.ADJUST) { @@ -318,21 +351,18 @@ public class StateMulticopterMovement implements UAVMovementModel { } // thrust includes energy calculation; - motor.requestThrust(estimateRequiredThrust(v), constantPhaseTime); + uav.thrust(uav.thrustForHorizontalVelocity(v), constantPhaseTime); // remaining time return time - constantPhaseTime; } /** - * The "negative" acceleration as part of a forward acceleration induced by drag. + * Reduce flight speed, straight level flight. * + * @param time * @return */ - private double accelerationDrag() { - return forwardDrag(getHorizontalMaxVelocity() / 2, maximumPitchAngleAllowed) / mass; - } - private long horizontalDeceleration(long time) { assert velocityVector.getZ() == 0; assert horizontalAngleToPosition(currentTargetPosition) == 0; @@ -340,7 +370,7 @@ public class StateMulticopterMovement implements UAVMovementModel { double v = velocityVector.getLength(); // rough approximation for velocity induced drag when accelerating (half of maximum) - double a_dec = getHorizontalMaxAcceleration() + accelerationDrag(); + double a_dec = uav.horizontalMaxDeceleration(); // calculate brake time double decTime = v / a_dec; @@ -366,9 +396,10 @@ public class StateMulticopterMovement implements UAVMovementModel { accelerationVector = directionVector.scale(-a_dec); // thrust includes energy calculation; accelerate against flight direction - motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust(), decelerationtPhaseTime); + uav.thrust(uav.thrustForHorizontalMaxVelocity(), decelerationtPhaseTime); if(Math.abs(positionVector.distanceTo(currentTargetPosition)) < MARGIN) { + // reached end of phase intrastate = IntraState.STOP; positionVector = currentTargetPosition.clone(); velocityVector = directionVector.scale(0); @@ -381,52 +412,6 @@ public class StateMulticopterMovement implements UAVMovementModel { return time - decelerationtPhaseTime; } - /////////////////// - /////////////////// - /////////////////// - private long approachTarget(long time) { - - switch(intrastate) { - case STOP: - case ADJUST: - // not directly in front of the UAV... turn or curve - if(horizontalAngleToPosition(currentTargetPosition) != 0) { - if(velocityVector.getLength() == 0) { - intrastate = IntraState.TURN; - } - else { - intrastate = IntraState.CURVE; - } - } - else { - intrastate = IntraState.ACCELERATION; - } - break; - case ACCELERATION: - time = horizontalAcceleration(time); - break; - case CONSTANT: - time = horizontalFlight(time); - break; - case DECELERATION: - time = horizontalDeceleration(time); - break; - case CURVE: - time = circularMovement(time); - break; - case TURN: - time = hoverTurn(time); - break; - default: throw new UnsupportedOperationException("Wrong State"); - } - - if(time > 0 && intrastate != IntraState.STOP) { - time = approachTarget(time); - } - - return time; - } - /** * Turn the front towards the target position while hovering. * @@ -442,6 +427,7 @@ public class StateMulticopterMovement implements UAVMovementModel { turnPhaseTime = time; } else { + // reached end of phase intrastate = IntraState.STOP; } @@ -458,18 +444,25 @@ public class StateMulticopterMovement implements UAVMovementModel { directionVector.normalize(); // thrust includes energy calculation; - motor.requestThrust(hoverThrustRequired(), turnPhaseTime); + uav.thrust(uav.hoverThrust(), turnPhaseTime); return time - turnPhaseTime; } + /** + * Fly a leveled turn towards the current target, such that the UAV faces directly towards the target at the end of the circular movement. + * + * TODO Dubin's curves? + * + * @param time + * @return + */ private long circularMovement(long time) { - double curveVelocity = Math.min(estimateOptimalSpeed(), flightVelocity()); + double curveVelocity = Math.min(flightVelocity(), uav.curveVelocity()); - // estimation for the turn radius based on the available, free thrust potential of the motors - double curveThrust = horizontalComponentMaxThrust() - estimateRequiredThrust(curveVelocity); - double turnRadius = ( mass * Math.pow(curveVelocity, 2)) / curveThrust; + // estimation for the turn radius based on the available, free thrust potential of the motors + double turnRadius = uav.getCurveRadius(curveVelocity); // calculate turn circle double angleToTarget = horizontalAngleToPosition(currentTargetPosition); @@ -525,7 +518,7 @@ public class StateMulticopterMovement implements UAVMovementModel { double circleLocationY = circleCenter.getY() + turnRadius * Math.sin(positionAngle + circleAngle); positionVector = new PositionVector(circleLocationX, circleLocationY, positionVector.getZ()); - currentAngleOfAttack = estimatePitchAngleForVelocity(curveVelocity); + currentAngleOfAttack = uav.pitchAngleForHorizontalVelocity(curveVelocity); // direction vector is the tangent at the new position // x' = x cos θ − y sin θ @@ -537,9 +530,10 @@ public class StateMulticopterMovement implements UAVMovementModel { directionVector.normalize(); velocityVector = directionVector.scale(curveVelocity); - accelerationVector = directionToCircleCenter.scale(curveThrust / mass); + accelerationVector = directionToCircleCenter.scale(uav.getCurveCentripedalAcceleration(curveVelocity)); - if(Math.abs(positionVector.distanceTo(tangentPoint)) < MARGIN) { + if(Math.abs(positionVector.distanceTo(tangentPoint)) < MARGIN) { + // reached end of phase positionVector = tangentPoint.clone(); directionVector = currentTargetPosition.minus(positionVector); directionVector.normalize(); @@ -547,109 +541,12 @@ public class StateMulticopterMovement implements UAVMovementModel { } // thrust includes energy calculation; accelerate against flight direction - motor.requestThrust(horizontalComponentMaxThrust(), constantPhaseTime); + uav.thrust(uav.thrustForHorizontalMaxVelocity(), constantPhaseTime); // remaining time return time - constantPhaseTime; } - private PositionVector calculateCircleTangentExitPoint(PositionVector circleCenter, double turnRadius, PositionVector target, double angleToTarget) { - // Calculate the tangent points from the target Position on the turn circle: Defines exit points for circle movement. - // https://stackoverflow.com/a/49987361 - double b = Math.sqrt(Math.pow(target.getX() - circleCenter.getX(), 2) + Math.pow(target.getY() - circleCenter.getY(), 2)); - // angle theta - double th = Math.acos(turnRadius / b); - // direction angle of point P from C - double d = Math.atan2(target.getY() - circleCenter.getY(), target.getX() - circleCenter.getX()); - double d1 = d + th; // direction angle of point T1 from C - double d2 = d - th; // direction angle of point T2 from C - - double T1x = circleCenter.getX() + turnRadius * Math.cos(d1); - double T1y = circleCenter.getY() + turnRadius * Math.sin(d1); - double T2x = circleCenter.getX() + turnRadius * Math.cos(d2); - double T2y = circleCenter.getY() + turnRadius * Math.sin(d2); - - PositionVector T1 = new PositionVector(T1x, T1y, positionVector.getZ()); - PositionVector T2 = new PositionVector(T2x, T2y, positionVector.getZ()); - - // now define which point to use for exiting - double aT1 = horizontalAngleToPosition(T1); - double aT2 = horizontalAngleToPosition(T2); - - PositionVector tangentPoint = null; - - // left -> neg angles - if(angleToTarget < 0) { - assert aT1 < 0 || aT2 < 0; - - if(aT1 < 0 && aT2 > 0) { - tangentPoint = T1; - } - else if(aT1 > 0 && aT2 < 0) { - tangentPoint = T2; - } - else if(aT1 < 0 && aT2 < 0) { - tangentPoint = Math.abs(aT1) < Math.abs(aT2) ? T1 : T2; - } - } - // right - else if(angleToTarget > 0) { - assert aT1 > 0 || aT2 > 0; - - if(aT1 < 0 && aT2 > 0) { - tangentPoint = T2; - } - else if(aT1 > 0 && aT2 < 0) { - tangentPoint = T1; - } - else if(aT1 > 0 && aT2 > 0) { - tangentPoint = Math.abs(aT1) < Math.abs(aT2) ? T1 : T2; - } - } - - return tangentPoint; - } - - /** - * Depending on the current state and available route waypoints, a deceleration may not be required at the currently approached target waypoint. - * @return - */ - private boolean decelerationAtNextTargetRequired() { - - if(route == null || route.size() == 0) { - throw new UnsupportedOperationException("Cannot be possible"); - } - - if(route.size() == 1) { - return true; - } - - switch(state) { - case HOVER: - case GROUND: - // not really interesting at this point! - return true; - // - case ASCEND: - if(route.get(0).getAltitude() < route.get(1).getAltitude()) { - return false; - } - break; - // - case DESCEND: - if(route.get(0).getAltitude() > route.get(1).getAltitude()) { - return false; - } - break; - case TARGET: - if(route.get(0).getAltitude() == route.get(1).getAltitude()) { - return false; - } - break; - } - - return true; - } /** * Static hovering @@ -658,7 +555,7 @@ public class StateMulticopterMovement implements UAVMovementModel { */ private void hover(long time) { this.state = MobilityState.HOVER; - motor.requestThrust(hoverThrustRequired(), time); + uav.thrust(uav.hoverThrust(), time); } /** @@ -668,16 +565,105 @@ public class StateMulticopterMovement implements UAVMovementModel { */ private void ground(long time) { this.state = MobilityState.GROUND; - motor.requestThrust(0, time); + uav.thrust(uav.minThrust(), time); } - private long verticalAcceleration(long time, double distance, double acceleration, double deceleration, double verticalMaximumVelocity, boolean upwards) { - double v0 = Math.abs(velocityVector.getZ()); - double accTime = (verticalMaximumVelocity - v0) / acceleration; - double accDist = 0.5 * acceleration * accTime * accTime + v0 * accTime; + ///////////// + // UP & DOWN (VERTICAL) + //////////// - double decTime = verticalMaximumVelocity / deceleration; + /** + * Vertical movement, upwards in a straight line. + * + * @param time + * @return + */ + private long ascend(long time) { + assert time > 0; + assert positionVector.getAltitude() < currentTargetPosition.getAltitude(); + assert this.state == MobilityState.ASCEND; + + double verticalDistance = currentTargetPosition.getAltitude() - positionVector.getAltitude(); + + switch(intrastate) { + case STOP: + intrastate = IntraState.ACCELERATION; + break; + case ACCELERATION: + time = verticalAcceleration(time, verticalDistance, uav.verticalAscentMaxAcceleration(), uav.verticalDescentMaxAcceleration(), uav.verticalAscentMaxVelocity(), true); + break; + case CONSTANT: + time = verticalFlight(time, verticalDistance, uav.verticalDescentMaxAcceleration(), true); + break; + case DECELERATION: + time = verticalDeceleration(time, verticalDistance, uav.verticalDescentMaxAcceleration(), true); + break; + default: throw new UnsupportedOperationException("wrong state"); + } + + if(time > 0 && intrastate != IntraState.STOP) { + time = ascend(time); + } + + return time; + } + + /** + * Vertical movement, downwards in a straight line. + * + * @param time + * @return + */ + private long descend(long time) { + assert time > 0; + assert positionVector.getAltitude() > currentTargetPosition.getAltitude(); + assert this.state == MobilityState.DESCEND; + + double verticalDistance = positionVector.getAltitude() - currentTargetPosition.getAltitude(); + + switch(intrastate) { + case STOP: + intrastate = IntraState.ACCELERATION; + break; + case ACCELERATION: + time = verticalAcceleration(time, verticalDistance, uav.verticalDescentMaxAcceleration(), uav.verticalAscentMaxAcceleration(), uav.verticalDescentMaxVelocity(), false); + break; + case CONSTANT: + time = verticalFlight(time, verticalDistance, uav.verticalAscentMaxAcceleration(), false); + break; + case DECELERATION: + time = verticalDeceleration(time, verticalDistance, uav.verticalAscentMaxAcceleration(), false); + break; + default: throw new UnsupportedOperationException("wrong state"); + } + + if(time > 0 && intrastate != IntraState.STOP) { + time = descend(time); + } + + return time; + } + + + /** + * Accelerate vertically, either downwards by lowering the thrust or upwards by increasing thrust. + * + * @param time + * @param distance + * @param acceleration + * @param deceleration + * @param verticalMaximumVelocity + * @param upwards + * @return + */ + private long verticalAcceleration(long time, double distance, double acceleration, double deceleration, double verticalMaximumVelocity, boolean upwards) { + double v0 = Math.abs(velocityVector.getZ()); + + double accTime = (verticalMaximumVelocity - v0) / acceleration; + double accDist = 0.5 * acceleration * accTime * accTime + v0 * accTime; + + double decTime = verticalMaximumVelocity / deceleration; double decDist = -0.5 * deceleration * decTime * decTime + verticalMaximumVelocity * decTime; // when the distance is short, we need to limit acceleration time / maximum velocity @@ -698,6 +684,7 @@ public class StateMulticopterMovement implements UAVMovementModel { accelerationPhaseTime = time; } else { + // reached end of phase intrastate = IntraState.CONSTANT; } @@ -711,7 +698,7 @@ public class StateMulticopterMovement implements UAVMovementModel { accelerationVector.setZ(acceleration); // thrust includes energy calculation; - motor.requestThrust(motor.getMaxThrust(), accelerationPhaseTime); + uav.thrust(uav.maxThrust(), accelerationPhaseTime); } else { positionVector.setZ(positionVector.getAltitude() - accDist); @@ -719,13 +706,23 @@ public class StateMulticopterMovement implements UAVMovementModel { accelerationVector.setZ(-acceleration); // thrust includes energy calculation; - motor.requestThrust(motor.getMinThrust(), accelerationPhaseTime); + uav.thrust(uav.minThrust(), accelerationPhaseTime); } // remaining step time return time - accelerationPhaseTime; } + /** + * Decelerate the current vertical movement, either by more thrust when moving downwards, + * or by reducing thrust when moving upwards. + * + * @param time + * @param distance + * @param deceleration + * @param upwards + * @return + */ private long verticalDeceleration(long time, double distance, double deceleration, boolean upwards) { double v0 = Math.abs(velocityVector.getZ()); @@ -739,6 +736,7 @@ public class StateMulticopterMovement implements UAVMovementModel { decelerationtPhaseTime = time; } else { + // reached end of phase intrastate = IntraState.STOP; } @@ -750,13 +748,13 @@ public class StateMulticopterMovement implements UAVMovementModel { positionVector.setZ(positionVector.getAltitude() + decDist); velocityVector.setZ(vint); accelerationVector.setZ(-deceleration); - motor.requestThrust(motor.getMinThrust(), decelerationtPhaseTime); + uav.thrust(uav.minThrust(), decelerationtPhaseTime); } else { positionVector.setZ(positionVector.getAltitude() - decDist); velocityVector.setZ(-vint); accelerationVector.setZ(deceleration); - motor.requestThrust(motor.getMaxThrust(), decelerationtPhaseTime); + uav.thrust(uav.maxThrust(), decelerationtPhaseTime); } // reached the position (within margin) @@ -772,6 +770,15 @@ public class StateMulticopterMovement implements UAVMovementModel { return time - decelerationtPhaseTime; } + /** + * Constant speed on vertical flight. + * + * @param time + * @param distance + * @param deceleration + * @param upwards + * @return + */ private long verticalFlight(long time, double distance, double deceleration, boolean upwards) { double v0 = Math.abs(velocityVector.getZ()); @@ -788,6 +795,7 @@ public class StateMulticopterMovement implements UAVMovementModel { constantPhaseTime = time; } else { + // reached end of phase intrastate = IntraState.DECELERATION; } @@ -797,81 +805,26 @@ public class StateMulticopterMovement implements UAVMovementModel { if(upwards) { positionVector.setZ(positionVector.getAltitude() + constDist); accelerationVector.setZ(0); - motor.requestThrust(motor.getMaxThrust(), constantPhaseTime); + uav.thrust(uav.verticalAscentThrust(v0), constantPhaseTime); } else { positionVector.setZ(positionVector.getAltitude() - constDist); accelerationVector.setZ(0); - motor.requestThrust(verticalDescentThrust(), constantPhaseTime); + uav.thrust(uav.verticalDescentThrust(v0), constantPhaseTime); } return time - constantPhaseTime; } - private long descend(long time) { - assert time > 0; - assert positionVector.getAltitude() > currentTargetPosition.getAltitude(); - assert this.state == MobilityState.DESCEND; - - double verticalDistance = positionVector.getAltitude() - currentTargetPosition.getAltitude(); - - switch(intrastate) { - case STOP: - intrastate = IntraState.ACCELERATION; - break; - case ACCELERATION: - time = verticalAcceleration(time, verticalDistance, GRAVITY, verticalAscentMaxAcceleration(), maximumVerticalVelocityAllowed, false); - break; - case CONSTANT: - time = verticalFlight(time, verticalDistance, verticalAscentMaxAcceleration(), false); - break; - case DECELERATION: - time = verticalDeceleration(time, verticalDistance, verticalAscentMaxAcceleration(), false); - break; - default: throw new UnsupportedOperationException("wrong state"); - } - - if(time > 0 && intrastate != IntraState.STOP) { - time = descend(time); - } - - return time; - } - private long ascend(long time) { - assert time > 0; - assert positionVector.getAltitude() < currentTargetPosition.getAltitude(); - assert this.state == MobilityState.ASCEND; - - double verticalDistance = currentTargetPosition.getAltitude() - positionVector.getAltitude(); - - switch(intrastate) { - case STOP: - intrastate = IntraState.ACCELERATION; - break; - case ACCELERATION: - time = verticalAcceleration(time, verticalDistance, verticalAscentMaxAcceleration(), GRAVITY, getVerticalAscentMaxVelocity(), true); - break; - case CONSTANT: - time = verticalFlight(time, verticalDistance, GRAVITY, true); - break; - case DECELERATION: - time = verticalDeceleration(time, verticalDistance, GRAVITY, true); - break; - default: throw new UnsupportedOperationException("wrong state"); - } - - if(time > 0 && intrastate != IntraState.STOP) { - time = ascend(time); - } - - return time; - } + /////////// /////////// /** + * Calculate the horizontal angle to a given target position from the current UAVs position. + * Angle is given w.r.t. the current UAV direction. * * @return >0 -> right; <0 -> left */ @@ -891,25 +844,142 @@ public class StateMulticopterMovement implements UAVMovementModel { } } - ////////// - ////////// - + /** + * Calculate the maximum length of the acceleration phase for limited distances to prevent overshooting the target. + * + * @param acceleration + * @param deceleration + * @param distanceToTarget + * @param v0 + * @return + */ + private double limitAccelerationTimeForShortDistances(double acceleration, double deceleration, double distanceToTarget, double v0) { + double A = 0.5 * acceleration + (acceleration * acceleration)/(2 * deceleration); + double B = v0 + (v0 * acceleration) / deceleration; + double C = (v0 * v0) / deceleration - distanceToTarget; + + return (-B + Math.sqrt(B * B - 4 * A * C))/(2*A); + } - @Override - public void setMotorControl(ActuatorComponent motor) { - uav.setMotor(motor); + /** + * Depending on the current state and available route waypoints, a deceleration may not be required at the currently approached target waypoint. + * @return + */ + private boolean decelerationAtNextTargetRequired() { + + if(route == null || route.size() == 0) { + throw new UnsupportedOperationException("Cannot be possible"); + } + + if(route.size() == 1) { + return true; + } + + switch(state) { + case HOVER: + case GROUND: + // not really interesting at this point! + return true; + // + case ASCEND: + if(route.get(0).getAltitude() < route.get(1).getAltitude()) { + return false; + } + break; + // + case DESCEND: + if(route.get(0).getAltitude() > route.get(1).getAltitude()) { + return false; + } + break; + case TARGET: + if(route.get(0).getAltitude() == route.get(1).getAltitude()) { + return false; + } + break; + } + + return true; } - @Override - public void setTargetVelocity(double v_pref) { - this.targetVelocity = v_pref; + /** + * Based on a given circle, a given target, and the angle of the UAV on the circle to the target, + * calculate the tangent point on the circle that the UAV must enter. + * + * @param circleCenter + * @param turnRadius + * @param target + * @param angleToTarget + * @return + */ + private PositionVector calculateCircleTangentExitPoint(PositionVector circleCenter, double radius, PositionVector target, double angleToTarget) { + // Calculate the tangent points from the target Position on the turn circle: Defines exit points for circle movement. + // https://stackoverflow.com/a/49987361 + double b = Math.sqrt(Math.pow(target.getX() - circleCenter.getX(), 2) + Math.pow(target.getY() - circleCenter.getY(), 2)); + // angle theta + double th = Math.acos(radius / b); + // direction angle of point P from C + double d = Math.atan2(target.getY() - circleCenter.getY(), target.getX() - circleCenter.getX()); + double d1 = d + th; // direction angle of point T1 from C + double d2 = d - th; // direction angle of point T2 from C + + double T1x = circleCenter.getX() + radius * Math.cos(d1); + double T1y = circleCenter.getY() + radius * Math.sin(d1); + double T2x = circleCenter.getX() + radius * Math.cos(d2); + double T2y = circleCenter.getY() + radius * Math.sin(d2); + + PositionVector T1 = new PositionVector(T1x, T1y, positionVector.getZ()); + PositionVector T2 = new PositionVector(T2x, T2y, positionVector.getZ()); + + // now define which point to use for exiting + double aT1 = horizontalAngleToPosition(T1); + double aT2 = horizontalAngleToPosition(T2); + + PositionVector tangentPoint = null; + + // left -> neg angles + if(angleToTarget < 0) { + assert aT1 < 0 || aT2 < 0; + + if(aT1 < 0 && aT2 > 0) { + tangentPoint = T1; + } + else if(aT1 > 0 && aT2 < 0) { + tangentPoint = T2; + } + else if(aT1 < 0 && aT2 < 0) { + tangentPoint = Math.abs(aT1) < Math.abs(aT2) ? T1 : T2; + } + } + // right + else if(angleToTarget > 0) { + assert aT1 > 0 || aT2 > 0; + + if(aT1 < 0 && aT2 > 0) { + tangentPoint = T2; + } + else if(aT1 > 0 && aT2 < 0) { + tangentPoint = T1; + } + else if(aT1 > 0 && aT2 > 0) { + tangentPoint = Math.abs(aT1) < Math.abs(aT2) ? T1 : T2; + } + } + + return tangentPoint; } - @Override - public double getCurrentVelocity() { - return velocityVector.getLength() ; + + ////////// + ////////// + + + // Put the current position and direction to the topology component + private void update() { + topologyComponent.updateCurrentLocation(new PositionVector(positionVector)); + topologyComponent.updateCurrentDirection(new PositionVector(directionVector)); } - + /** * Trigger the callback function, if there is a valid callback * @@ -934,14 +1004,55 @@ public class StateMulticopterMovement implements UAVMovementModel { } } + @Override + public void removeTargetLocations() { + route.clear(); + locationCallbacks.clear(); + } + + + /** + * Flight velocity for the multicopter. Either a target velocity is set, which must + * be within the margins of the available velocities of the UAV, or the optimal velocity is returned. + * + * @return + */ + private double flightVelocity() { + if(cruiseVelocity > 0 && cruiseVelocity < uav.horizontalMaxVelocity()) { + return cruiseVelocity; + } + else { + cruiseVelocity = -1; + return uav.optimalSpeed(); + } + } + + ////////// + // GETTERS AND SETTERS + ////////// + + @Override + public void setMotorControl(ActuatorComponent motor) { + uav.setMotor(motor); + } + + @Override + public void setTargetVelocity(double v_pref) { + this.cruiseVelocity = v_pref; + } + + @Override + public double getCurrentVelocity() { + return velocityVector.getLength() ; + } + @Override public void setTargetLocation(PositionVector target, ReachedLocationCallback reachedLocationCallback) { route.clear(); route.add(target); if(reachedLocationCallback != null) - locationCallbacks.put(target, reachedLocationCallback); - + locationCallbacks.put(target, reachedLocationCallback); } @Override @@ -972,25 +1083,31 @@ public class StateMulticopterMovement implements UAVMovementModel { return copy; } + public double getAngleOfAttack() { + return currentAngleOfAttack; + } + @Override - public void removeTargetLocations() { - route.clear(); - locationCallbacks.clear(); - } + public double getMinimumVelocity() { + return 0; + } - - - - - - private double flightVelocity() { - if(targetVelocity > 0 && targetVelocity < getHorizontalMaxVelocity()) { - return targetVelocity; - } - else { - targetVelocity = -1; - return estimateOptimalSpeed(); - } + + @Override + public double getMaximumVelocity() { + return uav.horizontalMaxVelocity(); + } + + + @Override + public double powerConsumptionWatt(double velocity) { + return uav.powerConsumptionWatt(velocity); + } + + + @Override + public double optimalSpeed() { + return uav.optimalSpeed(); } /** @@ -1001,14 +1118,14 @@ public class StateMulticopterMovement implements UAVMovementModel { */ public static class Factory implements AerialMovementModelFactory { - private MulticopterModel uav; + private MulticopterModel.Factory factory; - public void setUAVModel(MulticopterModel uav) { - this.uav = uav; + public void setUAVModel(MulticopterModel.Factory uavFactory) { + this.factory = uavFactory; } public UAVMovementModel createComponent(UAVTopologyComponent topologyComponent) { - return new StateMulticopterMovement(topologyComponent, uav); + return new StateMulticopterMovement(topologyComponent, factory.createComponent()); } } @@ -1053,34 +1170,11 @@ public class StateMulticopterMovement implements UAVMovementModel { } - public static EnergyModelPropertyMeasurement getPropoertyMeasurement(StateMulticopterMovement 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); + public static EnergyModelPropertyMeasurement getPropoertyMeasurement(StateMulticopterMovement m, double velocity, double thrust, double watt) { + // FIXME + double amp = watt / 14.8; + return new EnergyModelPropertyMeasurement(velocity, thrust, amp, watt); } } - - @Override - public double getMinimumVelocity() { - // TODO Auto-generated method stub - return 0; - } - - - @Override - public double getMaxiumumVelocity() { - // TODO Auto-generated method stub - return 0; - } - - } diff --git a/src/de/tud/kom/p2psim/impl/uav/IntelAeroRTF.java b/src/de/tud/kom/p2psim/impl/uav/IntelAeroRTF.java index 9854ecf1..82692950 100644 --- a/src/de/tud/kom/p2psim/impl/uav/IntelAeroRTF.java +++ b/src/de/tud/kom/p2psim/impl/uav/IntelAeroRTF.java @@ -27,6 +27,12 @@ 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; +/** + * A model for the Intel Aero Ready-to-Fly multicopter UAV. + * + * @author Julian Zobel + * @version 20.01.2022 + */ public class IntelAeroRTF implements MulticopterModel { private double mass; // kg @@ -36,6 +42,7 @@ public class IntelAeroRTF implements MulticopterModel { private double maximumPitchAngleAllowed; // ° max angle private double maximumVerticalVelocityAllowed; // m/s + private double optimalCurveVelocity; private double optimalVelocity = -1; private double maximumTurnAngle; // 90° per second turn angle @@ -44,14 +51,15 @@ public class IntelAeroRTF implements MulticopterModel { public IntelAeroRTF(double massTotal, double areaTop, double areaFront, double UAVDragCoefficient, double maximumPitchAngleAllowed, - double maximumDescentVelocityAllowed, double maximumTurnAngle) { + double maximumDescentVelocityAllowed, double maximumTurnAngle, double optimalCurveVelocity) { this.mass = massTotal; this.areaTop = areaTop; this.areaFront = areaFront; this.dragCoefficient = UAVDragCoefficient; this.maximumPitchAngleAllowed = maximumPitchAngleAllowed; this.maximumVerticalVelocityAllowed = maximumDescentVelocityAllowed; - this.maximumTurnAngle = maximumTurnAngle; + this.maximumTurnAngle = maximumTurnAngle; + this.optimalCurveVelocity = optimalCurveVelocity; } /** @@ -61,6 +69,7 @@ public class IntelAeroRTF implements MulticopterModel { * @param velocity * @return */ + @Override public double pitchAngleForHorizontalVelocity(double velocity) { int low = 0; @@ -70,7 +79,7 @@ public class IntelAeroRTF implements MulticopterModel { 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)); + double v2 = 2 * hoverThrust() * Math.tan(Math.toRadians(i)) / bodyDrag(Math.toRadians(i), new PositionVector(1,0,0)); if(v2 > vsquared && i < high) { high = i; @@ -97,7 +106,7 @@ public class IntelAeroRTF implements MulticopterModel { double d = lo + i * step; - double v2 = 2 * hoverThrustRequired() * Math.tan(Math.toRadians(d)) / bodyDrag(Math.toRadians(d), new PositionVector(1,0,0)); + double v2 = 2 * hoverThrust() * Math.tan(Math.toRadians(d)) / bodyDrag(Math.toRadians(d), new PositionVector(1,0,0)); double diff = Math.abs(((velocity * velocity) - v2)); @@ -113,9 +122,10 @@ public class IntelAeroRTF implements MulticopterModel { return maximumPitchAngleAllowed; } + @Override public double thrustForHorizontalVelocity(double velocity) { if(velocity == 0) { - return motor.estimatePowerConsumptionWatt(hoverThrustRequired()); + return hoverThrust(); } else if(velocity > horizontalMaxVelocity()) { return -1; @@ -126,21 +136,21 @@ public class IntelAeroRTF implements MulticopterModel { else { double estimateAngle = pitchAngleForHorizontalVelocity(velocity); double estimatedDrag = forwardDrag(velocity, estimateAngle); - double requiredThrust = Math.sqrt(hoverThrustRequired() * hoverThrustRequired() + estimatedDrag * estimatedDrag); + double requiredThrust = Math.sqrt(hoverThrust() * hoverThrust() + estimatedDrag * estimatedDrag); return requiredThrust; } } - + @Override public double horizontalMinVelocity() { - return Math.sqrt(2 * hoverThrustRequired() * Math.tan(Math.toRadians(0.25)) / bodyDrag(Math.toRadians(0.25), new PositionVector(1,0,0))); + return Math.sqrt(2 * hoverThrust() * Math.tan(Math.toRadians(0.25)) / bodyDrag(Math.toRadians(0.25), new PositionVector(1,0,0))); } - + @Override public double powerConsumptionWatt(double velocity) { if(velocity == 0) { - return motor.estimatePowerConsumptionWatt(hoverThrustRequired()); + return motor.estimatePowerConsumptionWatt(hoverThrust()); } else if(velocity > horizontalMaxVelocity()) { return -1; @@ -159,32 +169,51 @@ public class IntelAeroRTF implements MulticopterModel { * */ // Thrust required to hold the maximum allowed descent velocity - public double verticalDescentThrust() { - return hoverThrustRequired() - verticalDescentDrag(); + @Override + public double verticalDescentThrust(double velocity) { + return hoverThrust() - verticalDrag(velocity); } // Drag (upward lift) induced on the frame by moving with the maximum allowed descent velocity // 0.5 * p * C * A * v^2 - public double verticalDescentDrag() { - return 0.5 * bodyDrag(0, new PositionVector(0,0,1)) * maximumVerticalVelocityAllowed * maximumVerticalVelocityAllowed; + private double verticalDrag(double velocity) { + assert velocity <= maximumVerticalVelocityAllowed; + assert velocity > 0; + + return dragInducedByMovement(0, new PositionVector(0,0,1), velocity); + } + @Override public double verticalDescentMaxAcceleration() { - return verticalDescentDrag() / mass; + return GRAVITY - verticalDrag(maximumVerticalVelocityAllowed) / mass; + } + + @Override + public double verticalDescentMaxVelocity() { + double technicalMaximum = Math.sqrt(2.0 * (hoverThrust()) / bodyDrag(0, new PositionVector(0,0,-1))); + return Math.min(technicalMaximum, maximumVerticalVelocityAllowed); } + @Override public double verticalAscentMaxAcceleration() { - return (motor.getMaxThrust() - hoverThrustRequired()) / mass; + return (motor.getMaxThrust() - hoverThrust()) / mass; } @Override - public double verticalAscentMaxVelocity() { - double maxThrust = motor.getMaxThrust(); - return Math.sqrt(2.0 * (maxThrust - hoverThrustRequired()) / bodyDrag(0, new PositionVector(0,0,1))); + public double verticalAscentMaxVelocity() { + double technicalMaximum = Math.sqrt(2.0 * (motor.getMaxThrust() - hoverThrust()) / bodyDrag(0, new PositionVector(0,0,1))); + return Math.min(technicalMaximum, maximumVerticalVelocityAllowed); } @Override - public double hoverThrustRequired() { + public double verticalAscentThrust(double velocity) { + return hoverThrust() + verticalDrag(velocity); + } + + + @Override + public double hoverThrust() { return mass * GRAVITY; } @@ -194,28 +223,43 @@ public class IntelAeroRTF implements MulticopterModel { double maxVelocity = Math.sqrt( (2.0 * horizontalThrust) / bodyDrag(maximumPitchAngleAllowed, new PositionVector(1,0,0))); return maxVelocity; } - - protected double horizontalComponentMaxThrust() { + + private double horizontalComponentMaxThrust() { // hoverthrust / cos => amount of thrust in horizonal direction with °angle - double stableAltitudeMaximumTotalThrust = getHorizontalMaxVelocityRequiredTotalThrust(); + double stableAltitudeMaximumTotalThrust = thrustForHorizontalMaxVelocity(); // fraction of total thrust in horizonal (forward) direction with °angle double maximumHorizontalThrustStableAltitude = stableAltitudeMaximumTotalThrust * Math.sin(maximumPitchAngleAllowed); return maximumHorizontalThrustStableAltitude; } + /** + * Acceleration is thrust from motors minus drag from speed + * Drag: rough approximation for velocity induced drag when accelerating (half of maximum velocity drag) + */ + @Override public double horizontalMaxAcceleration() { - return horizontalComponentMaxThrust() / mass; + return horizontalComponentMaxThrust() - forwardDrag(horizontalMaxVelocity() / 2, maximumPitchAngleAllowed) / mass; + } + + /** + * Acceleration is thrust from motors minus drag from speed + * Drag: rough approximation for velocity induced drag when accelerating (half of maximum velocity drag) + */ + @Override + public double horizontalMaxDeceleration() { + return horizontalComponentMaxThrust() + forwardDrag(horizontalMaxVelocity() / 2, maximumPitchAngleAllowed) / mass; } - protected double getHorizontalMaxVelocityRequiredTotalThrust() { - return hoverThrustRequired() / Math.cos(maximumPitchAngleAllowed); + @Override + public double thrustForHorizontalMaxVelocity() { + return hoverThrust() / Math.cos(maximumPitchAngleAllowed); } - public double bodyDrag(double angleRadians, PositionVector direction) { + private double bodyDrag(double angleRadians, PositionVector direction) { return AIRDENSITY * dragCoefficient * areaExposedToDrag(angleRadians, direction); } - protected double areaExposedToDrag(double angleRadians, PositionVector direction) { + private 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(); @@ -229,7 +273,7 @@ public class IntelAeroRTF implements MulticopterModel { /* * F_drag [N] = 0.5 * p * C_drag * A * v^2 */ - public double dragInducedByMovement(double angleOfAttack, PositionVector direction, double velocity) { + private double dragInducedByMovement(double angleOfAttack, PositionVector direction, double velocity) { return 0.5 * bodyDrag(angleOfAttack, direction) * velocity * velocity; } @@ -240,7 +284,7 @@ public class IntelAeroRTF implements MulticopterModel { * @param angleInRadians * @return */ - public double forwardDrag(double velocity, double angleInRadians) { + private double forwardDrag(double velocity, double angleInRadians) { return dragInducedByMovement(angleInRadians, new PositionVector(1,0,0), velocity); } @@ -273,17 +317,55 @@ public class IntelAeroRTF implements MulticopterModel { return optimalVelocity; } + @Override + public boolean isOn() { + return motor.isOn(); + } ///// @Override public void setMotor(ActuatorComponent actuator) { assert actuator instanceof StatelessActuatorComponent; - this.motor = (StatelessActuatorComponent) motor; + this.motor = (StatelessActuatorComponent) actuator; } + @Override + public double getMaximumTurnAngle() { + return maximumTurnAngle; + } + + @Override + public void thrust(double thrust, long phaseTime) { + motor.requestThrust(thrust, phaseTime); + } + + @Override + public double getCurveRadius(double curveVelocity) { + return Math.pow(curveVelocity, 2) / getCurveCentripedalAcceleration(curveVelocity); + } + + @Override + public double getCurveCentripedalAcceleration(double curveVelocity) { + return (horizontalComponentMaxThrust() - thrustForHorizontalVelocity(curveVelocity)) / mass; + } + + @Override + public double maxThrust() { + return motor.getMaxThrust(); + } + + @Override + public double minThrust() { + return motor.getMinThrust(); + } + + @Override + public double curveVelocity() { + return optimalCurveVelocity; + } /** - * Factory for this movement model + * Factory for the Intel UAV * * @author Julian Zobel * @version 1.0, 14.01.2020 @@ -298,6 +380,7 @@ public class IntelAeroRTF implements MulticopterModel { private double maximumPitchAngleAllowed = Math.toRadians(60); // ° max angle private double maximumVerticalVelocityAllowed = 10; // m/s private double maximumTurnAngle = Math.toRadians(90); // 90° per second turn angle + private double optimalCurveVelocity = 8; public void setMassTotal(double massTotal) { this.massTotal = massTotal; @@ -330,7 +413,11 @@ public class IntelAeroRTF implements MulticopterModel { public MulticopterModel createComponent() { return new IntelAeroRTF(massTotal, areaTop, areaFront, - UAVDragCoefficient, maximumPitchAngleAllowed, maximumVerticalVelocityAllowed, maximumTurnAngle); + UAVDragCoefficient, maximumPitchAngleAllowed, maximumVerticalVelocityAllowed, maximumTurnAngle, optimalCurveVelocity); + } + + public void setOptimalCurveVelocity(double optimalCurveVelocity) { + this.optimalCurveVelocity = optimalCurveVelocity; } } } -- GitLab