Add Autopilot. Update SimConsole. Add new examples.

.gitignore

@@ -1,6 +1,8 @@
 __pycache__/
 *.py[cod]
 .pytest_cache/
+
 htmlcov/
+
 logs/
 *.log

example.py

@@ -10,33 +10,38 @@
 import numpy as np
 
 from simulator.aircraft import AircraftDynamics, load_airframe_parameters_from_yaml
+from simulator.autopilot import Autopilot
 from simulator.cli import SimConsole
 from simulator.gui import AttitudePositionPanel
-from simulator.utils import wait_animation
 
 params_file = r"config/aerosonde_parameters.yaml"
 aerosonde_params = load_airframe_parameters_from_yaml(params_file)
 
 dt = 0.01
 uav = AircraftDynamics(dt, aerosonde_params, use_quat=True)
-uav.trim(25.0, np.deg2rad(10.0), 500, update=True)
-wait_animation(10.0)  # wait 10 seconds to visualize trim vars
+uav.trim(25.0, np.deg2rad(10.0), 500, update=True, verbose=False)
+
+ap = Autopilot(dt, aerosonde_params, uav.state)
 
 cli = SimConsole()
 gui = AttitudePositionPanel(use_blit=True, pos_3d=True)
 
-sim_time = 0.0  # simulation time
-sim_iter = 0  # simulation steps
-real_t0 = time.time()
+t_sim = 0.0  # simulation time
+k_sim = 0  # simulation steps
+t0 = time.time()
 while True:
-    sim_time += dt
-    sim_iter += 1
+    t_sim += dt
+    k_sim += 1
 
-    uav.update()  # update simulation states
+    ap.control_course_altitude(course_target=np.deg2rad(180.0), altitude_target=1e3)
+    uav.update(ap.control_deltas)  # update simulation states
 
-    gui.add_data(uav.state, sim_time)
-
-    if sim_iter % 10 == 0:  # update interface each 10 steps
-        real_time = time.time() - real_t0
-        cli.print_state(sim_time, real_time, uav.state)
-        gui.update(uav.state, pause=0.01)
+    gui.add_data(time=t_sim, state=uav.state)
+
+    if k_sim % 10 == 0:  # update interface each 10 steps
+        t_real = time.time() - t0
+        cli.print_time(t_sim, t_real, dt, k_sim)
+        cli.print_aircraft_state(uav.state)
+        cli.print_control_deltas(uav.control_deltas)
+        cli.print_autopilot_status(ap.status)
+        gui.update(state=uav.state, pause=0.01)

examples/example_aircraft_dynamics.py

@@ -35,6 +35,6 @@
     state = uav.kinematics_dynamics(uav.state.x, u)
     uav.state.update(state)
 
-    cli.print_state(t, uav.state, style="table")
+    cli.print_aircraft_state(t, uav.state, style="table")
 
     gui.update(uav.state.x, pause=0.01)

examples/example_autopilot.py

