# 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...")