from dynamics_linear.compute_models import compute_model
from tools.signals import signals
# initialize the visualization
VIDEO = False # True==write video, False==don't write video
mav_view = mavViewer() # initialize the mav viewer
data_view = dataViewer() # initialize view of data plots
if VIDEO is True:
from video.video_writer import videoWriter
video = videoWriter(video_name="chap5_video.avi",
bounding_box=(0, 0, 1000, 1000),
output_rate=SIM.ts_video)
# initialize elements of the architecture
wind = windSimulation(SIM.ts_simulation)
mav = mavDynamics(SIM.ts_simulation)
# use compute_trim function to compute trim state and trim input
Va = 25.
gamma = 10.*np.pi/180.
trim_state, trim_input = compute_trim(mav, Va, gamma)
mav._state = trim_state # set the initial state of the mav to the trim state
delta = trim_input # set input to constant constant trim input
# # compute the state space model linearized about trim
compute_model(mav, trim_state, trim_input)
# this signal will be used to excite modes
input_signal = signals(amplitude=.05,
duration=0.01,
start_time=2.0)
#sensor_view = SensorViewer() # initialize view of sensor data plots
#obsv = observer(SIM.ts_simulation)
path_follow = path_follower()
logger = log('Test flight.txt')
# path definition
from message_types.msg_path import msgPath
from message_types.msg_state import msgState
from message_types.msg_delta import msgDelta
path = msgPath()
state = msgState() # instantiate state message
delta = msgDelta()
commandWindow = autopilotCommand()
commands = msgAutopilot()
wind = windSimulation(SIM.ts_simulation)
wind._steady_state = np.array([[5., 2., 0.]]).T # Steady wind in NED frame
mav = mavDynamics(SIM.ts_simulation, localIP, raspIP)
# path.type = 'line'
path.type = 'orbit'
if path.type == 'line':
path.line_origin = np.array([[0.0, 0.0, -100.0]]).T
path.line_direction = np.array([[0.5, 1.0, -0.05]]).T
path.line_direction = path.line_direction / np.linalg.norm(
path.line_direction)
path.airspeed = 35
else: # path.type == 'orbit'
path.orbit_center = np.array([[0.0, 0.0, -100.0]]).T # center of the orbit
path.orbit_radius = 500.0 # radius of the orbit
path.orbit_direction = 'CW' # orbit direction: 'CW'==clockwise, 'CCW'==counter clockwise
path.airspeed = 35
# initialize the simulation time