@@ -0,0 +1,78 @@
+"""
+ Copyright (c) 2024 Pablo Ramirez Escudero
+ 
+ This software is released under the MIT License.
+ https://opensource.org/licenses/MIT
+"""
+
+import time
+
+import numpy as np
+from scipy import signal
+
+from simulator.aircraft import (
+    AircraftDynamics,
+    ControlDeltas,
+    load_airframe_parameters_from_yaml,
+)
+from simulator.autopilot import Autopilot
+from simulator.cli import SimConsole
+from simulator.gui import AttitudePositionPanel, FlightControlPanel
+from simulator.utils import wait_animation
+
+params_file = r"config/aerosonde_parameters.yaml"
+aerosonde_params = load_airframe_parameters_from_yaml(params_file)
+
+dt = 0.01
+x0 = np.array(
+    [
+        0.0,  # pd
+        0.0,  # pe
+        0.0,  # pn
+        25.0,  # u
+        0.0,  # v
+        0.0,  # w
+        1.0,  # q0
+        0.0,  # q1
+        0.0,  # q2
+        0.0,  # q3
+        0.0,  # p
+        0.0,  # q
+        0.0,  # r
+    ]
+)
+uav = AircraftDynamics(dt, aerosonde_params, use_quat=True, x0=x0)
+x_trim, delta_trim = uav.trim(Va=25.0)
+
+autopilot = Autopilot(dt, aerosonde_params, uav.state)
+
+cli = SimConsole()
+gui = AttitudePositionPanel(use_blit=True, pos_3d=True)
+# gui = FlightControlPanel(use_blit=True)
+
+t_sim = 0.0  # simulation time
+k_sim = 0  # simulation steps
+t0 = time.time()
+while True:
+    t_sim += dt
+    k_sim += 1
+
+    roll_cmd = np.deg2rad(0.0) * signal.square(2*np.pi*0.5*t_sim)
+    pitch_cmd = np.deg2rad(0.0) * signal.sawtooth(2*np.pi*0.1*t_sim, width=0.5)
+    Va_cmd = 25.0 + 10.0 * signal.sawtooth(2*np.pi*0.05*t_sim, width=0.5)
+    h_cmd = 0.0 * signal.sawtooth(2*np.pi*0.01*t_sim, width=0.5)
+    X_cmd = np.deg2rad(90.0) * signal.square(2*np.pi*0.01*t_sim)
+    # autopilot.control_pitch_roll(roll_target=roll_cmd, pitch_target=pitch_cmd, airspeed_target=Va_cmd)
+    autopilot.control_course_altitude(altitude_target=h_cmd, course_target=X_cmd, airspeed_target=25.0)
+
+    uav.update(autopilot.control_deltas)  # update simulation states
+
+    gui.add_data(time=t_sim, state=uav.state, autopilot_status=autopilot.status)
+
+    if k_sim % 10 == 0:  # update interface each 10 steps
+        t_real = time.time() - t0
+        cli.print_time(t_sim, t_real, dt, k_sim, style='simple')
+        cli.print_aircraft_state(uav.state, style='simple')
+        cli.print_control_deltas(uav.control_deltas, style='simple')
+        cli.print_autopilot_status(autopilot.status, style='simple')
+        gui.update(state=uav.state, pause=0.01)

examples/example_fugoid.py

@@ -53,5 +53,5 @@
 
     if sim_iter % 10 == 0:  # update interface each 10 steps
         real_time = time.time() - real_t0
-        cli.print_state(sim_time, real_time, uav.state)
+        cli.print_aircraft_state(sim_time, real_time, uav.state)
         gui.update(uav.state, pause=0.01)

examples/example_main_panel.py

@@ -37,5 +37,5 @@
 
     if sim_step % 10 == 0:
         t_real = time.time() - t0
-        cli.print_state(t_sim, t_real, uav.state)
+        cli.print_aircraft_state(t_sim, t_real, uav.state)
         gui.update(uav.state, pause=0.01)

examples/example_quat_view.py

@@ -36,5 +36,5 @@
     x = np.array([north, east, down, 0, 0, 0, q[0], q[1], q[2], q[3], 0, 0, 0])
     state = AircraftState(x, use_quat=True)
 
-    cli.print_state(t, state)
+    cli.print_aircraft_state(t, state)
     gui.update(state, pause=0.01)

examples/example_sim_visualization.py

@@ -31,5 +31,5 @@
     x = np.array([north, east, down, 0, 0, 0, roll, pitch, yaw, 0, 0, 0])
     state = AircraftState(x)
 
-    cli.print_state(t, state)
+    cli.print_aircraft_state(t, state)
     gui.update(x, pause=0.01)

requirements.txt

@@ -2,6 +2,6 @@ matplotlib==3.8.4
 numpy==2.1.0
 pytest==8.2.2
 PyYAML==6.0.2
-rich==13.7.1
+rich==13.8.0
 scipy==1.14.1
 toml==0.10.2

simulator/aircraft/aerodynamic_model.py

