182 lines
6.3 KiB
Python
182 lines
6.3 KiB
Python
import numpy as np
|
|
import sys
|
|
|
|
"""
|
|
Below are some default wind objects that might be useful inputs to the system.
|
|
"""
|
|
|
|
class NoWind(object):
|
|
"""
|
|
This wind profile is the trivial case of no wind. It will output
|
|
zero wind speed on all axes for all time.
|
|
Alternatively, you can use ConstantWind with wx=wy=wz=0.
|
|
"""
|
|
|
|
def __init__(self):
|
|
"""
|
|
Inputs:
|
|
Nothing
|
|
"""
|
|
|
|
def update(self, t, position):
|
|
"""
|
|
Given the present time and position of the multirotor, return the
|
|
current wind speed on all three axes.
|
|
|
|
The wind should be expressed in the world coordinates.
|
|
"""
|
|
return np.array([0,0,0])
|
|
|
|
class ConstantWind(object):
|
|
"""
|
|
This wind profile is constant both spatially and temporally.
|
|
Wind speed is specified on each axis.
|
|
"""
|
|
|
|
def __init__(self, wx, wy, wz):
|
|
"""
|
|
"""
|
|
|
|
self.wind = np.array([wx, wy, wz])
|
|
|
|
|
|
def update(self, t, position):
|
|
"""
|
|
Given the present time and position of the multirotor, return the
|
|
current wind speed on all three axes.
|
|
|
|
The wind should be expressed in the world coordinates.
|
|
"""
|
|
|
|
return self.wind
|
|
|
|
class SinusoidWind(object):
|
|
"""
|
|
Wind will vary subject to a sine function with appropriate amplitude, frequency, and phase offset.
|
|
"""
|
|
|
|
def __init__(self, amplitudes=np.array([1,1,1]), frequencies=np.array([1,1,1]), phase=np.array([0,0,0])):
|
|
"""
|
|
Inputs:
|
|
amplitudes := array of amplitudes on each axis
|
|
frequencies := array of frequencies for the wind pattern on each axis
|
|
phase := relative phase offset on each axis in seconds
|
|
"""
|
|
self.Ax, self.Ay, self.Az = amplitudes[0], amplitudes[1], amplitudes[2]
|
|
self.fx, self.fy, self.fz = frequencies[0], frequencies[1], frequencies[2]
|
|
self.px, self.py, self.pz = phase[0], phase[1], phase[2]
|
|
|
|
def update(self, t, position):
|
|
"""
|
|
Given the present time and position of the multirotor, return the
|
|
current wind speed on all three axes.
|
|
|
|
The wind should be expressed in the world coordinates.
|
|
"""
|
|
|
|
wind = np.array([self.Ax*np.sin(2*np.pi*self.fx*(t+self.px)),
|
|
self.Ay*np.sin(2*np.pi*self.fy*(t+self.py)),
|
|
self.Az*np.sin(2*np.pi*self.fz*(t+self.pz))])
|
|
|
|
return wind
|
|
|
|
class LadderWind(object):
|
|
"""
|
|
The wind will step up and down between a minimum and maximum speed for a specified duration.
|
|
Visualized below...
|
|
|
|
| | <- duration
|
|
max -> --- ---
|
|
--- ---
|
|
--- ---
|
|
min -> --- ---
|
|
--------------------------> t
|
|
|
|
** Normally the wind will start at min and increase sequentially, but if the random flag is set true,
|
|
the wind will step to a random sublevel after each duration is up.
|
|
|
|
"""
|
|
|
|
def __init__(self, min=np.array([-1,-1,-1]), max=np.array([1,1,1]), duration=np.array([1,1,1]), Nstep=np.array([5,5,5]), random_flag=False):
|
|
"""
|
|
Inputs:
|
|
min := array of minimum wind speeds across each axis
|
|
max := array of maximum wind speeds across each axis
|
|
duration := array of durations for each step
|
|
Nstep := array for the integer number of discretized steps between min and max across each axis
|
|
start_step :=
|
|
"""
|
|
|
|
# Check the inputs for consistency, quit and raise a flag if the inputs aren't physically realizable
|
|
if np.any(Nstep <= 0):
|
|
print("LadderWind Error: The number of steps must be greater than or equal to 1")
|
|
sys.exit(1)
|
|
|
|
if np.any(max - min < 0):
|
|
print("LadderWind Error: The max value must be greater than the min value.")
|
|
sys.exit(1)
|
|
|
|
self.random_flag = random_flag
|
|
self.durx, self.dury, self.durz = duration[0], duration[1], duration[2]
|
|
self.nx, self.ny, self.nz = Nstep[0], Nstep[1], Nstep[2]
|
|
|
|
# Compute arrays of intermediate wind speeds for each axis
|
|
self.wx_arr = np.linspace(min[0], max[0], self.nx)
|
|
self.wy_arr = np.linspace(min[1], max[1], self.ny)
|
|
self.wz_arr = np.linspace(min[2], max[2], self.nz)
|
|
|
|
# Initialize the amplitude id.. these numbers are used to index the arrays above to get the appropriate wind speed on each axis
|
|
if self.random_flag:
|
|
self.xid = np.random.choice(self.nx)
|
|
self.yid = np.random.choice(self.ny)
|
|
self.zid = np.random.choice(self.nz)
|
|
else:
|
|
self.xid, self.yid, self.zid = 0, 0, 0
|
|
|
|
# Initialize the timers... since we don't yet know the starting time, we'll set them in the first call
|
|
self.timerx = None
|
|
self.timery = None
|
|
self.timerz = None
|
|
|
|
# Initialize the winds
|
|
self.wx, self.wy, self.wz = self.wx_arr[self.xid], self.wy_arr[self.yid], self.wz_arr[self.zid]
|
|
|
|
def update(self, t, position):
|
|
"""
|
|
Given the present time and position of the multirotor, return the
|
|
current wind speed on all three axes.
|
|
|
|
The wind should be expressed in the world coordinates.
|
|
"""
|
|
if self.timerx is None:
|
|
self.timerx, self.timery, self.timerz = t, t, t
|
|
|
|
if (t - self.timerx) >= self.durx:
|
|
if self.random_flag:
|
|
self.xid = np.random.choice(self.nx)
|
|
else:
|
|
self.xid = (self.xid + 1) % self.nx
|
|
self.wx = self.wx_arr[self.xid]
|
|
self.timerx = t
|
|
|
|
if (t - self.timery) >= self.dury:
|
|
if self.random_flag:
|
|
self.yid = np.random.choice(self.ny)
|
|
else:
|
|
self.yid = (self.yid + 1) % self.ny
|
|
self.wy = self.wy_arr[self.yid]
|
|
self.timery = t
|
|
|
|
if (t - self.timerz) >= self.durz:
|
|
if self.random_flag:
|
|
self.zid = np.random.choice(self.nz)
|
|
else:
|
|
self.zid = (self.zid + 1) % self.nz
|
|
self.wz = self.wz_arr[self.zid]
|
|
self.timerz = t
|
|
|
|
return np.array([self.wx, self.wy, self.wz])
|
|
|
|
if __name__=="__main__":
|
|
wind = ConstantWind(wx=1,wy=1,wz=1)
|
|
print(wind.update(0,np.array([0,0,0]))) |