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, MAV.pe0, MAV.pd0, MAV.u0, MAV.v0, MAV.w0, MAV.e0, MAV.e1,
MAV.e2, MAV.e3, MAV.p0, MAV.q0, MAV.r0
])
self._state = self._state.reshape((13, 1))
self.msg_true_state = StateMsg()
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, MAV.pe0, MAV.pd0, MAV.u0, MAV.v0, MAV.w0,
MAV.e0, MAV.e1, MAV.e2, MAV.e3, MAV.p0, MAV.q0, MAV.r0])
self._state = self._state.reshape((13, 1))
self._wind = np.zeros((3, 1))
self.updateVelocityData()
#store the forces
self._forces = np.zeros((3, 1))
self._Va = MAV.u0
self._alpha = 0
self._beta = 0
#true state
self.msg_true_state = StateMsg()
def __init__(self, ts_control):
#Can i just use the PID control class with 0 for certain gain?
# instantiate lateral controllers
self.roll_from_aileron = pid_control( #was pd_control_with_rate
kp=AP.roll_kp,
kd=AP.roll_kd,
limit_h=np.radians(45),
limit_l=-np.radians(45))
self.course_from_roll = pid_control( # was pi
kp=AP.course_kp,
ki=AP.course_ki,
Ts=ts_control,
limit_h=np.radians(30),
limit_l=-np.radians(30))
self.sideslip_from_rudder = pid_control( # was pi
kp=AP.sideslip_kp,
ki=AP.sideslip_ki,
Ts=ts_control,
limit_h=np.radians(45),
limit_l=-np.radians(45))
self.yaw_damper = transfer_function(
num=np.array([[AP.yaw_damper_kp, 0]]),
den=np.array([[1, 1 / AP.yaw_damper_tau_r]]),
Ts=ts_control)
# instantiate longitudinal controllers
self.pitch_from_elevator = pid_control( # was pd_control with rate
kp=AP.pitch_kp,
kd=AP.pitch_kd,
limit_h=np.radians(45),
limit_l=-np.radians(45))
self.altitude_from_pitch = pid_control( # was pi
kp=AP.altitude_kp,
ki=AP.altitude_ki,
Ts=ts_control,
limit_h=np.radians(30),
limit_l=-np.radians(30))
self.airspeed_from_throttle = pid_control( # was pi
kp=AP.airspeed_throttle_kp,
ki=AP.airspeed_throttle_ki,
Ts=ts_control,
limit_h=1.0,
limit_l=0.0)
self.commanded_state = StateMsg()
def __init__(self, ts_control):
# initialized estimated state message
self.estimated_state = StateMsg()
# 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.5)
self.lpf_accel_y = alpha_filter(alpha=0.5)
self.lpf_accel_z = alpha_filter(alpha=0.5)
# 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
# _state = [pn, pe, pd, u, v, w, e0, e1, e2, e3, p, q, r]
self._state = np.array([
MAV.pn0, MAV.pe0, MAV.pd0, MAV.u0, MAV.v0, MAV.w0, MAV.e0, MAV.e1,
MAV.e2, MAV.e3, MAV.p0, MAV.q0, MAV.r0
])
self._state = self._state.reshape((13, 1))
self._wind = np.zeros((3, 1))
self.updateVelocityData()
#store the forces
self._forces = np.zeros((3, 1))
self._Va = MAV.u0
self._alpha = 0
self._beta = 0
#true state
self.msg_true_state = StateMsg()
self.sensors = msg_sensors()
self._gps_eta_n = 0.0
self._gps_eta_e = 0.0
self._gps_eta_h = 0.0
self._t_gps = 999. #timer so that gps only updates every ts_gps seconds
from mav_viewer import MAV_Viewer
# from video_writer import video_writer
#import the dynamics
from chp3.mav_dynamics import mav_dynamics as Dynamics
# import parameters
import parameters.sim_params as SIM
# import message types
from messages.state_msg import StateMsg
import numpy as np
# initialize messages
state = StateMsg() # instantiate state message
# initialize dynamics object
dyn = Dynamics(SIM.ts_sim)
# initialize viewers and video
VIDEO = False # True==write video, False==don't write video
mav_view = MAV_Viewer()
if VIDEO == True:
video = video_writer(video_name="chap3_video.avi",
bounding_box=(0, 0, 1000, 1000),
output_rate=SIM.ts_video)
# initialize the simulation time
sim_time = SIM.t0
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)
mav_view = MAV_Viewer()
data_view = data_viewer()
# initialize the simulation time
sim_time = SIM.t0
temp = StateMsg()
# main simulation loop
print("Press Ctrl-Q to exit...")
while sim_time < SIM.t_end:
#-------get commanded values-------------
commands.airspeed_command = Va_command.square(sim_time)
commands.course_command = chi_command.square(sim_time)
commands.altitude_command = h_command.square(sim_time)
#-----------controller---------------------
measurements = dyn.sensors
estimated_state = obsv.update(measurements)
delta, commanded_state = ctrl.update(commands, estimated_state)
#------------Physical System----------------------