@@ -15,7 +15,8 @@
 class AerodynamicModel:
 
     def __init__(self, params: AirframeParameters) -> None:
-        """Initialize the ForcesMoments class.
+        """
+        Initialize the ForcesMoments class.
 
         Parameters
         ----------
@@ -27,7 +28,8 @@ def __init__(self, params: AirframeParameters) -> None:
     def calculate_forces_moments(
         self, state: AircraftState, deltas: ControlDeltas
     ) -> np.ndarray:
-        """Calcuate aerodynamic forces and moments acting on the aircraft.
+        """
+        Calcuate aerodynamic forces and moments acting on the aircraft.
 
         Parameters
         ----------
@@ -67,7 +69,8 @@ def calculate_forces_moments(
         return u
 
     def lift_coefficient_vs_alpha(self, alpha: float, model: str = "accurate") -> float:
-        """Calculate the lift coefficient as a function of angle of attack.
+        """
+        Calculate the lift coefficient as a function of angle of attack.
 
         Parameters
         ----------
@@ -105,7 +108,8 @@ def lift_coefficient_vs_alpha(self, alpha: float, model: str = "accurate") -> fl
         return CL_vs_alpha
 
     def lift_force(self, state: AircraftState, deltas: ControlDeltas) -> float:
-        """Calculate the lift force acting on the aircraft.
+        """
+        Calculate the lift force acting on the aircraft.
 
         Parameters
         ----------
@@ -134,7 +138,8 @@ def lift_force(self, state: AircraftState, deltas: ControlDeltas) -> float:
     def drag_coefficient_vs_alpha(
         self, alpha: float, model: str = "quadratic"
     ) -> float:
-        """Calculate the drag coefficient as a function of angle of attack
+        """
+        Calculate the drag coefficient as a function of angle of attack
 
         Parameters
         ----------
@@ -166,7 +171,8 @@ def drag_coefficient_vs_alpha(
         return CD_vs_alpha
 
     def drag_force(self, state: AircraftState, deltas: ControlDeltas) -> float:
-        """Calculate the drag force acting on the aircraft.
+        """
+        Calculate the drag force acting on the aircraft.
 
         Parameters
         ----------
@@ -193,7 +199,8 @@ def drag_force(self, state: AircraftState, deltas: ControlDeltas) -> float:
         return F_drag
 
     def pitch_moment(self, state: AircraftState, deltas: ControlDeltas) -> float:
-        """Calculate the pitching moment acting on the aircraft.
+        """
+        Calculate the pitching moment acting on the aircraft.
 
         Parameters
         ----------
@@ -221,7 +228,8 @@ def pitch_moment(self, state: AircraftState, deltas: ControlDeltas) -> float:
         return m
 
     def lateral_force(self, state: AircraftState, deltas: ControlDeltas) -> float:
-        """Calculate the lateral force acting on the aircraft.
+        """
+        Calculate the lateral force acting on the aircraft.
 
         Parameters
         ----------
@@ -250,7 +258,8 @@ def lateral_force(self, state: AircraftState, deltas: ControlDeltas) -> float:
         return fy
 
     def roll_moment(self, state: AircraftState, deltas: ControlDeltas) -> float:
-        """Calculate the roll moment acting on the aircraft.
+        """
+        Calculate the roll moment acting on the aircraft.
 
         Parameters
         ----------

simulator/aircraft/aircraft_dynamics.py

@@ -35,7 +35,8 @@ def __init__(
         x0: np.ndarray = None,
         delta0: np.ndarray = None,
     ) -> None:
-        """Initialize the AircraftDynamics class.
+        """
+        Initialize the AircraftDynamics class.
 
         Parameters
         ----------
@@ -67,7 +68,7 @@ def __init__(
 
         self.params = params
         self.control_deltas = ControlDeltas(delta0)
-        self.state = AircraftState(x0, wind0, use_quat, self.control_deltas)
+        self.state = AircraftState(x0, wind0, use_quat)
         self.aerodynamics = AerodynamicModel(params)
         self.propulsion = PropulsionModel(params)
 
@@ -78,7 +79,8 @@ def set_control_deltas(self, delta: np.ndarray) -> None:
         self.control_deltas.update(delta)
 
     def update(self, deltas: ControlDeltas = None) -> AircraftState:
-        """Update aircraft's state simulating the flight dynamics.
+        """
+        Update aircraft's state simulating the flight dynamics.
         Firstly, forces and moments are calculated using gravity, aerodynamics and propulsion models.
         Then, numeric integration of the kinematic and dynamic equations provide the new updated state.
 
@@ -105,7 +107,8 @@ def update(self, deltas: ControlDeltas = None) -> AircraftState:
         self.state.update(x, x_dot)
 
     def forces_moments(self, state: AircraftState, deltas: ControlDeltas) -> np.ndarray:
-        """Calcuate external forces and moments acting on the aircraft due to gravity, aerodynamics and propulsion.
+        """
+        Calcuate external forces and moments acting on the aircraft due to gravity, aerodynamics and propulsion.
 
         Parameters
         ----------
@@ -133,7 +136,8 @@ def forces_moments(self, state: AircraftState, deltas: ControlDeltas) -> np.ndar
         return ug + ua + up
 
     def kinematics_dynamics(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
-        """Integrate the kinematics and dynamics equations to calculate the new state.
+        """
+        Integrate the kinematics and dynamics equations to calculate the new state.
 
         Parameters
         ----------
@@ -186,7 +190,8 @@ def kinematics_dynamics(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
         return x2
 
     def state_derivatives(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
-        """State transition or dynamics function for numeric integration: dx/dt = f(x, u)
+        """
+        State transition or dynamics function for numeric integration: dx/dt = f(x, u)
 
         Parameters
         ----------
@@ -235,7 +240,8 @@ def state_derivatives(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
             return self._state_derivatives_euler(x, u)
 
     def _state_derivatives_quat(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
-        """State transition function for quaternion representation of attitude.
+        """
+        State transition function for quaternion representation of attitude.
 
         Parameters
         ----------
@@ -282,7 +288,8 @@ def _state_derivatives_quat(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
         return x_dot
 
     def _state_derivatives_euler(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
-        """State transition function for euler angles representation of attitude.
+        """
+        State transition function for euler angles representation of attitude.
 
         Parameters
         ----------
@@ -331,24 +338,26 @@ def _state_derivatives_euler(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
     def trim(
         self,
         Va: float,
-        gamma: float,
-        R_orb: float,
+        gamma: float = 0.0,
+        R_orb: float = np.inf,
         update: bool = True,
         verbose: bool = True,
     ) -> tuple[np.ndarray, np.ndarray]:
-        """Calculate the trimmed states and deltas for the trim conditions,
+        """
+        Calculate the trimmed states and deltas for the trim conditions,
         such that the aircraft maintains a steady flight.
 
-        If `update` is True, the internal `state` and `deltas` of the aircraft are updated with the computed trim values.
+        If `update` is True, the internal `state` and `deltas` of the aircraft are updated
+        with the computed trim values.
 
         Parameters
         ----------
         Va : float
             The trim airspeed value in m/s
-        gamma : float
-            The trim path angle in radians
-        R_orb : float
-            The trim orbit radius in meters
+        gamma : float, optional
+            The trim path angle in radians, by default 0.0
+        R_orb : float, optional
+            The trim orbit radius in meters, by default np.inf
         update : bool, optional
             To update internal state and deltas with calculated trim,
             by default True
@@ -416,7 +425,7 @@ def cons_ineq_u(v: np.ndarray) -> np.ndarray:
                 {"type": "eq", "fun": cons_eq_x},
                 {"type": "ineq", "fun": cons_ineq_u},
             ],
-            options={"maxiter": 1000, "disp": True},
+            options={"maxiter": 1000, "disp": verbose},
         )
         state_trim = AircraftState(result.x[0:12], use_quat=False)
         deltas_trim = ControlDeltas(result.x[12:16])