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: