diff --git a/src/de/tud/kom/p2psim/api/topology/movement/SimUAVLocationActuator.java b/src/de/tud/kom/p2psim/api/topology/movement/SimUAVLocationActuator.java
index a96c95193043b87da859361b0328c5710fcf04ec..9d77e7f5e76d606ebba504fba05932e16aee4dcd 100644
--- a/src/de/tud/kom/p2psim/api/topology/movement/SimUAVLocationActuator.java
+++ b/src/de/tud/kom/p2psim/api/topology/movement/SimUAVLocationActuator.java
@@ -21,12 +21,13 @@
package de.tud.kom.p2psim.api.topology.movement;
import de.tud.kom.p2psim.api.topology.component.ControllableLocationActuator;
-import de.tud.kom.p2psim.impl.energy.components.ActuatorEnergyComponent;
+import de.tud.kom.p2psim.impl.energy.components.StateActuatorEnergyComponent;
+import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
public interface SimUAVLocationActuator extends SimLocationActuator, ControllableLocationActuator {
public UAVMovementModel getUAVMovement();
public void setUAVMovement(UAVMovementModel uavMovement);
- public ActuatorEnergyComponent getActuatorEnergyComponent();
+
}
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 35e1968855fb8f6dffb48951d15a3975958a9692..b4b1b4429a60b2770483d9437a4164d0cea49cdb 100644
--- a/src/de/tud/kom/p2psim/api/topology/movement/UAVMovementModel.java
+++ b/src/de/tud/kom/p2psim/api/topology/movement/UAVMovementModel.java
@@ -21,16 +21,21 @@
package de.tud.kom.p2psim.api.topology.movement;
import java.util.LinkedList;
+
+import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;
import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;
public interface UAVMovementModel {
- public void setPreferredCruiseSpeed(double v_pref);
+ public void setMotorControl(StatelessMotorComponent motor);
+
+ public void setPreferredSpeed(double v_pref);
- public double getMaxCruiseSpeed();
- public double getMinCruiseSpeed();
+ public double verticalAscentMaxVelocity();
+ public double horizontalMaxVelocity();
public double getCurrentSpeed();
+ public double minimumVelocity();
public void move(long timeBetweenMovementOperations);
diff --git a/src/de/tud/kom/p2psim/impl/energy/components/MotorCharacteristic.java b/src/de/tud/kom/p2psim/impl/energy/components/MotorCharacteristic.java
new file mode 100644
index 0000000000000000000000000000000000000000..6f55cf35277a95e38e4f85f570d4eb18e58c2db4
--- /dev/null
+++ b/src/de/tud/kom/p2psim/impl/energy/components/MotorCharacteristic.java
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2005-2010 KOM – Multimedia Communications Lab
+ *
+ * This file is part of PeerfactSim.KOM.
+ *
+ * PeerfactSim.KOM is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * any later version.
+ *
+ * PeerfactSim.KOM is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with PeerfactSim.KOM. If not, see .
+ *
+ */
+
+package de.tud.kom.p2psim.impl.energy.components;
+
+import de.tudarmstadt.maki.simonstrator.api.util.XMLConfigurableConstructor;
+
+public class MotorCharacteristic {
+
+ private final double thrust;
+ private final double current;
+
+ @XMLConfigurableConstructor({ "thrust", "current" })
+ public MotorCharacteristic(double thrust, double current) {
+ this.thrust = thrust;
+ this.current = current;
+ }
+
+ public double getCurrent() {
+ return current;
+ }
+
+ public double getThrust() {
+ return thrust;
+ }
+
+ @Override
+ public String toString() {
+ return "["+ thrust + "N @ " + current + "A]";
+ }
+}
diff --git a/src/de/tud/kom/p2psim/impl/energy/components/ActuatorEnergyComponent.java b/src/de/tud/kom/p2psim/impl/energy/components/StateActuatorEnergyComponent.java
similarity index 95%
rename from src/de/tud/kom/p2psim/impl/energy/components/ActuatorEnergyComponent.java
rename to src/de/tud/kom/p2psim/impl/energy/components/StateActuatorEnergyComponent.java
index e74f9a87754cea89a7b0bc9da99d2dd4785f393e..99318b1acfdf5e3f8eb5a7e0aa6358ac97bff910 100644
--- a/src/de/tud/kom/p2psim/impl/energy/components/ActuatorEnergyComponent.java
+++ b/src/de/tud/kom/p2psim/impl/energy/components/StateActuatorEnergyComponent.java
@@ -36,7 +36,7 @@ import de.tudarmstadt.maki.simonstrator.api.Time;
* @author Julian Zobel
* @version 1.0, 11.09.2018
*/
-public class ActuatorEnergyComponent implements EnergyComponent {
+public class StateActuatorEnergyComponent implements EnergyComponent {
/**
* States supported by this energy component
@@ -53,7 +53,7 @@ public class ActuatorEnergyComponent implements EnergyComponent {
private double actuatorLoad;
- public ActuatorEnergyComponent(int numberOfActuators, double volt, double hoverAmp, double maxAmp) {
+ public StateActuatorEnergyComponent(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);
diff --git a/src/de/tud/kom/p2psim/impl/energy/components/StatelessMotorComponent.java b/src/de/tud/kom/p2psim/impl/energy/components/StatelessMotorComponent.java
new file mode 100644
index 0000000000000000000000000000000000000000..5fd7821fa076f584313eff5e93826bd76d582e0b
--- /dev/null
+++ b/src/de/tud/kom/p2psim/impl/energy/components/StatelessMotorComponent.java
@@ -0,0 +1,279 @@
+/*
+ * Copyright (c) 2005-2010 KOM � Multimedia Communications Lab
+ *
+ * This file is part of PeerfactSim.KOM.
+ *
+ * PeerfactSim.KOM is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * any later version.
+ *
+ * PeerfactSim.KOM is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with PeerfactSim.KOM. If not, see .
+ *
+ */
+
+package de.tud.kom.p2psim.impl.energy.components;
+
+
+import java.util.Comparator;
+import java.util.LinkedList;
+import de.tud.kom.p2psim.api.energy.ComponentType;
+import de.tud.kom.p2psim.api.energy.EnergyComponent;
+import de.tud.kom.p2psim.api.energy.EnergyEventListener;
+import de.tud.kom.p2psim.api.energy.EnergyState;
+import de.tud.kom.p2psim.impl.energy.DefaultEnergyState;
+import de.tudarmstadt.maki.simonstrator.api.Time;
+
+
+/**
+ * Component for devices that provide thrust, such as electrical motors for UAV propulsion.
+ * Is configured by {@link MotorCharacteristic}s with a given thrust (N) and a given current (A).
+ * Values in between given characteristics are calculated by linear interpolation.
+ *
+ *
+ * @author Julian Zobel
+ * @version 1.0, 05.03.2019
+ */
+public class StatelessMotorComponent implements EnergyComponent {
+
+ public enum componentState {OFF, ON}
+
+ private componentState state;
+
+ private EnergyEventListener energyModel;
+
+ private long lastEnergyConsumationTime;
+
+ private double volts;
+ private final double uJconversionFactor = 1000000;
+ private final int numberOfActuators;
+
+ private double thrust;
+ private double amps;
+ private EnergyState energyState;
+ private LinkedList characteristics = new LinkedList<>();
+
+
+ public StatelessMotorComponent(int numberOfActuators, double volt) {
+ this.volts = volt;
+ this.numberOfActuators = numberOfActuators;
+ this.state = componentState.OFF;
+ this.lastEnergyConsumationTime = Time.getCurrentTime();
+
+ thrust = 0;
+ amps = 0;
+ energyState = new DefaultEnergyState("OFF", 0);
+ }
+
+ /**
+ * Get the maximum thrust provided by this component.
+ * @return
+ */
+ public double getMaxThrust() {
+ return characteristics.getLast().getThrust();
+ }
+
+ /**
+ * Set the new energy state and calculate the energy consumption from the last state
+ */
+ private void setEnergyState() {
+
+ // set the new energy state
+ EnergyState newState;
+
+ if(state == componentState.ON) {
+ newState = new DefaultEnergyState("Actuator", numberOfActuators * (amps * volts) * uJconversionFactor);
+ }
+ else {
+ newState = new DefaultEnergyState("OFF", 0);
+ }
+
+ // calculate energy consumption for the previous state
+ long timeSpentInState = Time.getCurrentTime() - lastEnergyConsumationTime;
+
+ double cons = calculateEnergyConsumation(energyState, timeSpentInState);
+
+ System.out.println("consumed energy : " + (cons/uJconversionFactor) + " in " + Time.getFormattedTime(timeSpentInState));
+
+ energyModel.componentConsumedEnergy(this, cons);
+
+ // set new state
+ energyState = newState;
+ lastEnergyConsumationTime = Time.getCurrentTime();
+ }
+
+ /**
+ * Request a given amount of thrust to be provided from this component. If the amount is less than the minimum
+ * or more than the maximum, the minimum or maximum thrust values, respectively, are enforced.
+ *
+ * @param targetThrust
+ * @return The amount of thrust this component now generates.
+ */
+ public double requestThrust(double targetThrust) {
+
+ if(targetThrust == 0 || targetThrust <= characteristics.getFirst().getThrust()) {
+ setLoad(characteristics.getFirst());
+ }
+ else if(targetThrust >= characteristics.getLast().getThrust()) {
+ setLoad(characteristics.getLast());
+ }
+ else {
+ approximateThrustCurrentRelation(targetThrust);
+ }
+
+ System.out.println("Now providing Thrust: " + this.thrust + " N");
+
+ return this.thrust;
+ }
+
+ /**
+ * Given an amount of thrust between the minimum and maximum values, the required current
+ * to provide this amount of thrust is calculated by linear interpolation by the nearest lower
+ * and upper {@link MotorCharacteristic}s.
+ *
+ * @param targetThrust
+ */
+ private void approximateThrustCurrentRelation(double targetThrust) {
+
+ MotorCharacteristic lower = null, upper = null;
+
+ // find the lower and upper bounding characteristics
+ for (MotorCharacteristic ch : characteristics) {
+ //
+ if(ch.getThrust() == targetThrust) {
+ setLoad(ch);
+ return;
+ }
+ else {
+ // list is sorted, lower bound is the biggest that is lower
+ if(ch.getThrust() < targetThrust) {
+ lower = ch;
+ }
+ // the first that is greater is used as upper bound
+ else if(ch.getThrust() > targetThrust) {
+ upper = ch;
+ break;
+ }
+ }
+ }
+
+ if(upper == null || lower == null) {
+ throw new UnsupportedOperationException("Lower or upper bounds cannot be null");
+ }
+
+ if(upper.getThrust() < targetThrust || lower.getThrust() > targetThrust) {
+ throw new UnsupportedOperationException("Lower or upper bound do not match");
+ }
+
+ /*
+ * Calculate the approximated current with the upper and lower bounds:
+ * Amp_approx = Amp_lower + (T_target - T_lower)/(T_upper - T_lower) * (Amp_upper - Amp_lower)
+ */
+ double delta = (targetThrust - lower.getThrust())/(upper.getThrust() - lower.getThrust());
+
+ double calculatedAmps = lower.getCurrent() + delta * (upper.getCurrent() - lower.getCurrent());
+
+ System.out.println(targetThrust + " N targeted => " + calculatedAmps + " A calculated | " + delta + " <> " + lower + " " + upper);
+
+ setLoad(targetThrust, calculatedAmps);
+ }
+
+ private void setLoad(double thrust, double amps) {
+ this.thrust = thrust;
+ this.amps = amps;
+ setEnergyState();
+ }
+
+ private void setLoad(MotorCharacteristic ch) {
+ this.thrust = ch.getThrust();
+ this.amps = ch.getCurrent();
+ setEnergyState();
+ }
+
+ /**
+ * Add a {@link MotorCharacteristic} for this motor.
+ *
+ * @param c
+ */
+ public void addChar(MotorCharacteristic c) {
+
+ characteristics.add(c);
+
+ // sort the characteristics starting from low to high thrust
+ characteristics.sort(new Comparator() {
+ @Override
+ public int compare(MotorCharacteristic o1,
+ MotorCharacteristic o2) {
+ return (int) (o1.getThrust() - o2.getThrust());
+ }
+ });
+
+ }
+
+
+ @Override
+ public void eventOccurred(Object content, int type) {
+ // TODO Auto-generated method stub
+
+ }
+
+ @Override
+ public ComponentType getType() {
+ return ComponentType.ACTUATOR;
+ }
+
+ @Override
+ public boolean turnOff() {
+ this.thrust = 0;
+ this.amps = 0;
+ this.state = componentState.OFF;
+ setEnergyState();
+
+ return true;
+ }
+
+ @Override
+ public boolean turnOn() {
+ if (isAvailable()) {
+ if(this.state != componentState.ON) {
+ this.state = componentState.ON;
+ requestThrust(0);
+ }
+ return true;
+ }
+ return false;
+ }
+
+ public boolean isAvailable() {
+ if (energyModel.componentCanBeActivated(this)) {
+ return true;
+ }
+ return false;
+ }
+
+ @Override
+ public boolean isOn() {
+ if(this.state != componentState.OFF && isAvailable()) {
+ return true;
+ }
+ return false;
+ }
+
+ @Override
+ public void setEnergyEventListener(EnergyEventListener listener) {
+ energyModel = listener;
+ }
+
+ public EnergyState getCurrentState() {
+ return energyState;
+ }
+
+
+
+}
diff --git a/src/de/tud/kom/p2psim/impl/energy/configs/ActuatorConfiguration.java b/src/de/tud/kom/p2psim/impl/energy/configs/ActuatorConfiguration.java
new file mode 100644
index 0000000000000000000000000000000000000000..9ac9a9edc1c4b31abfe3d743dce25a3c695a2dd9
--- /dev/null
+++ b/src/de/tud/kom/p2psim/impl/energy/configs/ActuatorConfiguration.java
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2005-2010 KOM � Multimedia Communications Lab
+ *
+ * This file is part of PeerfactSim.KOM.
+ *
+ * PeerfactSim.KOM is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * any later version.
+ *
+ * PeerfactSim.KOM is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with PeerfactSim.KOM. If not, see .
+ *
+ */
+
+package de.tud.kom.p2psim.impl.energy.configs;
+
+import java.util.LinkedList;
+import de.tud.kom.p2psim.api.common.SimHost;
+import de.tud.kom.p2psim.api.energy.EnergyConfiguration;
+import de.tud.kom.p2psim.impl.energy.components.MotorCharacteristic;
+import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
+
+
+/**
+ *
+ *
+ * @author Julian Zobel
+ * @version 1.0, 05.03.2019
+ */
+public class ActuatorConfiguration implements EnergyConfiguration {
+
+ private int numberOfActuators;
+ private double volt;
+
+ private LinkedList characteristics = new LinkedList<>();
+
+ @Override
+ public StatelessMotorComponent getConfiguredEnergyComponent(SimHost host) {
+ StatelessMotorComponent comp = new StatelessMotorComponent(numberOfActuators, volt);
+
+ for (MotorCharacteristic c : characteristics) {
+ comp.addChar(c);
+ }
+
+ return comp;
+ }
+
+ @Override
+ public String getHelp() {
+ return "Fix actuator energy consumption config";
+ }
+
+ @Override
+ public boolean isWellConfigured() {
+ if(numberOfActuators >= 1 && volt > 0 && characteristics.size() >= 2) {
+ return true;
+ }
+
+ return false;
+ }
+
+ public void setNumberOfActuators(int num) {
+ numberOfActuators = num;
+ }
+
+ public void setVolt(double voltage) {
+ this.volt = voltage;
+ }
+
+ public void setMotorCharacteristic(MotorCharacteristic c) {
+ characteristics.add(c);
+ }
+
+}
diff --git a/src/de/tud/kom/p2psim/impl/energy/configs/ActuatorEnergyConsumptionConfig.java b/src/de/tud/kom/p2psim/impl/energy/configs/ActuatorEnergyConsumptionConfig.java
index 354586b55989fb9506e6ceb5eb4ef5bdf2eecb06..553567768dac30989ff0f87dd98768c9a4f0be0c 100644
--- a/src/de/tud/kom/p2psim/impl/energy/configs/ActuatorEnergyConsumptionConfig.java
+++ b/src/de/tud/kom/p2psim/impl/energy/configs/ActuatorEnergyConsumptionConfig.java
@@ -22,15 +22,15 @@ package de.tud.kom.p2psim.impl.energy.configs;
import de.tud.kom.p2psim.api.common.SimHost;
import de.tud.kom.p2psim.api.energy.EnergyConfiguration;
-import de.tud.kom.p2psim.impl.energy.components.ActuatorEnergyComponent;
+import de.tud.kom.p2psim.impl.energy.components.StateActuatorEnergyComponent;
/**
- * Energy Configuration for {@link ActuatorEnergyComponent}s.
+ * Energy Configuration for {@link StateActuatorEnergyComponent}s.
*
* @author Julian Zobel
* @version 1.0, 11.09.2018
*/
-public class ActuatorEnergyConsumptionConfig implements EnergyConfiguration {
+public class ActuatorEnergyConsumptionConfig implements EnergyConfiguration {
private int numberOfActuators;
private double volt;
@@ -38,8 +38,8 @@ public class ActuatorEnergyConsumptionConfig implements EnergyConfiguration getAllAttractionPoints() {
throw new UnsupportedOperationException();
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
new file mode 100644
index 0000000000000000000000000000000000000000..db55d8fe20626e5e9a5787f8079dcc2b32235e35
--- /dev/null
+++ b/src/de/tud/kom/p2psim/impl/topology/movement/aerial/MulticopterMovement.java
@@ -0,0 +1,315 @@
+/*
+ * Copyright (c) 2005-2010 KOM – Multimedia Communications Lab
+ *
+ * This file is part of PeerfactSim.KOM.
+ *
+ * PeerfactSim.KOM is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * any later version.
+ *
+ * PeerfactSim.KOM is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with PeerfactSim.KOM. If not, see .
+ *
+ */
+
+package de.tud.kom.p2psim.impl.topology.movement.aerial;
+
+import java.util.LinkedHashMap;
+import java.util.LinkedList;
+import java.util.Map;
+
+import org.apache.commons.math3.geometry.Vector;
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
+import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
+
+import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
+import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
+import de.tud.kom.p2psim.impl.topology.component.UAVTopologyComponent;
+import de.tud.kom.p2psim.impl.topology.util.PositionVector;
+import de.tudarmstadt.maki.simonstrator.api.Time;
+import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;
+
+public class MulticopterMovement implements UAVMovementModel {
+
+
+ private UAVTopologyComponent topologyComponent;
+
+ private PositionVector direction;
+ private double currentAngleOfAttack;
+ private double currentSpeed;
+
+ private double preferredSpeed;
+
+
+ private LinkedList route = new LinkedList<>();
+ private Map locationCallbacks = new LinkedHashMap<>(); // TODO callback interface
+
+ private StatelessMotorComponent motor;
+
+
+ private double mass = 1.465; // kg
+ private final double airdensity = 1.2255; // 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.04; // m^2
+ private double dragCoefficient = 0.5;
+ private double maxPitchAngle = Math.toRadians(45); // 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;
+ this.direction = new PositionVector(0,0);
+
+ }
+
+
+ /*
+ *
+ */
+
+ public double verticalDescentMaxThrust() {
+ // m * g - 0.5 * p * C * A * v^2
+ return hoverThrustRequired() - 0.5 * bodyDrag(0, new PositionVector(0,0,1)) * descentVelocityMax * descentVelocityMax;
+ }
+
+ public double verticalAscentMaxAcceleration() {
+ return (motor.getMaxThrust() - hoverThrustRequired()) / mass;
+ }
+
+ public double verticalAscentMaxVelocity() {
+ double maxThrust = motor.getMaxThrust();
+ return Math.sqrt(2.0 * (maxThrust - hoverThrustRequired()) / bodyDrag(0, new PositionVector(0,0,1)));
+ }
+
+
+ public double hoverThrustRequired() {
+ return mass * gravity;
+ }
+
+
+ public double horizontalMaxVelocity() {
+
+ double horizontalThrust = horizontalMaxThrust();
+
+ double maxVelocity = Math.sqrt( (2.0 * horizontalThrust) / bodyDrag(maxPitchAngle, new PositionVector(1,0,0)));
+
+ return maxVelocity;
+ }
+
+ public double horizontalMaxThrust() {
+
+ // hoverthrust / cos => amount of thrust in horizonal direction with °angle
+ double stableAltitudeMaximumTotalThrust = hoverThrustRequired() / Math.cos(maxPitchAngle);
+
+ // fraction of total thrust in horizonal (forward) direction with °angle
+ double maximumHorizontalThrustStableAltitude = stableAltitudeMaximumTotalThrust * Math.sin(maxPitchAngle);
+
+ return maximumHorizontalThrustStableAltitude;
+ }
+
+ public double bodyDrag(double angleRadians, PositionVector direction) {
+ return airdensity * dragCoefficient * areaExposedToDrag(angleRadians, direction);
+ }
+
+ public 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);
+
+ return areaExposedFront + areaExposedTop;
+ }
+
+ /*
+ * F_drag [N] = 0.5 * p * C_drag * A * v^2
+ */
+ public double currentDrag() {
+ return 0.5 * bodyDrag(currentAngleOfAttack, direction) * currentSpeed * currentSpeed;
+ }
+
+
+
+ /*
+ *
+ */
+
+ @Override
+ public void setMotorControl(StatelessMotorComponent motor) {
+ this.motor = motor;
+ }
+
+ @Override
+ public void move(long timeBetweenMovementOperations) {
+
+ if(motor.isOn() && !route.isEmpty()) {
+ PositionVector position = new PositionVector(topologyComponent.getRealPosition());
+
+ double thrust = 0;
+ direction = new PositionVector(0,0,1);
+
+ double distz = 100 - position.getZ();
+
+ if(distz < 100) {
+
+ double time = ((double) timeBetweenMovementOperations) / ((double) Time.SECOND);
+
+ // ASCENSION TO SAFETY HEIGHT
+ thrust = motor.requestThrust(motor.getMaxThrust());
+
+ double acc = ((thrust - hoverThrustRequired()) / mass ) - currentDrag();
+ double speed = 0;
+
+ if(acc < 0.1) {
+ // normal flight
+ speed = verticalAscentMaxVelocity();
+ }
+ else {
+ // acceleration
+ speed = currentSpeed + acc * time;
+ }
+
+
+ System.out.println("ACCELERATION : " + acc);
+ System.out.println("SPEED : " + speed);
+
+ //double dist = 0.5 * acc * (timeBetweenMovementOperations / Time.SECOND) * (timeBetweenMovementOperations / Time.SECOND);
+ double dist = currentSpeed * time;
+
+ //double safetydistance = speed
+
+ //System.out.println();
+
+ position.add(new PositionVector(0,0,dist));
+
+ System.out.println("NEW POSITION : " + position);
+
+ topologyComponent.updateCurrentLocation(position);
+ }
+ else {
+ thrust = motor.requestThrust(hoverThrustRequired());
+ topologyComponent.updateCurrentLocation(position);
+ }
+
+
+
+
+// PositionVector directionToTarget = new PositionVector(route.getFirst());
+// directionToTarget.subtract(position);
+//
+// double distanceToTargetPosition = directionToTarget.getLength();
+
+
+// // If target point is reached within a 1 meter margin, we remove that point from the list
+// if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < currentSpeed)
+// {
+// route.removeFirst();
+//
+// topologyComponent.updateCurrentLocation(position);
+// currentSpeed = 0;
+//
+// locationReached(position);
+// return;
+// }
+//
+// double timefactor = timeBetweenMovementOperations / Time.SECOND;
+//
+// currentSpeed = Math.min(distanceToTargetPosition, preferredSpeed);
+//
+// direction.normalize();
+// direction.multiplyScalar(currentSpeed * timefactor);
+//
+// PositionVector newPosition = new PositionVector(position);
+// newPosition.add(direction);
+//
+// topologyComponent.updateCurrentLocation(newPosition);
+
+ }
+
+ //System.out.println(Simulator.getFormattedTime(Simulator.getCurrentTime()) + " | " + topologyComponent);
+ }
+
+
+ @Override
+ public void setPreferredSpeed(double v_pref) {
+ this.preferredSpeed = v_pref;
+ }
+
+ @Override
+ public double getCurrentSpeed() {
+ return currentSpeed;
+ }
+
+ /**
+ * Trigger the callback function, if there is a valid callback
+ *
+ * @param position
+ */
+ private void locationReached(PositionVector position) {
+ if(locationCallbacks.containsKey(position)) {
+ locationCallbacks.get(position).reachedLocation();
+ }
+ }
+
+ @Override
+ public void setTargetLocation(PositionVector target,
+ ReachedLocationCallback reachedLocationCallback) {
+ route.clear();
+ route.add(target);
+ if(reachedLocationCallback != null)
+ locationCallbacks.put(target, reachedLocationCallback);
+
+ }
+
+ @Override
+ public void setTargetLocationRoute(LinkedList route,
+ ReachedLocationCallback reachedLocationCallback) {
+ this.route.clear();
+ this.route.addAll(route);
+ if(reachedLocationCallback != null)
+ locationCallbacks.put(route.getLast(), reachedLocationCallback);
+ }
+
+ @Override
+ public void addTargetLocation(PositionVector target,
+ ReachedLocationCallback reachedLocationCallback) {
+ route.add(target);
+ if(reachedLocationCallback != null)
+ locationCallbacks.put(target, reachedLocationCallback);
+ }
+
+ @Override
+ public LinkedList getTargetLocations() {
+
+ LinkedList copy = new LinkedList<>();
+ for (PositionVector pv : route) {
+ copy.add(pv.clone());
+ }
+
+ return copy;
+ }
+
+ @Override
+ public void removeTargetLocations() {
+ route.clear();
+ locationCallbacks.clear();
+ }
+
+
+ @Override
+ public double minimumVelocity() {
+ return 0;
+ }
+
+
+
+}
diff --git a/src/de/tud/kom/p2psim/impl/topology/movement/aerial/SimpleMutlicopterMovement.java b/src/de/tud/kom/p2psim/impl/topology/movement/aerial/SimpleMutlicopterMovement.java
index 870a9f9c1bd49b597e760f376e0b437da99e1908..ed2634fcd7cb3cfb1cc3390bf1fdb00d0cdbba01 100644
--- a/src/de/tud/kom/p2psim/impl/topology/movement/aerial/SimpleMutlicopterMovement.java
+++ b/src/de/tud/kom/p2psim/impl/topology/movement/aerial/SimpleMutlicopterMovement.java
@@ -28,6 +28,7 @@ import java.util.Map;
import org.joda.time.tz.ZoneInfoProvider;
import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
+import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
import de.tud.kom.p2psim.impl.topology.component.UAVTopologyComponent;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;
import de.tudarmstadt.maki.simonstrator.api.Time;
@@ -52,6 +53,7 @@ public class SimpleMutlicopterMovement implements UAVMovementModel {
private LinkedList route = new LinkedList<>();
private Map locationCallbacks = new LinkedHashMap<>(); // TODO callback interface
+ private StatelessMotorComponent motor;
public SimpleMutlicopterMovement(UAVTopologyComponent topologyComponent, double maxCruiseSpeed, double minCruiseSpeed) {
this.topologyComponent = topologyComponent;
@@ -62,18 +64,13 @@ public class SimpleMutlicopterMovement implements UAVMovementModel {
}
@Override
- public void setPreferredCruiseSpeed(double v_pref) {
- this.preferredCruiseSpeed = v_pref;
- }
-
- @Override
- public double getMaxCruiseSpeed() {
- return maxCruiseSpeed;
+ public void setMotorControl(StatelessMotorComponent motor) {
+ this.motor = motor;
}
-
+
@Override
- public double getMinCruiseSpeed() {
- return minCruiseSpeed;
+ public void setPreferredSpeed(double v_pref) {
+ this.preferredCruiseSpeed = v_pref;
}
@Override
@@ -96,10 +93,10 @@ public class SimpleMutlicopterMovement implements UAVMovementModel {
if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < currentSpeed)
{
route.removeFirst();
- topologyComponent.getActuatorEnergyComponent().useActuator(0.5);
+ motor.requestThrust(16.3);
topologyComponent.updateCurrentLocation(targetPosition); // triggers energy consumption for last distance
currentSpeed = 0;
- topologyComponent.getActuatorEnergyComponent().useActuator(0); // now hover
+ motor.requestThrust(14.4); // now hover
locationReached(targetPosition);
return;
}
@@ -116,7 +113,7 @@ public class SimpleMutlicopterMovement implements UAVMovementModel {
PositionVector newPosition = new PositionVector(currentPosition);
newPosition.add(direction);
- topologyComponent.getActuatorEnergyComponent().useActuator(1);
+ motor.requestThrust(22);
topologyComponent.updateCurrentLocation(newPosition);
}
@@ -179,6 +176,21 @@ public class SimpleMutlicopterMovement implements UAVMovementModel {
locationCallbacks.clear();
}
+ @Override
+ public double verticalAscentMaxVelocity() {
+ return maxCruiseSpeed;
+ }
+
+ @Override
+ public double horizontalMaxVelocity() {
+ return maxCruiseSpeed;
+ }
+
+ @Override
+ public double minimumVelocity() {
+ return 0;
+ }
+