change plot (euler angles)

mavion/ctlfun.py

 
 import numpy as np
 
-def pulse(t, t0, tf):
+def upulse(t, t0, tf):
     u = 0 if (t<t0 or t>=tf) else 1
     return u
 
-def step(t, t0) :
+def ustep(t, t0) :
     u = 0 if t<t0 else 1
     return u
 
-def ramp(t, t0) :
+def uramp(t, t0) :
     u = 0 if t<t0 else (t - t0)
     return u
-
-time = np.arange(0, 20, 0.01)
-
-p = pulse(time, 1, 2)
-
-print(p)

mavion/mavion.py

@@ -5,6 +5,7 @@ from gymnasium.envs.classic_control import utils
 import yaml
 from scipy.optimize import fsolve
 from scipy.integrate import solve_ivp
+from ctlfun import *
 
 
 deg2rad = 2 * np.pi / 360
@@ -69,16 +70,8 @@ def eul2dcm(a):
                    
     return Rx @ Ry @ Rz
 
-
-def s2q(x):
-    quat = eul2quat(x[3:6])
-    x = np.concatenate([x[0:3], quat, x[6:12]])
-    return x
-
-def q2s(x):
-    eul = quat2eul(x[3:7])
-    x = np.concatenate([x[0:3], eul, x[7:13]])
-    return x
+s2q = lambda x : np.concatenate([x[0:3], eul2quat(x[3:6]), x[6:12]])
+q2s = lambda x : np.concatenate([x[0:3], quat2eul(x[3:7]), x[7:13]])
 
 
 class Mavion:
@@ -324,7 +317,7 @@ class Mavion:
         return sol.y.T[-1]
 
     def sim(self, t_span, x0, fctrl, fwind):
-        func = lambda t, s: self.dyn(s, fctrl(t, s, w), fwind(t, s))
+        func = lambda t, s: self.dyn(s, fctrl(t, s, fwind(t, s)), fwind(t, s))
         sol = solve_ivp(func, t_span, x0, method='RK45', max_step=0.01)
         return sol
 
@@ -390,7 +383,7 @@ class MavionEnv(gym.Env):
 if __name__ == "__main__":
     mavion = Mavion()
 
-    vh = 5
+    vh = 0
     vz = 0
     dx, de, theta = mavion.trim(vh, vz)
     theta = (theta + np.pi) % (2*np.pi) - np.pi
@@ -401,25 +394,43 @@ if __name__ == "__main__":
     vel = np.array([vh, 0, vz])
     rot = np.array([0, 0, 0])
     st = np.concatenate([pos, eul, vel, rot])
-    x = s2q(st)
+    x0 = s2q(st)
 
-    u = np.array([-dx, dx, de, de])
-    w = np.zeros(3)
+    def ctl(t, s, w):
+        d = upulse(t, 5, 6)*dx*0.0
+        u = np.array([-dx-d, dx+d, de, de])
+        return u
+
+    def wnd(t, s):
+        w = np.zeros(3)
+        return w 
 
-    def ctl(t, s, w):   return u
-    def wnd(t, s):      return w 
+    sim = mavion.sim((0, 60), x0, ctl, wnd)
 
-    sim = mavion.sim((0, 20), x, ctl, wnd)
+    plt.style.use('_mpl-gallery')
+    fig, axs = plt.subplots(2, 2)
 
-    fig, axs = plt.subplots(2, 2, layout='constrained')
     ax = axs[0][0]
-    ax.plot(sim.t, sim.y[0:3].T)
+    ax.plot(sim.t, sim.y[0:3].T, label=('x','y','z'))
+    ax.legend()
+    ax.set_title(label='positions')
+
     ax = axs[1][0]
-    ax.plot(sim.t, sim.y[3:7].T)
+    quat = sim.y[3:7].T
+    eul = np.apply_along_axis(quat2eul, 1, quat)
+    ax.plot(sim.t, eul*57.3, label=('phi','theta','psi'))
+    ax.legend()
+    ax.set_title(label='euler angles')
+
     ax = axs[0][1]
-    ax.plot(sim.t, sim.y[7:10].T)
+    ax.plot(sim.t, sim.y[7:10].T, label=('u','v','w'))
+    ax.legend()
+    ax.set_title(label='vitesses')
+
     ax = axs[1][1]
-    ax.plot(sim.t, sim.y[10:13].T)
+    ax.plot(sim.t, sim.y[10:13].T, label=('p','q','r'))
+    ax.legend()
+    ax.set_title(label='rotations')
     plt.show()
 
 

mavion/mavion.yaml

@@ -19,6 +19,6 @@ DRY_SURFACE : 0.0
 PHI_n : 0.0
 PROP_RADIUS : 0.105
 ELEVON_MEFFICIENCY : [0, 0.66, 0] 
-ELEVON_FEFFICIENCY : [0, 0.33,0] 
+ELEVON_FEFFICIENCY : [0, 0.33, 0] 
 PROP_KP : 7.84e-06 
 PROP_KM : 2.400e-7