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()
if VIDEO is True:
from video.video_writer import videoWriter
video = videoWriter(video_name="chap4_video.avi",
bounding_box=(0, 0, 1000, 1000),
output_rate=SIM.ts_video)
# initialize elements of the architecture
wind = windSimulation(SIM.ts_simulation)
wind._steady_state = np.array([[5., 2., 0.]]).T # Steady wind in NED frame
mav = mavDynamics(SIM.ts_simulation)
# initialize the simulation time
sim_time = SIM.start_time
plot_time = sim_time
delta = msgDelta()
# main simulation loop
while sim_time < SIM.end_time:
delta_e, delta_a, delta_r, delta_t, autopilot = controlJoystick.getInputs()
# -------set control surfaces-------------
#delta_e += 0.001 * (np.random.random() - 0.5)
#delta_a += 0.001 * (np.random.random() - 0.5)
#delta_r += 0.001 * (np.random.random() - 0.5)
#delta_t += 0.001 * (np.random.random() - 0.5)
delta.from_array(np.array([[delta_e, delta_a, delta_r, delta_t]]).T)
# transpose to make it a column vector
# -------physical system-------------
current_wind = wind.update() # get the new wind vector
mav.update(delta, current_wind) # propagate the MAV dynamics