diff --git a/mavion/__pycache__/ctlfun.cpython-311.pyc b/mavion/__pycache__/ctlfun.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..69fa0ba76d54ea451fc7acd6bf533fec8e04f48a
Binary files /dev/null and b/mavion/__pycache__/ctlfun.cpython-311.pyc differ
diff --git a/mavion/ctlfun.py b/mavion/ctlfun.py
index 2f78993e41a3741ed38160c72ac94071ef91e1b8..80f61a7d255902cab71ac5e77cd46436d6bdbf77 100644
--- a/mavion/ctlfun.py
+++ b/mavion/ctlfun.py
@@ -1,20 +1,14 @@
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)
diff --git a/mavion/mavion.py b/mavion/mavion.py
index 65326433f6b440945254606b0f3c974515a60895..992b76047012a5c3bc9d84d7d16c6dbc7a0a36cd 100644
--- a/mavion/mavion.py
+++ b/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()
diff --git a/mavion/mavion.yaml b/mavion/mavion.yaml
index a1abdd630a3c168970b367c4ea81c9924815df11..63ef25c774aa46d725b1d894e07d057ea883ecab 100644
--- a/mavion/mavion.yaml
+++ b/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