Mbranch

mavsim_python/chap2/draw_mav.py

@@ -91,9 +91,34 @@ def get_points(self):
 
         # points are in NED coordinates
         # define the points on the aircraft following diagram Fig 2.14
-        points = np.array([[0, 0, 0],  # point 1 [0]
-                           [1, 1, 1],  # point 2 [1]
-                           [1, 1, 0],  # point 3 [2]
+        points = np.array([[1,1,0],  # point 1 [0]
+                           [1,-1,0],  # point 2 [1]
+                           [-1,-1,0],  # point 3 [2]
+                           [-1,1,0],#point 4
+                           [1,1,0],#point 1
+                           [1,1,-2],#point 5
+                           [1,-1,-2],#point 6
+                           [1,-1,0],#point 2
+                           [1,-1,-2],#point 6
+                           [-1,-1,-2],#point 7
+                           [-1,-1,0],#point 3
+                           [-1,-1,-2],#point 7
+                           [-1,1,-2],#point 8
+                           [-1,1,0],#point 4
+                           [-1,1,-2],#point 8
+                           [1,1,-2],#point 5
+                           [1,1,0],#point 1
+                           [1.5,1.5,0],#point 9
+                           [1.5,-1.5,0],#point 10
+                           [1,-1,0],#point 2
+                           [1.5,-1.5, 0],#point 10
+                           [-1.5,-1.5,0],#point 11
+                           [-1,-1,0],#point 3
+                           [-1.5,-1.5, 0],#point 11
+                           [-1.5,1.5,0],#point 12
+                           [-1,1,0],#point 4
+                           [-1.5,1.5,0],#point 12
+                           [1.5,1.5,0]#point 9
                            ]).T
 
         # scale points for better rendering
@@ -117,6 +142,6 @@ def points_to_mesh(self, points):
           (a rectangle requires two triangular mesh faces)
         """
         points = points.T
-        mesh = np.array([[points[0], points[1], points[2]]])
+        mesh = np.array([[points[0], points[1], points[2], points[3],points[4],points[5],points[6],points[7],points[8],points[9],points[10],points[11],points[12],points[13],points[14],points[15],points[16],points[17],points[18],points[19],points[20],points[21],points[22],points[23],points[24],points[25],points[26],points[27]]])
 
         return mesh

mavsim_python/chap2/mav_viewer.py

@@ -18,7 +18,7 @@
 class MavViewer():
     def __init__(self):
         # initialize Qt gui application and window
-        self.app = pg.QtGui.QApplication([])  # initialize QT
+        self.app = pg.QtWidgets.QApplication([])  # initialize QT
         self.window = gl.GLViewWidget()  # initialize the view object
         self.window.setWindowTitle('MAV Viewer')
         self.window.setGeometry(0, 0, 1000, 1000)  # args: upper_left_x, upper_right_y, width, height

mavsim_python/chap3/mav_dynamics.py

@@ -101,46 +101,54 @@ def _derivatives(self, state, forces_moments):
         n = forces_moments.item(5)
 
         # position kinematics
+<<<<<<< HEAD
         pos_dot = np.array([
-            [e1**2+e0**2 - e2**2 - e3**2, 2*(e1*e2-e3*e0), 2*(e1*e3+e2*e0)],
-            [2*(e1*e2+e3*e0), e2**2+e0**2-e1**2-e3**2, 2*(e2*e3-e1*e0)],
-            [2*(e1*e3-e2*e0), 2*(e2*e3+e1*e0), e3**2+e0**2-e1**2-e2**2]
-        ])@np.array([
-            [u],[v],[w]
-        ])
+=======
+         pos_dot = np.array([
+>>>>>>> 4f7f17404f03a3319c2dde47cae0adb424f3567a
+            [(e1**2)+(e0**2) - (e2**2) - (e3**2), 2*((e1*e2)-(e3*e0)), 2*((e1*e3)+(e2*e0))],
+            [2*((e1*e2)+(e3*e0)), (e2**2)+(e0**2)-(e1**2)-(e3**2), 2*((e2*e3)-(e1*e0))],
+            [2*((e1*e3)-(e2*e0)), 2*((e2*e3)+(e1*e0)), (e3**2)+(e0**2)-(e1**2)-(e2**2)]
+        ])@np.array([[u],[v],[w]])
+<<<<<<< HEAD
+
+=======
+>>>>>>> 4f7f17404f03a3319c2dde47cae0adb424f3567a
         north_dot = pos_dot[0]
         east_dot = pos_dot[1]
         down_dot = pos_dot[2]
 
         # position dynamics
-        vel = np.array([[r*v - q*w],
-                          [p*w - r*u],
-                          [q*u - p*v]]) + (1/m)*np.array([[fx], [fy], [fz]])
+        vel = np.array([
+                          [(r*v) - (q*w)],
+                          [(p*w) - (r*u)],
+                          [(q*u) - (p*v)]
+                          ]) + (1/mass)*np.array([[fx], [fy], [fz]])
+
         u_dot = vel[0]
         v_dot = vel[1]
         w_dot = vel[2]
 
         # rotational kinematics
         e_vel = .5*np.array([
-            [0,-p,-q,-r],
-            [p,0,r,-q],
-            [q,-r, 0, p],
-            [r,q,-p,0]
+            [0.,-p,-q,-r],
+            [p,0.,r,-q],
+            [q,-r, 0., p],
+            [r,q,-p,0.]
         ])@np.array([
             [e0],
             [e1],
             [e2],
-            [e3]
-        ])
+            [e3]])
         e0_dot = e_vel[0]
         e1_dot = e_vel[1]
         e2_dot = e_vel[2]
         e3_dot = e_vel[3]
 
         # rotatonal dynamics
-        p_dot = 0
-        q_dot = 0
-        r_dot = 0
+        p_dot = r1*p*q-r2*q*r+r3*l+r4*n 
+        q_dot = r5*p*r-r6*(p**2-r**2)+m/jy
+        r_dot = r7*p*q-r1*q*r+r4*l+r8*n
 
         # collect the derivative of the states
         x_dot = np.array([[north_dot, east_dot, down_dot, u_dot, v_dot, w_dot,
@@ -158,4 +166,4 @@ def _update_true_state(self):
         self.true_state.psi = psi
         self.true_state.p = self._state.item(10)
         self.true_state.q = self._state.item(11)
-        self.true_state.r = self._state.item(12)
+        self.true_state.r = self._state.item(12)