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:
