def __init__(self, Ts): self._ts_simulation = Ts # set initial states based on parameter file # _state is the 13x1 internal state of the aircraft that is being propagated: # _state = [pn, pe, pd, u, v, w, e0, e1, e2, e3, p, q, r] # We will also need a variety of other elements that are functions of the _state and the wind. # self.true_state is a 19x1 vector that is estimated and used by the autopilot to control the aircraft: # true_state = [pn, pe, h, Va, alpha, beta, phi, theta, chi, p, q, r, Vg, wn, we, psi, gyro_bx, gyro_by, gyro_bz] self._state = np.array([ [MAV.pn0], # (0) [MAV.pe0], # (1) [MAV.pd0], # (2) [MAV.u0], # (3) [MAV.v0], # (4) [MAV.w0], # (5) [MAV.phi0], # (6) [MAV.theta0], # (7) [MAV.psi0], # (8) [MAV.p0], # (9) [MAV.q0], # (10) [MAV.r0] ]) # (11) # store wind data for fast recall since it is used at various points in simulation self._wind = np.array([[0.], [0.], [0.]]) # wind in NED frame in meters/sec self._update_velocity_data() # store forces to avoid recalculation in the sensors function self._forces = np.array([[], [], []]) self._Va = math.sqrt( self._state.item(3)**2 + self._state.item(4)**2 + self._state.item(5)**2) self._alpha = np.arctan2(self._state.item(5), self._state.item(3)) self._beta = np.arcsin(self._state.item(4) / self._Va) # initialize true_state message self.true_state = msgState()
def __init__(self, ts_control): # instantiate lateral controllers self.roll_from_aileron = pdControlWithRate(kp=AP.roll_kp, kd=AP.roll_kd, limit=np.radians(45)) self.course_from_roll = piControl(kp=AP.course_kp, ki=AP.course_ki, Ts=ts_control, limit=np.radians(30)) self.yaw_damper = matlab.tf([0.5, 0.], [ 1.0, ], ts_control) # # num=np.array([[AP.yaw_damper_kp, 0]]), # den=np.array([[1, 1/AP.yaw_damper_tau_r]]), # Ts=ts_control) # instantiate lateral controllers self.pitch_from_elevator = pdControlWithRate(kp=AP.pitch_kp, kd=AP.pitch_kd, limit=np.radians(45)) self.altitude_from_pitch = piControl(kp=AP.altitude_kp, ki=AP.altitude_ki, Ts=ts_control, limit=np.radians(30)) self.airspeed_from_throttle = piControl(kp=AP.airspeed_throttle_kp, ki=AP.airspeed_throttle_ki, Ts=ts_control, limit=1.0) self.commanded_state = msgState()
def __init__(self, ts_control): # instantiate lateral controllers self.aileron_from_roll = pidControl(kp=AP.roll_kp, ki=AP.roll_ki, kd=AP.roll_kd, Ts=ts_control, sigma=0.05, low_limit=-AP.delta_a_max, high_limit=AP.delta_a_max) self.roll_from_course = pidControl(kp=AP.course_kp, ki=AP.course_ki, kd=0, Ts=ts_control, sigma=0.05, low_limit=-np.pi / 7.2, high_limit=np.pi / 7.2) # instantiate longitudinal controllers self.elevator_from_pitch = pidControl(kp=AP.pitch_kp, ki=0, kd=AP.pitch_kd, Ts=ts_control, sigma=0.05, low_limit=-AP.delta_e_max, high_limit=AP.delta_e_max) self.throttle_from_airspeed = pidControl(kp=AP.airspeed_throttle_kp, ki=AP.airspeed_throttle_ki, kd=0, Ts=ts_control, sigma=0.05, low_limit=0, high_limit=AP.throttle_max) self.pitch_from_airspeed = pidControl(kp=AP.airspeed_pitch_kp, ki=AP.airspeed_pitch_ki, kd=0, Ts=ts_control, sigma=0.05, low_limit=-np.pi / 6, high_limit=np.pi / 6) self.pitch_from_altitude = pidControl(kp=AP.altitude_kp, ki=AP.altitude_ki, kd=0, Ts=ts_control, sigma=0.05, low_limit=-np.pi / 6, high_limit=np.pi / 6) # inicialize message self.commanded_state = msgState() self.delta = msgDelta()
def __init__(self, Ts): # OLD STUFF here # initialize the sensors message self._sensors = msgSensors() # random walk parameters for GPS self._gps_eta_n = 0. self._gps_eta_e = 0. self._gps_eta_h = 0. # timer so that gps only updates every ts_gps seconds self._t_gps = 999. # large value ensures gps updates at initial time. # update velocity data and forces and moments self._ts_simulation = Ts # set initial states based on parameter file # _state is the 13x1 internal state of the aircraft that is being propagated: # _state = [pn, pe, pd, u, v, w, e0, e1, e2, e3, p, q, r] # We will also need a variety of other elements that are functions of the _state and the wind. # self.true_state is a 19x1 vector that is estimated and used by the autopilot to control the aircraft: # true_state = [pn, pe, h, Va, alpha, beta, phi, theta, chi, p, q, r, Vg, wn, we, psi, gyro_bx, gyro_by, gyro_bz] self._state = np.array([ [MAV.pn0], # (0) [MAV.pe0], # (1) [MAV.pd0], # (2) [MAV.u0], # (3) [MAV.v0], # (4) [MAV.w0], # (5) [MAV.e0], # (6) [MAV.e1], # (7) [MAV.e2], # (8) [MAV.e3], # (9) [MAV.p0], # (10) [MAV.q0], # (11) [MAV.r0] ]) # (12) # store wind data for fast recall since it is used at various points in simulation self._wind = np.array([[0.], [0.], [0.]]) # wind in NED frame in meters/sec self._forces = np.array([[0], [0], [0]]) self._update_velocity_data() # store forces to avoid recalculation in the sensors function self._forces_moments(delta=np.array([[0.0], [0.0], [0.0], [0.5]])) self._Va = MAV.u0 self._alpha = 0 self._beta = 0 # initialize true_state message self.true_state = msgState()
def __init__(self, ts_control): # initialized estimated state message self.estimated_state = msgState() # use alpha filters to low pass filter gyros and accels self.lpf_gyro_x = alpha_filter(alpha=0.5) self.lpf_gyro_y = alpha_filter(alpha=0.5) self.lpf_gyro_z = alpha_filter(alpha=0.5) self.lpf_accel_x = alpha_filter(alpha=0.9) self.lpf_accel_y = alpha_filter(alpha=0.9) self.lpf_accel_z = alpha_filter(alpha=0.9) # use alpha filters to low pass filter static and differential pressure self.lpf_static = alpha_filter(alpha=0.9) self.lpf_diff = alpha_filter(alpha=0.5) # ekf for phi and theta self.attitude_ekf = ekf_attitude() # ekf for pn, pe, Vg, chi, wn, we, psi self.position_ekf = ekf_position()
def __init__(self, Ts): self.ts_simulation = Ts # set initial states based on parameter file # _state is the 13x1 internal state of the aircraft that is being propagated: # _state = [pn, pe, pd, u, v, w, e0, e1, e2, e3, p, q, r] self._state = np.array([[MAV.pn0], # (0) [MAV.pe0], # (1) [MAV.pd0], # (2) [MAV.u0], # (3) [MAV.v0], # (4) [MAV.w0], # (5) [MAV.e0], # (6) [MAV.e1], # (7) [MAV.e2], # (8) [MAV.e3], # (9) [MAV.p0], # (10) [MAV.q0], # (11) [MAV.r0]]) # (12) self.true_state = msgState()
""" mavSimPy - Chapter 2 assignment for Beard & McLain, PUP, 2012 - Update history: 2/24/2020 - RWB """ import sys sys.path.append('..') from mav_viewer import mavViewer import parameters.simulation_parameters as SIM from message_types.msg_state import msgState # initialize messages state = msgState() # instantiate state message # initialize viewers and video VIDEO = False # True==write video, False==don't write video mav_view = mavViewer() if VIDEO is True: from video_writer import videoWriter video = videoWriter(video_name="chap2_video.avi", bounding_box=(0, 0, 1000, 1000), output_rate=SIM.ts_video) # initialize the simulation time sim_time = SIM.start_time # main simulation loop while sim_time < SIM.end_time: # -------vary states to check viewer------------- if sim_time < SIM.end_time / 6: