# 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)
ctrl = autopilot(SIM.ts_simulation)
obsv = observer(SIM.ts_simulation)
# 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.,
frequency=0.02)
chi_command = signals(dc_offset=np.radians(180),amplitude=np.radians(45), \
start_time=5.0, frequency = 0.015)
# initialize the simulation time
sim_time = SIM.start_time
# main simulation loop
while sim_time < SIM.end_time:
#-----autopilot commands-------
commands.airspeed_command = Va_command.square(sim_time)
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)
# initialize the simulation time
sim_time = SIM.start_time
# main simulation loop
print("Press Command-Q to exit...")
while sim_time < SIM.end_time:
# -------physical system-------------
current_wind = np.zeros((6,1))
# this input excites the phugoid mode by adding an impulse at t=5.0
# delta[0][0] += input_signal.impulse(sim_time)
mav.update(delta, current_wind) # propagate the MAV dynamics
# initialize the simulation time
sim_time = SIM.start_time
# main simulation loop
print("Press Command-Q to exit...")
while sim_time < SIM.end_time:
if sim_time < 5:
brakes_on = True
at_rest = True
commands.airspeed_command = 0
commands.altitude_command = 0
elif 5 < sim_time < 70:
at_rest = False
if commands.airspeed_command < 20:
Va_command = signals(dc_offset=0.0, amplitude=1.0, start_time=5.0,
frequency=0.01)
commands.airspeed_command = Va_command.sawtooth(sim_time)
else:
commands.airspeed_command = 20
commands.altitude_command = 100
else:
commands.airspeed_command = 0
# commands.altitude_command = 0
if commands.altitude_command > 0:
h_command = signals(dc_offset=100.0, amplitude=-1.0, start_time=70.0,
frequency=0.01)
commands.altitude_command = h_command.sawtooth(sim_time)
else:
commands.altitude_command = 0
#-------controller-------------
T0 = 0
Tf = 10
n = int(np.floor((Tf - T0) / dt))
alpha = [uav._alpha * 180 / np.pi]
beta = [uav._beta * 180 / np.pi]
phi = [MAV.phi0 * 180 / np.pi]
theta = [MAV.theta0 * 180 / np.pi]
psi = [MAV.psi0 * 180 / np.pi]
gamma = [uav.true_state.gamma * 180 / np.pi]
t_history = [T0]
elev_doublet = sigs.signals(amplitude=0.2,
frequency=1.0,
start_time=1,
duration=1,
dc_offset=chap5.u_trim[0])
for ii in range(n):
delta_e = elev_doublet.doublet(dt * ii)
delta = np.array(
[delta_e, chap5.u_trim[1], chap5.u_trim[2], chap5.u_trim[3]])
wind = np.array([[0.], [0.], [0.], [0.], [0.], [0.]])
uav.update(delta, wind)
alpha.append(uav._alpha * 180 / np.pi)
beta.append(uav._beta * 180 / np.pi)
t_history.append(ii * dt)
phi_t, theta_t, psi_t = Quaternion2Euler(uav._state[6:10])
phi.append(phi_t * 180 / np.pi)
theta.append(theta_t * 180 / np.pi)
psi.append(psi_t * 180 / np.pi)
# autopilot commands
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(180),
# amplitude=np.radians(45),
# start_time=5.0,
# frequency=0.015)
Va_command = signals(dc_offset=25.0, amplitude=10.0, start_time=2.0, frequency = 0.02)
h_command = signals(dc_offset=100.0, amplitude=20.0, start_time=0.0, frequency = 0.02)
chi_command = signals(dc_offset=np.radians(0), amplitude=np.radians(200), start_time=5.0, frequency = 0.02)
# initialize the simulation time
sim_time = SIM.start_time
#Make initial trim conditions
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)
flag = False
# main simulation loop
print("Press Command-Q to exit...")
