Skip to content
GitLab
Menu
Projects
Groups
Snippets
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Simonstrator
PeerfactSim.KOM
Commits
a860aadf
Commit
a860aadf
authored
Jan 20, 2020
by
Julian Zobel
Browse files
More renaming to make energy and movement moidels for UAVs more lenient.
parent
5813d44d
Changes
14
Hide whitespace changes
Inline
Side-by-side
src/de/tud/kom/p2psim/api/energy/Battery.java
View file @
a860aadf
...
...
@@ -38,7 +38,7 @@ public interface Battery extends Cloneable {
*
* @return the maximum capacity of the battery in uJ
*/
public
double
getMaximumEnergy
Level
();
public
double
getMaximumEnergy
();
/**
* Returns the current status of the Battery as a Percentage-Value
...
...
@@ -52,7 +52,7 @@ public interface Battery extends Cloneable {
*
* @return the current capacity of the battery in uJ
*/
public
double
getCurrentEnergy
Level
();
public
double
getCurrentEnergy
();
/**
* Returns the consumed energy in uJoule
...
...
src/de/tud/kom/p2psim/api/energy/EnergyComponent.java
View file @
a860aadf
...
...
@@ -28,8 +28,8 @@ import de.tudarmstadt.maki.simonstrator.api.Time;
* State-switching is event-based in this new version of the Model to allow more
* fine-grained analyzing.
*
* @author Bjoern Richerzhagen
* @version 1.
0
, 2
1
.0
2
.20
1
2
* @author Bjoern Richerzhagen
, Julian Zobel
* @version 1.
1
, 2
0
.0
1
.202
0
*/
public
interface
EnergyComponent
extends
EventHandler
{
...
...
src/de/tud/kom/p2psim/api/topology/component/ControllableLocationActuator.java
View file @
a860aadf
...
...
@@ -87,5 +87,7 @@ public interface ControllableLocationActuator extends Actuator {
public
double
getMovementSpeed
();
public
double
estimatePowerConsumption
(
double
velocity
);
public
double
estimatePowerConsumptionWatt
(
double
velocity
);
public
double
estimateFlightDistance
(
double
velocity
,
double
batterylevel
,
double
batterythreshold
);
}
src/de/tud/kom/p2psim/api/topology/movement/UAVMovementModel.java
View file @
a860aadf
...
...
@@ -21,24 +21,42 @@
package
de.tud.kom.p2psim.api.topology.movement
;
import
java.util.LinkedList
;
import
de.tud.kom.p2psim.impl.energy.components.ActuatorComponent
;
import
de.tud.kom.p2psim.impl.energy.components.StatelessActuatorComponent
;
import
de.tud.kom.p2psim.impl.topology.util.PositionVector
;
import
de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback
;
/**
* UAV Movement Models provide basic functionality for the simulation of flying UAVs.
*
* @author Julian Zobel
* @version 1.0, 20.01.2020
*/
public
interface
UAVMovementModel
{
// NO - NO CHANGY
public
final
double
AIRDENSITY
=
1.2045
;
// kg/m^3
public
final
double
GRAVITY
=
9.807
;
// m/s^2
public
void
setMotorControl
(
ActuatorComponent
motor
);
public
void
setPreferredSpeed
(
double
v_pref
);
/**
* Set the preferred/target speed of this UAV.
*
* @param targetVelocity
*/
public
void
setTargetVelocity
(
double
targetVelocity
);
public
double
v
erticalAscentMaxVelocity
();
public
double
h
orizontalMaxVelocity
();
public
double
getCurrent
Speed
();
public
double
h
orizontalMinVelocity
();
public
double
getV
erticalAscentMaxVelocity
();
public
double
getH
orizontalMaxVelocity
();
public
double
getCurrent
Velocity
();
public
double
getH
orizontalMinVelocity
();
public
double
estimatePowerConsumption
(
double
velocity
);
/**
* Estimate the power consumption for a given velocity
* @param velocity
* @return The power consumption in W for the given velocity.
*/
public
double
estimatePowerConsumptionWatt
(
double
velocity
);
public
void
move
(
long
timeBetweenMovementOperations
);
...
...
src/de/tud/kom/p2psim/impl/energy/EnergyModelFactory.java
View file @
a860aadf
...
...
@@ -94,12 +94,12 @@ public class EnergyModelFactory implements HostComponentFactory {
double
percentageOfFullBattery
=
(
1
-
random
.
nextDouble
());
if
(
percentageOfFullBattery
<
minimumStartEnergyLevel
)
percentageOfFullBattery
=
minimumStartEnergyLevel
;
double
initialEnergy
=
batteryModel
.
getMaximumEnergy
Level
()
*
percentageOfFullBattery
;
double
initialEnergy
=
batteryModel
.
getMaximumEnergy
()
*
percentageOfFullBattery
;
bat
=
batteryModel
.
copy
(
initialEnergy
);
}
else
{
bat
=
batteryModel
.
copy
(
batteryModel
.
getMaximumEnergy
Level
());
bat
=
batteryModel
.
copy
(
batteryModel
.
getMaximumEnergy
());
}
EnergyModel
em
=
new
ComponentBasedEnergyModel
(
host
,
bat
);
...
...
src/de/tud/kom/p2psim/impl/energy/RechargeableBattery.java
View file @
a860aadf
...
...
@@ -63,6 +63,6 @@ public class RechargeableBattery extends SimpleBattery {
@Override
public
String
toString
()
{
return
"Rechargeable Battery ["
+
getCurrentPercentage
()+
"%] ["
+
getCurrentEnergy
Level
()+
"/"
+
getMaximumEnergy
Level
()+
"]"
;
return
"Rechargeable Battery ["
+
getCurrentPercentage
()+
"%] ["
+
getCurrentEnergy
()+
"/"
+
getMaximumEnergy
()+
"]"
;
}
}
src/de/tud/kom/p2psim/impl/energy/SimpleBattery.java
View file @
a860aadf
...
...
@@ -64,7 +64,7 @@ public class SimpleBattery implements Battery {
}
@Override
public
double
getCurrentEnergy
Level
()
{
public
double
getCurrentEnergy
()
{
if
(
isEmpty
())
{
return
0.0
;
}
...
...
@@ -104,7 +104,7 @@ public class SimpleBattery implements Battery {
}
@Override
public
double
getMaximumEnergy
Level
()
{
public
double
getMaximumEnergy
()
{
return
initialEnergy
;
}
...
...
src/de/tud/kom/p2psim/impl/energy/components/ActuatorComponent.java
View file @
a860aadf
...
...
@@ -24,5 +24,6 @@ import de.tud.kom.p2psim.api.energy.EnergyComponent;
// Interface Tag
public
interface
ActuatorComponent
extends
EnergyComponent
{
}
src/de/tud/kom/p2psim/impl/energy/components/StatefulActuatorComponent.java
View file @
a860aadf
...
...
@@ -21,6 +21,7 @@
package
de.tud.kom.p2psim.impl.energy.components
;
import
de.tud.kom.p2psim.api.energy.Battery
;
import
de.tud.kom.p2psim.api.energy.ComponentType
;
import
de.tud.kom.p2psim.api.energy.EnergyEventListener
;
import
de.tud.kom.p2psim.api.energy.EnergyState
;
...
...
@@ -54,8 +55,8 @@ public class StatefulActuatorComponent implements ActuatorComponent {
public
StatefulActuatorComponent
(
int
numberOfActuators
,
double
volt
,
double
hoverAmp
,
double
maxAmp
)
{
OFF
=
new
DefaultEnergyState
(
"OFF"
,
0
);
FLY
=
new
DefaultEnergyState
(
"FLY"
,
numberOfActuators
*
(
hoverAmp
*
volt
)
*
1000000
);
MAX
=
new
DefaultEnergyState
(
"MAX"
,
numberOfActuators
*
(
maxAmp
*
volt
)
*
1000000
);
FLY
=
new
DefaultEnergyState
(
"FLY"
,
numberOfActuators
*
(
hoverAmp
*
volt
)
*
Battery
.
uJconverison
);
MAX
=
new
DefaultEnergyState
(
"MAX"
,
numberOfActuators
*
(
maxAmp
*
volt
)
*
Battery
.
uJconverison
);
this
.
currentState
=
OFF
;
this
.
lastEnergyConsumationEvent
=
Time
.
getCurrentTime
();
...
...
@@ -92,11 +93,20 @@ public class StatefulActuatorComponent implements ActuatorComponent {
}
}
public
double
estimatePowerConsumption
(
double
load
)
{
/**
* Estimates the energy consumption based on a given load.
* @param load: Actuator load between 0.0 and 1.0
* @return Energy consumption in J/s
*/
public
double
estimateEnergyConsumptionWatt
(
double
load
)
{
double
consumationDelta
=
MAX
.
getEnergyConsumption
()
-
FLY
.
getEnergyConsumption
();
double
estimation
=
FLY
.
getEnergyConsumption
()
+
consumationDelta
*
load
;
return
estimation
;
System
.
out
.
println
(
"MAX "
+
((
MAX
.
getEnergyConsumption
()
/
14.8
)
/
Battery
.
uJconverison
));
System
.
out
.
println
(
"MIN"
+
((
FLY
.
getEnergyConsumption
()
/
14.8
)
/
Battery
.
uJconverison
));
System
.
out
.
println
(
load
);
// this estimation is in uJ, but has to return J, thus, do conversion
return
estimation
/
Battery
.
uJconverison
;
}
@Override
...
...
src/de/tud/kom/p2psim/impl/energy/components/StatelessActuatorComponent.java
View file @
a860aadf
...
...
@@ -129,7 +129,7 @@ public class StatelessActuatorComponent implements ActuatorComponent {
/**
*
* @param targetThrust
* @return The power consumption for the target thrust in
uW (micro w
att
)
* @return The power consumption for the target thrust in
W
att
*/
public
double
estimatePowerConsumptionWatt
(
double
targetThrust
)
{
if
(
targetThrust
==
0
||
targetThrust
<=
numberOfActuators
*
characteristics
.
getFirst
().
getThrust
())
{
...
...
@@ -143,6 +143,8 @@ public class StatelessActuatorComponent implements ActuatorComponent {
else
{
double
amps
=
approximateAmpereDraw
(
targetThrust
);
System
.
out
.
println
(
amps
);
return
numberOfActuators
*
amps
*
volts
;
}
}
...
...
src/de/tud/kom/p2psim/impl/energy/models/AbstractEnergyModel.java
View file @
a860aadf
...
...
@@ -173,7 +173,7 @@ public abstract class AbstractEnergyModel implements EnergyModel, BatterySensor,
@Override
public
double
getCurrentEnergyLevel
()
{
return
bat
.
getCurrentEnergy
Level
();
return
bat
.
getCurrentEnergy
();
}
@Override
...
...
src/de/tud/kom/p2psim/impl/topology/component/UAVTopologyComponent.java
View file @
a860aadf
...
...
@@ -23,6 +23,7 @@ package de.tud.kom.p2psim.impl.topology.component;
import
java.util.LinkedList
;
import
java.util.Set
;
import
de.tud.kom.p2psim.api.common.SimHost
;
import
de.tud.kom.p2psim.api.energy.Battery
;
import
de.tud.kom.p2psim.api.energy.ComponentType
;
import
de.tud.kom.p2psim.api.energy.EnergyModel
;
import
de.tud.kom.p2psim.api.network.SimNetInterface
;
...
...
@@ -38,6 +39,7 @@ import de.tud.kom.p2psim.impl.energy.models.AbstractEnergyModel;
import
de.tud.kom.p2psim.impl.topology.placement.UAVBasePlacement
;
import
de.tud.kom.p2psim.impl.topology.util.PositionVector
;
import
de.tudarmstadt.maki.simonstrator.api.Monitor
;
import
de.tudarmstadt.maki.simonstrator.api.Time
;
import
de.tudarmstadt.maki.simonstrator.api.component.ComponentNotAvailableException
;
import
de.tudarmstadt.maki.simonstrator.api.component.overlay.OverlayComponent
;
import
de.tudarmstadt.maki.simonstrator.api.component.sensor.location.AttractionPoint
;
...
...
@@ -137,12 +139,12 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
@Override
public
double
getMovementSpeed
()
{
return
movement
.
getCurrent
Speed
();
return
movement
.
getCurrent
Velocity
();
}
@Override
public
void
setMovementSpeed
(
double
speed
)
{
movement
.
set
PreferredSpeed
(
speed
);
movement
.
set
TargetVelocity
(
speed
);
}
@Override
...
...
@@ -197,13 +199,18 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
@Override
public
double
getCurrentBatteryEnergy
()
{
return
battery
.
getCurrentEnergy
Level
();
return
battery
.
getCurrentEnergy
();
}
public
RechargeableBattery
getBattery
()
{
return
battery
;
}
@Override
public
double
getMaximumBatteryCapacity
()
{
return
battery
.
getMaximumEnergy
();
}
@Override
public
UAVMovementModel
getUAVMovement
()
{
return
movement
;
...
...
@@ -278,7 +285,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
}
};
movement
.
set
PreferredSpeed
(
movement
.
horizontalMaxVelocity
());
movement
.
set
TargetVelocity
(
movement
.
horizontalMaxVelocity
());
movement
.
setTargetLocation
(
baseLocation
,
returnCallback
);
}
...
...
@@ -339,17 +346,26 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
public
void
updateCurrentDirection
(
PositionVector
direction
)
{
this
.
direction
.
set
(
direction
);
}
@Override
public
double
getMaximumBatteryCapa
city
(
)
{
return
battery
.
getMaximumEnergyLevel
(
);
public
double
estimatePowerConsumptionWatt
(
double
velo
city
)
{
return
movement
.
estimatePowerConsumptionWatt
(
velocity
);
}
@Override
public
double
estimatePowerConsumption
(
double
velocity
)
{
return
movement
.
estimatePowerConsumption
(
velocity
);
public
double
estimateFlightDistance
(
double
velocity
,
double
batterylevel
,
double
batterythreshold
)
{
assert
batterylevel
>
batterythreshold
;
assert
batterylevel
<=
1.0
&&
batterylevel
>=
0.0
;
assert
batterythreshold
<=
1.0
&&
batterythreshold
>=
0.0
;
double
availableEnergy
=
(
battery
.
getMaximumEnergy
()
*
(
batterylevel
-
batterythreshold
))
/
Battery
.
uJconverison
;
// since battery energy is in uJ, conversion in J is required
double
powerconsumption
=
estimatePowerConsumptionWatt
(
velocity
);
// J/s (or Watt)
double
distance
=
(
availableEnergy
/
powerconsumption
)
*
velocity
;
// d = (E/P)* v [m]
return
distance
;
}
@Override
public
PositionVector
getBaseLocation
()
{
return
baseLocation
.
clone
();
...
...
src/de/tud/kom/p2psim/impl/topology/movement/aerial/MulticopterMovement.java
View file @
a860aadf
...
...
@@ -45,32 +45,41 @@ import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocation
* @version 1.0, 11.09.2018
*/
public
class
MulticopterMovement
implements
UAVMovementModel
{
private
UAVTopologyComponent
topologyComponent
;
private
double
currentAngleOfAttack
;
private
double
currentSpeed
;
private
double
velocity
;
private
double
target
Speed
;
private
double
target
Velocity
;
private
LinkedList
<
PositionVector
>
route
=
new
LinkedList
<>();
private
Map
<
PositionVector
,
ReachedLocationCallback
>
locationCallbacks
=
new
LinkedHashMap
<>();
// TODO callback interface
private
StatelessActuatorComponent
motor
;
private
double
mass
=
1.465
;
// kg
private
final
double
airdensity
=
1.2045
;
// kg/m^3
private
final
double
gravity
=
9.807
;
// m/s^2
private
double
A_top
=
0.245
;
// m^2
private
double
A_front
=
0.1
;
// m^2
private
double
dragCoefficient
=
0.7
;
private
double
maxPitchAngle
=
Math
.
toRadians
(
60
);
// 45° max angle
private
double
descentVelocityMax
=
5
;
// m/s
private
double
maxTurnAngle
=
Math
.
toRadians
(
90
);
// 90° per second turn angle
public
MulticopterMovement
(
UAVTopologyComponent
topologyComponent
)
{
this
.
topologyComponent
=
topologyComponent
;
private
double
mass
;
// kg
private
double
areaTop
;
// m^2
private
double
areaFront
;
// m^2
private
double
dragCoefficient
;
private
double
maximumPitchAngleAllowed
;
// ° max angle
private
double
maximumDecentVelocityAllowed
;
// m/s
// FIXME currently not used
private
double
maximumTurnAngle
;
// 90° per second turn angle
public
MulticopterMovement
(
UAVTopologyComponent
topologyComponent
,
double
massTotal
,
double
areaTop
,
double
areaFront
,
double
UAVDragCoefficient
,
double
maximumPitchAngleAllowed
,
double
maximumDecentVelocityAllowed
,
double
maximumTurnAngle
)
{
this
.
topologyComponent
=
topologyComponent
;
this
.
mass
=
massTotal
;
this
.
areaTop
=
areaTop
;
this
.
areaFront
=
areaFront
;
this
.
dragCoefficient
=
UAVDragCoefficient
;
this
.
maximumPitchAngleAllowed
=
maximumPitchAngleAllowed
;
this
.
maximumDecentVelocityAllowed
=
maximumDecentVelocityAllowed
;
this
.
maximumTurnAngle
=
maximumTurnAngle
;
}
boolean
first
=
true
;
...
...
@@ -87,7 +96,7 @@ public class MulticopterMovement implements UAVMovementModel {
// If target point is reached within a 1 meter margin, we remove that point from the list
if
(
distanceToTargetPosition
<
0.1
||
distanceToTargetPosition
<
currentSpeed
)
if
(
distanceToTargetPosition
<
0.1
||
distanceToTargetPosition
<
velocity
)
{
target
=
route
.
removeFirst
();
...
...
@@ -96,7 +105,7 @@ public class MulticopterMovement implements UAVMovementModel {
// go to hover mode
topologyComponent
.
updateCurrentLocation
(
target
.
clone
());
currentSpeed
=
0
;
velocity
=
0
;
motor
.
requestThrust
(
hoverThrustRequired
());
PositionVector
direction
=
topologyComponent
.
getCurrentDirection
().
clone
();
...
...
@@ -109,16 +118,16 @@ public class MulticopterMovement implements UAVMovementModel {
else
{
// get to speed
if
(
target
Speed
>
0
&&
target
Speed
<
h
orizontalMaxVelocity
())
{
motor
.
requestThrust
(
estimateRequiredThrust
(
target
Speed
));
currentSpeed
=
target
Speed
;
if
(
target
Velocity
>
0
&&
target
Velocity
<
getH
orizontalMaxVelocity
())
{
motor
.
requestThrust
(
estimateRequiredThrust
(
target
Velocity
));
velocity
=
target
Velocity
;
}
else
{
motor
.
requestThrust
(
h
orizontalMaxVelocityRequiredTotalThrust
());
currentSpeed
=
h
orizontalMaxVelocity
();
motor
.
requestThrust
(
getH
orizontalMaxVelocityRequiredTotalThrust
());
velocity
=
getH
orizontalMaxVelocity
();
}
long
timeUntilReachedLocation
=
(
long
)
(
distanceToTargetPosition
/
currentSpeed
)
*
Time
.
SECOND
;
long
timeUntilReachedLocation
=
(
long
)
(
distanceToTargetPosition
/
velocity
)
*
Time
.
SECOND
;
target
=
route
.
getFirst
();
PositionVector
directionToTarget
=
new
PositionVector
(
target
);
...
...
@@ -128,7 +137,7 @@ public class MulticopterMovement implements UAVMovementModel {
directionToTarget
.
normalize
();
topologyComponent
.
updateCurrentDirection
(
directionToTarget
.
clone
());
directionToTarget
.
multiplyScalar
(
currentSpeed
*
timefactor
);
directionToTarget
.
multiplyScalar
(
velocity
*
timefactor
);
PositionVector
newPosition
=
new
PositionVector
(
position
);
newPosition
.
add
(
directionToTarget
);
...
...
@@ -146,13 +155,13 @@ public class MulticopterMovement implements UAVMovementModel {
double
timefactor
=
timeBetweenMovementOperations
/
Time
.
SECOND
;
// get to speed
if
(
target
Speed
>
0
&&
target
Speed
<
h
orizontalMaxVelocity
())
{
motor
.
requestThrust
(
estimateRequiredThrust
(
target
Speed
));
currentSpeed
=
target
Speed
;
if
(
target
Velocity
>
0
&&
target
Velocity
<
getH
orizontalMaxVelocity
())
{
motor
.
requestThrust
(
estimateRequiredThrust
(
target
Velocity
));
velocity
=
target
Velocity
;
}
else
{
motor
.
requestThrust
(
h
orizontalMaxVelocityRequiredTotalThrust
());
currentSpeed
=
h
orizontalMaxVelocity
();
motor
.
requestThrust
(
getH
orizontalMaxVelocityRequiredTotalThrust
());
velocity
=
getH
orizontalMaxVelocity
();
}
PositionVector
directionToTarget
=
new
PositionVector
(
target
);
...
...
@@ -164,7 +173,7 @@ public class MulticopterMovement implements UAVMovementModel {
topologyComponent
.
updateCurrentDirection
(
directionToTarget
.
clone
());
}
directionToTarget
.
multiplyScalar
(
currentSpeed
*
timefactor
);
directionToTarget
.
multiplyScalar
(
velocity
*
timefactor
);
PositionVector
newPosition
=
new
PositionVector
(
position
);
newPosition
.
add
(
directionToTarget
);
...
...
@@ -175,7 +184,7 @@ public class MulticopterMovement implements UAVMovementModel {
}
else
if
(
motor
.
isOn
())
{
if
(
currentSpeed
!=
0
)
{
if
(
velocity
!=
0
)
{
throw
new
UnsupportedOperationException
(
"no route but speed not 0?"
);
}
...
...
@@ -194,61 +203,62 @@ public class MulticopterMovement implements UAVMovementModel {
*
*/
p
ublic
double
verticalDescentMaxThrust
()
{
p
rotected
double
verticalDescentMaxThrust
()
{
// m * g - 0.5 * p * C * A * v^2
return
hoverThrustRequired
()
-
0.5
*
bodyDrag
(
0
,
new
PositionVector
(
0
,
0
,
1
))
*
des
centVelocity
Max
*
des
centVelocity
Max
;
return
hoverThrustRequired
()
-
0.5
*
bodyDrag
(
0
,
new
PositionVector
(
0
,
0
,
1
))
*
maximumDe
centVelocity
Allowed
*
maximumDe
centVelocity
Allowed
;
}
p
ublic
double
verticalAscentMaxAcceleration
()
{
p
rotected
double
verticalAscentMaxAcceleration
()
{
return
(
motor
.
getMaxThrust
()
-
hoverThrustRequired
())
/
mass
;
}
public
double
verticalAscentMaxVelocity
()
{
@Override
public
double
getVerticalAscentMaxVelocity
()
{
double
maxThrust
=
motor
.
getMaxThrust
();
return
Math
.
sqrt
(
2.0
*
(
maxThrust
-
hoverThrustRequired
())
/
bodyDrag
(
0
,
new
PositionVector
(
0
,
0
,
1
)));
}
p
ublic
double
hoverThrustRequired
()
{
return
mass
*
gravity
;
p
rotected
double
hoverThrustRequired
()
{
return
mass
*
GRAVITY
;
}
public
double
h
orizontalMaxVelocity
()
{
@Override
public
double
getH
orizontalMaxVelocity
()
{
double
horizontalThrust
=
h
orizontalComponentMaxThrust
();
double
horizontalThrust
=
getH
orizontalComponentMaxThrust
();
double
maxVelocity
=
Math
.
sqrt
(
(
2.0
*
horizontalThrust
)
/
bodyDrag
(
maxPitchAngle
,
new
PositionVector
(
1
,
0
,
0
)));
double
maxVelocity
=
Math
.
sqrt
(
(
2.0
*
horizontalThrust
)
/
bodyDrag
(
max
imum
PitchAngle
Allowed
,
new
PositionVector
(
1
,
0
,
0
)));
return
maxVelocity
;
}
p
ublic
double
h
orizontalComponentMaxThrust
()
{
p
rotected
double
getH
orizontalComponentMaxThrust
()
{
// hoverthrust / cos => amount of thrust in horizonal direction with °angle
double
stableAltitudeMaximumTotalThrust
=
h
orizontalMaxVelocityRequiredTotalThrust
();
double
stableAltitudeMaximumTotalThrust
=
getH
orizontalMaxVelocityRequiredTotalThrust
();
// fraction of total thrust in horizonal (forward) direction with °angle
double
maximumHorizontalThrustStableAltitude
=
stableAltitudeMaximumTotalThrust
*
Math
.
sin
(
maxPitchAngle
);
double
maximumHorizontalThrustStableAltitude
=
stableAltitudeMaximumTotalThrust
*
Math
.
sin
(
max
imum
PitchAngle
Allowed
);
return
maximumHorizontalThrustStableAltitude
;
}
p
ublic
double
h
orizontalMaxVelocityRequiredTotalThrust
()
{
return
hoverThrustRequired
()
/
Math
.
cos
(
maxPitchAngle
);
p
rotected
double
getH
orizontalMaxVelocityRequiredTotalThrust
()
{
return
hoverThrustRequired
()
/
Math
.
cos
(
max
imum
PitchAngle
Allowed
);
}
p
ublic
double
bodyDrag
(
double
angleRadians
,
PositionVector
direction
)
{
return
airdensity
*
dragCoefficient
*
areaExposedToDrag
(
angleRadians
,
direction
);
p
rotected
double
bodyDrag
(
double
angleRadians
,
PositionVector
direction
)
{
return
AIRDENSITY
*
dragCoefficient
*
areaExposedToDrag
(
angleRadians
,
direction
);
}
p
ublic
double
areaExposedToDrag
(
double
angleRadians
,
PositionVector
direction
)
{
p
rotected
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_t
op
+
Math
.
cos
(
angleRadians
)
*
A_f
ront
);
double
areaExposedTop
=
v
.
getY
()
*
(
Math
.
cos
(
angleRadians
)
*
A_t
op
+
Math
.
sin
(
angleRadians
)
*
A_f
ront
);
double
areaExposedFront
=
v
.
getX
()
*
(
Math
.
sin
(
angleRadians
)
*
areaT
op
+
Math
.
cos
(
angleRadians
)
*
areaF
ront
);
double
areaExposedTop
=
v
.
getY
()
*
(
Math
.
cos
(
angleRadians
)
*
areaT
op
+
Math
.
sin
(
angleRadians
)
*
areaF
ront
);
return
areaExposedFront
+
areaExposedTop
;
}
...
...
@@ -256,8 +266,8 @@ public class MulticopterMovement implements UAVMovementModel {
/*
* F_drag [N] = 0.5 * p * C_drag * A * v^2
*/
p
ublic
double
currentDrag
()
{
return
0.5
*
bodyDrag
(
currentAngleOfAttack
,
topologyComponent
.
getCurrentDirection
())
*
currentSpeed
*
currentSpeed
;
p
rotected
double
currentDrag
()
{
return
0.5
*
bodyDrag
(
currentAngleOfAttack
,
topologyComponent
.
getCurrentDirection
())
*
velocity
*
velocity
;
}
/**
...
...
@@ -267,7 +277,7 @@ public class MulticopterMovement implements UAVMovementModel {
* @param angleInRadians
* @return
*/
pr
ivate
double
forwardDrag
(
double
velocity
,
double
angleInRadians
)
{
pr
otected
double
forwardDrag
(
double
velocity
,
double
angleInRadians
)
{
return
0.5
*
bodyDrag
(
angleInRadians
,
new
PositionVector
(
1
,
0
,
0
))
*
velocity
*
velocity
;
}
...
...
@@ -278,20 +288,17 @@ public class MulticopterMovement implements UAVMovementModel {
@Override
public
void
setMotorControl
(
ActuatorComponent
motor
)
{
this
.
motor
=
(
StatelessActuatorComponent
)
motor
;
this
.
motor
=
(
StatelessActuatorComponent
)
motor
;
}
@Override
public
void
set
PreferredSpeed
(
double
v_pref
)
{
this
.
target
Speed
=
v_pref
;
public
void
set
TargetVelocity
(
double
v_pref
)
{
this
.
target
Velocity
=
v_pref
;
}
@Override
public
double
getCurrent
Speed
()
{
return
currentSpeed
;
public
double
getCurrent
Velocity
()
{
return
velocity
;
}
/**
...
...
@@ -350,43 +357,38 @@ public class MulticopterMovement implements UAVMovementModel {
}
@Override
public
double
h
orizontalMinVelocity
()
{
public
double
getH
orizontalMinVelocity
()
{
return
Math
.
sqrt
(
2
*
hoverThrustRequired
()
*
Math
.
tan
(
Math
.
toRadians
(
0.25
))
/
bodyDrag
(
Math
.
toRadians
(
0.25
),
new
PositionVector
(
1
,
0
,
0
)));
}
@Override
public
double
estimatePowerConsumption
(
double
velocity
)
{
public
double
estimatePowerConsumption
Watt
(
double
velocity
)
{
if
(
velocity
==
0
)
{
return
motor
.
estimatePowerConsumptionWatt
(
hoverThrustRequired
());
}
else
if
(
velocity
>
h
orizontalMaxVelocity
())
{
else
if
(
velocity
>
getH
orizontalMaxVelocity
())
{
return
-
1
;
}
else
if
(
velocity
<
h
orizontalMinVelocity
())
{
else
if
(
velocity
<
getH
orizontalMinVelocity
())
{
return
-
1
;
}
else
{
double
estimateAngle
=
estimatePitchAngleForVelocity
(
velocity
);
double
estimatedDrag
=
forwardDrag
(
velocity
,
estimateAngle
);
double
requiredThrust
=
Math
.
sqrt
(
hoverThrustRequired
()
*
hoverThrustRequired
()
+
estimatedDrag
*
estimatedDrag
);
double
wattage
=
motor
.
estimatePowerConsumptionWatt
(
requiredThrust
);
double
requiredThrust
=
estimateRequiredThrust
(
velocity
);
double
wattage
=
motor
.
estimatePowerConsumptionWatt
(
requiredThrust
);
return
wattage
;
}
}
}
p
ublic
double
estimateRequiredThrust
(
double
velocity
)
{
p
rotected
double
estimateRequiredThrust
(
double
velocity
)
{
if
(
velocity
==
0
)
{
return
motor
.
estimatePowerConsumptionWatt
(
hoverThrustRequired
());
}
else
if
(
velocity
>
h
orizontalMaxVelocity
())
{
else
if
(
velocity
>
getH
orizontalMaxVelocity
())
{
return
-
1
;
}
else
if
(
velocity
<
h
orizontalMinVelocity
())
{
else
if
(
velocity
<
getH
orizontalMinVelocity
())
{
return
-
1
;
}
else
{
...
...
@@ -404,14 +406,14 @@ public class MulticopterMovement implements UAVMovementModel {
* @param velocity
* @return
*/
pr
ivate
double
estimatePitchAngleForVelocity
(
double
velocity
)
{
pr
otected
double
estimatePitchAngleForVelocity
(
double
velocity
)
{
int
low
=
0
;
int
high
=
Integer
.
MAX_VALUE
;
double
vsquared
=
(
velocity
*
velocity
);
for
(
int
i
=
0
;
i
<=
((
int
)
Math
.
toDegrees
(
maxPitchAngle
));
i
++)
{
for
(
int
i
=
0
;
i
<=
((
int
)
Math
.
toDegrees
(
max
imum
PitchAngle
Allowed
));
i
++)
{
double
v2
=
2
*
hoverThrustRequired
()
*
Math
.
tan
(
Math
.
toRadians
(
i
))
/
bodyDrag
(
Math
.
toRadians
(
i
),
new
PositionVector
(
1
,
0
,
0
));
...
...
@@ -453,7 +455,7 @@ public class MulticopterMovement implements UAVMovementModel {
return
Math
.
toRadians
(
nearest
);
}
return
maxPitchAngle
;
return
max
imum
PitchAngle
Allowed
;
}
/**
...
...
@@ -463,8 +465,47 @@ public class MulticopterMovement implements UAVMovementModel {
* @version 1.0, 14.01.2020
*/
public
static
class
Factory
implements
AerialMovementModelFactory
{
private
double
massTotal
=
1.465
;
// kg
private
double
areaTop
=
0.245
;
// m^2
private
double
areaFront
=
0.1
;
// m^2
private
double
UAVDragCoefficient
=
0.7
;
private
double
maximumPitchAngleAllowed
=
Math
.
toRadians
(
60
);
// ° max angle
private
double
maximumDecentVelocityAllowed
=
5
;
// m/s
private
double
maximumTurnAngle
=
Math
.
toRadians
(
90
);
// 90° per second turn angle
public
void
setMassTotal
(
double
massTotal
)
{
this
.
massTotal
=
massTotal
;
}
public
void
setAreaTop
(
double
areaTop
)
{
this
.
areaTop
=
areaTop
;
}
public
void
setAreaFront
(
double
areaFront
)
{
this
.
areaFront
=
areaFront
;
}
public
void
setUAVDragCoefficient
(
double
uAVDragCoefficient
)
{
UAVDragCoefficient
=
uAVDragCoefficient
;
}
public
void
setMaximumPitchAngleAllowed
(
double
maximumPitchAngleAllowed
)
{
this
.
maximumPitchAngleAllowed
=
Math
.
toRadians
(
maximumPitchAngleAllowed
);
}
public
void
setMaximumDecentVelocityAllowed
(
double
maximumDecentVelocityAllowed
)
{
this
.
maximumDecentVelocityAllowed
=
maximumDecentVelocityAllowed
;
}
public
void
setMaximumTurnAngle
(
double
maximumTurnAngle
)
{
this
.
maximumTurnAngle
=
Math
.
toRadians
(
maximumTurnAngle
);
}
public
UAVMovementModel
createComponent
(
UAVTopologyComponent
topologyComponent
)
{
return
new
MulticopterMovement
(
topologyComponent
);
return
new
MulticopterMovement
(
topologyComponent
,
massTotal
,
areaTop
,
areaFront
,
UAVDragCoefficient
,
maximumPitchAngleAllowed
,
maximumDecentVelocityAllowed
,
maximumTurnAngle
);
}
}
}
src/de/tud/kom/p2psim/impl/topology/movement/aerial/SimpleMulticopterMovement.java
View file @
a860aadf
...
...
@@ -46,19 +46,22 @@ import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocation
public
class
SimpleMulticopterMovement
implements
UAVMovementModel
{
private
UAVTopologyComponent
topologyComponent
;
private
double
speed
;
private
double
target
Speed
;
private
double
velocity
;
private
double
target
Velocity
;
private
double
maximumHorizontal
Speed
=
15
;
private
double
maximumVertical
Speed
=
5
;
private
double
maximumHorizontal
Velocity
;
private
double
maximumVertical
Velocity
;
private
LinkedList
<
PositionVector
>
route
=
new
LinkedList
<>();
private
Map
<
PositionVector
,
ReachedLocationCallback
>
locationCallbacks
=
new
LinkedHashMap
<>();
private
StatefulActuatorComponent
motor
;
public
SimpleMulticopterMovement
(
UAVTopologyComponent
topologyComponent
)
{
this
.
topologyComponent
=
topologyComponent
;
public
SimpleMulticopterMovement
(
UAVTopologyComponent
topologyComponent
,
double
maximumHorizontalVelocity
,
double
maximumVerticalVelocity
)
{
this
.
topologyComponent
=
topologyComponent
;
this
.
maximumHorizontalVelocity
=
maximumHorizontalVelocity
;
this
.
maximumVerticalVelocity
=
maximumVerticalVelocity
;
}
@Override
...
...
@@ -72,12 +75,12 @@ public class SimpleMulticopterMovement implements UAVMovementModel {
Double
distanceToTargetPosition
=
targetPosition
.
distanceTo
(
currentPosition
);
// If target point is reached within a 1 meter margin, we remove that point from the list
if
(
distanceToTargetPosition
<
0.1
||
distanceToTargetPosition
<
speed
)
if
(
distanceToTargetPosition
<
0.1
||
distanceToTargetPosition
<
velocity
)
{
route
.
removeFirst
();
topologyComponent
.
updateCurrentLocation
(
targetPosition
);
// triggers energy consumption for last distance
speed
=
0
;
velocity
=
0
;
motor
.
useActuator
(
0
);
locationReached
(
targetPosition
);
return
;
...
...
@@ -85,12 +88,12 @@ public class SimpleMulticopterMovement implements UAVMovementModel {
double
timefactor
=
timeBetweenMovementOperations
/
Time
.
SECOND
;
speed
=
Math
.
min
(
distanceToTargetPosition
,
target
Speed
);
velocity
=
Math
.
min
(
distanceToTargetPosition
,
target
Velocity
);
PositionVector
direction
=
new
PositionVector
(
targetPosition
);
direction
.
subtract
(
currentPosition
);
direction
.
normalize
();
direction
.
multiplyScalar
(
speed
*
timefactor
);
direction
.
multiplyScalar
(
velocity
*
timefactor
);
PositionVector
newPosition
=
new
PositionVector
(
currentPosition
);
newPosition
.
add
(
direction
);
...
...
@@ -100,7 +103,7 @@ public class SimpleMulticopterMovement implements UAVMovementModel {
}
else
if
(
motor
.
isOn
())
{
if
(
speed
!=
0
)
{
if
(
velocity
!=
0
)
{
throw
new
UnsupportedOperationException
(
"no route but speed not 0?"
);
}
...
...
@@ -116,13 +119,13 @@ public class SimpleMulticopterMovement implements UAVMovementModel {
}
@Override
public
void
set
PreferredSpeed
(
double
v_pref
)
{
this
.
target
Speed
=
v_pref
;
public
void
set
TargetVelocity
(
double
v_pref
)
{
this
.
target
Velocity
=
v_pref
;
}
@Override
public
double
getCurrent
Speed
()
{
return
speed
;
public
double
getCurrent
Velocity
()
{
return
velocity
;
}
/**
...
...
@@ -181,15 +184,18 @@ public class SimpleMulticopterMovement implements UAVMovementModel {
}
@Override
public
double
estimatePowerConsumption
(
double
velocity
)
{
public
double
estimatePowerConsumptionWatt
(
double
velocity
)
{
double
wattage
=
0
;
if
(
velocity
==
0
)
{
return
motor
.
estimate
Power
Consumption
(
0
);
wattage
=
motor
.
estimate
Energy
Consumption
Watt
(
0
);
}
else
{
return
motor
.
estimate
Power
Consumption
(
1.0
);
else
{
wattage
=
motor
.
estimate
Energy
Consumption
Watt
(
1.0
);
}
return
wattage
;
}
@Override
...
...
@@ -198,17 +204,17 @@ public class SimpleMulticopterMovement implements UAVMovementModel {
}
@Override
public
double
v
erticalAscentMaxVelocity
()
{
return
maximumVertical
Speed
;
public
double
getV
erticalAscentMaxVelocity
()
{
return
maximumVertical
Velocity
;
}
@Override
public
double
h
orizontalMaxVelocity
()
{
return
maximumHorizontal
Speed
;
public
double
getH
orizontalMaxVelocity
()
{
return
maximumHorizontal
Velocity
;
}
@Override
public
double
h
orizontalMinVelocity
()
{
public
double
getH
orizontalMinVelocity
()
{
return
0
;
}
...
...
@@ -219,8 +225,21 @@ public class SimpleMulticopterMovement implements UAVMovementModel {
* @version 1.0, 14.01.2020
*/
public
static
class
Factory
implements
AerialMovementModelFactory
{
private
double
maximumHorizontalVelocity
=
15
;
public
void
setMaximumHorizontalVelocity
(
double
maximumHorizontalVelocity
)
{
this
.
maximumHorizontalVelocity
=
maximumHorizontalVelocity
;
}
private
double
maximumVerticalVelocity
=
5
;
public
void
setMaximumVerticalVelocity
(
double
maximumVerticalVelocity
)
{
this
.
maximumVerticalVelocity
=
maximumVerticalVelocity
;
}
public
UAVMovementModel
createComponent
(
UAVTopologyComponent
topologyComponent
)
{
return
new
SimpleMulticopterMovement
(
topologyComponent
);
}
return
new
SimpleMulticopterMovement
(
topologyComponent
,
maximumHorizontalVelocity
,
maximumVerticalVelocity
);
}
}
}
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment