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)