def __init__(self, chapter):
self.chapter = chapter
# The current state of the UAV
self.state = np.array([
[P.Pn0], # (0)
[P.Pe0], # (1)
[P.Pd0], # (2)
[P.u0], # (3)
[P.v0], # (4)
[P.w0], # (5)
[P.e0], # (6)
[P.e1], # (7)
[P.e2], # (8)
[P.e3], # (9)
[P.p0], # (10)
[P.q0], # (11)
[P.r0]
]) # (12)
# The flight state of the UAV
self.flight_state = np.array([[P.Va0], [P.alpha0], [P.beta0],
[P.path_angle0], [P.heading0]])
# The forces that are applied to the body of the UAV
self.bodyForces = np.array([[P.Fx], [P.Fy], [P.Fz], [P.Ml], [P.Mm],
[P.Mn]])
# The state of the control surfaces
self.controlState = np.array([[P.deltaA0], [P.deltaE0], [P.deltaR0],
[P.deltaT0]])
# The commanded aircraft path following values
self.autopilot = np.array([[P.Va0], [P.altitude0], [P.heading0]])
# self.autopilot = np.matrix([[P.Va0],
# [P.altitude0],
# [3.14159]])
# The attitude of the UAV in euler angles (phi, theta, psi)
self.eulerAttitude = Quaternion2Euler(self.state[6:10])
self.euler_state = self.euler_states_array()
self.kinematics = Dynamics()
self.windsim = WindSimulation()
self.forces = Forces()
self.lat_control = lateral_control()
self.lon_control = longitudinal_control()
self.sensors = Sensors()
self.gps = Gps()
self.va = np.array([[P.u0], [P.v0], [P.w0]])
self.wind = np.array([[0], [0], [0]])
self.sensor_values = np.array([[0], [0], [0], [0], [0], [0], [0], [0]])
self.measured_gps = np.array([[0], [0], [0], [0], [0]])
self.kalman_attitude = self.eulerAttitude[0:2]
self.attitude_filter = AttitudeFilter(self.kalman_attitude)
self.kalman_loc = np.array([[0], [0], [P.u0], [0], [1], [1], [0]])
self.loc_filter = GPSFilter(self.kalman_loc)
self.sensor_LPF = SensorFilter(
np.array([[0.1], [0.1], [0.1], [0.81], [0.81], [0.81], [0.51],
[0.81]]))
def __init__(self, parent=None):
super(MAVModel, self).__init__(parent)
# QtCore.QThread.__init__(self)
# The current state of the UAV
# self.viewer = viewer
self.view_thread = Viewer()
# self.view_thread.draw_mav(self.Output())
self.view_thread.start()
self.state = np.array([
[P.Pn0], # (0)
[P.Pe0], # (1)
[P.Pd0], # (2)
[P.u0], # (3)
[P.v0], # (4)
[P.w0], # (5)
[P.e0], # (6)
[P.e1], # (7)
[P.e2], # (8)
[P.e3], # (9)
[P.p0], # (10)
[P.q0], # (11)
[P.r0]
]) # (12)
# The flight state of the UAV
self.flight_state = np.array([[P.Va0], [P.alpha0], [P.beta0],
[P.path_angle0], [P.heading0]])
# The forces that are applied to the body of the UAV
self.bodyForces = np.array([[P.Fx], [P.Fy], [P.Fz], [P.Ml], [P.Mm],
[P.Mn]])
# The state of the control surfaces
self.controlState = np.array([[P.deltaA0], [P.deltaE0], [P.deltaR0],
[P.deltaT0]])
# The commanded aircraft path following values
self.autopilot = np.array([[P.Va0], [P.altitude0], [P.heading0]])
# self.autopilot = np.matrix([[P.Va0],
# [P.altitude0],
# [3.14159]])
# The attitude of the UAV in euler angles (phi, theta, psi)
self.eulerAttitude = Quaternion2Euler(self.state[6:10])
self.euler_state = self.euler_states_array()
self.kinematics = Dynamics()
self.windsim = WindSimulation()
self.forces = Forces()
self.lat_control = lateral_control()
self.lon_control = longitudinal_control()
self.sensors = Sensors()
self.gps = Gps()
self.va = np.array([[P.u0], [P.v0], [P.w0]])
self.wind = np.array([[0], [0], [0]])
self.sensor_values = np.array([[0], [0], [0], [0], [0], [0], [0], [0]])
self.measured_gps = np.array([[0], [0], [0], [0], [0]])
self.kalman_attitude = self.eulerAttitude[0:2]
self.attitude_filter = AttitudeFilter(self.kalman_attitude)
self.kalman_loc = np.array([[0], [0], [P.u0], [0], [1], [1], [0]])
self.loc_filter = GPSFilter(self.kalman_loc)
self.sensor_LPF = SensorFilter(
np.array([[0.1], [0.1], [0.1], [0.81], [0.81], [0.81], [0.51],
[0.81]]))
self.signal_gen = Signals()
self.t_gps = 0.0
self.t_plot = 0.0
self.t_control = 0.0
self.t_elapse = 0.0
self.view_thread.init_plots(self.EulerStates(),
self.get_flight_state(),
np.linalg.norm(self.va), self.wind[0:2],
self.autopilot, self.sensor_values[3:8],
self.kalman_attitude, self.kalman_loc,
self.measured_gps)