from tools.signals import signals
# initialize the visualization
mav_view = mav_viewer() # initialize the mav viewer
data_view = data_viewer() # initialize view of data plots
# initialize elements of the architecture
wind = wind_simulation(SIM.ts_simulation)
mav = mav_dynamics(SIM.ts_simulation)
# use compute_trim function to compute trim state and trim input
Va0 = 25
gamma = 0 #10.0*np.pi/180.
delta0 = np.array([[0], [0], [0],
[0.5]]) # [delta_e, delta_a, delta_r, delta_t]
trim_state, trim_input = compute_trim(mav, delta0, Va0, gamma)
mav.set_state(trim_state) # set the initial state of the mav to the trim state
delta = trim_input # set input to constant trim input
# # compute the state space model linearized about trim
compute_model(trim_state, trim_input)
# this signal will be used to excite modes
input_signal = signals(amplitude=.05, duration=0.01, start_time=2.0)
# initialize the simulation time
sim_time = SIM.start_time
# main simulation loop
print("Press Command-Q to exit...")
while sim_time < SIM.end_time:
mav_view = mavViewer() # initialize the mav viewer
data_view = dataViewer() # initialize view of data plots
if VIDEO is True:
from chap2.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 = 15. * 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)
# initialize the simulation time
sim_time = SIM.start_time
# main simulation loop
print("Press Command-Q to exit...")
while sim_time < SIM.end_time:
mav_view = mav_viewer() # initialize the mav viewer
data_view = data_viewer() # initialize view of data plots
# initialize elements of the architecture
wind = wind_simulation(SIM.ts_simulation)
mav = mav_dynamics(SIM.ts_simulation)
# use compute_trim function to compute trim state and trim input
Va = 25.
gamma = 0. * np.pi / 180.
R = np.inf
display_transfer = True
display_ss = True
display_trim = True
trim_state, trim_input = compute_trim(mav, Va, gamma, R, display=display_trim)
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
A_lon, B_lon, A_lat, B_lat = compute_ss_model(mav,
trim_state,
trim_input,
display=display_transfer)
T_phi_delta_a, T_chi_phi, T_theta_delta_e, T_h_theta, \
T_h_Va, T_Va_delta_t, T_Va_theta, T_beta_delta_r \
= compute_tf_model(mav, trim_state, trim_input, display=display_ss)
# initialize the simulation time
sim_time = SIM.start_time
h = 0.001
f_f, _ = mav.calcMotorDynamics(Va, delta_t + h)
f_b, _ = mav.calcMotorDynamics(Va, delta_t - h)
dThrust = (f_f - f_b) / (2.0 * h)
return dThrust
if __name__ == "__main__":
from chap4.mav_dynamics import mav_dynamics
from chap5.trim import compute_trim
dyn = mav_dynamics(0.02)
Va = 25.
gamma = 0. * np.pi / 180.0
trim_state, trim_input = compute_trim(dyn, Va, gamma)
dyn._state = trim_state
# trim_euler = euler_state(trim_state)
# trim_quat = quaternion_state(trim_euler)
# print('trim: \n',trim_state)
# print('euler: \n',trim_euler)
# print('quat: \n',trim_quat)
# A_lat = getALat(dyn,trim_state,trim_input)
# A_lon, B_lon, A_lat, B_lat = compute_ss_model(dyn, trim_state, trim_input)
# print('A_lon: \n',A_lon)
# print('B_lon: \n',B_lon)
# print('A_lat: \n',A_lat)
# print('B_lat: \n',B_lat)
frequency=0.05)
h_command = signals(dc_offset=100.0,
amplitude=30.0,
start_time=0.0,
frequency=0.02)
chi_command = signals(dc_offset=np.radians(0.0),
amplitude=np.radians(200),
start_time=5.0,
frequency=0.05)
# initialize the simulation time
sim_time = SIM.start_time
# main simulation loop
print("Press Command-Q to exit...")
trim_state, trim_input = compute_trim(mav, 25.0, 0.0) #git rid of this
while sim_time < SIM.end_time:
#-------controller-------------
estimated_state = mav.msg_true_state # uses true states in the control
commands.airspeed_command = Va_command.square(sim_time)
commands.course_command = chi_command.square(sim_time)
commands.altitude_command = h_command.square(sim_time)
delta, commanded_state = ctrl.update(commands, estimated_state)
#do I need to include a command state for roll feed forward?
#-------physical system-------------
current_wind = wind.update() # get the new wind vector
# current_wind = np.zeros((6, 1)) #get rid of this
input = np.array(
[[delta.item(0),
# initialize the visualization
VIDEO = False # True==write video, False==don't write video
mav_view = mav_viewer() # initialize the mav viewer
data_view = data_viewer() # initialize view of data plots
if VIDEO == True:
from chap2.video_writer import video_writer
video = video_writer(video_name="chap6_video.avi",
bounding_box=(0, 0, 1000, 1000),
output_rate=SIM.ts_video)
# initialize elements of the architecture
wind = wind_simulation(SIM.ts_simulation)
mav = mav_dynamics(SIM.ts_simulation)
ctrl = autopilot(SIM.ts_simulation)
trim_state, trim_input = compute_trim(mav, 30, 10.*np.pi/180)
# autopilot commands
from message_types.msg_autopilot import msg_autopilot
commands = msg_autopilot()
Va_command = signals(dc_offset=25.0, amplitude=3.0, start_time=2.0, frequency = 0.01)
h_command = signals(dc_offset=100.0, amplitude=10.0, start_time=0.0, frequency = 0.02)
chi_command = signals(dc_offset=np.radians(0), amplitude=np.radians(45), start_time=5.0, frequency = 0.015)
# initialize the simulation time
sim_time = SIM.start_time
# main simulation loop
print("Press Command-Q to exit...")
while sim_time < SIM.end_time:
